Food/FoodDelivery
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

【类 型】：fix

Browse Source 
【主	题】：修复bug
【描	述】：
	[原因]：不需要订阅多主题 制定粤'cmd/macadd'  参数的主题数组就不需要了
	[过程]：mqtt订阅函数参数删除 ;维持心跳函数 参数删除
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
szdot 2024-07-02 03:16:26 +08:00
parent b708b47c4b
commit db3e2ec931
3 changed files with 378 additions and 382 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

736
FoodDelivery.ino
View File

@ -38,7 +38,7 @@ void setup() {
Serial1.begin(115200, SERIAL_8N1, 18, 5); //声音模块引串口脚映射 Serial1.begin(115200, SERIAL_8N1, 18, 5); //声音模块引串口脚映射
/*初始化*/ /*初始化*/
fc.connectWifi(); //连接wifi fc.connectWifi(); //连接wifi
fc.connectMqtt(topicSub, topicSubCount); //连接mqtt fc.connectMqtt("cmd"); //连接mqtt
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调 fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
@ -54,7 +54,7 @@ void loop() {
/*从飞控拿数据*/ /*从飞控拿数据*/
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
/*保持mqtt心跳*/ /*保持mqtt心跳*/
fc.mqttLoop(topicSub, topicSubCount); fc.mqttLoop("cmd");
} }
/** /**
@ -100,7 +100,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
*第10位：航点继续 *第10位：航点继续
*第11位：磁罗盘校准 *第11位：磁罗盘校准
*/ */
String todoJson = value; //转换值 String todoJson = value; //转换值
/* json 反序列化 */ /* json 反序列化 */
DynamicJsonDocument doc(500); DynamicJsonDocument doc(500);
deserializeJson(doc, todoJson); deserializeJson(doc, todoJson);
@ -142,8 +142,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
} else if (key == "getPlaneState") { //获取飞机状态 } else if (key == "getPlaneState") { //获取飞机状态
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态 fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态
} else if (key == "resetState") { //恢复飞机为初始状态 } else if (key == "resetState") { //恢复飞机为初始状态
String todo = value; //转换值 String todo = value; //转换值
topicPubMsg[10] = todo; //恢复初始状态 topicPubMsg[10] = todo; //恢复初始状态
} else if (key == "chan1") { } else if (key == "chan1") {
uint16_t todo = value; //转换值 uint16_t todo = value; //转换值
fc.channels[0] = todo; //恢复初始状态 fc.channels[0] = todo; //恢复初始状态
@ -162,32 +162,32 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_channels_override(fc.channels); //油门控制
} else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 } else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
uint16_t todo = value; uint16_t todo = value;
switch(todo){ switch (todo) {
case 0: case 0:
{ {
//收钩 //收钩
} }
break; break;
case 1: case 1:
{ {
//放钩 //放钩
} }
break; break;
case 2: case 2:
{ {
//暂停 //暂停
} }
break; break;
case 3: case 3:
{ {
//继续 //继续
} }
break; break;
case 4: case 4:
{ {
//重置重量 //重置重量
} }
break; break;
} }
} else if (key == "cameraController") { //云台相机控制 } else if (key == "cameraController") { //云台相机控制
String todoJson = value; //转换值 String todoJson = value; //转换值
@ -202,7 +202,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
//相机控制 //相机控制
if (item == 0) { //0:一键回中 if (item == 0) { //0:一键回中
uint8_t stopCommand[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; //回中前 强制 俯仰旋转 停止 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); fc.udpSendToCamera(stopCommand, stopLen);
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
size_t len = sizeof(command); size_t len = sizeof(command);
@ -220,388 +220,386 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} }
} }
}
/** /**
* @description: 写入航点 * @description: 写入航点
* @param {String} todo mqtt订阅执行任务的Json字符串 * @param {String} todo mqtt订阅执行任务的Json字符串
*/ */
void writeRoute(String todoJson) { void writeRoute(String todoJson) {
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln("正在写航点"); // 提示正在写入中 fc.logln("正在写航点"); // 提示正在写入中
return; 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<JsonObject>();
// 写入航点
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);
}
} }
//改变飞机状态 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<JsonObject>();
// 写入航点
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缓存数据之后 解析数据执行的回调 * @description: 从串口拿到mavlink缓存数据之后 解析数据执行的回调
* @param {mavlink_message_t*} pMsg mavlink数据信息指针 * @param {mavlink_message_t*} pMsg mavlink数据信息指针
* @param {mavlink_status_t*} pStatus * @param {mavlink_status_t*} pStatus
* @param {uint8_t} c 串口读取的缓存 * @param {uint8_t} c 串口读取的缓存
*/ */
void mavlink_receiveCallback(uint8_t c) { void mavlink_receiveCallback(uint8_t c) {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
// 尝试从数据流里 解析数据 // 尝试从数据流里 解析数据
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
// 通过msgid来判断 数据流的类别 // 通过msgid来判断 数据流的类别
char buf[10]; char buf[10];
switch (msg.msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
{ {
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据 mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据
sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态 sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态
topicPubMsg[0] = buf; //心跳主题 topicPubMsg[0] = buf; //心跳主题
//从心跳里判断 飞机是否解锁 解锁改变飞机状态 //从心跳里判断 飞机是否解锁 解锁改变飞机状态
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
} else { //心跳里面 判断没有解锁 } else { //心跳里面 判断没有解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态
} else { } else {
topicPubMsg[10] = "1"; //没写航点 设置为初始化状态 topicPubMsg[10] = "1"; //没写航点 设置为初始化状态
}
}
//从心跳里面 判断飞机当前的模式
switch (heartbeat.custom_mode) {
case 0:
{
topicPubMsg[12] = "Stabilize自稳";
} }
} break;
//从心跳里面 判断飞机当前的模式 case 2:
switch (heartbeat.custom_mode) { {
case 0: topicPubMsg[12] = "AltHold定高";
{ }
topicPubMsg[12] = "Stabilize自稳"; break;
} case 3:
break; {
case 2: topicPubMsg[12] = "Auto自动";
{ }
topicPubMsg[12] = "AltHold定高"; break;
} case 4:
break; {
case 3: topicPubMsg[12] = "Guided引导";
{ }
topicPubMsg[12] = "Auto自动"; break;
} case 5:
break; {
case 4: topicPubMsg[12] = "Loiter留待";
{ }
topicPubMsg[12] = "Guided引导"; break;
} case 6:
break; {
case 5: topicPubMsg[12] = "RTL返航";
{ }
topicPubMsg[12] = "Loiter留待"; break;
} case 9:
break; {
case 6: topicPubMsg[12] = "Land降落";
{ }
topicPubMsg[12] = "RTL返航"; break;
} case 16:
break; {
case 9: topicPubMsg[12] = "PosHold定点";
{ }
topicPubMsg[12] = "Land降落"; break;
} default:
break; {
case 16: topicPubMsg[12] = "Unknown未知";
{ }
topicPubMsg[12] = "PosHold定点"; break;
}
break;
default:
{
topicPubMsg[12] = "Unknown未知";
}
break;
}
} }
break; }
break;
case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS
{ {
mavlink_sys_status_t sys_status; // 解构的数据放到这个对象 mavlink_sys_status_t sys_status; // 解构的数据放到这个对象
mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据 mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
// 电压 // 电压
sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000); sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
if (topicPubMsg[1] != buf) { // 有更新 则更新数据 if (topicPubMsg[1] != buf) { // 有更新 则更新数据
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; // 电流
sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE if (topicPubMsg[2] != buf) {
{ topicPubMsg[2] = buf;
mavlink_param_value_t param_value;
mavlink_msg_param_value_decode(&msg, &param_value);
} }
break; // 电池电量
sprintf(buf, "%d", sys_status.battery_remaining);
case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU if (topicPubMsg[3] != buf) {
{ topicPubMsg[3] = buf;
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg, &raw_imu);
} }
break; }
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置 case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE
{ {
mavlink_global_position_int_t global_position_int; mavlink_param_value_t param_value;
mavlink_msg_global_position_int_decode(&msg, &global_position_int); mavlink_msg_param_value_decode(&msg, &param_value);
// 高度 }
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); break;
if (topicPubMsg[4] != buf) {
topicPubMsg[4] = buf; case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
} {
//{经度，维度，海拔高度} mavlink_raw_imu_t raw_imu;
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", mavlink_msg_raw_imu_decode(&msg, &raw_imu);
global_position_int.lon, }
global_position_int.lat, break;
(double)global_position_int.alt / 1000);
if (topicPubMsg[15] != buf) { case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置
topicPubMsg[15] = buf; {
} 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; //{经度，维度，海拔高度}
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}",
case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 global_position_int.lon,
{ global_position_int.lat,
mavlink_vfr_hud_t vfr_hud; (double)global_position_int.alt / 1000);
mavlink_msg_vfr_hud_decode(&msg, &vfr_hud); if (topicPubMsg[15] != buf) {
// 对地速度 ps:飞行速度 topicPubMsg[15] = buf;
sprintf(buf, "%.2f", vfr_hud.groundspeed);
if (topicPubMsg[5] != buf) {
topicPubMsg[5] = buf;
}
} }
break; }
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘
{ {
mavlink_gps_raw_int_t gps_raw_int; mavlink_vfr_hud_t vfr_hud;
mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int); mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
// 卫星数 // 对地速度 ps:飞行速度
sprintf(buf, "%d", gps_raw_int.satellites_visible); sprintf(buf, "%.2f", vfr_hud.groundspeed);
if (topicPubMsg[6] != buf) { if (topicPubMsg[5] != buf) {
topicPubMsg[6] = buf; topicPubMsg[5] = 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; }
break;
case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS
{ {
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_gps_raw_int_t gps_raw_int;
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
//日志 // 卫星数
fc.logln("MsgID:"); sprintf(buf, "%d", gps_raw_int.satellites_visible);
fc.logln(msg.msgid); if (topicPubMsg[6] != buf) {
fc.logln("No:"); topicPubMsg[6] = buf;
fc.logln(mission_request.seq);
//飞控 反馈当前要写入航点的序号
fc.writeSeq = mission_request.seq;
} }
break; // 纬度
sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 if (topicPubMsg[7] != buf) {
{ topicPubMsg[7] = buf;
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; // 经度
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: case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈
break; {
} 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: 发送主题线程 * @description: 发送主题线程
*/ */
void pubThread() { void pubThread() {
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
// 创建一个JSON对象 // 创建一个JSON对象
DynamicJsonDocument doc(2000); // 缓冲区 DynamicJsonDocument doc(2000); // 缓冲区
//遍历 有更新的数据 组成一个json对象 //遍历 有更新的数据 组成一个json对象
for (int i = 0; i < topicPubCount; i++) { for (int i = 0; i < topicPubCount; i++) {
if (topicPubMsg[i] != oldMsg[i]) { if (topicPubMsg[i] != oldMsg[i]) {
if (i == 0) { //心跳包 每每向心跳主题发布信息 if (i == 0) { //心跳包 每每向心跳主题发布信息
//启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 //启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
if (fc.getIsInit()) { if (fc.getIsInit()) {
fc.setIsInit(false); fc.setIsInit(false);
fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容 fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容
}
//设置对象成员 ps:心跳
doc[topicPub[0]] = topicPubMsg[0];
} else { //非心跳 有更新 录入成员
doc[topicPub[i]] = topicPubMsg[i];
oldMsg[i] = topicPubMsg[i];
} }
//设置对象成员 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:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() { void flashThread() {
if (digitalRead(23) == LOW) { if (digitalRead(23) == LOW) {
if (isPush) { //点击之后 if (isPush) { //点击之后
//请求注册 ps:发送esp8266的物理地址 到对频主题 //请求注册 ps:发送esp8266的物理地址 到对频主题
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
}
isPush = false; //复位按钮
} else {
//FLASH按下状态
isPush = true;
} }
isPush = false; //复位按钮
} else {
//FLASH按下状态
isPush = true;
} }
}
/** /**
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
*/ */
void mavThread() { void mavThread() {
fc.mav_request_data(); fc.mav_request_data();
} }
/** /**
* @description: 向飞控 发送油门指令 * @description: 向飞控 发送油门指令
*/ */
void chanThread() { void chanThread() {
//mav_channels_override(channels); //mav_channels_override(channels);
} }

20
FoodDeliveryBase.cpp
View File

@ -121,10 +121,9 @@ void FoodCube::connectWifi() {
/** /**
* @description: 连接mqtt * @description: 连接mqtt
* @param {String[]} topicSub 主题数组 * @param {String} topicSub 订阅主题
* @param {int} topicSubCount 数组总数
*/ */
void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { void FoodCube::connectMqtt(String topicSub) {
/*尝试连接mqtt*/ /*尝试连接mqtt*/
if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) { if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) {
logln("MQTT Server Connected."); logln("MQTT Server Connected.");
@ -134,7 +133,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
logln(macAdd); logln(macAdd);
/*连接成功 订阅主题*/ /*连接成功 订阅主题*/
//订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback //订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
subscribeTopic("cmd", 1); subscribeTopic(topicSub, 1);
playText("服务器,已连接"); playText("服务器,已连接");
} else { } else {
//失败返回状态码 //失败返回状态码
@ -146,14 +145,13 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
/** /**
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳 * @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
* @param {String[]} topicSub 主题数组 * @param {String} topicSub 订阅主题
* @param {int} topicSubCount 数组总数
*/ */
void FoodCube::mqttLoop(String topicSub[], int topicSubCount) { void FoodCube::mqttLoop(String topicSub) {
if (mqttClient->connected()) { //检测 如果开发板成功连接服务器 if (mqttClient->connected()) { //检测 如果开发板成功连接服务器
mqttClient->loop(); // 保持心跳 mqttClient->loop(); // 保持心跳
} else { // 如果开发板未能成功连接服务器 } else { // 如果开发板未能成功连接服务器
connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器 connectMqtt(topicSub); // 则尝试连接服务器
} }
} }

4
FoodDeliveryBase.h
View File

@ -46,8 +46,8 @@ public:
void connectWifi(); void connectWifi();
/*mqtt*/ /*mqtt*/
PubSubClient* mqttClient; //指向 mqtt服务器连接 对象 PubSubClient* mqttClient; //指向 mqtt服务器连接 对象
void connectMqtt(String topicSub[], int topicSubCount); void connectMqtt(String topicSub);
void mqttLoop(String topicSub[], int topicSubCount); void mqttLoop(String topicSub);
void subscribeTopic(String topicString, int Qos); void subscribeTopic(String topicString, int Qos);
void pubMQTTmsg(String topicString, String messageString); void pubMQTTmsg(String topicString, String messageString);
/*串口输出*/ /*串口输出*/