From db3e2ec9310ef1beee170311a6a27bc0699bc618 Mon Sep 17 00:00:00 2001 From: szdot Date: Tue, 2 Jul 2024 03:16:26 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=09=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Afix=20=E3=80=90=E4=B8=BB=09=E9=A2=98=E3=80=91=EF=BC=9A?= =?UTF-8?q?=E4=BF=AE=E5=A4=8Dbug=20=E3=80=90=E6=8F=8F=09=E8=BF=B0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=20=09[=E5=8E=9F=E5=9B=A0]=EF=BC=9A=E4=B8=8D=E9=9C=80?= =?UTF-8?q?=E8=A6=81=E8=AE=A2=E9=98=85=E5=A4=9A=E4=B8=BB=E9=A2=98=20?= =?UTF-8?q?=E5=88=B6=E5=AE=9A=E7=B2=A4'cmd/macadd'=20=20=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E7=9A=84=E4=B8=BB=E9=A2=98=E6=95=B0=E7=BB=84=E5=B0=B1=E4=B8=8D?= =?UTF-8?q?=E9=9C=80=E8=A6=81=E4=BA=86=20=09[=E8=BF=87=E7=A8=8B]=EF=BC=9Am?= =?UTF-8?q?qtt=E8=AE=A2=E9=98=85=E5=87=BD=E6=95=B0=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E5=88=A0=E9=99=A4=20;=E7=BB=B4=E6=8C=81=E5=BF=83=E8=B7=B3?= =?UTF-8?q?=E5=87=BD=E6=95=B0=20=E5=8F=82=E6=95=B0=E5=88=A0=E9=99=A4=20=09?= =?UTF-8?q?[=E5=BD=B1=E5=93=8D]=EF=BC=9A=20=E3=80=90=E7=BB=93=09=E6=9D=9F?= =?UTF-8?q?=E3=80=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- FoodDelivery.ino | 736 +++++++++++++++++++++---------------------- FoodDeliveryBase.cpp | 20 +- FoodDeliveryBase.h | 4 +- 3 files changed, 378 insertions(+), 382 deletions(-) diff --git a/FoodDelivery.ino b/FoodDelivery.ino index 87c55de..4d432c6 100644 --- a/FoodDelivery.ino +++ b/FoodDelivery.ino @@ -38,7 +38,7 @@ void setup() { Serial1.begin(115200, SERIAL_8N1, 18, 5); //声音模块引串口脚映射 /*初始化*/ fc.connectWifi(); //连接wifi - fc.connectMqtt(topicSub, topicSubCount); //连接mqtt + fc.connectMqtt("cmd"); //连接mqtt fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调 fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) @@ -54,7 +54,7 @@ void loop() { /*从飞控拿数据*/ fc.comm_receive(mavlink_receiveCallback); /*保持mqtt心跳*/ - fc.mqttLoop(topicSub, topicSubCount); + fc.mqttLoop("cmd"); } /** @@ -100,7 +100,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { *第10位：航点继续 *第11位：磁罗盘校准 */ - String todoJson = value; //转换值 + String todoJson = value; //转换值 /* json 反序列化 */ DynamicJsonDocument doc(500); deserializeJson(doc, todoJson); @@ -142,8 +142,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { } else if (key == "getPlaneState") { //获取飞机状态 fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态 } else if (key == "resetState") { //恢复飞机为初始状态 - String todo = value; //转换值 - topicPubMsg[10] = todo; //恢复初始状态 + String todo = value; //转换值 + topicPubMsg[10] = todo; //恢复初始状态 } else if (key == "chan1") { uint16_t todo = value; //转换值 fc.channels[0] = todo; //恢复初始状态 @@ -162,32 +162,32 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { fc.mav_channels_override(fc.channels); //油门控制 } else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 uint16_t todo = value; - switch(todo){ + switch (todo) { case 0: - { - //收钩 - } - break; + { + //收钩 + } + break; case 1: - { - //放钩 - } - break; + { + //放钩 + } + break; case 2: - { - //暂停 - } - break; + { + //暂停 + } + break; case 3: - { - //继续 - } - break; + { + //继续 + } + break; case 4: - { - //重置重量 - } - break; + { + //重置重量 + } + break; } } else if (key == "cameraController") { //云台相机控制 String todoJson = value; //转换值 @@ -202,7 +202,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { //相机控制 if (item == 0) { //0:一键回中 uint8_t stopCommand[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; //回中前 强制 俯仰旋转 停止 - size_t stopLen = sizeof(StopCommand); + 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); @@ -220,388 +220,386 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { fc.udpSendToCamera(command, len); } } - - /** +} +/** * @description: 写入航点 * @param {String} todo mqtt订阅执行任务的Json字符串 */ - void writeRoute(String todoJson) { - if (fc.writeState) // 如果正在写入状态 跳出 - { - fc.logln("正在写航点"); // 提示正在写入中 - return; - } - //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 - fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}")//发送正在写入的飞机状态 - // json 反序列化 - DynamicJsonDocument doc(0x2FFF); - deserializeJson(doc, todoJson); - JsonObject obj = doc.as(); - // 写入航点 - uint8_t taskcount = obj["taskcount"]; //获取航点总数 - fc.mav_mission_count(taskcount); //向飞控请求写入航点的数量 - fc.writeState = true; //锁定写入状态 - //监听飞控航点写入情况 - while (true) { - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 - fc.comm_receive(mavlink_receiveCallback); - if (fc.missionArkType == 0) { //写入成功 - fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 - fc.logln("misson_bingo..."); - //改变飞机状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 - 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); //结束写入状态 - //当有成果反馈之后 初始化下列数据 - 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 != "") { - 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(200); - } +void writeRoute(String todoJson) { + if (fc.writeState) // 如果正在写入状态 跳出 + { + fc.logln("正在写航点"); // 提示正在写入中 + return; } - - /** + //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 + fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //发送正在写入的飞机状态 + // json 反序列化 + DynamicJsonDocument doc(0x2FFF); + deserializeJson(doc, todoJson); + JsonObject obj = doc.as(); + // 写入航点 + uint8_t taskcount = obj["taskcount"]; //获取航点总数 + fc.mav_mission_count(taskcount); //向飞控请求写入航点的数量 + fc.writeState = true; //锁定写入状态 + //监听飞控航点写入情况 + while (true) { + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 + fc.comm_receive(mavlink_receiveCallback); + if (fc.missionArkType == 0) { //写入成功 + fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 + fc.logln("misson_bingo..."); + //改变飞机状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 + 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); //结束写入状态 + //当有成果反馈之后 初始化下列数据 + 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 != "") { + 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(200); + } +} +/** * @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; - // 尝试从数据流里 解析数据 - if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 - // 通过msgid来判断 数据流的类别 - char buf[10]; - switch (msg.msgid) { - 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) { //从心跳里面 判断已经解锁 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 - } else { //心跳里面 判断没有解锁 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 - if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态 - } else { - topicPubMsg[10] = "1"; //没写航点 设置为初始化状态 +void mavlink_receiveCallback(uint8_t c) { + mavlink_message_t msg; + mavlink_status_t status; + // 尝试从数据流里 解析数据 + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 + // 通过msgid来判断 数据流的类别 + char buf[10]; + switch (msg.msgid) { + 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) { //从心跳里面 判断已经解锁 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 + } else { //心跳里面 判断没有解锁 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 + if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态 + } else { + topicPubMsg[10] = "1"; //没写航点 设置为初始化状态 + } + } + //从心跳里面 判断飞机当前的模式 + switch (heartbeat.custom_mode) { + case 0: + { + topicPubMsg[12] = "Stabilize自稳"; } - } - //从心跳里面 判断飞机当前的模式 - switch (heartbeat.custom_mode) { - case 0: - { - topicPubMsg[12] = "Stabilize自稳"; - } - break; - case 2: - { - topicPubMsg[12] = "AltHold定高"; - } - break; - case 3: - { - topicPubMsg[12] = "Auto自动"; - } - break; - case 4: - { - topicPubMsg[12] = "Guided引导"; - } - break; - case 5: - { - topicPubMsg[12] = "Loiter留待"; - } - break; - case 6: - { - topicPubMsg[12] = "RTL返航"; - } - break; - case 9: - { - topicPubMsg[12] = "Land降落"; - } - break; - case 16: - { - topicPubMsg[12] = "PosHold定点"; - } - break; - default: - { - topicPubMsg[12] = "Unknown未知"; - } - break; - } + break; + case 2: + { + topicPubMsg[12] = "AltHold定高"; + } + break; + case 3: + { + topicPubMsg[12] = "Auto自动"; + } + break; + case 4: + { + topicPubMsg[12] = "Guided引导"; + } + break; + case 5: + { + topicPubMsg[12] = "Loiter留待"; + } + break; + case 6: + { + topicPubMsg[12] = "RTL返航"; + } + break; + case 9: + { + topicPubMsg[12] = "Land降落"; + } + break; + case 16: + { + topicPubMsg[12] = "PosHold定点"; + } + break; + default: + { + topicPubMsg[12] = "Unknown未知"; + } + break; } - break; + } + 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); - if (topicPubMsg[2] != buf) { - topicPubMsg[2] = buf; - } - // 电池电量 - sprintf(buf, "%d", sys_status.battery_remaining); - if (topicPubMsg[3] != buf) { - topicPubMsg[3] = buf; - } + 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; } - break; - - case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE - { - mavlink_param_value_t param_value; - mavlink_msg_param_value_decode(&msg, ¶m_value); + // 电流 + sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); + if (topicPubMsg[2] != buf) { + topicPubMsg[2] = 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); + // 电池电量 + sprintf(buf, "%d", sys_status.battery_remaining); + if (topicPubMsg[3] != buf) { + topicPubMsg[3] = buf; } - break; + } + 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.relative_alt / 1000); - if (topicPubMsg[4] != buf) { - topicPubMsg[4] = buf; - } - //{经度，维度，海拔高度} - sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", - global_position_int.lon, - global_position_int.lat, - (double)global_position_int.alt / 1000); - if (topicPubMsg[15] != buf) { - topicPubMsg[15] = buf; - } + case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE + { + mavlink_param_value_t param_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(&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.relative_alt / 1000); + if (topicPubMsg[4] != buf) { + topicPubMsg[4] = buf; } - 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; - } + //{经度，维度，海拔高度} + sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", + global_position_int.lon, + global_position_int.lat, + (double)global_position_int.alt / 1000); + if (topicPubMsg[15] != buf) { + topicPubMsg[15] = buf; } - break; + } + 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; - } - // 纬度 - sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); - if (topicPubMsg[7] != buf) { - topicPubMsg[7] = buf; - } - // 经度 - sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); - if (topicPubMsg[8] != buf) { - topicPubMsg[8] = buf; - } - // 卫星状态 - switch (gps_raw_int.fix_type) { - case 0: - { - topicPubMsg[9] = "No Fix"; - } - break; - case 1: - { - topicPubMsg[9] = "No Fix"; - } - break; - case 2: - { - topicPubMsg[9] = "Fix 2D"; - } - break; - case 3: - { - topicPubMsg[9] = "Fix 3D"; - } - break; - case 4: - { - topicPubMsg[9] = "Low GPS"; - } - break; - case 5: - { - topicPubMsg[9] = "Float"; - } - break; - default: - { - topicPubMsg[9] = "Fixed"; - } - 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; + } + 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("MsgID:"); - fc.logln(msg.msgid); - fc.logln("No:"); - fc.logln(mission_request.seq); - //飞控 反馈当前要写入航点的序号 - fc.writeSeq = mission_request.seq; + 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; } - 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("MsgID:"); - fc.logln(msg.msgid); - fc.logln("re:"); - fc.logln(mission_ark.type); - /*记录航点的写入状态 */ - fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 - /*当有成果反馈之后 初始化下列数据*/ - fc.writeState = false; //是否是写入状态 - fc.writeSeq = -1; //飞控反馈 需写入航点序列号 - fc.futureSeq = 0; //记录将来要写入 航点序列号 + // 纬度 + sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); + if (topicPubMsg[7] != buf) { + topicPubMsg[7] = buf; } - break; + // 经度 + sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); + if (topicPubMsg[8] != buf) { + topicPubMsg[8] = buf; + } + // 卫星状态 + switch (gps_raw_int.fix_type) { + case 0: + { + topicPubMsg[9] = "No Fix"; + } + break; + case 1: + { + topicPubMsg[9] = "No Fix"; + } + break; + case 2: + { + topicPubMsg[9] = "Fix 2D"; + } + break; + case 3: + { + topicPubMsg[9] = "Fix 3D"; + } + break; + case 4: + { + topicPubMsg[9] = "Low GPS"; + } + break; + case 5: + { + topicPubMsg[9] = "Float"; + } + break; + default: + { + topicPubMsg[9] = "Fixed"; + } + break; + } + } + break; - default: - 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("MsgID:"); + fc.logln(msg.msgid); + fc.logln("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("MsgID:"); + fc.logln(msg.msgid); + fc.logln("re:"); + fc.logln(mission_ark.type); + /*记录航点的写入状态 */ + fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 + /*当有成果反馈之后 初始化下列数据*/ + fc.writeState = false; //是否是写入状态 + fc.writeSeq = -1; //飞控反馈 需写入航点序列号 + fc.futureSeq = 0; //记录将来要写入 航点序列号 + } + break; + + default: + break; } } - - /** +} +/** * @description: 发送主题线程 */ - void pubThread() { - /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ - // 创建一个JSON对象 - DynamicJsonDocument doc(2000); // 缓冲区 - //遍历 有更新的数据 组成一个json对象 - for (int i = 0; i < topicPubCount; i++) { - if (topicPubMsg[i] != oldMsg[i]) { - if (i == 0) { //心跳包 每每向心跳主题发布信息 - //启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 - if (fc.getIsInit()) { - fc.setIsInit(false); - fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容 - } - //设置对象成员 ps:心跳 - doc[topicPub[0]] = topicPubMsg[0]; - } else { //非心跳 有更新 录入成员 - doc[topicPub[i]] = topicPubMsg[i]; - oldMsg[i] = topicPubMsg[i]; +void pubThread() { + /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ + // 创建一个JSON对象 + DynamicJsonDocument doc(2000); // 缓冲区 + //遍历 有更新的数据 组成一个json对象 + for (int i = 0; i < topicPubCount; i++) { + if (topicPubMsg[i] != oldMsg[i]) { + if (i == 0) { //心跳包 每每向心跳主题发布信息 + //启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 + if (fc.getIsInit()) { + fc.setIsInit(false); + fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容 } + //设置对象成员 ps:心跳 + doc[topicPub[0]] = topicPubMsg[0]; + } else { //非心跳 有更新 录入成员 + doc[topicPub[i]] = topicPubMsg[i]; + oldMsg[i] = topicPubMsg[i]; } } - // 将JSON对象序列化为JSON字符串 - String jsonString; - serializeJson(doc, jsonString); - fc.pubMQTTmsg("planeState", jsonString); - /*更新4G网络测速ping值*/ - //pingNetTest(); - - //八达岭需求 过后删除 - fc.pubMQTTmsg("heartBeat", topicPubMsg[0]); - fc.pubMQTTmsg("position", topicPubMsg[15]); } + // 将JSON对象序列化为JSON字符串 + String jsonString; + serializeJson(doc, jsonString); + fc.pubMQTTmsg("planeState", jsonString); + /*更新4G网络测速ping值*/ + //pingNetTest(); - /** + //八达岭需求 过后删除 + fc.pubMQTTmsg("heartBeat", topicPubMsg[0]); + fc.pubMQTTmsg("position", topicPubMsg[15]); +} + +/** * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 */ - void flashThread() { - if (digitalRead(23) == LOW) { - if (isPush) { //点击之后 - //请求注册 ps:发送esp8266的物理地址 到对频主题 - fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); - } - isPush = false; //复位按钮 - } else { - //FLASH按下状态 - isPush = true; +void flashThread() { + if (digitalRead(23) == LOW) { + if (isPush) { //点击之后 + //请求注册 ps:发送esp8266的物理地址 到对频主题 + fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); } + isPush = false; //复位按钮 + } else { + //FLASH按下状态 + isPush = true; } +} - /** +/** * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 */ - void mavThread() { - fc.mav_request_data(); - } +void mavThread() { + fc.mav_request_data(); +} - /** +/** * @description: 向飞控 发送油门指令 */ - void chanThread() { - //mav_channels_override(channels); - } +void chanThread() { + //mav_channels_override(channels); +} diff --git a/FoodDeliveryBase.cpp b/FoodDeliveryBase.cpp index 71ec21e..24cd407 100644 --- a/FoodDeliveryBase.cpp +++ b/FoodDeliveryBase.cpp @@ -121,10 +121,9 @@ void FoodCube::connectWifi() { /** * @description: 连接mqtt -* @param {String[]} topicSub 主题数组 -* @param {int} topicSubCount 数组总数 +* @param {String} topicSub 订阅主题 */ -void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { +void FoodCube::connectMqtt(String topicSub) { /*尝试连接mqtt*/ if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) { logln("MQTT Server Connected."); @@ -134,7 +133,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { logln(macAdd); /*连接成功 订阅主题*/ //订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback - subscribeTopic("cmd", 1); + subscribeTopic(topicSub, 1); playText("服务器,已连接"); } else { //失败返回状态码 @@ -146,14 +145,13 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { /** * @description: 写在loop函数里 检测mqtt连接情况 并保持心跳 -* @param {String[]} topicSub 主题数组 -* @param {int} topicSubCount 数组总数 +* @param {String} topicSub 订阅主题 */ -void FoodCube::mqttLoop(String topicSub[], int topicSubCount) { - if (mqttClient->connected()) { //检测 如果开发板成功连接服务器 - mqttClient->loop(); // 保持心跳 - } else { // 如果开发板未能成功连接服务器 - connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器 +void FoodCube::mqttLoop(String topicSub) { + if (mqttClient->connected()) { //检测 如果开发板成功连接服务器 + mqttClient->loop(); // 保持心跳 + } else { // 如果开发板未能成功连接服务器 + connectMqtt(topicSub); // 则尝试连接服务器 } } diff --git a/FoodDeliveryBase.h b/FoodDeliveryBase.h index 596b1fd..c126d9d 100644 --- a/FoodDeliveryBase.h +++ b/FoodDeliveryBase.h @@ -46,8 +46,8 @@ public: void connectWifi(); /*mqtt*/ PubSubClient* mqttClient; //指向 mqtt服务器连接 对象 - void connectMqtt(String topicSub[], int topicSubCount); - void mqttLoop(String topicSub[], int topicSubCount); + void connectMqtt(String topicSub); + void mqttLoop(String topicSub); void subscribeTopic(String topicString, int Qos); void pubMQTTmsg(String topicString, String messageString); /*串口输出*/