From 2647663cae2ba81e76d6faa7e4df81a391808d13 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 14 Jun 2023 13:36:35 +0200 Subject: [PATCH] mavlink_mission: Update mission checksum to calculate checksum based on mavlink_endianness. Send the mission checksum constantly at a frequency of 1 Hz. --- boards/px4/fmu-v5x/default.px4board | 1 + msg/Mission.msg | 2 +- src/modules/mavlink/mavlink_mission.cpp | 45 ++++++++++++++++++------ src/modules/mavlink/mavlink_mission.h | 7 +++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 5 files changed, 43 insertions(+), 14 deletions(-) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 5c662fbca8b0..ce125bd04a67 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -68,6 +68,7 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/msg/Mission.msg b/msg/Mission.msg index c97a9c07640f..7e244be989d4 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -3,4 +3,4 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest -uint32 checksum # crc32 checksum of all items +uint32 checksum # crc32 checksum of all items as defined by the MAVlink message MISSION_CHECKSUM. diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 381e944cc907..288daee21183 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -67,7 +68,15 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; -static constexpr int MISSION_ITEM_CRC_SIZE = 32; +typedef struct __attribute__((packed)) CrcMissionItem { + uint8_t frame; + uint16_t command; + uint8_t autocontinue; + float params[7]; +} CrcMissionItem_t; + +static constexpr int MISSION_ITEM_CRC_SIZE = sizeof(CrcMissionItem_t); + #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ ((_msg.target_component == mavlink_system.compid) || \ @@ -78,15 +87,22 @@ static constexpr int MISSION_ITEM_CRC_SIZE = 32; static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32) { union { - struct __attribute__((packed)) { - uint8_t frame; - uint16_t command; - uint8_t autocontinue; - float params[7]; - } item; + CrcMissionItem_t item; uint8_t raw[MISSION_ITEM_CRC_SIZE]; } u; +#if MAVLINK_NEED_BYTE_SWAP + _mav_put_uint8_t(u.raw, 0, mission_item.frame); + _mav_put_uint16_t(u.raw, 1, mission_item.command); + _mav_put_uint8_t(u.raw, 3, mission_item.autocontinue); + _mav_put_float(u.raw, 4, mission_item.param1); + _mav_put_float(u.raw, 8, mission_item.param2); + _mav_put_float(u.raw, 12, mission_item.param3); + _mav_put_float(u.raw, 16, mission_item.param4); + _mav_put_float(u.raw, 20, mission_item.x); + _mav_put_float(u.raw, 24, mission_item.y); + _mav_put_float(u.raw, 28, mission_item.z); +#else u.item.frame = mission_item.frame; u.item.command = mission_item.command; u.item.autocontinue = mission_item.autocontinue; @@ -97,6 +113,7 @@ static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_ite u.item.params[4] = mission_item.x; u.item.params[5] = mission_item.y; u.item.params[6] = mission_item.z; +#endif return crc32part(u.raw, sizeof(u), prev_crc32); } @@ -569,6 +586,8 @@ MavlinkMissionManager::send() return; } + hrt_abstime t_now{hrt_absolute_time()}; + mission_result_s mission_result{}; if (_mission_result_sub.update(&mission_result)) { @@ -615,7 +634,7 @@ MavlinkMissionManager::send() } } - } else if (_slow_rate_limiter.check(hrt_absolute_time())) { + } else if (_slow_rate_limiter.check(t_now)) { if ((_count[MAV_MISSION_TYPE_MISSION] > 0) && (_current_seq >= 0)) { send_mission_current(_current_seq); @@ -628,6 +647,13 @@ MavlinkMissionManager::send() } } + /* Send mission checksum at a low rate */ + if (_checksum_rate_limiter.check(t_now)) { + send_mission_checksum(MAV_MISSION_TYPE_MISSION); + send_mission_checksum(MAV_MISSION_TYPE_FENCE); + send_mission_checksum(MAV_MISSION_TYPE_RALLY); + } + /* check for timed-out operations */ if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0) && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { @@ -696,9 +722,6 @@ MavlinkMissionManager::handle_message(const mavlink_message_t *msg) handle_mission_clear_all(msg); break; - case MAVLINK_MSG_ID_COMMAND_INT: - break; - default: break; } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 7658e9dd9104..187f93c216ba 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -53,6 +53,10 @@ #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" +#include + +using namespace time_literals; + enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -139,7 +143,8 @@ class MavlinkMissionManager static uint16_t _safepoint_update_counter; bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) - MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz + MavlinkRateLimiter _slow_rate_limiter{100_ms}; ///< Rate limit sending of the current WP sequence to 10 Hz + MavlinkRateLimiter _checksum_rate_limiter{1_s}; ///< Rate limit sending the checksum of current mission, rally point and geofence Mavlink *_mavlink; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a3704a4cf70d..74e3999b1dfd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -534,7 +534,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const if (message_id == MAVLINK_MSG_ID_MISSION_CHECKSUM) { result = _mission_manager.send_mission_checksum((MAV_MISSION_TYPE)roundf(vehicle_command.param2)) ? - vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; } else { #else