【类 型】:fix 改用 不检测是否数据是否更新 直接全部发送
【原 因】:解决控制端 或者 飞机掉线问题 保证数据最新 且同步 【过 程】: 【影 响】: # 类型 包含: # feat:新功能(feature) # fix:修补bug # docs:文档(documentation) # style: 格式(不影响代码运行的变动) # refactor:重构(即不是新增功能,也不是修改bug的代码变动) # test:增加测试 # chore:构建过程或辅助工具的变动
This commit is contained in:
parent
ed39e11154
commit
816f3a4366
@ -38,7 +38,6 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
|
|||||||
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "parameter", "homePorsion"};
|
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "parameter", "homePorsion"};
|
||||||
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
||||||
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
||||||
String oldMsg[topicPubCount] = {""}; // 记录旧的值 用来对比有没有更新
|
|
||||||
/*触发发送 主题*/
|
/*触发发送 主题*/
|
||||||
// 0:对频信息
|
// 0:对频信息
|
||||||
String topicHandle[] = {"crosFrequency"};
|
String topicHandle[] = {"crosFrequency"};
|
||||||
@ -153,10 +152,6 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
fc.mav_command(n, param);
|
fc.mav_command(n, param);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (key == "getPlaneState") // 终端主动get飞机状态
|
|
||||||
{
|
|
||||||
forcePubMsg(); // 强制发送飞机 现在的所有状态值
|
|
||||||
}
|
|
||||||
else if (key == "resetState")
|
else if (key == "resetState")
|
||||||
{ // 恢复飞机为初始状态
|
{ // 恢复飞机为初始状态
|
||||||
String todo = value; // 转换值
|
String todo = value; // 转换值
|
||||||
@ -733,7 +728,9 @@ void pubThread()
|
|||||||
if (topicPubMsg[1] == "") // 检测电压字段
|
if (topicPubMsg[1] == "") // 检测电压字段
|
||||||
{
|
{
|
||||||
fc.mav_request_data(); // 请求 设定飞控输出数据流内容
|
fc.mav_request_data(); // 请求 设定飞控输出数据流内容
|
||||||
|
delay(100);
|
||||||
fc.mav_request_homePosition(); // 请求 飞控返航点
|
fc.mav_request_homePosition(); // 请求 飞控返航点
|
||||||
|
delay(100);
|
||||||
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
|
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
|
||||||
delay(100);
|
delay(100);
|
||||||
}
|
}
|
||||||
@ -742,18 +739,9 @@ void pubThread()
|
|||||||
DynamicJsonDocument doc(2000); // 缓冲区
|
DynamicJsonDocument doc(2000); // 缓冲区
|
||||||
// 遍历 有更新的数据 组成一个json对象
|
// 遍历 有更新的数据 组成一个json对象
|
||||||
for (int i = 0; i < topicPubCount; i++)
|
for (int i = 0; i < topicPubCount; i++)
|
||||||
{
|
|
||||||
if (i == 0 || i == 10) // 0心跳包 10飞机状态state包 每每向心跳主题发布信息
|
|
||||||
{
|
{
|
||||||
doc[topicPub[i]] = topicPubMsg[i];
|
doc[topicPub[i]] = topicPubMsg[i];
|
||||||
}
|
}
|
||||||
else if (topicPubMsg[i] != oldMsg[i])
|
|
||||||
{
|
|
||||||
// 非心跳 非飞机状态 有更新 录入成员
|
|
||||||
doc[topicPub[i]] = topicPubMsg[i];
|
|
||||||
oldMsg[i] = topicPubMsg[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// 将JSON对象序列化为JSON字符串
|
// 将JSON对象序列化为JSON字符串
|
||||||
String jsonString;
|
String jsonString;
|
||||||
serializeJson(doc, jsonString);
|
serializeJson(doc, jsonString);
|
||||||
@ -762,24 +750,6 @@ void pubThread()
|
|||||||
// pingNetTest();
|
// pingNetTest();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: 强制遍历发送主题线程 ps:不管内容如何哪怕是空字符
|
|
||||||
*/
|
|
||||||
void forcePubMsg()
|
|
||||||
{
|
|
||||||
// 创建一个JSON对象
|
|
||||||
DynamicJsonDocument doc(2000); // 缓冲区
|
|
||||||
// 遍历 有更新的数据 组成一个json对象
|
|
||||||
for (int i = 0; i < topicPubCount; i++)
|
|
||||||
{
|
|
||||||
doc[topicPub[i]] = topicPubMsg[i];
|
|
||||||
}
|
|
||||||
// 将JSON对象序列化为JSON字符串
|
|
||||||
String jsonString;
|
|
||||||
serializeJson(doc, jsonString);
|
|
||||||
fc.pubMQTTmsg("planeState", jsonString);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @description: 向飞控 发送油门指令
|
* @description: 向飞控 发送油门指令
|
||||||
*/
|
*/
|
||||||
|
@ -8,7 +8,6 @@
|
|||||||
|
|
||||||
extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
|
extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
|
||||||
extern void pubThread();
|
extern void pubThread();
|
||||||
extern void forcePubMsg();
|
|
||||||
extern void writeRoute(String topicStr);
|
extern void writeRoute(String topicStr);
|
||||||
extern void mavlink_receiveCallback(uint8_t c);
|
extern void mavlink_receiveCallback(uint8_t c);
|
||||||
extern const String topicPub[];
|
extern const String topicPub[];
|
||||||
|
Loading…
Reference in New Issue
Block a user