#include "commser.h" #include "motocontrol.h" extern void Hook_stop(); extern void up_action(float motospeed); extern Motocontrol motocontrol; extern void Hook_resume(); extern void begin_tare(); extern bool curr_armed; extern uint8_t curr_mode; extern Initstatus initstatus; extern void Hook_autodown(float length_cm); extern void Hook_recovery(); static const char *MOUDLENAME = "COMMSER"; /*项目对象*/ // char *ssid = "szMate40pro"; // wifi帐号 // char *password = "63587839ab"; // wifi密码 // char* ssid = "szdot"; //wifi帐号 // char* password = "63587839ab"; //wifi密码 // char *ssid = "flicube"; // wifi帐号 // char *password = "fxmf0622"; // wifi密码 char *ssid = "fxmf_sc02"; // 4g wifi帐号 char *password = "12345678"; // 4g wifi密码 char *mqttServer = "wxsky.com"; // mqtt地址 int mqttPort = 1883; // mqtt端口 char *mqttName = "admin"; // mqtt帐号 char *mqttPassword = "123456"; // mqtt密码 uint8_t mavlinkSerial = 2; // 飞控占用的串口号 uint8_t voiceSerial = 1; // 声音模块占用串口 char *udpServerIP = "192.168.3.92"; // 云台相机ip 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:返航点 18:文本信息 19：返航速度 20:{参数名：值} const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "fixType", "completionPct", "reportCal", "questState", "acceState", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed", "parameterValue"}; /* *其中topicPubMsg[10]既飞机任务状态的值 *二进制00 0000 *第0位：初始状态 &1 *第1位：正在写入航线 &2 *第2位：已经写入航点 &4 *第3位：正在解锁 &8 *第4位：解锁成功 &16 *第5位：执行航点任务 &32 */ const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数 String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值对应topicPub /*触发发送主题*/ // 0:对频信息 String topicHandle[] = {"crosFrequency"}; boolean isPush = false; // 记得删除板子按钮状态 ps:D3引脚下拉微动开关 /*异步线程对象*/ Ticker pubTicker; // 定时发布主题线程 Ticker mavTicker; // 定时指定飞控mavlink 串口返回的数据类别防止飞控启动滞后 // Ticker chanTicker; //定时向飞控发送油门指定 /** * @description: mqtt订阅主题收到信息的回调函数 * @param {char*} topic 主题名称 msg/macadd * @param {byte*} topic 订阅获取的内容 * @param {unsigned int} length 内容的长度 */ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) { /*解构mqtt发过来的内容*/ String jsonStr = ""; for (int i = 0; i < length; i++) { jsonStr += (char)payload[i]; } /*解构内容*/ DynamicJsonDocument doc(0x2FFF); // 创建 JSON 文档 deserializeJson(doc, jsonStr); // 解析 JSON 数据 // 遍历 JSON 对象 String key; // 键 JsonVariant value; // 值 for (JsonPair kv : doc.as()) { key = kv.key().c_str(); // 获取键 value = kv.value(); // 获取值取值时还需指定类型 } /*根据订阅内容键值做具体操作*/ if (key == "questAss") { // 飞行航点任务 questAss String todo = value; // 转换值 writeRoute(todo); // 写入航点 } else if (key == "setQuestState") { // 设置飞机任务状态 String todoJson = value; // 转换值 /* json 反序列化 */ DynamicJsonDocument doc(50); deserializeJson(doc, todoJson); JsonObject obj = doc.as(); uint8_t n = obj["bit"]; // 状态位数 uint8_t state = obj["state"]; // 标记飞机状态 0 or 1 // 标记飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); if (n = 5 && state == 1) // 执行任务 ps:切自动 { // 切模式 set_mode(AUTO); // 设置飞控为自动模式 // 油门 delay(500); uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁油门到底 fc.mav_channels_override(chan); // 控制油门 } } else if (key == "unlock") // 解锁 { set_mode(ALT_HOLD); // 设置飞控为定高模式 delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效 lock_or_unlock(false); // 解锁 } else if (key == "lock") // 加锁 { // 油门 uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁油门到底 fc.mav_channels_override(chan); // 控制油门 // 加锁 lock_or_unlock(true); delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效 set_mode(ALT_HOLD); // 设置飞控为定高模式 } else if (key == "guidedMode") // 指点 { // 设置飞控为 Guided 模式 set_mode(GUIDED); // 可以根据需要调整延迟时间，确保模式切换生效 delay(2000); // JSON 反序列化取经纬高 String todoJson = value; // 转换值 DynamicJsonDocument doc(512); // 适当增加缓冲区大小 deserializeJson(doc, todoJson); JsonObject obj = doc.as(); double alt = obj["alt"].as(); // 传过来的高度值 double lat = 0; double lon = 0; if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度 { lat = obj["lat"].as(); lon = obj["lon"].as(); } // 飞至指定位置 fly_to_localtion(lat, lon, alt); } else if (key == "autoMode") // 自动auto模式 { // 切模式 set_mode(AUTO); // 设置飞控为定高模式 // 油门 delay(500); uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁油门到底 fc.mav_channels_override(chan); // 控制油门 } else if (key == "loiterMode") // 悬停定点loiter模式 { // 油门 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁油门到底 fc.mav_channels_override(chan); // 控制油门 delay(500); // 切模式 set_mode(LOITER); // 设置飞控为定高模式 // 油门 delay(500); fc.mav_channels_override(chan); // 控制油门 } else if (key == "holdAltMode") // 定高 { // 油门 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁油门到底 fc.mav_channels_override(chan); // 控制油门 delay(500); // 切模式 set_mode(ALT_HOLD); // 油门 delay(500); fc.mav_channels_override(chan); // 控制油门 } else if (key == "landMode") // 降落 { set_mode(LAND); // 切模式 } else if (key == "rtlMode") // 返航 { set_mode(RTL); // 切模式 } else if (key == "resetCompass") // 校准磁罗盘 { resetCompass(); } else if (key == "initAcce") { uint32_t todo = value.as(); // 转换值 if (todo == 1) { initAcce(); // 发起校准 } else { fc.sendCommandAck(1, 1); // 摆好校准 } } else if (key == "setParam") { String todoJson = value; // 转换值 /* json 反序列化 */ DynamicJsonDocument doc(128); deserializeJson(doc, todoJson); JsonObject obj = doc.as(); // 提取参数 const char *item = obj["item"]; // 获取 item 字段 float paramValue = obj["value"]; // 获取 value 字段 // 调用 setParam 函数 fc.setParam(item, paramValue); } else if (key == "getParam") { String todo = value; // 转换值 fc.mav_parameter_data(todo.c_str()); // 获取指定参数值 } else if (key == "refreshRequest") { refreshRequest(); // 刷新各种请求 } else if (key == "resetQuestState") { // 恢复飞机为初始状态 String todo = value; // 转换值 topicPubMsg[10] = todo; // 恢复初始状态 } else if (key == "hookConteroller") { // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 uint16_t todo = value; switch (todo) { case 0: { // 收钩 ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP"); if (motocontrol.getcontrolstatus().is_autogoodsdown) up_action(SPEED_BTN_FAST); } break; case 1: { // 放钩 } break; case 2: { // 暂停 ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_stop(); } break; case 3: { // 继续 ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_resume(); } break; case 4: { // 重置重量 if (motocontrol.gethooktatus() == HS_Stop) // 先判断钩子状态 “已停止” begin_tare(); else fc.playText("未能校准，请保持挂钩，空钩并且没有入仓的状态。再尝试"); } break; } } else if (key == "cameraController") { // 云台相机控制 String todoJson = value; // 转换值 /* json 反序列化 */ DynamicJsonDocument doc(500); deserializeJson(doc, todoJson); 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 (item == 0) { // 0:一键回中 uint8_t stopCommand[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; // 回中前强制俯仰旋转停止 size_t stopLen = sizeof(stopCommand); fc.udpSendToCamera(stopCommand, stopLen); uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}; size_t len = sizeof(command); 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); fc.udpSendToCamera(command, len); } else if (item == 2) { // 2:俯仰旋转 uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; command[8] = yaw; command[9] = pitch; size_t len = sizeof(command); fc.udpSendToCamera(command, len); } } } /** * @description: 写入航点 * @param {String} todo mqtt订阅执行任务的Json字符串 */ void writeRoute(String todoJson) { if (fc.writeState) // 如果正在写入状态跳出 { fc.logln("正在写航点"); // 提示正在写入中 return; } // json 反序列化 DynamicJsonDocument doc(0x2FFF); deserializeJson(doc, todoJson); JsonObject obj = doc.as(); // 写入航点 uint8_t taskcount = obj["taskcount"]; // 获取航点总数 fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量 fc.writeState = true; // 锁定写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 // 监听飞控航点写入情况 while (true) { fc.comm_receive(mavlink_receiveCallback); if (fc.missionArkType == 0) { // 写入成功 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.logln("misson_bingo..."); // 改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 航点写入成功状态 break; } else if (fc.missionArkType > 0) { // 写入失败 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.logln("misson_error..."); // 改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); // 航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 1); // 回到初始状态 // 当有成果反馈之后初始化下列数据 return; // 写入失败中断函数 } // 飞控返回新的写入航点序号 if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) { // 从订阅信息里拿航点参数 uint8_t frame = obj["tasks"][fc.writeSeq]["frame"]; uint8_t command = obj["tasks"][fc.writeSeq]["command"]; uint8_t current = obj["tasks"][fc.writeSeq]["current"]; uint8_t autocontinue = obj["tasks"][fc.writeSeq]["autocontinue"]; double param1 = obj["tasks"][fc.writeSeq]["param1"]; double param2 = obj["tasks"][fc.writeSeq]["param2"]; double param3 = obj["tasks"][fc.writeSeq]["param3"]; double param4 = obj["tasks"][fc.writeSeq]["param4"]; double x = obj["tasks"][fc.writeSeq]["x"]; double y = obj["tasks"][fc.writeSeq]["y"]; double z = obj["tasks"][fc.writeSeq]["z"]; String str = obj["tasks"][fc.writeSeq]["sound"]; if (str.length() > 0 && str != "null") { fc.questVoiceStr = str; } fc.logln("frame--"); fc.logln(frame); // 写入航点 fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.futureSeq++; // 下一个航点序号 } delay(20); } } /** * @description: 从串口拿到mavlink缓存数据之后解析数据执行的回调 * @param {mavlink_message_t*} pMsg mavlink数据信息指针 * @param {mavlink_status_t*} pStatus * @param {uint8_t} c 串口读取的缓存 */ void mavlink_receiveCallback(uint8_t c) { mavlink_message_t msg; mavlink_status_t status; // printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱 // 尝试从数据流里解析数据 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 // 通过msgid来判断数据流的类别 char buf[50]; // printf("mav_id:%d\n",msg.msgid); switch (msg.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: // #242 返航点位置 { char buf[120]; mavlink_home_position_t home_pos; mavlink_msg_home_position_decode(&msg, &home_pos); // 返航点经纬高 sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}", home_pos.longitude, // 经度，以degE7单位传递 home_pos.latitude, // 纬度，以degE7单位传递 home_pos.z); // 相对高度，单位为米 topicPubMsg[17] = buf; } case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: // #49 飞机原点 { mavlink_gps_global_origin_t gps_org; // 解构的数据放到这个对象 mavlink_msg_gps_global_origin_decode(&msg, &gps_org); // 解构msg数据 // 原点点经纬高 sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}", gps_org.longitude, gps_org.latitude, (double)gps_org.altitude / 1000); // if (topicPubMsg[x] != buf) // { // topicPubMsg[x] = buf; // } } break; case MAVLINK_MSG_ID_MAG_CAL_REPORT: // #192 磁罗盘校准结果 { mavlink_mag_cal_report_t report; mavlink_msg_mag_cal_report_decode(&msg, &report); if (report.cal_status == MAG_CAL_SUCCESS) { topicPubMsg[9] = "successful"; // 校准成功 } else { topicPubMsg[9] = "failed"; // 校准失败 } } break; case MAVLINK_MSG_ID_MAG_CAL_PROGRESS: // #191 磁罗盘校准进度 { mavlink_mag_cal_progress_t progress; mavlink_msg_mag_cal_progress_decode(&msg, &progress); sprintf(buf, "%d", progress.completion_pct); topicPubMsg[8] = buf; } break; case MAVLINK_MSG_ID_COMMAND_ACK: // #77 处理 COMMAND_ACK 消息 { mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&msg, &ack); // 飞控接收到校准加速度计命令 if (ack.command == MAV_CMD_PREFLIGHT_CALIBRATION) { if (ack.result == 0) { fc.isAnalyzeAcce = true; // 标记进入校准状态此时匹配日志 } } } break; case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 { mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据 sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态 topicPubMsg[0] = buf; // 心跳主题 // 从心跳里判断飞机是否解锁解锁改变飞机状态 if (heartbeat.base_mode & 128) { // 从心跳里面判断已经解锁 if (!curr_armed) { printf("armed\n"); fc.playText("[v1]已解锁"); } curr_armed = true; topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭 } else { if (curr_armed) { printf("disarm\n"); fc.playText("[v1]已加锁"); } curr_armed = false; // 心跳里面判断没有解锁 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); // 设置为航点写入成功状态 } else { topicPubMsg[10] = "1"; // 没写航点设置为初始化状态 } } // 从心跳里面判断飞机当前的模式 if (curr_mode != heartbeat.custom_mode) { curr_mode = heartbeat.custom_mode; switch (heartbeat.custom_mode) { case 0: { topicPubMsg[12] = "姿态"; } break; case 2: { topicPubMsg[12] = "定高"; } break; case 3: { topicPubMsg[12] = "自动"; } break; case 4: { topicPubMsg[12] = "指点"; } break; case 5: { topicPubMsg[12] = "悬停"; // Loiter 定点 } break; case 6: { topicPubMsg[12] = "返航"; } break; case 9: { topicPubMsg[12] = "降落"; } break; case 16: { topicPubMsg[12] = "定点"; // PosHold } break; default: { topicPubMsg[12] = "未知模式"; } break; } fc.playText(topicPubMsg[12]); } } break; case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS { mavlink_sys_status_t sys_status; // 解构的数据放到这个对象 mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据 // 电压 sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000); if (topicPubMsg[1] != buf) { // 有更新则更新数据 topicPubMsg[1] = buf; } // 电流 sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); // 解构中 1=10毫安所以这里/100 最终单位是A 安培 if (topicPubMsg[2] != buf) { topicPubMsg[2] = buf; } // 电池电量 sprintf(buf, "%d", sys_status.battery_remaining); if (topicPubMsg[3] != buf) { topicPubMsg[3] = buf; } } break; case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU { mavlink_raw_imu_t 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(&msg, &global_position_int); // 海拔高度 sprintf(buf, "%.2f", (double)global_position_int.alt / 1000); if (topicPubMsg[4] != buf) { topicPubMsg[4] = buf; } // 经纬高 sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}", global_position_int.lon, global_position_int.lat, (double)global_position_int.relative_alt / 1000); if (topicPubMsg[15] != buf) { topicPubMsg[15] = 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[18] = buf; } // 解析加速度计校准状态 if (fc.isAnalyzeAcce) { // 根据不同的校准状态更新 topicPubMsg[11]，并将结果发送到前端 if (strstr(buf, "Place vehicle level and press any key") != nullptr) { topicPubMsg[11] = "level"; } else if (strstr(buf, "Place vehicle on its LEFT side and press any key") != nullptr) { topicPubMsg[11] = "left"; } else if (strstr(buf, "Place vehicle on its RIGHT side and press any key") != nullptr) { topicPubMsg[11] = "right"; } else if (strstr(buf, "Place vehicle nose DOWN and press any key") != nullptr) { topicPubMsg[11] = "down"; } else if (strstr(buf, "Place vehicle nose UP and press any key") != nullptr) { topicPubMsg[11] = "up"; } else if (strstr(buf, "Place vehicle on its BACK and press any key") != nullptr) { topicPubMsg[11] = "back"; } else if (strstr(buf, "Calibration successful") != nullptr) { topicPubMsg[11] = "successful"; fc.isAnalyzeAcce = false; // 结束校准状态 } else if (strstr(buf, "Calibration FAILED") != nullptr) { topicPubMsg[11] = "failed"; fc.isAnalyzeAcce = false; // 结束校准状态 } } } break; case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 { mavlink_vfr_hud_t vfr_hud; mavlink_msg_vfr_hud_decode(&msg, &vfr_hud); // 对地速度 ps:飞行速度 sprintf(buf, "%.2f", vfr_hud.groundspeed); if (topicPubMsg[5] != buf) { topicPubMsg[5] = buf; } } break; case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS { mavlink_gps_raw_int_t 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) { topicPubMsg[6] = buf; } // 卫星状态 switch (gps_raw_int.fix_type) { case 0: topicPubMsg[7] = "No Fix"; // 无定位 break; case 1: topicPubMsg[7] = "Bad Fix"; // 差的定位（无效数据） break; case 2: topicPubMsg[7] = "2D Fix"; // 2D定位（只有经纬度） break; case 3: topicPubMsg[7] = "3D Fix"; // 3D定位（经纬度和高度） break; case 4: topicPubMsg[7] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS） break; case 5: topicPubMsg[7] = "Float"; // RTK浮动解（精度较高，但不如固定解） break; case 6: topicPubMsg[7] = "RTK Fixed"; // RTK固定解（高精度定位） break; default: topicPubMsg[7] = "Unknown"; // 未知状态 break; } // 纬度 // sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); // 经度 // sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); } break; case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 { mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 // 日志 fc.logln((char *)"MsgID:"); fc.logln(msg.msgid); fc.logln((char *)"No:"); fc.logln(mission_request.seq); // 飞控反馈当前要写入航点的序号 fc.writeSeq = mission_request.seq; } break; case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 { mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据 /*日志*/ fc.logln((char *)"MsgID:"); fc.logln(msg.msgid); fc.logln((char *)"re:"); fc.logln(mission_ark.type); /*记录航点的写入状态 */ fc.missionArkType = mission_ark.type; // 0：写入成功非0表示失败 /*当有成果反馈之后初始化下列数据*/ fc.writeState = false; // 是否是写入状态 fc.writeSeq = -1; // 飞控反馈需写入航点序列号 fc.futureSeq = 0; // 记录将来要写入航点序列号 } break; case MAVLINK_MSG_ID_COMMAND_LONG: // #47: 航点提交成果反馈 { uint16_t fc_hook_cmd = mavlink_msg_command_long_get_command(&msg); uint16_t rngalt_cm = (uint16_t)mavlink_msg_command_long_get_param1(&msg); printf("COMMAND_LONG ID:%d,rng:%dcm \n", fc_hook_cmd, rngalt_cm); switch (fc_hook_cmd) { // 自动放线 case MAV_CMD_FC_HOOK_AUTODOWN: { HookStatus hss = motocontrol.gethooktatus(); printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm); // 没有激光高度直接退出 if (rngalt_cm == 0) { printf("exit rngalt_cm==0"); break; } if (hss == HS_Locked) { Hook_autodown(rngalt_cm); // 语音播报3次 if (fc.questVoiceStr != "") fc.playText(fc.questVoiceStr + "。" + fc.questVoiceStr + "。" + fc.questVoiceStr); } else { if (hss == HS_Stop) Hook_resume(); else printf("exit hooktatus error"); } break; } // 暂停 case MAV_CMD_FC_HOOK_PAUSE: { printf("MAV_CMD_FC_HOOK_PAUSE \n"); Hook_stop(); break; } // 暂停 case MAV_CMD_FC_HOOK_RECOVERY: { printf("MAV_CMD_FC_HOOK_RECOVERY \n"); Hook_recovery(); break; } } } break; case MAVLINK_MSG_ID_PARAM_VALUE: // #22:获取飞控参数信息 { mavlink_param_value_t param_value; mavlink_msg_param_value_decode(&msg, ¶m_value); // "BATT_CAPACITY" 参数电池总容量 if (strcmp(param_value.param_id, "BATT_CAPACITY") == 0) { // 将参数值转换为字符串 dtostrf(param_value.param_value, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串 if (String(buf) != topicPubMsg[16]) { topicPubMsg[16] = buf; } } // "WPNAV_SPEED" 参数返航速度 else if (strcmp(param_value.param_id, "WPNAV_SPEED") == 0) { // 将参数值转换为字符串 dtostrf(param_value.param_value / 1000, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串 if (String(buf) != topicPubMsg[19]) { topicPubMsg[19] = buf; } } // 其余参数 char jsonStr[100]; // 用于存储 JSON 格式的字符串 dtostrf(param_value.param_value, 0, 2, buf); // 将浮点数转换为字符串 // 构造 JSON 格式的字符串 {"param_name": value} snprintf(jsonStr, sizeof(jsonStr), "{\"%s\":%.2f}", param_value.param_id, atof(buf)); // 检查生成的 JSON 字符串是否与 topicPubMsg[20] 不相同，若不同则更新 if (String(jsonStr) != topicPubMsg[20]) { topicPubMsg[20] = jsonStr; } } break; default: break; } } } /** * @description: 发送主题线程 */ void pubThread() { /*检测飞控是否返回数据（此处检测的心跳字段）没有数据就像飞控请求*/ if (topicPubMsg[0] == "") // 检测心跳字段 { refreshRequest(); } /*解析mavlink 数据流等此函数内会把解析的数据按信息类型发布到mqtt服务器 planeState/macadd主题 */ // 创建一个JSON对象 DynamicJsonDocument doc(512); // 缓冲区 // 遍历有更新的数据组成一个json对象 for (int i = 0; i < topicPubCount; i++) { if (i == 0 || i == 10) // 每次必发的写在这里 { doc[topicPub[i]] = topicPubMsg[i]; } else if (i == 8 || i == 9) // 有新值发但是不重置 { if (topicPubMsg[i] != "") { doc[topicPub[i]] = topicPubMsg[i]; } } else // 其余有新值就发发出后重置 { if (topicPubMsg[i] != "") { doc[topicPub[i]] = topicPubMsg[i]; topicPubMsg[i] = ""; // 重置 } } } // 将JSON对象序列化为JSON字符串 String jsonString; serializeJson(doc, jsonString); fc.pubMQTTmsg("planeState", jsonString); } /** * @description: 刷新各请求 */ void refreshRequest() { fc.mav_request_home_position(); delay(50); fc.mav_request_data(); // 请求设定飞控输出数据流内容 delay(50); fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控返回参数电池容量值 delay(50); fc.mav_parameter_data("WPNAV_SPEED"); // 请求飞控返回参数返航速度 delay(50); } /** * @description: 向飞控发送油门指令 */ void chanThread() { // mav_channels_override(channels); } /** * @brief 设置模式 * @param 模式 */ void set_mode(FlightMode mode) { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_DO_SET_MODE; msg_cmd.param1 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; msg_cmd.param2 = mode; msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); } /** * @brief 加解锁 * @param islock true加锁 false解锁 */ void lock_or_unlock(bool islock) { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_COMPONENT_ARM_DISARM; if (islock) { msg_cmd.param1 = 0; msg_cmd.param2 = 21196; } else { msg_cmd.param1 = 1; msg_cmd.param2 = 0; } msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); } /** * @brief 飞到指定位置 * @param lat 维度 * @param lon 经度 * @param alt 高度 */ void fly_to_localtion(double lat, double lon, double alt) { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_NAV_TAKEOFF; msg_cmd.param1 = 0; msg_cmd.param2 = 0; msg_cmd.param3 = 0; msg_cmd.param4 = 0; msg_cmd.param5 = lat; msg_cmd.param6 = lon; msg_cmd.param7 = alt; msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); } /** * @brief 校准磁罗盘 */ void resetCompass() { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_DO_START_MAG_CAL; msg_cmd.param1 = 0; msg_cmd.param2 = 1; msg_cmd.param3 = 0; msg_cmd.param4 = 0; msg_cmd.param5 = 0; msg_cmd.param6 = 0; msg_cmd.param7 = 0; msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); } /** * @brief 发起校准 */ void initAcce() { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_PREFLIGHT_CALIBRATION; // 校准 msg_cmd.param1 = 0; // Gyro Temperature calibration msg_cmd.param2 = 0; // Magnetometer calibration msg_cmd.param3 = 0; // Ground Pressure calibration msg_cmd.param4 = 0; // Remote Control calibration msg_cmd.param5 = 1; // 1:加速度计 msg_cmd.param6 = 0; // Compass/Motor or Airspeed calibration msg_cmd.param7 = 0; // ESC or Baro calibration msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; // 发送校准命令 fc.mav_send_command(msg_cmd); }