From f3583e17e5ce4bf77e1a0f9f148cc1d5c7c7a22d Mon Sep 17 00:00:00 2001 From: xu Date: Mon, 1 Jul 2024 22:40:12 +0800 Subject: [PATCH] =?UTF-8?q?[feat]=20=E8=9E=8D=E5=90=88=E6=96=B0=E6=94=B9?= =?UTF-8?q?=E7=9A=84mqtt=E4=BB=A3=E7=A0=81=20=E5=B0=86mqtt=E5=92=8C?= =?UTF-8?q?=E5=B0=8F=E7=94=B0=E4=BB=A3=E7=A0=81=E5=8D=95=E7=8B=AC=E6=94=BE?= =?UTF-8?q?=E5=88=B0commer.cpp=E9=87=8C=E9=9D=A2=EF=BC=8C=E4=B8=BB?= =?UTF-8?q?=E7=A8=8B=E5=BA=8F=E8=B0=83=E7=94=A8=20=E4=BB=85=E8=9E=8D?= =?UTF-8?q?=E5=90=88,=E4=BB=A3=E7=A0=81=E6=9C=AA=E5=AE=8C=E5=96=84?= 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：构建过程或辅助工具的变动 --- src/commser.cpp | 598 ++++++++++++++++++++++++++++++++++++++ src/commser.h | 21 ++ src/config.h | 19 ++ src/main.cpp | 742 ++---------------------------------------------- 4 files changed, 656 insertions(+), 724 deletions(-) create mode 100644 src/commser.cpp create mode 100644 src/commser.h diff --git a/src/commser.cpp b/src/commser.cpp new file mode 100644 index 0000000..470ce3f --- /dev/null +++ b/src/commser.cpp @@ -0,0 +1,598 @@ +#include "commser.h" +#include "motocontrol.h" +//////////////////////////////MQTT_语音_MAVLINK 部分 +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 = "szdot"; //wifi帐号 +//char* password = "Ttaj@#*.com"; //wifi密码 +char* ssid = "flicube"; //wifi帐号 +char* password = "fxmf0622"; //wifi密码 +char* mqttServer = "szdot.top"; //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:{经度，维度，海拔高度} +String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position" }; +int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //登记 json成员总数 +String topicPubMsg[16]; //登记 json成员的值 对应topicPub +String oldMsg[16]; //记录旧的值 用来对比有没有更新 +/*触发发送 主题*/ +//0:对频信息 +String topicHandle[] = { "crosFrequency" }; +boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关 + +/*异步线程对象*/ +Ticker pubTicker(pubThread,1000); //定时发布主题 线程 +Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 +Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程 + +//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 == "setPlaneState") { //设置飞机状态 + /* + *其中topicPubMsg[10]既飞机状态的值 + *二进制0000 0000 0000 0000 + *第0位：初始状态 + *第1位：是否正在写入航线 + *第2位：是否已经写入航点 + *第3位：是否正在解锁 + *第4位：解锁是否成功 + *第5位：执行航点任务 + *第6位：起飞 + *第7位: 悬停 + *第8位：降落 + *第9位：返航 + *第10位：航点继续 + *第11位：磁罗盘校准 + */ + String todoJson = value; //转换值 + /* json 反序列化 */ + DynamicJsonDocument doc(500); + deserializeJson(doc, todoJson); + JsonObject obj = doc.as(); + uint8_t n = obj["bit"]; //状态位数 + uint8_t state = obj["state"]; //标记飞机状态 0 or 1 + uint8_t count = obj["count"]; //传过来 + //解构val数组参数 + uint16_t param[count]; + for (int i = 0; i < count; i++) { + param[i] = obj["param"][i]; + } + //标记飞机状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); + //飞控执行 + if (n == 3) { //3操作飞机加解锁 + uint16_t chan[] = { 1500, 1500, 1100, 1500 }; //加解锁 油门到底 + fc.mav_channels_override(chan); //控制油门 + fc.mav_set_mode(2); //飞控设置成AltHold定高 + fc.mav_command(n, param); + } else { + uint16_t chan[] = { 1500, 1500, 1500, 1500 }; //除了加解锁模式 油门全部控制居中 + fc.mav_channels_override(chan); //控制油门 + } + if (n == 6) { //6测试起飞 + fc.mav_set_mode(4); //飞控设置成Guided引导模式 + fc.mav_command(n, param); //起飞 + } else if (n == 7) { //7 悬停 + fc.mav_set_mode(5); //飞控设置成Loiter留待模式 + } else if (n == 5) { //5 航点执行 + fc.mav_set_mode(3); //飞控设置成auto自动模式 + } else if (n == 8) { //8降落* + fc.mav_command(n, param); + } else if (n == 9) { //9返航 + fc.mav_set_mode(6); //飞控设置成RTL返航 + } else if (n == 11) { //11磁罗盘校准 + fc.mav_command(n, param); + } + } else if (key == "getPlaneState") { //获取飞机状态 + fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态 + } else if (key == "resetState") { //恢复飞机为初始状态 + String todo = value; //转换值 + topicPubMsg[10] = todo; //恢复初始状态 + } else if (key == "chan1") { + uint16_t todo = value; //转换值 + fc.channels[0] = todo; //恢复初始状态 + fc.mav_channels_override(fc.channels); //油门控制 + } else if (key == "chan2") { + uint16_t todo = value; //转换值 + fc.channels[1] = todo; //恢复初始状态 + fc.mav_channels_override(fc.channels); //油门控制 + } else if (key == "chan3") { + uint16_t todo = value; //转换值 + fc.channels[2] = todo; //恢复初始状态 + fc.mav_channels_override(fc.channels); //油门控制 + } else if (key == "chan4") { + uint16_t todo = value; //转换值 + fc.channels[3] = todo; //恢复初始状态 + fc.mav_channels_override(fc.channels); //油门控制 + } else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 + uint16_t todo = value; + switch(todo){ + case 0: + { + //收钩 + } + break; + case 1: + { + //放钩 + } + break; + case 2: + { + //暂停 + } + break; + case 3: + { + //继续 + } + break; + case 4: + { + //重置重量 + } + 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; + } + //改变飞机状态 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"; //没写航点 设置为初始化状态 + } + } + //从心跳里面 判断飞机当前的模式 + 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 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; + } + } + break; + + 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; + } + //{经度，维度，海拔高度} + 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; + + 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; + } + // 纬度 + 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; + } + } + 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]; + } + } + } + // 将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; + } + } + + /** +* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 +*/ + void mavThread() { + fc.mav_request_data(); + } + + /** +* @description: 向飞控 发送油门指令 +*/ + void chanThread() { + //mav_channels_override(channels); + } diff --git a/src/commser.h b/src/commser.h new file mode 100644 index 0000000..a0890fb --- /dev/null +++ b/src/commser.h @@ -0,0 +1,21 @@ +#ifndef COMMSER_H +#define COMMSER_H + +#include "Arduino.h" +#include //调用Ticker.h库 +#include "FoodDeliveryBase.h" +#include "config.h" + +void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length); +void mavThread(); +void pubThread(); +void flashThread() ; +void writeRoute(String topicStr); +void mavlink_receiveCallback(uint8_t c) ; +extern String topicPub[]; +extern int topicPubCount; +extern FoodCube fc; //创建项目对象 +extern Ticker pubTicker; //定时发布主题 线程 +extern Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 +extern Ticker flashTicker; //单片机主动 按钮主动发布主题 线程 +#endif \ No newline at end of file diff --git a/src/config.h b/src/config.h index a4ef3db..56e1dc0 100644 --- a/src/config.h +++ b/src/config.h @@ -54,3 +54,22 @@ enum HookStatus HS_Locked, // 已到顶部锁定 HS_Stop // 已停止 }; +enum Initstatus +{ + IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线 + IS_Start, // 0. 刚上电 + IS_Wait_Locked, + IS_LengthZero, // 1.已确定零点 + IS_begin_WeightZero, + IS_Wait_WeightZero, + IS_WeightZero, // 2.已确定称重传感器0点 + IS_InStoreLocked, // 3. 挂钩入仓顶锁定 + IS_CheckWeightZero, // 检查称重传感器是否归0 + IS_OK, // 4.所有初始化已经OK，可以正常操作 + IS_Error // 5.初始化遇到错误 +} ; +const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线 +const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停 +const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 +const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 +const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 diff --git a/src/main.cpp b/src/main.cpp index fb39600..cf80e34 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,6 +11,7 @@ #include #include "FoodDeliveryBase.h" #include +#include "commser.h" // 角度传感器 // 收放线电机控制 @@ -83,73 +84,15 @@ enum Weightalign_status } _weightalign_status; unsigned long _weightalign_begintm; //校准开始时间 // 需要做的初始化工作 -enum Initstatus -{ - IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线 - IS_Start, // 0. 刚上电 - IS_Wait_Locked, - IS_LengthZero, // 1.已确定零点 - IS_begin_WeightZero, - IS_Wait_WeightZero, - IS_WeightZero, // 2.已确定称重传感器0点 - IS_InStoreLocked, // 3. 挂钩入仓顶锁定 - IS_CheckWeightZero, // 检查称重传感器是否归0 - IS_OK, // 4.所有初始化已经OK，可以正常操作 - IS_Error // 5.初始化遇到错误 -} initstatus; + +Initstatus initstatus; unsigned long _tm_waitinit; -const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线 -const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停 -const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 -const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 -const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 -/////////////////////// - /////////////////////////////////MQTT_语音_MAVLINK 部分 -/*项目对象*/ -//char* ssid = "szdot"; //wifi帐号 -//char* password = "Ttaj@#*.com"; //wifi密码 -char* ssid = (char*)"fxmf_sc01"; //wifi帐号 "flicube";// "fxmf_sc01" -char* password = (char*)"12345678"; //wifi密码 "fxmf0622"; //"12345678" -char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //114.115.137.239 mqtt地址 -int mqttPort = 1883; //mqtt端口 -char* mqttName = (char*)"admin"; //mqtt帐号 -char* mqttPassword = (char*)"123456"; //mqtt密码 -uint8_t mavlinkSerial = 2; //飞控占用的串口号 -uint8_t voiceSerial = 1; //声音模块占用串口 -char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip -uint32_t udpServerPort = 37260; //云台相机端口 -void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length); -void mavThread(); -void pubThread(); -void flashThread() ; -void writeRoute(String topicStr); -void mavlink_receiveCallback(uint8_t c) ; -FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象 -/*订阅 主题*/ -//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制 -String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController", "cmd" }; //订阅主题 -int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数 -/*有更新主动发送 主题*/ -//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度) -String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode","loadweight","hookstatus","position"}; -int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数 -String topicPubMsg[16]; //发送数据存放 对应topicPub -String oldMsg[16]; //记录旧的数据 用来对比有没有更新 -/*触发发送 主题*/ -//0:对频信息 -String topicHandle[] = { "crosFrequency" }; -boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关 -/*异步线程对象*/ -Ticker pubTicker(pubThread,1000); //定时发布主题 线程 -Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 -//Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程 - /////////////////////////////////MQTT_语音_MAVLINK 部分结束 void setup() { @@ -208,15 +151,15 @@ void setup() Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射 fc.playText("开始启动"); fc.connectWifi(); //连接wifi - // fc.playText("正在连接服务器"); - // fc.connectMqtt(topicSub, topicSubCount); //连接mqtt + fc.playText("正在连接服务器"); + fc.connectMqtt(topicPub, topicPubCount); //连接mqtt fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调 fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) /*异步线程*/ pubTicker.start(); //定时 发布主题 mavTicker.start(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 - //flashTicker.start(); //监听 按flash键时 主动发布对频主题 + flashTicker.start(); //监听 按flash键时 主动发布对频主题 /////////////////////////////////MQTT_语音_MAVLINK 部分结束 @@ -281,8 +224,8 @@ void showinfo() // if (pullweight > 10) // printf("PullWeight:%d\n", pullweight); //发送重量到mqtt - topicPubMsg[14]=motocontrol.gethooktatus_str() ; - topicPubMsg[13]=pullweight; + //topicPubMsg[14]=motocontrol.gethooktatus_str() ; + //topicPubMsg[13]=pullweight; // control_status_t cs=motocontrol.getcontrolstatus() ; @@ -468,8 +411,8 @@ void set_locked(bool locked) void loop() { tksendinfo.update(); // 定时发送信息任务 - pubTicker.update(); //定时 发布主题 - mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 + //pubTicker.update(); //定时 发布主题 + //mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 // sercomm.getcommand(); // 得到控制命令 @@ -509,6 +452,8 @@ void loop() /////////////////////////////////MQTT_语音_MAVLINK 部分 /*从飞控拿数据*/ fc.comm_receive(mavlink_receiveCallback); + /*保持mqtt心跳*/ + fc.mqttLoop(topicPub, topicPubCount); /////////////////////////////////MQTT_语音_MAVLINK 部分结束 delay(1); @@ -535,9 +480,11 @@ void Task1(void *pvParameters) /*保持mqtt心跳*/ // fc.mqttLoop(topicSub, topicSubCount); - if (fc.checkWiFiStatus()) + + ///px1 + // if (fc.checkWiFiStatus()) /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/ - fc.mqttLoop(topicSub, topicSubCount); + // fc.mqttLoop(topicSub, topicSubCount); vTaskDelay(10); } } @@ -775,7 +722,8 @@ void btn_presssatonce() ESP_LOGD(MOUDLENAME,"UP_presssatonce"); led_show(255,255, 255); // 高亮一下 fc.playText("发送对频信息"); - fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); + ///px1 + //fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); } // 收线按钮-长按 void upbtn_pressstart() @@ -865,658 +813,4 @@ void testbtn_click() //////////////////////////////MQTT_语音_MAVLINK 部分 -/** -* @description: mqtt订阅主题 收到信息 的回调函数 -* @param {char*} topic 主题名称 -* @param {byte*} topic 订阅获取的内容 -* @param {unsigned int} length 内容的长度 -*/ -void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { - /*获取 "订阅主题/macadd" 截取/macadd后的长度 */ - int count = strlen(topic); - String cutTopic = ""; //记录订阅主题 - for (int i = 0; i < count - 13; i++) { - cutTopic += topic[i]; - } - /*解构mqtt发过来的内容*/ - String topicStr = ""; - for (int i = 0; i < length; i++) { - topicStr += (char)payload[i]; - } - /*订阅回调主体*/ - if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss - writeRoute(topicStr); //写入航点 - } else - if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState - /* - *其中topicPubMsg[10]既飞机状态的值 - *二进制0000 0000 0000 0000 - *第0位：初始状态 - *第1位：是否正在写入航线 - *第2位：是否已经写入航点 - *第3位：是否正在解锁 - *第4位：解锁是否成功 - *第5位：执行航点任务 - *第6位：起飞 - *第7位: 悬停 - *第8位：降落 - *第9位：返航 - *第10位：航点继续 - *第11位：磁罗盘校准 - */ - //解构参数 - DynamicJsonDocument doc(0x2FFF); - deserializeJson(doc, topicStr); - JsonObject obj = doc.as(); - uint8_t n = obj["bit"]; //状态位数 - uint8_t state = obj["state"]; //标记飞机状态 0 or 1 - uint8_t count = obj["count"]; //传过来 - //解构val数组参数 - uint16_t param[count]; - for (int i = 0; i < count; i++) { - param[i] = obj["param"][i]; - } - //标记飞机状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); - //飞控执行 - if (n == 3) { //3操作飞机加解锁 - uint16_t chan[] = { 1500, 1500, 1100, 1500 }; //加解锁 油门到底 - fc.mav_channels_override(chan); //控制油门 - fc.mav_set_mode(2); //飞控设置成AltHold定高 - fc.mav_command(n, param); - } else { - uint16_t chan[] = { 1500, 1500, 1500, 1500 }; //除了加解锁模式 油门全部控制居中 - fc.mav_channels_override(chan); //控制油门 - } - if (n == 6) { //6测试起飞 - fc.mav_set_mode(4); //飞控设置成Guided引导模式 - fc.mav_command(n, param); //起飞 - } - if (n == 7) { //7 悬停 - fc.mav_set_mode(5); //飞控设置成Loiter留待模式 - } - if (n == 5) { //5 航点执行 - fc.mav_set_mode(3); //飞控设置成auto自动模式 - } - if (n == 8) { //8降落* - fc.mav_command(n, param); - } - if (n == 9) { //9返航 - fc.mav_set_mode(6); //飞控设置成RTL返航 - } - if (n == 11) { //11磁罗盘校准 - fc.mav_command(n, param); - } - } - if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState - fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态 - }else - if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState - topicPubMsg[10] = "1"; //恢复初始状态 - }else - if (cutTopic == topicSub[4]) { //4:油门通道1 - uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 - fc.channels[0] = strInt; //恢复初始状态 - fc.mav_channels_override(fc.channels); //油门控制 - }else - if (cutTopic == topicSub[5]) { //5:油门通道2 - uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 - fc.channels[1] = strInt; //恢复初始状态 - fc.mav_channels_override(fc.channels); //油门控制 - }else - if (cutTopic == topicSub[6]) { //6:油门通道3 - uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 - fc.channels[2] = strInt; //恢复初始状态 - fc.mav_channels_override(fc.channels); //油门控制 - }else - if (cutTopic == topicSub[7]) { //7:油门通道4 - uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 - fc.channels[3] = strInt; //恢复初始状态 - fc.mav_channels_override(fc.channels); //油门控制 - }else - if (cutTopic == topicSub[8]) { //8:钩子控制 - uint16_t strInt = (uint16_t)topicStr.toInt(); // - ESP_LOGD(MOUDLENAME,"hookcontrol command: %d", strInt); - switch (strInt) - { - case 0: //收 - /* code */ - ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_UP"); - if (motocontrol.getcontrolstatus().is_autogoodsdown) - up_action(SPEED_BTN_FAST); - break; - case 1: //放 - /* code */ - break; - case 2: //暂停 - /* code */ - ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_PAUSE"); - if (motocontrol.getcontrolstatus().is_autogoodsdown) - Hook_stop(); - break; - case 3: //继续 - /* code */ - ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_resume"); - if (motocontrol.getcontrolstatus().is_autogoodsdown) - Hook_resume(); - break; - } - }else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController - // json 反序列化 - DynamicJsonDocument doc(0x2FFF); - deserializeJson(doc, topicStr); - JsonObject obj = doc.as(); - //相机控制 - uint8_t item=obj["item"]; - switch (item) - { - //一键回中 - case 0: - fc.Camera_action_ret(); - break; - //1:变焦 - case 1: - fc.Camera_action_zoom(obj["val"]); - break; - //2:俯仰,旋转 - case 2: - fc.Camera_action_move(obj["pitch"],obj["yaw"]); - break; - } - }else if (cutTopic == topicSub[10]) { //通用命令 - // json 反序列化 - DynamicJsonDocument doc(0x2FFF); - deserializeJson(doc, topicStr); - JsonObject obj = doc.as(); - uint8_t cmd_id=obj["id"]; - ESP_LOGD(MOUDLENAME,"Mqtt_cmd: %d",cmd_id); - switch (cmd_id) - { - //校准称重 - case 10: - if (motocontrol.gethooktatus() == HS_Stop) - begin_tare(); - break; - } - } -} - -/** -* @description: 写入航点 -* @param {String} topicStr mqtt订阅执行任务的Json字符串 -*/ -void writeRoute(String topicStr) { - if (fc.writeState) // 如果正在写入状态 跳出 - { - ESP_LOGD(MOUDLENAME,"正在写航点"); // 提示正在写入中 - return; - } - //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 - fc.pubMQTTmsg("planeState", topicPubMsg[10]); //发送正在写入的飞机状态 - // json 反序列化 - DynamicJsonDocument doc(0x2FFF); - deserializeJson(doc, topicStr); - 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默认状态 - ESP_LOGD(MOUDLENAME,"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默认状态 - ESP_LOGE(MOUDLENAME,"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!=NULL)&&(str != "")&&(str != "null")) - { - fc.questVoiceStr = str; - //printf("writevoice str: %s \n", fc.questVoiceStr); - // if (fc.writeSeq==0) - //fc.playText(fc.questVoiceStr); - } - ESP_LOGD(MOUDLENAME,"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; - //printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱 - // 尝试从数据流里 解析数据 - if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 - // 通过msgid来判断 数据流的类别 - char buf[10]; - - // printf("mav_id:%d\n",msg.msgid); - - 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) { //从心跳里面 判断已经解锁 - if (!curr_armed) - { - ESP_LOGD(MOUDLENAME,"armed"); - fc.playText("已解锁"); - } - curr_armed=true; - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 - } else { - if (curr_armed) - { - ESP_LOGD(MOUDLENAME,"disarm"); - fc.playText("已加锁"); - } - curr_armed=false; //心跳里面 判断没有解锁 - 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"; //没写航点 设置为初始化状态 - } - } - //从心跳里面 判断飞机当前的模式 - 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] = "悬停"; - } - break; - case 6: - { - topicPubMsg[12] = "返航"; - } - break; - case 9: - { - topicPubMsg[12] = "降落"; - } - break; - case 16: - { - topicPubMsg[12] = "定点"; - } - break; - default: - { - topicPubMsg[12] = "未知模式"; - } - break; - } - fc.playText(topicPubMsg[12],true); - } - } - 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; - } - } - break; - - 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; - } - //海拔高度 - 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; - - 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; - } - // 纬度 - 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; - } - } - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 - { - mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 - mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 - //日志 - ESP_LOGD(MOUDLENAME,"MsgID:%d No:%d",msg.msgid,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数据 - /*日志*/ - ESP_LOGD(MOUDLENAME,"MsgID:%d re:",msg.msgid,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); - HookStatus hss=motocontrol.gethooktatus(); - ESP_LOGD(MOUDLENAME,"COMMAND_LONG ID:%d,rng:%dcm",fc_hook_cmd,rngalt_cm); - //如果没初始化就退出 - if (initstatus != IS_OK) - { - ESP_LOGE(MOUDLENAME,"not initstatus:%d",initstatus); - break; - } - switch (fc_hook_cmd) - { - //自动放线 - case MAV_CMD_FC_HOOK_AUTODOWN: - { - // HookStatus hss=motocontrol.gethooktatus(); - ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm",hss,rngalt_cm); - //没有激光高度直接退出 - if (rngalt_cm==0) - { - ESP_LOGE(MOUDLENAME,"exit rngalt_cm==0"); - break; - } - if (hss==HS_Locked) - { - Hook_autodown(rngalt_cm); - //最大音量语音播报1次 - if (fc.questVoiceStr!="") - fc.playText(fc.questVoiceStr,true); - } - else - { - if (hss==HS_Stop) - Hook_resume(); - else - ESP_LOGE(MOUDLENAME,"exit hooktatus error"); - } - break; - } - //暂停 - case MAV_CMD_FC_HOOK_PAUSE: - { - ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_PAUSE"); - Hook_stop(); - break; - } - //暂停 - case MAV_CMD_FC_HOOK_RECOVERY: - { - ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_RECOVERY "); - Hook_recovery(); - break; - } - //开始下降 - case MAV_CMD_FC_DESCENTSTART: - { - ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_DESCENTSTART "); - fc.Camera_action_down(); - break; - } - } - - } - break; - //当前执行到的任务号 - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - { - - break; - } - default: - break; - } - } -} - -/** -* @description: 发送主题线程 -*/ -void pubThread() { - /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ - // 创建一个JSON对象 - DynamicJsonDocument doc(2000); // 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(); -} - -/** -* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 -*/ -void flashThread() { - if (digitalRead(23) == HIGH) { - if (isPush) { //点击之后 - //请求注册 ps:发送esp8266的物理地址 到对频主题 - fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); - } - isPush = false; //复位按钮 - } else { - //FLASH按下状态 - isPush = true; - } -} - -/** -* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 -*/ -void mavThread() { - fc.mav_request_data(); -} - -/** -* @description: 向飞控 发送油门指令 -*/ -void chanThread() { - //mav_channels_override(channels); -}