【类 型】:factor 电池总容量发送不用json格式
【原 因】:不用在控制端过滤了 【过 程】:发送时直接过滤出来 放到“电池总容量”字段里 【影 响】: # 类型 包含: # feat:新功能(feature) # fix:修补bug # docs:文档(documentation) # style: 格式(不影响代码运行的变动) # refactor:重构(即不是新增功能,也不是修改bug的代码变动) # test:增加测试 # chore:构建过程或辅助工具的变动
This commit is contained in:
parent
bd776a59d6
commit
5d8f28e545
@ -551,7 +551,7 @@ void FoodCube::mav_parameter_data(const char *param_id)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
// 构造PARAM_REQUEST_READ消息
|
||||
mavlink_msg_param_request_read_pack(1, 200, &msg, 1, 1, param_id, -1);
|
||||
mavlink_msg_param_request_read_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, param_id, -1);
|
||||
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
@ -565,7 +565,7 @@ void FoodCube::mav_request_homePosition()
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
// 构造命令 MAV_CMD_GET_HOME_POSITION 请求
|
||||
mavlink_msg_command_long_pack(1, 200, &msg, 1, 1, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0);
|
||||
mavlink_msg_command_long_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0);
|
||||
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
@ -679,7 +679,6 @@ void FoodCube::mav_send_text(uint8_t severity, const char *text)
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
printf("0x%02X ", buf[i]);
|
||||
;
|
||||
}
|
||||
printf(" \n");
|
||||
// 通过串口发送
|
||||
|
@ -34,8 +34,8 @@ 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:{经度,维度,海拔高度} 16:飞控参数{键:值} 17:返航点
|
||||
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "parameter", "homePorsion"};
|
||||
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度,维度,海拔高度} 16:电池总容量 17:返航点
|
||||
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePorsion"};
|
||||
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
||||
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
||||
/*触发发送 主题*/
|
||||
@ -700,12 +700,15 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
mavlink_param_value_t param_value;
|
||||
mavlink_msg_param_value_decode(&msg, ¶m_value);
|
||||
|
||||
// 获取参数 格式化 JSON 字符串
|
||||
sprintf(buf, "{\"%s\":%.2f}", param_value.param_id, param_value.param_value);
|
||||
|
||||
if (topicPubMsg[16] != buf)
|
||||
// 只处理 "BATT_CAPACITY" 参数
|
||||
if (strcmp(param_value.param_id, "BATT_CAPACITY") == 0)
|
||||
{
|
||||
topicPubMsg[16] = buf;
|
||||
// 将参数值转换为字符串
|
||||
dtostrf(param_value.param_value, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串
|
||||
if (String(buf) != topicPubMsg[16])
|
||||
{
|
||||
topicPubMsg[16] = buf;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
Loading…
Reference in New Issue
Block a user