【类 型】:feat 接收到控制端发送的refreshRequest主题 重新向飞控发送 各种请求
【原 因】: 【过 程】: 【影 响】:
This commit is contained in:
parent
2814793479
commit
5b4f1454c0
@ -152,6 +152,10 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
||||
fc.mav_command(n, param);
|
||||
}
|
||||
}
|
||||
else if (key == "refreshRequest")
|
||||
{
|
||||
refreshRequest(); // 刷新各种请求
|
||||
}
|
||||
else if (key == "resetState")
|
||||
{ // 恢复飞机为初始状态
|
||||
String todo = value; // 转换值
|
||||
@ -727,12 +731,7 @@ void pubThread()
|
||||
/*检测飞控是否返回数据 (此处检测的电压字段) 没有数据 就像飞控请求*/
|
||||
if (topicPubMsg[1] == "") // 检测电压字段
|
||||
{
|
||||
fc.mav_request_data(); // 请求 设定飞控输出数据流内容
|
||||
delay(100);
|
||||
fc.mav_request_homePosition(); // 请求 飞控返航点
|
||||
delay(100);
|
||||
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
|
||||
delay(100);
|
||||
refreshRequest();
|
||||
}
|
||||
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
|
||||
// 创建一个JSON对象
|
||||
@ -750,6 +749,19 @@ void pubThread()
|
||||
// pingNetTest();
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 刷新各请求
|
||||
*/
|
||||
void refreshRequest()
|
||||
{
|
||||
fc.mav_request_data(); // 请求 设定飞控输出数据流内容
|
||||
delay(100);
|
||||
fc.mav_request_homePosition(); // 请求 飞控返航点
|
||||
delay(100);
|
||||
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
|
||||
delay(100);
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 向飞控 发送油门指令
|
||||
*/
|
||||
|
Loading…
Reference in New Issue
Block a user