diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 6107d38..557db7c 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -559,14 +559,30 @@ void FoodCube::mav_parameter_data(const char *param_id) /** * @description: 向飞控请求返航点的位置数据 */ -void FoodCube::mav_request_homePosition() +void FoodCube::mav_request_home_position() { mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - // 构造命令 MAV_CMD_GET_HOME_POSITION 请求 - mavlink_msg_command_long_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0); - uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + // 组装命令 + mavlink_command_long_t command = {0}; + command.target_system = 1; // 目标飞控的系统ID + command.target_component = 1; // 目标飞控的组件ID + command.command = MAV_CMD_GET_HOME_POSITION; + command.confirmation = 0; + command.param1 = 0; // 参数保留，不使用 + command.param2 = 0; + command.param3 = 0; + command.param4 = 0; + command.param5 = 0; + command.param6 = 0; + command.param7 = 0; + + // 打包命令消息 + mavlink_msg_command_long_encode(1, 1, &msg, &command); + + // 将消息转换为字节流并发送 + int len = mavlink_msg_to_send_buffer(buf, &msg); SWrite(buf, len, mavlinkSerial); } diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h index eb0552a..49f8c28 100644 --- a/src/FoodDeliveryBase.h +++ b/src/FoodDeliveryBase.h @@ -64,7 +64,7 @@ public: String setNBit(String str, uint8_t n, uint8_t i); void mav_request_data(); void mav_parameter_data(const char *param_id); - void mav_request_homePosition(); + void mav_request_home_position(); void comm_receive(void (*pFun)(uint8_t)); void mav_mission_count(uint8_t taskcount); void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z); diff --git a/src/commser.cpp b/src/commser.cpp index 453f389..d3d9a94 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -34,8 +34,8 @@ uint32_t udpServerPort = 37260; // 云台相机端口 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象 /* 发布 主题 ps:以下是登记发布json内容的组成元素 */ // 登记 json成员名字 -// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度} 16:电池总容量 17:返航点 -const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePorsion"}; +// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度} 16:电池总容量 17:返航点 18:文本信息 +const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePorsion", "statusText"}; const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数 String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub /*触发发送 主题*/ @@ -383,6 +383,7 @@ void mavlink_receiveCallback(uint8_t c) } curr_armed = true; topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭 } else { @@ -392,7 +393,8 @@ void mavlink_receiveCallback(uint8_t c) fc.playText("[v1]已加锁"); } curr_armed = false; // 心跳里面 判断没有解锁 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为未解锁状态 0xxxx 第四位为0 代表未解锁 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭 if ((uint8_t)topicPubMsg[10].toInt() & 4) { // 如果已经写入了航点 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态 @@ -516,24 +518,39 @@ void mavlink_receiveCallback(uint8_t c) } break; - case MAVLINK_MSG_ID_HOME_POSITION: // #242 返航点位置 + case 242: // #242 返航点位置MAVLINK_MSG_ID_HOME_POSITION { fc.pubMQTTmsg("demo", "homep"); - mavlink_home_position_t home_position; - mavlink_msg_home_position_decode(&msg, &home_position); + // mavlink_home_position_t home_position; + // mavlink_msg_home_position_decode(&msg, &home_position); - // 提取经纬度并转换为度数 - double latitude = home_position.latitude / 1e7; - double longitude = home_position.longitude / 1e7; + // // 提取经纬度并转换为度数 + // double latitude = home_position.latitude / 1e7; + // double longitude = home_position.longitude / 1e7; - // 格式化为 JSON 字符串，包含经纬度 - sprintf(buf, "{\"lng\":%.7f,\"lat\":%.7f}", - longitude, latitude); + // // 格式化为 JSON 字符串，包含经纬度 + // sprintf(buf, "{\"lng\":%.7f,\"lat\":%.7f}", + // longitude, latitude); - // 更新 topicPubMsg 中的相关字段 - if (topicPubMsg[17] != buf) + // // 更新 topicPubMsg 中的相关字段 + // if (topicPubMsg[17] != buf) + // { + // topicPubMsg[17] = buf; + // } + } + break; + + case MAVLINK_MSG_ID_STATUSTEXT: // #253 文本信息 + { + char buf[51]; // 定义一个局部缓冲区，大小为 51 + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(&msg, &statustext); + // 解析文本信息 + strncpy(buf, statustext.text, sizeof(statustext.text)); + buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾 + if (topicPubMsg[18] != buf) { - topicPubMsg[17] = buf; + topicPubMsg[18] = buf; } } break; @@ -730,6 +747,7 @@ void pubThread() { doc[topicPub[i]] = topicPubMsg[i]; } + // fc.mav_request_home_position(); // 请求 飞控返航点 // 将JSON对象序列化为JSON字符串 String jsonString; serializeJson(doc, jsonString); @@ -743,12 +761,13 @@ void pubThread() */ void refreshRequest() { - fc.mav_request_data(); // 请求 设定飞控输出数据流内容 + fc.mav_request_home_position(); delay(100); - fc.mav_request_homePosition(); // 请求 飞控返航点 + fc.mav_request_data(); // 请求 设定飞控输出数据流内容 delay(100); fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值 delay(100); + fc.pubMQTTmsg("demo", "init"); } /**