Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

【类 型】：feat 添加MAVLINK_MSG_ID_STATUSTEXT 信息播报

Browse Source 
【原  因】：
【过  程】：
【影  响】：
This commit is contained in:
tk 2024-08-09 17:26:04 +08:00
parent 9741ec5d70
commit d200fc0f46
3 changed files with 57 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/FoodDeliveryBase.cpp
View File

@ -559,14 +559,30 @@ void FoodCube::mav_parameter_data(const char *param_id)
/** /**
* @description: 向飞控请求返航点的位置数据 * @description: 向飞控请求返航点的位置数据
*/ */
void FoodCube::mav_request_homePosition() void FoodCube::mav_request_home_position()
{ {
mavlink_message_t msg; mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// 构造命令 MAV_CMD_GET_HOME_POSITION 请求 // 组装命令
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); mavlink_command_long_t command = {0};
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); command.target_system = 1; // 目标飞控的系统ID
command.target_component = 1; // 目标飞控的组件ID
command.command = MAV_CMD_GET_HOME_POSITION;
command.confirmation = 0;
command.param1 = 0; // 参数保留，不使用
command.param2 = 0;
command.param3 = 0;
command.param4 = 0;
command.param5 = 0;
command.param6 = 0;
command.param7 = 0;
// 打包命令消息
mavlink_msg_command_long_encode(1, 1, &msg, &command);
// 将消息转换为字节流并发送
int len = mavlink_msg_to_send_buffer(buf, &msg);
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }

2
src/FoodDeliveryBase.h
View File

@ -64,7 +64,7 @@ public:
String setNBit(String str, uint8_t n, uint8_t i); String setNBit(String str, uint8_t n, uint8_t i);
void mav_request_data(); void mav_request_data();
void mav_parameter_data(const char *param_id); void mav_parameter_data(const char *param_id);
void mav_request_homePosition(); void mav_request_home_position();
void comm_receive(void (*pFun)(uint8_t)); void comm_receive(void (*pFun)(uint8_t));
void mav_mission_count(uint8_t taskcount); void mav_mission_count(uint8_t taskcount);
void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z); void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z);

53
src/commser.cpp
View File

@ -34,8 +34,8 @@ uint32_t udpServerPort = 37260; // 云台相机端口
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */ /* 发布 主题 ps:以下是登记发布json内容的组成元素 */
// 登记 json成员名字 // 登记 json成员名字
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度} 16:电池总容量 17:返航点 // 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度} 16:电池总容量 17:返航点 18:文本信息
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePorsion"}; const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePorsion", "statusText"};
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
/*触发发送 主题*/ /*触发发送 主题*/
@ -383,6 +383,7 @@ void mavlink_receiveCallback(uint8_t c)
} }
curr_armed = true; curr_armed = true;
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭
} }
else else
{ {
@ -392,7 +393,8 @@ void mavlink_receiveCallback(uint8_t c)
fc.playText("[v1]已加锁"); fc.playText("[v1]已加锁");
} }
curr_armed = false; // 心跳里面 判断没有解锁 curr_armed = false; // 心跳里面 判断没有解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为未解锁状态 0xxxx 第四位为0 代表未解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 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); // 设置为航点写入成功状态
@ -516,24 +518,39 @@ void mavlink_receiveCallback(uint8_t c)
} }
break; break;
case MAVLINK_MSG_ID_HOME_POSITION: // #242 返航点位置 case 242: // #242 返航点位置MAVLINK_MSG_ID_HOME_POSITION
{ {
fc.pubMQTTmsg("demo", "homep"); fc.pubMQTTmsg("demo", "homep");
mavlink_home_position_t home_position; // mavlink_home_position_t home_position;
mavlink_msg_home_position_decode(&msg, &home_position); // mavlink_msg_home_position_decode(&msg, &home_position);
// 提取经纬度并转换为度数 // // 提取经纬度并转换为度数
double latitude = home_position.latitude / 1e7; // double latitude = home_position.latitude / 1e7;
double longitude = home_position.longitude / 1e7; // double longitude = home_position.longitude / 1e7;
// 格式化为 JSON 字符串，包含经纬度 // // 格式化为 JSON 字符串，包含经纬度
sprintf(buf, "{\"lng\":%.7f,\"lat\":%.7f}", // sprintf(buf, "{\"lng\":%.7f,\"lat\":%.7f}",
longitude, latitude); // longitude, latitude);
// 更新 topicPubMsg 中的相关字段 // // 更新 topicPubMsg 中的相关字段
if (topicPubMsg[17] != buf) // if (topicPubMsg[17] != buf)
// {
// topicPubMsg[17] = buf;
// }
}
break;
case MAVLINK_MSG_ID_STATUSTEXT: // #253 文本信息
{ {
topicPubMsg[17] = buf; char buf[51]; // 定义一个局部缓冲区，大小为 51
mavlink_statustext_t statustext;
mavlink_msg_statustext_decode(&msg, &statustext);
// 解析文本信息
strncpy(buf, statustext.text, sizeof(statustext.text));
buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾
if (topicPubMsg[18] != buf)
{
topicPubMsg[18] = buf;
} }
} }
break; break;
@ -730,6 +747,7 @@ void pubThread()
{ {
doc[topicPub[i]] = topicPubMsg[i]; doc[topicPub[i]] = topicPubMsg[i];
} }
// fc.mav_request_home_position(); // 请求 飞控返航点
// 将JSON对象序列化为JSON字符串 // 将JSON对象序列化为JSON字符串
String jsonString; String jsonString;
serializeJson(doc, jsonString); serializeJson(doc, jsonString);
@ -743,12 +761,13 @@ void pubThread()
*/ */
void refreshRequest() void refreshRequest()
{ {
fc.mav_request_data(); // 请求 设定飞控输出数据流内容 fc.mav_request_home_position();
delay(100); delay(100);
fc.mav_request_homePosition(); // 请求 飞控返航点 fc.mav_request_data(); // 请求 设定飞控输出数据流内容
delay(100); delay(100);
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值 fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
delay(100); delay(100);
fc.pubMQTTmsg("demo", "init");
} }
/** /**