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 70fc68ccf2c1..7e244be989d4 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -3,3 +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 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 e89f0f1f42fd..288daee21183 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -50,27 +50,75 @@ #include #include #include +#include #include #include #include +#include using matrix::wrap_2pi; dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; bool MavlinkMissionManager::_dataman_init = false; uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; +uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 }; int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; +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) || \ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_msg.target_component == MAV_COMP_ID_ALL))) + +static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32) +{ + union { + 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; + u.item.params[0] = mission_item.param1; + u.item.params[1] = mission_item.param2; + u.item.params[2] = mission_item.param3; + u.item.params[3] = mission_item.param4; + 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); +} + + MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink) { @@ -101,6 +149,7 @@ MavlinkMissionManager::init_offboard_mission() if (ret > 0) { _dataman_id = (dm_item_t)mission_state.dataman_id; _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; + _crc32[MAV_MISSION_TYPE_MISSION] = mission_state.checksum; _current_seq = mission_state.current_seq; } else if (ret < 0) { @@ -124,6 +173,7 @@ MavlinkMissionManager::load_geofence_stats() if (ret == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; + _crc32[MAV_MISSION_TYPE_FENCE] = stats.checksum; _geofence_update_counter = stats.update_counter; } @@ -139,6 +189,7 @@ MavlinkMissionManager::load_safepoint_stats() if (ret == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; + _crc32[MAV_MISSION_TYPE_RALLY] = stats.checksum; } return ret; @@ -148,7 +199,7 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ int -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32) { // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; @@ -156,6 +207,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; + mission.checksum = crc32; /* update mission state in dataman */ @@ -177,6 +229,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun /* update active mission state */ _dataman_id = dataman_id; _count[MAV_MISSION_TYPE_MISSION] = count; + _crc32[MAV_MISSION_TYPE_MISSION] = crc32; _current_seq = seq; _my_dataman_id = _dataman_id; @@ -199,17 +252,19 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun } int -MavlinkMissionManager::update_geofence_count(unsigned count) +MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32) { mission_stats_entry_s stats; stats.num_items = count; stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data + stats.checksum = crc32; /* update stats in dataman */ int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); if (res == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_FENCE] = count; + _crc32[MAV_MISSION_TYPE_FENCE] = crc32; } else { @@ -226,17 +281,19 @@ MavlinkMissionManager::update_geofence_count(unsigned count) } int -MavlinkMissionManager::update_safepoint_count(unsigned count) +MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32) { mission_stats_entry_s stats; stats.num_items = count; stats.update_counter = ++_safepoint_update_counter; + stats.checksum = crc32; /* update stats in dataman */ int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); if (res == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_RALLY] = count; + _crc32[MAV_MISSION_TYPE_RALLY] = crc32; } else { @@ -277,6 +334,57 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); } +/** + * Combines two independently computed CRC32 checksums to a single checksum + * that is the same as if the checksum was computed over the concatenation + * of both blocks. + * Reference: https://stackoverflow.com/questions/23122312/crc-calculation-of-a-mostly-static-data-stream + */ +uint32_t crc32combine(uint32_t crc_a, uint32_t crc_b, size_t len_b) +{ + size_t i; + uint8_t zero = 0; + + for (i = 0; i < len_b; i++) { + crc_a = crc32part(&zero, 1, crc_a); + } + + return (crc_a ^ crc_b); +} + +#if !defined(CONSTRAINED_FLASH) +bool MavlinkMissionManager::send_mission_checksum(MAV_MISSION_TYPE mission_type) +{ + if (mission_type != MAV_MISSION_TYPE_MISSION && + mission_type != MAV_MISSION_TYPE_FENCE && + mission_type != MAV_MISSION_TYPE_RALLY && + mission_type != MAV_MISSION_TYPE_ALL + ) { + return false; + } + + _time_last_sent = hrt_absolute_time(); + mavlink_mission_checksum_t wpc{}; + wpc.mission_type = mission_type; + + if (mission_type == MAV_MISSION_TYPE_ALL) { + // combine all individual checksums + uint32_t checksum = _crc32[MAV_MISSION_TYPE_MISSION]; + checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_FENCE], + _count[MAV_MISSION_TYPE_FENCE] * MISSION_ITEM_CRC_SIZE); + checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_RALLY], + _count[MAV_MISSION_TYPE_RALLY] * MISSION_ITEM_CRC_SIZE); + wpc.checksum = checksum; + + } else { + wpc.checksum = _crc32[mission_type]; + } + + mavlink_msg_mission_checksum_send_struct(_mavlink->get_channel(), &wpc); + return true; +} +#endif + void MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) { @@ -478,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)) { @@ -524,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); @@ -537,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) { @@ -679,7 +796,8 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { - if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { + if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq, + _crc32[MAV_MISSION_TYPE_MISSION]) == PX4_OK) { PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } else { @@ -901,6 +1019,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _transfer_in_progress = true; _mission_type = (MAV_MISSION_TYPE)wpc.mission_type; + _transfer_current_crc32 = 0; if (wpc.count > current_max_item_count()) { PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count()); @@ -919,20 +1038,20 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { - update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0); + update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0, 0); } else { - update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); } break; case MAV_MISSION_TYPE_FENCE: - update_geofence_count(0); + update_geofence_count(0, 0); break; case MAV_MISSION_TYPE_RALLY: - update_safepoint_count(0); + update_safepoint_count(0, 0); break; default: @@ -1113,6 +1232,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) return; } + _transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32); + bool write_failed = false; bool check_failed = false; @@ -1157,7 +1278,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (mission_item.vertex_count < 3) { // feasibility check PX4_ERR("Fence: too few vertices"); check_failed = true; - update_geofence_count(0); + update_geofence_count(0, 0); } } else { @@ -1226,15 +1347,15 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(_transfer_count); + ret = update_geofence_count(_transfer_count, _transfer_current_crc32); break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(_transfer_count); + ret = update_safepoint_count(_transfer_count, _transfer_current_crc32); break; default: @@ -1248,6 +1369,9 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (ret == PX4_OK) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); +#if !defined(CONSTRAINED_FLASH) + send_mission_checksum(_mission_type); +#endif } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1281,22 +1405,22 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(0); + ret = update_geofence_count(0, 0); break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(0); + ret = update_safepoint_count(0, 0); break; case MAV_MISSION_TYPE_ALL: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); - ret = update_geofence_count(0) || ret; - ret = update_safepoint_count(0) || ret; + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); + ret = update_geofence_count(0, 0) || ret; + ret = update_safepoint_count(0, 0) || ret; break; default: @@ -1400,8 +1524,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_TAKEOFF: mission_item->nav_cmd = NAV_CMD_TAKEOFF; + mission_item->params[0] = mavlink_mission_item->param1; // Pitch mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); - break; case MAV_CMD_NAV_LOITER_TO_ALT: @@ -1435,8 +1559,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * break; case MAV_CMD_NAV_VTOL_TAKEOFF: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + break; + case MAV_CMD_NAV_VTOL_LAND: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->params[0] = mavlink_mission_item->param1; // Land options + mission_item->params[2] = mavlink_mission_item->param3; // Approach altitude mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 64f6607ef343..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, @@ -91,6 +95,8 @@ class MavlinkMissionManager void check_active_mission(void); + bool send_mission_checksum(MAV_MISSION_TYPE mission_type); + private: enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) @@ -110,6 +116,8 @@ class MavlinkMissionManager static bool _dataman_init; ///< Dataman initialized static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE + static uint32_t _crc32[3]; ///< CRC32 checksum of (active) mission for each MAV_MISSION_TYPE + static int32_t _current_seq; ///< Current item sequence in active mission int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) @@ -120,6 +128,7 @@ class MavlinkMissionManager uint16_t _transfer_seq{0}; ///< Item sequence in current transmission int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized) + uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission @@ -134,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; @@ -159,13 +169,13 @@ class MavlinkMissionManager void init_offboard_mission(); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32); /** store the geofence count to dataman */ - int update_geofence_count(unsigned count); + int update_geofence_count(unsigned count, uint32_t crc32); /** store the safepoint count to dataman */ - int update_safepoint_count(unsigned count); + int update_safepoint_count(unsigned count, uint32_t crc32); /** load geofence stats from dataman */ int load_geofence_stats(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 26103f656aea..74e3999b1dfd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -529,11 +529,21 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const get_message_interval((int)roundf(cmd_mavlink.param1)); } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { - uint16_t message_id = (uint16_t)roundf(vehicle_command.param1); - result = handle_request_message_command(message_id, - vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, - vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); +#if !defined(CONSTRAINED_FLASH) + + 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_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + + } else { +#else + { +#endif + result = handle_request_message_command(message_id, + vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + } } else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) { if (_mavlink->failure_injection_enabled()) { diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index c194b8053256..bd0f860e4ba8 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -203,7 +203,8 @@ struct mission_item_s { */ struct mission_stats_entry_s { uint16_t num_items; /**< total number of items stored (excluding this one) */ - uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */ + uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */ + uint32_t checksum; /**< CRC32 checksum of the fence / safe points */ }; /**