From 3c1655957851abc13c860191a620875019ad678d Mon Sep 17 00:00:00 2001 From: AIR Date: Sun, 25 Jun 2023 18:48:22 +0800 Subject: [PATCH] =?UTF-8?q?mavlink=20=E5=8F=96=E6=B6=88=E6=8C=87=E9=92=88?= =?UTF-8?q?=E5=8F=82=E6=95=B0=20=E5=9C=A8comm=5Freceive=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E5=86=85=E9=83=A8=E5=A3=B0=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- FoodDelivery.ino | 58 ++++++++++++++++++++++++++---------------------- 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/FoodDelivery.ino b/FoodDelivery.ino index e5d27be..2027da9 100644 --- a/FoodDelivery.ino +++ b/FoodDelivery.ino @@ -58,7 +58,6 @@ void loop() { fc.comm_receive(mavlink_receiveCallback); /*保持mqtt心跳*/ fc.mqttLoop(topicSub, topicSubCount); - delay(50); } /** @@ -164,24 +163,27 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { DynamicJsonDocument doc(0x2FFF); deserializeJson(doc, topicStr); JsonObject obj = doc.as(); + int8_t item=obj["item"]; + int8_t val=obj["val"]; + int8_t pitch=obj["pitch"]; + int8_t yaw=obj["yaw"]; //相机控制 - if (obj["item"] = "0") { //0:一键回中 + if (item == 0) { //0:一键回中 uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; size_t len = sizeof(command); - } else if (obj["item"] = "1") { //1:变焦 - uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x0 }; - command[8] = (uint16_t)obj["val"].toInt(); + fc.udpSendToCamera(command, len); + } else if (item == 1) { //1:变焦 + uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; + command[8] = val; size_t len = sizeof(command); - } else if (obj["item"] = "2") { //2:俯仰 + fc.udpSendToCamera(command, len); + } else if (item == 2) { //2:俯仰 旋转 uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; - command[9] = (uint16_t)obj["val"].toInt(); - size_t len = sizeof(command); - }else if (obj["item"] = "3") { //2:旋转 - uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; - command[8] = (uint16_t)obj["val"].toInt(); + command[9] = pitch; + command[8] = yaw; size_t len = sizeof(command); + fc.udpSendToCamera(command, len); } - fc.udpSendToCamera(command, len); } } @@ -258,16 +260,18 @@ void writeRoute(String topicStr) { * @param {mavlink_status_t*} pStatus * @param {uint8_t} c 串口读取的缓存 */ -void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) { +void mavlink_receiveCallback(uint8_t c) { + mavlink_message_t msg; + mavlink_status_t status; // 尝试从数据流里 解析数据 - if (mavlink_parse_char(MAVLINK_COMM_0, c, pMsg, pStatus)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 // 通过msgid来判断 数据流的类别 char buf[10]; - switch (pMsg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 { mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 - mavlink_msg_heartbeat_decode(pMsg, &heartbeat); // 解构msg数据 + mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据 sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态 topicPubMsg[0] = buf; //心跳主题 //从心跳里判断 飞机是否解锁 解锁改变飞机状态 @@ -335,7 +339,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS { mavlink_sys_status_t sys_status; // 解构的数据放到这个对象 - mavlink_msg_sys_status_decode(pMsg, &sys_status); // 解构msg数据 + mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据 // 电压 sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000); if (topicPubMsg[1] != buf) { // 有更新 则更新数据 @@ -357,21 +361,21 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE { mavlink_param_value_t param_value; - mavlink_msg_param_value_decode(pMsg, ¶m_value); + mavlink_msg_param_value_decode(&msg, ¶m_value); } break; case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU { mavlink_raw_imu_t raw_imu; - mavlink_msg_raw_imu_decode(pMsg, &raw_imu); + mavlink_msg_raw_imu_decode(&msg, &raw_imu); } break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置 { mavlink_global_position_int_t global_position_int; - mavlink_msg_global_position_int_decode(pMsg, &global_position_int); + mavlink_msg_global_position_int_decode(&msg, &global_position_int); // 高度 sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); if (topicPubMsg[4] != buf) { @@ -383,7 +387,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 { mavlink_vfr_hud_t vfr_hud; - mavlink_msg_vfr_hud_decode(pMsg, &vfr_hud); + mavlink_msg_vfr_hud_decode(&msg, &vfr_hud); // 对地速度 ps:飞行速度 sprintf(buf, "%.2f", vfr_hud.groundspeed); if (topicPubMsg[5] != buf) { @@ -395,7 +399,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS { mavlink_gps_raw_int_t gps_raw_int; - mavlink_msg_gps_raw_int_decode(pMsg, &gps_raw_int); + mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int); // 卫星数 sprintf(buf, "%d", gps_raw_int.satellites_visible); if (topicPubMsg[6] != buf) { @@ -455,10 +459,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 { mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 - mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据 + mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 //日志 fc.logln("MsgID:"); - fc.logln(pMsg->msgid); + fc.logln(msg.msgid); fc.logln("No:"); fc.logln(mission_request.seq); //飞控 反馈当前要写入航点的序号 @@ -469,10 +473,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 { mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 - mavlink_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据 + mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据 /*日志*/ fc.logln("MsgID:"); - fc.logln(pMsg->msgid); + fc.logln(msg.msgid); fc.logln("re:"); fc.logln(mission_ark.type); /*记录航点的写入状态 */ @@ -519,7 +523,7 @@ void pubThread() { * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 */ void flashThread() { - if (digitalRead(23) == HIGH) { + if (digitalRead(23) == LOW) { if (isPush) { //点击之后 //请求注册 ps:发送esp8266的物理地址 到对频主题 fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());