From e10fe665b981d713ca488cc1e333d6c09624e499 Mon Sep 17 00:00:00 2001 From: tk Date: Tue, 20 Aug 2024 20:32:48 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Afaat=20=E9=A3=9E=E6=8E=A7=E7=A3=81=E7=BD=97=E7=9B=98?= =?UTF-8?q?=E6=A0=A1=E5=87=86=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A?= =?UTF-8?q?1=E6=8E=A5=E6=94=B6=E5=88=B0=E5=91=BD=E4=BB=A4=E5=BC=80?= =?UTF-8?q?=E5=A7=8B=E6=A0=A1=E5=87=86=202=E9=80=9A=E8=BF=87mqtt=E5=90=91?= =?UTF-8?q?=E5=89=8D=E7=AB=AF=E5=8F=91=E9=80=81=E8=BF=9B=E5=BA=A6=20?= =?UTF-8?q?=E5=92=8C=20=E7=BB=93=E6=9E=9C=20=E3=80=90=E5=BD=B1=20=20?= =?UTF-8?q?=E5=93=8D=E3=80=91=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mavlink_msg_mag_cal_report.h | 204 +++++++++--------- src/FoodDeliveryBase.cpp | 19 +- src/commser.cpp | 104 +++++---- 3 files changed, 180 insertions(+), 147 deletions(-) diff --git a/lib/mavlink/ardupilotmega/mavlink_msg_mag_cal_report.h b/lib/mavlink/ardupilotmega/mavlink_msg_mag_cal_report.h index 3e6c287..8e276ae 100644 --- a/lib/mavlink/ardupilotmega/mavlink_msg_mag_cal_report.h +++ b/lib/mavlink/ardupilotmega/mavlink_msg_mag_cal_report.h @@ -4,22 +4,23 @@ #define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 MAVPACKED( -typedef struct __mavlink_mag_cal_report_t { - float fitness; /*< [mgauss] RMS milligauss residuals.*/ - float ofs_x; /*< X offset.*/ - float ofs_y; /*< Y offset.*/ - float ofs_z; /*< Z offset.*/ - float diag_x; /*< X diagonal (matrix 11).*/ - float diag_y; /*< Y diagonal (matrix 22).*/ - float diag_z; /*< Z diagonal (matrix 33).*/ - float offdiag_x; /*< X off-diagonal (matrix 12 and 21).*/ - float offdiag_y; /*< Y off-diagonal (matrix 13 and 31).*/ - float offdiag_z; /*< Z off-diagonal (matrix 32 and 23).*/ - uint8_t compass_id; /*< Compass being calibrated.*/ - uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/ - uint8_t cal_status; /*< Calibration Status.*/ - uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/ -}) mavlink_mag_cal_report_t; + typedef struct __mavlink_mag_cal_report_t { + float fitness; /*< [mgauss] RMS milligauss residuals.*/ + float ofs_x; /*< X offset.*/ + float ofs_y; /*< Y offset.*/ + float ofs_z; /*< Z offset.*/ + float diag_x; /*< X diagonal (matrix 11).*/ + float diag_y; /*< Y diagonal (matrix 22).*/ + float diag_z; /*< Z diagonal (matrix 33).*/ + float offdiag_x; /*< X off-diagonal (matrix 12 and 21).*/ + float offdiag_y; /*< Y off-diagonal (matrix 13 and 31).*/ + float offdiag_z; /*< Z off-diagonal (matrix 32 and 23).*/ + uint8_t compass_id; /*< Compass being calibrated.*/ + uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/ + uint8_t cal_status; /*< Calibration Status.*/ + uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/ + }) +mavlink_mag_cal_report_t; #define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44 #define MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN 44 @@ -29,49 +30,51 @@ typedef struct __mavlink_mag_cal_report_t { #define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 #define MAVLINK_MSG_ID_192_CRC 36 - - #if MAVLINK_COMMAND_24BIT -#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ - 192, \ - "MAG_CAL_REPORT", \ - 14, \ - { { "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_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) }, \ - { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ - { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ - { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ - { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ - { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ - { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ - { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ - { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ - { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ - { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ - } \ -} +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT \ + { \ + 192, \ + "MAG_CAL_REPORT", \ + 14, \ + { \ + {"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_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)}, \ + {"fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness)}, \ + {"ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x)}, \ + {"ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y)}, \ + {"ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z)}, \ + {"diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x)}, \ + {"diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y)}, \ + {"diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z)}, \ + {"offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x)}, \ + {"offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y)}, \ + {"offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z)}, \ + } \ + } #else -#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ - "MAG_CAL_REPORT", \ - 14, \ - { { "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_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) }, \ - { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ - { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ - { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ - { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ - { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ - { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ - { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ - { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ - { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ - { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ - } \ -} +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT \ + { \ + "MAG_CAL_REPORT", \ + 14, \ + { \ + {"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_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)}, \ + {"fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness)}, \ + {"ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x)}, \ + {"ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y)}, \ + {"ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z)}, \ + {"diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x)}, \ + {"diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y)}, \ + {"diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z)}, \ + {"offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x)}, \ + {"offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y)}, \ + {"offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z)}, \ + } \ + } #endif /** @@ -96,8 +99,8 @@ typedef struct __mavlink_mag_cal_report_t { * @param offdiag_z Z off-diagonal (matrix 32 and 23). * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t *msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; @@ -116,7 +119,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 42, cal_status); _mav_put_uint8_t(buf, 43, autosaved); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); #else mavlink_mag_cal_report_t packet; packet.fitness = fitness; @@ -134,7 +137,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_ packet.cal_status = cal_status; packet.autosaved = autosaved; - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); #endif msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; @@ -164,8 +167,8 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_ * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z) + mavlink_message_t *msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; @@ -184,7 +187,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, u _mav_put_uint8_t(buf, 42, cal_status); _mav_put_uint8_t(buf, 43, autosaved); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); #else mavlink_mag_cal_report_t packet; packet.fitness = fitness; @@ -202,7 +205,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, u packet.cal_status = cal_status; packet.autosaved = autosaved; - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); #endif msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; @@ -217,7 +220,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, u * @param msg The MAVLink message to compress the data into * @param mag_cal_report C-struct to read the message contents from */ -static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t *msg, const mavlink_mag_cal_report_t *mag_cal_report) { return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); } @@ -231,7 +234,7 @@ static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint * @param msg The MAVLink message to compress the data into * @param mag_cal_report C-struct to read the message contents from */ -static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t *msg, const mavlink_mag_cal_report_t *mag_cal_report) { return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); } @@ -303,7 +306,7 @@ static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8 * @param chan MAVLink channel to send the message * @param struct The MAVLink struct to serialize */ -static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan, const mavlink_mag_cal_report_t* mag_cal_report) +static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan, const mavlink_mag_cal_report_t *mag_cal_report) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mavlink_msg_mag_cal_report_send(chan, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); @@ -320,7 +323,7 @@ static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -366,15 +369,14 @@ static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf // MESSAGE MAG_CAL_REPORT UNPACKING - /** * @brief Get field compass_id from mag_cal_report message * * @return Compass being calibrated. */ -static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t *msg) { - return _MAV_RETURN_uint8_t(msg, 40); + return _MAV_RETURN_uint8_t(msg, 40); } /** @@ -382,9 +384,9 @@ static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_me * * @return Bitmask of compasses being calibrated. */ -static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t *msg) { - return _MAV_RETURN_uint8_t(msg, 41); + return _MAV_RETURN_uint8_t(msg, 41); } /** @@ -392,9 +394,9 @@ static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_mess * * @return Calibration Status. */ -static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t *msg) { - return _MAV_RETURN_uint8_t(msg, 42); + return _MAV_RETURN_uint8_t(msg, 42); } /** @@ -402,9 +404,9 @@ static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_me * * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. */ -static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t *msg) { - return _MAV_RETURN_uint8_t(msg, 43); + return _MAV_RETURN_uint8_t(msg, 43); } /** @@ -412,9 +414,9 @@ static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_mes * * @return [mgauss] RMS milligauss residuals. */ -static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -422,9 +424,9 @@ static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message * * @return X offset. */ -static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -432,9 +434,9 @@ static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t * * @return Y offset. */ -static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -442,9 +444,9 @@ static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t * * @return Z offset. */ -static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -452,9 +454,9 @@ static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t * * @return X diagonal (matrix 11). */ -static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -462,9 +464,9 @@ static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_ * * @return Y diagonal (matrix 22). */ -static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -472,9 +474,9 @@ static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_ * * @return Z diagonal (matrix 33). */ -static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -482,9 +484,9 @@ static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_ * * @return X off-diagonal (matrix 12 and 21). */ -static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -492,9 +494,9 @@ static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_messa * * @return Y off-diagonal (matrix 13 and 31). */ -static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -502,9 +504,9 @@ static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_messa * * @return Z off-diagonal (matrix 32 and 23). */ -static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t *msg) { - return _MAV_RETURN_float(msg, 36); + return _MAV_RETURN_float(msg, 36); } /** @@ -513,7 +515,7 @@ static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_messa * @param msg The message to decode * @param mag_cal_report C-struct to decode the message contents into */ -static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t *msg, mavlink_mag_cal_report_t *mag_cal_report) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); @@ -531,8 +533,8 @@ static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* ms mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); #else - uint8_t len = msg->len < MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN? msg->len : MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN; - memset(mag_cal_report, 0, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN ? msg->len : MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN; + memset(mag_cal_report, 0, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); memcpy(mag_cal_report, _MAV_PAYLOAD(msg), len); #endif } diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 72f765e..6bad452 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -753,7 +753,7 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) cmd.param1 = param[0]; // 0:加锁 1：解锁 cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁 } - if (controlType == 6) + else if (controlType == 6) { // 测试起飞 float p = (float)param[0]; cmd.command = MAV_CMD_NAV_TAKEOFF; @@ -765,16 +765,21 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) cmd.param6 = 0; cmd.param7 = p; // 起飞高度 } - if (controlType == 8) + else if (controlType == 8) { // 降落 cmd.command = MAV_CMD_NAV_LAND; } - if (controlType == 11) - { // 磁罗盘校准 + else if (controlType == 11) + { + // 磁罗盘校准 cmd.command = MAV_CMD_DO_START_MAG_CAL; - cmd.param1 = 0; - cmd.param2 = 1; - cmd.param3 = 1; + cmd.param1 = 0; // 磁罗盘的bitmask（0表示所有） + cmd.param2 = 0; // 自动重试（0=不重试, 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); int len = mavlink_msg_to_send_buffer(buf, &msg); diff --git a/src/commser.cpp b/src/commser.cpp index d9a7b4b..4044d8c 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -14,12 +14,12 @@ extern void Hook_recovery(); static const char *MOUDLENAME = "COMMSER"; /*项目对象*/ -// char *ssid = "szMate40pro"; // wifi帐号 -// char *password = "63587839ab"; // wifi密码 +char *ssid = "szMate40pro"; // wifi帐号 +char *password = "63587839ab"; // wifi密码 // char* ssid = "szdot"; //wifi帐号 // char* password = "63587839ab"; //wifi密码 -char *ssid = "flicube"; // wifi帐号 -char *password = "fxmf0622"; // wifi密码 +// char *ssid = "flicube"; // wifi帐号 +// char *password = "fxmf0622"; // wifi密码 // char *ssid = "fxmf_sc01"; // 4g wifi帐号 // char *password = "12345678"; // 4g wifi密码 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); // 创建项目对象 /* 发布 主题 ps:以下是登记发布json内容的组成元素 */ // 登记 json成员名字 -// 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"}; +// 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", "fixType", "completionPct", "reportCal", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"}; const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数 String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub /*触发发送 主题*/ @@ -123,7 +123,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) } else { - uint16_t chan[] = {1500, 1500, 1300, 1500}; // 除了加解锁模式 油门全部控制居中 + uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中 fc.mav_channels_override(chan); // 控制油门 } if (n == 6) @@ -139,17 +139,22 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) { // 5 航点执行 fc.mav_set_mode(3); // 飞控设置成auto自动模式 } - else if (n == 8) - { // 8降落* + else if (n == 8) // 8降落 + { fc.mav_command(n, param); } else if (n == 9) { // 9返航 fc.mav_set_mode(6); // 飞控设置成RTL返航 } - else if (n == 11) - { // 11磁罗盘校准 + else if (n == 11) // 11磁罗盘校准 + { + /*初始化磁罗盘校准进度 和 校准结果*/ + topicPubMsg[8] = ""; + topicPubMsg[9] = ""; + /*校准*/ fc.mav_command(n, param); + fc.playText("校准磁罗盘"); } } else if (key == "refreshRequest") @@ -378,10 +383,7 @@ void mavlink_receiveCallback(uint8_t c) home_pos.latitude, // 纬度，以degE7单位传递 home_pos.z); // 相对高度，单位为米 - if (topicPubMsg[17] != buf) - { - topicPubMsg[17] = buf; - } + topicPubMsg[17] = buf; } case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: // #49 飞机原点 { @@ -399,6 +401,31 @@ void mavlink_receiveCallback(uint8_t c) // } } 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 心跳 { mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 @@ -588,46 +615,38 @@ void mavlink_receiveCallback(uint8_t c) { 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) { case 0: - topicPubMsg[9] = "No Fix"; // 无定位 + topicPubMsg[7] = "No Fix"; // 无定位 break; case 1: - topicPubMsg[9] = "Bad Fix"; // 差的定位（无效数据） + topicPubMsg[7] = "Bad Fix"; // 差的定位（无效数据） break; case 2: - topicPubMsg[9] = "2D Fix"; // 2D定位（只有经纬度） + topicPubMsg[7] = "2D Fix"; // 2D定位（只有经纬度） break; case 3: - topicPubMsg[9] = "3D Fix"; // 3D定位（经纬度和高度） + topicPubMsg[7] = "3D Fix"; // 3D定位（经纬度和高度） break; case 4: - topicPubMsg[9] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS） + topicPubMsg[7] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS） break; case 5: - topicPubMsg[9] = "Float"; // RTK浮动解（精度较高，但不如固定解） + topicPubMsg[7] = "Float"; // RTK浮动解（精度较高，但不如固定解） break; case 6: - topicPubMsg[9] = "RTK Fixed"; // RTK固定解（高精度定位） + topicPubMsg[7] = "RTK Fixed"; // RTK固定解（高精度定位） break; default: - topicPubMsg[9] = "Unknown"; // 未知状态 + topicPubMsg[7] = "Unknown"; // 未知状态 break; } + // 纬度 + // sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); + // 经度 + // sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); } break; @@ -761,20 +780,27 @@ void pubThread() } /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ // 创建一个JSON对象 - DynamicJsonDocument doc(2000); // 缓冲区 + DynamicJsonDocument doc(512); // 缓冲区 // 遍历 有更新的数据 组成一个json对象 for (int i = 0; i < topicPubCount; i++) { - if (i == 0 || i == 10) // 心跳每次发 飞机状态 需要每次必发的写在这里 + if (i == 0 || i == 10) // 每次必发的写在这里 { doc[topicPub[i]] = topicPubMsg[i]; } - else // 其余有新值就发 + else if (i == 8 || i == 9) // 有新值发 但是不重置 { if (topicPubMsg[i] != "") { doc[topicPub[i]] = topicPubMsg[i]; - topicPubMsg[i] = ""; + } + } + else // 其余有新值就发 发出后重置 + { + if (topicPubMsg[i] != "") + { + doc[topicPub[i]] = topicPubMsg[i]; + topicPubMsg[i] = ""; // 重置 } } }