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

【类 型】：faat 飞控磁罗盘校准

Browse Source 
【原  因】：
【过  程】：1接收到命令开始校准 2通过mqtt向前端发送进度 和 结果
【影  响】：
This commit is contained in:
tk 2024-08-20 20:32:48 +08:00
parent 443111cc8d
commit e10fe665b9
3 changed files with 180 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
lib/mavlink/ardupilotmega/mavlink_msg_mag_cal_report.h
View File

@ -19,7 +19,8 @@ typedef struct __mavlink_mag_cal_report_t {
uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/ uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/
uint8_t cal_status; /*< Calibration Status.*/ uint8_t cal_status; /*< Calibration Status.*/
uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/ uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/
}) mavlink_mag_cal_report_t; })
mavlink_mag_cal_report_t;
#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44 #define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44
#define MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN 44 #define MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN 44
@ -29,14 +30,14 @@ typedef struct __mavlink_mag_cal_report_t {
#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 #define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36
#define MAVLINK_MSG_ID_192_CRC 36 #define MAVLINK_MSG_ID_192_CRC 36
#if MAVLINK_COMMAND_24BIT #if MAVLINK_COMMAND_24BIT
#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ #define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT \
{ \
192, \ 192, \
"MAG_CAL_REPORT", \ "MAG_CAL_REPORT", \
14, \ 14, \
{ { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ { \
{"compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id)}, \
{"cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask)}, \ {"cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask)}, \
{"cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status)}, \ {"cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status)}, \
{"autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved)}, \ {"autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved)}, \
@ -53,10 +54,12 @@ typedef struct __mavlink_mag_cal_report_t {
} \ } \
} }
#else #else
#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ #define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT \
{ \
"MAG_CAL_REPORT", \ "MAG_CAL_REPORT", \
14, \ 14, \
{ { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ { \
{"compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id)}, \
{"cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask)}, \ {"cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask)}, \
{"cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status)}, \ {"cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status)}, \
{"autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved)}, \ {"autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved)}, \
@ -366,7 +369,6 @@ static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf
// MESSAGE MAG_CAL_REPORT UNPACKING // MESSAGE MAG_CAL_REPORT UNPACKING
/** /**
* @brief Get field compass_id from mag_cal_report message * @brief Get field compass_id from mag_cal_report message
* *

19
src/FoodDeliveryBase.cpp
View File

@ -753,7 +753,7 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[])
cmd.param1 = param[0]; // 0:加锁 1：解锁 cmd.param1 = param[0]; // 0:加锁 1：解锁
cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁 cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁
} }
if (controlType == 6) else if (controlType == 6)
{ // 测试起飞 { // 测试起飞
float p = (float)param[0]; float p = (float)param[0];
cmd.command = MAV_CMD_NAV_TAKEOFF; cmd.command = MAV_CMD_NAV_TAKEOFF;
@ -765,16 +765,21 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[])
cmd.param6 = 0; cmd.param6 = 0;
cmd.param7 = p; // 起飞高度 cmd.param7 = p; // 起飞高度
} }
if (controlType == 8) else if (controlType == 8)
{ // 降落 { // 降落
cmd.command = MAV_CMD_NAV_LAND; cmd.command = MAV_CMD_NAV_LAND;
} }
if (controlType == 11) else if (controlType == 11)
{ // 磁罗盘校准 {
// 磁罗盘校准
cmd.command = MAV_CMD_DO_START_MAG_CAL; cmd.command = MAV_CMD_DO_START_MAG_CAL;
cmd.param1 = 0; cmd.param1 = 0; // 磁罗盘的bitmask（0表示所有）
cmd.param2 = 1; cmd.param2 = 0; // 自动重试（0=不重试, 1=重试）
cmd.param3 = 1; cmd.param3 = 0; // 是否自动保存（0=需要用户输入, 1=自动保存）
cmd.param4 = 0; // 延迟（秒）
cmd.param5 = 0; // 自动重启（0=用户重启, 1=自动重启）
cmd.param6 = 0; // 保留空参数
cmd.param7 = 0; // 保留空参数
} }
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd); mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);

102
src/commser.cpp
View File

@ -14,12 +14,12 @@ extern void Hook_recovery();
static const char *MOUDLENAME = "COMMSER"; static const char *MOUDLENAME = "COMMSER";
/*项目对象*/ /*项目对象*/
// char *ssid = "szMate40pro"; // wifi帐号 char *ssid = "szMate40pro"; // wifi帐号
// char *password = "63587839ab"; // wifi密码 char *password = "63587839ab"; // wifi密码
// char* ssid = "szdot"; //wifi帐号 // char* ssid = "szdot"; //wifi帐号
// char* password = "63587839ab"; //wifi密码 // char* password = "63587839ab"; //wifi密码
char *ssid = "flicube"; // wifi帐号 // char *ssid = "flicube"; // wifi帐号
char *password = "fxmf0622"; // wifi密码 // char *password = "fxmf0622"; // wifi密码
// char *ssid = "fxmf_sc01"; // 4g wifi帐号 // char *ssid = "fxmf_sc01"; // 4g wifi帐号
// char *password = "12345678"; // 4g wifi密码 // char *password = "12345678"; // 4g wifi密码
char *mqttServer = "szdot.top"; // mqtt地址 char *mqttServer = "szdot.top"; // mqtt地址
@ -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:返航点 18:文本信息 19：返航速度 // 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:定位状态 8:磁罗盘校准进度 9：磁罗盘校准是否成功 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息 19：返航速度
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"}; const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "fixType", "completionPct", "reportCal", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
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
/*触发发送 主题*/ /*触发发送 主题*/
@ -123,7 +123,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
} }
else else
{ {
uint16_t chan[] = {1500, 1500, 1300, 1500}; // 除了加解锁模式 油门全部控制居中 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
fc.mav_channels_override(chan); // 控制油门 fc.mav_channels_override(chan); // 控制油门
} }
if (n == 6) if (n == 6)
@ -139,17 +139,22 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
{ // 5 航点执行 { // 5 航点执行
fc.mav_set_mode(3); // 飞控设置成auto自动模式 fc.mav_set_mode(3); // 飞控设置成auto自动模式
} }
else if (n == 8) else if (n == 8) // 8降落
{ // 8降落* {
fc.mav_command(n, param); fc.mav_command(n, param);
} }
else if (n == 9) else if (n == 9)
{ // 9返航 { // 9返航
fc.mav_set_mode(6); // 飞控设置成RTL返航 fc.mav_set_mode(6); // 飞控设置成RTL返航
} }
else if (n == 11) else if (n == 11) // 11磁罗盘校准
{ // 11磁罗盘校准 {
/*初始化磁罗盘校准进度 和 校准结果*/
topicPubMsg[8] = "";
topicPubMsg[9] = "";
/*校准*/
fc.mav_command(n, param); fc.mav_command(n, param);
fc.playText("校准磁罗盘");
} }
} }
else if (key == "refreshRequest") else if (key == "refreshRequest")
@ -378,11 +383,8 @@ void mavlink_receiveCallback(uint8_t c)
home_pos.latitude, // 纬度，以degE7单位传递 home_pos.latitude, // 纬度，以degE7单位传递
home_pos.z); // 相对高度，单位为米 home_pos.z); // 相对高度，单位为米
if (topicPubMsg[17] != buf)
{
topicPubMsg[17] = buf; topicPubMsg[17] = buf;
} }
}
case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: // #49 飞机原点 case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: // #49 飞机原点
{ {
mavlink_gps_global_origin_t gps_org; // 解构的数据放到这个对象 mavlink_gps_global_origin_t gps_org; // 解构的数据放到这个对象
@ -399,6 +401,31 @@ void mavlink_receiveCallback(uint8_t c)
// } // }
} }
break; break;
case MAVLINK_MSG_ID_MAG_CAL_REPORT: // #192 磁罗盘校准结果
{
mavlink_mag_cal_report_t report;
mavlink_msg_mag_cal_report_decode(&msg, &report);
if (report.cal_status == MAG_CAL_SUCCESS)
{
topicPubMsg[9] = "successful"; // 校准成功
}
else
{
topicPubMsg[9] = "failed"; // 校准失败
}
}
break;
case MAVLINK_MSG_ID_MAG_CAL_PROGRESS: // #191 磁罗盘校准进度
{
mavlink_mag_cal_progress_t progress;
mavlink_msg_mag_cal_progress_decode(&msg, &progress);
sprintf(buf, "%d", progress.completion_pct);
topicPubMsg[8] = buf;
}
break;
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
{ {
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
@ -588,46 +615,38 @@ void mavlink_receiveCallback(uint8_t c)
{ {
topicPubMsg[6] = buf; topicPubMsg[6] = 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) switch (gps_raw_int.fix_type)
{ {
case 0: case 0:
topicPubMsg[9] = "No Fix"; // 无定位 topicPubMsg[7] = "No Fix"; // 无定位
break; break;
case 1: case 1:
topicPubMsg[9] = "Bad Fix"; // 差的定位（无效数据） topicPubMsg[7] = "Bad Fix"; // 差的定位（无效数据）
break; break;
case 2: case 2:
topicPubMsg[9] = "2D Fix"; // 2D定位（只有经纬度） topicPubMsg[7] = "2D Fix"; // 2D定位（只有经纬度）
break; break;
case 3: case 3:
topicPubMsg[9] = "3D Fix"; // 3D定位（经纬度和高度） topicPubMsg[7] = "3D Fix"; // 3D定位（经纬度和高度）
break; break;
case 4: case 4:
topicPubMsg[9] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS） topicPubMsg[7] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS）
break; break;
case 5: case 5:
topicPubMsg[9] = "Float"; // RTK浮动解（精度较高，但不如固定解） topicPubMsg[7] = "Float"; // RTK浮动解（精度较高，但不如固定解）
break; break;
case 6: case 6:
topicPubMsg[9] = "RTK Fixed"; // RTK固定解（高精度定位） topicPubMsg[7] = "RTK Fixed"; // RTK固定解（高精度定位）
break; break;
default: default:
topicPubMsg[9] = "Unknown"; // 未知状态 topicPubMsg[7] = "Unknown"; // 未知状态
break; break;
} }
// 纬度
// sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
// 经度
// sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
} }
break; break;
@ -761,20 +780,27 @@ void pubThread()
} }
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
// 创建一个JSON对象 // 创建一个JSON对象
DynamicJsonDocument doc(2000); // 缓冲区 DynamicJsonDocument doc(512); // 缓冲区
// 遍历 有更新的数据 组成一个json对象 // 遍历 有更新的数据 组成一个json对象
for (int i = 0; i < topicPubCount; i++) for (int i = 0; i < topicPubCount; i++)
{ {
if (i == 0 || i == 10) // 心跳每次发 飞机状态 需要每次必发的写在这里 if (i == 0 || i == 10) // 每次必发的写在这里
{ {
doc[topicPub[i]] = topicPubMsg[i]; doc[topicPub[i]] = topicPubMsg[i];
} }
else // 其余有新值就发 else if (i == 8 || i == 9) // 有新值发 但是不重置
{ {
if (topicPubMsg[i] != "") if (topicPubMsg[i] != "")
{ {
doc[topicPub[i]] = topicPubMsg[i]; doc[topicPub[i]] = topicPubMsg[i];
topicPubMsg[i] = ""; }
}
else // 其余有新值就发 发出后重置
{
if (topicPubMsg[i] != "")
{
doc[topicPub[i]] = topicPubMsg[i];
topicPubMsg[i] = ""; // 重置
} }
} }
} }