#include "FoodDeliveryBase.h" /*项目对象*/ //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; //定时发布主题 线程 Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 Ticker flashTicker; //单片机主动 按钮主动发布主题 线程 Ticker chanTicker; //定时向飞控 发送油门指定 void setup() { Serial.begin(115200); //日志串口 Serial1.begin(115200, SERIAL_8N1, 18, 5); //声音模块引串口脚映射 /*初始化*/ fc.connectWifi(); //连接wifi fc.connectMqtt(topicSub, topicSubCount); //连接mqtt fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调 fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) /*异步线程*/ pubTicker.attach(1, pubThread); //定时 发布主题 mavTicker.attach(10, mavThread); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 flashTicker.attach_ms(50, flashThread); //监听 按flash键时 主动发布对频主题 pinMode(23, INPUT_PULLUP); // 记得删除 } void loop() { /*从飞控拿数据*/ fc.comm_receive(mavlink_receiveCallback); /*保持mqtt心跳*/ fc.mqttLoop(topicSub, topicSubCount); } /** * @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") { //恢复飞机为初始状态 topicPubMsg[10] = "1"; //恢复初始状态 } 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); }