diff --git a/msg/Mission.msg b/msg/Mission.msg index 546959a3b3..a2479d86aa 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -7,6 +7,6 @@ int32 current_seq # default -1, start at the one changed latest int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise int32 land_index # Index of the land item, -1 otherwise -uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased -uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased -uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased +uint32 mission_id # indicates updates to the mission, reload from dataman if changed +uint32 geofence_id # indicates updates to the geofence, reload from dataman if changed +uint32 safe_points_id # indicates updates to the safe points, reload from dataman if changed diff --git a/msg/MissionResult.msg b/msg/MissionResult.msg index 463232e96f..aaa840762d 100644 --- a/msg/MissionResult.msg +++ b/msg/MissionResult.msg @@ -1,7 +1,7 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint16 mission_update_counter # Counter for the mission for which the result was generated -uint16 geofence_update_counter # Counter for the corresponding geofence for which the result was generated (used for mission feasibility) +uint16 mission_id # Id for the mission for which the result was generated +uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) int32 seq_reached # Sequence of the mission item which has been reached, default -1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1511b011e6..e0a270e835 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1935,19 +1935,19 @@ void Commander::checkForMissionUpdate() if (_mission_result_sub.updated()) { const mission_result_s &mission_result = _mission_result_sub.get(); - const auto prev_mission_mission_update_counter = mission_result.mission_update_counter; + const auto prev_mission_mission_id = mission_result.mission_id; _mission_result_sub.update(); // if mission_result is valid for the current mission const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) - && (mission_result.mission_update_counter > 0); + && (mission_result.mission_id > 0); bool auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { /* Only evaluate mission state if home is set */ if (!_failsafe_flags.home_position_invalid && - (prev_mission_mission_update_counter != mission_result.mission_update_counter)) { + (prev_mission_mission_id != mission_result.mission_id)) { if (!auto_mission_available) { /* the mission is invalid */ diff --git a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp index 12bbc817a7..05648c5ce0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp @@ -56,6 +56,6 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter) } // This is a mode requirement, no need to report - reporter.failsafeFlags().auto_mission_missing = mission_result.mission_update_counter <= 0; + reporter.failsafeFlags().auto_mission_missing = mission_result.mission_id <= 0; } } diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index 57f5cdfcf8..08ff29a4ee 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -574,10 +574,13 @@ _file_initialize(unsigned max_offset) mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.count = 0; mission.current_seq = 0; + mission.mission_id = 0u; + mission.geofence_id = 0u; + mission.safe_points_id = 0u; mission_stats_entry_s stats; stats.num_items = 0; - stats.update_counter = 1; + stats.opaque_id = 0; g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); g_dm_ops->write(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d895bdd31d..f3ec4d55e8 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -53,19 +53,17 @@ #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::_mission_update_counter = 0; -uint16_t MavlinkMissionManager::_geofence_update_counter = 0; -uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; - #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ ((_msg.target_component == mavlink_system.compid) || \ @@ -96,14 +94,14 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : } void -MavlinkMissionManager::init_offboard_mission(mission_s mission_state) +MavlinkMissionManager::init_offboard_mission(const mission_s &mission_state) { _dataman_id = (dm_item_t)mission_state.dataman_id; _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; + _crc32[MAV_MISSION_TYPE_MISSION] = mission_state.mission_id; _current_seq = mission_state.current_seq; _land_start_marker = mission_state.land_start_index; _land_marker = mission_state.land_index; - _mission_update_counter = mission_state.mission_update_counter; } bool @@ -116,7 +114,7 @@ MavlinkMissionManager::load_geofence_stats() if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; - _geofence_update_counter = stats.update_counter; + _crc32[MAV_MISSION_TYPE_FENCE] = stats.opaque_id; } return success; @@ -132,7 +130,7 @@ MavlinkMissionManager::load_safepoint_stats() if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; - _safepoint_update_counter = stats.update_counter; + _crc32[MAV_MISSION_TYPE_RALLY] = stats.opaque_id; } return success; @@ -142,25 +140,27 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ void -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman) +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32, + bool write_to_dataman) { + /* 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; + mission_s mission{}; mission.timestamp = hrt_absolute_time(); mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; - mission.mission_update_counter = _mission_update_counter; - mission.geofence_update_counter = _geofence_update_counter; - mission.safe_points_update_counter = _safepoint_update_counter; + mission.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; + mission.geofence_id = _crc32[MAV_MISSION_TYPE_FENCE]; + mission.safe_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; mission.land_start_index = _land_start_marker; mission.land_index = _land_marker; - /* update active mission state */ - _dataman_id = dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = count; - _current_seq = seq; - _my_dataman_id = _dataman_id; - if (write_to_dataman) { bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); @@ -174,11 +174,11 @@ 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; + stats.opaque_id = crc32; /* update stats in dataman */ bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), @@ -186,6 +186,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) if (success) { _count[MAV_MISSION_TYPE_FENCE] = count; + _crc32[MAV_MISSION_TYPE_FENCE] = crc32; } else { @@ -198,16 +199,17 @@ MavlinkMissionManager::update_geofence_count(unsigned count) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION], + false); return PX4_OK; } 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.opaque_id = crc32; /* update stats in dataman */ bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), @@ -215,6 +217,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) if (success) { _count[MAV_MISSION_TYPE_RALLY] = count; + _crc32[MAV_MISSION_TYPE_RALLY] = crc32; } else { @@ -227,12 +230,13 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION], + false); return PX4_OK; } void -MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type, uint32_t opaque_id) { mavlink_mission_ack_t wpa{}; @@ -240,6 +244,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t wpa.target_component = compid; wpa.type = type; wpa.mission_type = _mission_type; + wpa.opaque_id = opaque_id; mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); @@ -251,13 +256,17 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) { mavlink_mission_current_t wpc{}; wpc.seq = seq; + wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; + wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE]; + wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); } void -MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type, + uint32_t opaque_id) { _time_last_sent = hrt_absolute_time(); @@ -267,6 +276,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ wpc.target_component = compid; wpc.count = count; wpc.mission_type = mission_type; + wpc.opaque_id = opaque_id; mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); @@ -390,6 +400,17 @@ MavlinkMissionManager::current_item_count() return _count[_mission_type]; } +uint32_t +MavlinkMissionManager::get_current_mission_type_crc() +{ + if (_mission_type >= sizeof(_crc32) / sizeof(_crc32[0])) { + PX4_ERR("WPM: _crc32 out of bounds (%u)", _mission_type); + return 0; + } + + return _crc32[_mission_type]; +} + void MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) { @@ -649,7 +670,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq); + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq, _crc32[MAV_MISSION_TYPE_MISSION]); } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); @@ -718,7 +739,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type); } - send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type); + send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type, get_current_mission_type_crc()); } else { PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy"); @@ -862,6 +883,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()); @@ -879,25 +901,24 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _land_start_marker = -1; _land_marker = -1; - ++_mission_update_counter; /* 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: @@ -1016,7 +1037,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (_transfer_seq == wp.seq + 1) { // Assume this is a duplicate, where we already successfully got all mission items, // but the GCS did not receive the last ack and sent the same item again - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); } else { PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); @@ -1056,6 +1077,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; @@ -1115,7 +1138,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 { @@ -1179,18 +1202,17 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: - ++_mission_update_counter; _land_start_marker = _transfer_land_start_marker; _land_marker = _transfer_land_marker; - update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + 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: @@ -1203,7 +1225,7 @@ 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); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1236,29 +1258,27 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: - ++_mission_update_counter; _land_start_marker = -1; _land_marker = -1; 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: - ++_mission_update_counter; _land_start_marker = -1; _land_marker = -1; 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 = update_safepoint_count(0) || ret; + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); + ret = update_geofence_count(0, 0); + ret = update_safepoint_count(0, 0) || ret; break; default: @@ -1761,21 +1781,42 @@ void MavlinkMissionManager::check_active_mission() if (_mission_sub.updated()) { _mission_sub.update(); - if (_mission_sub.get().geofence_update_counter != _geofence_update_counter) { + if (_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) { load_geofence_stats(); } - if (_mission_sub.get().safe_points_update_counter != _safepoint_update_counter) { + if (_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) { load_safepoint_stats(); } - if ((_mission_sub.get().mission_update_counter != _mission_update_counter) + if ((_mission_sub.get().mission_id != _crc32[MAV_MISSION_TYPE_MISSION]) || (_my_dataman_id != (dm_item_t)_mission_sub.get().dataman_id)) { PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); init_offboard_mission(_mission_sub.get()); _my_dataman_id = _dataman_id; send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], - MAV_MISSION_TYPE_MISSION); + MAV_MISSION_TYPE_MISSION, _crc32[MAV_MISSION_TYPE_MISSION]); } } } + +uint32_t MavlinkMissionManager::crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32) +{ + union { + CrcMissionItem_t item; + uint8_t raw[sizeof(CrcMissionItem_t)]; + } u; + + 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; + + return crc32part(u.raw, sizeof(u), prev_crc32); +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b4bb8bccaa..0a98ea80f8 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -112,6 +112,7 @@ private: 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]; ///< Checksum of items in (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) @@ -119,6 +120,7 @@ private: dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission uint16_t _transfer_count{0}; ///< Items count in current transmission + uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission 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) @@ -135,9 +137,6 @@ private: uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; - static uint16_t _mission_update_counter; - static uint16_t _geofence_update_counter; - static uint16_t _safepoint_update_counter; int32_t _land_start_marker{-1}; ///< index of loaded land start mission item (if unavailable, index of land mission item, -1 otherwise) int32_t _land_marker{-1}; ///< index of loaded land mission item (-1 if unavailable) @@ -160,19 +159,23 @@ private: /** get the number of item count for the current _mission_type */ uint16_t current_item_count(); + /** get the crc32 checksum for the current _mission_type */ + uint32_t get_current_mission_type_crc(); + /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager &operator = (const MavlinkMissionManager &); - void init_offboard_mission(mission_s mission_state); + void init_offboard_mission(const mission_s &mission_state); - void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman = true); + void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32, + bool write_to_dataman = true); /** 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 */ bool load_geofence_stats(); @@ -183,7 +186,7 @@ private: /** * @brief Sends an waypoint ack message */ - void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type, uint32_t opaque_id = 0U); /** * @brief Broadcasts the new target waypoint and directs the MAV to fly there @@ -196,7 +199,8 @@ private: */ void send_mission_current(uint16_t seq); - void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type); + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type, + uint32_t opaque_id); void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); @@ -267,4 +271,12 @@ private: */ void copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item, const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx = 1, int8_t end_idx = 7); + + /** + * Update crc calculation including new mission item + * @param[in] mission_item new mission item + * @param[in] prev_crc32 crc32 checksum of all previous mission item + * @return updated crc32 checksum of mission items + */ + static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32); }; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5876b8a513..5572d9b474 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -40,8 +40,10 @@ */ #include "geofence.h" #include "navigator.h" +#include "navigation.h" #include +#include #include #include @@ -51,6 +53,27 @@ #include "navigator.h" +static uint32_t crc32_for_fence_point(const mission_fence_point_s &fence_point, uint32_t prev_crc32) +{ + union { + CrcMissionItem_t item; + uint8_t raw[sizeof(CrcMissionItem_t)]; + } u; + + u.item.frame = fence_point.frame; + u.item.command = fence_point.nav_cmd; + u.item.autocontinue = 0U; + u.item.params[0] = 0.f; + u.item.params[1] = 0.f; + u.item.params[2] = 0.f; + u.item.params[3] = 0.f; + u.item.params[4] = static_cast(fence_point.lat); + u.item.params[5] = static_cast(fence_point.lon); + u.item.params[6] = fence_point.alt; + + return crc32part(u.raw, sizeof(u), prev_crc32); +} + Geofence::Geofence(Navigator *navigator) : ModuleParams(navigator), _navigator(navigator) @@ -105,9 +128,9 @@ void Geofence::run() _error_state = DatamanState::ReadWait; _dataman_state = DatamanState::Error; - } else if (_update_counter != _stats.update_counter) { + } else if (_opaque_id != _stats.opaque_id) { - _update_counter = _stats.update_counter; + _opaque_id = _stats.opaque_id; _fence_updated = false; _dataman_cache.invalidate(); @@ -559,6 +582,7 @@ Geofence::loadFromFile(const char *filename) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t"); events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported"); ret_val = PX4_ERROR; + uint32_t crc32{0U}; /* do a second pass, now that we know the number of vertices */ for (int seq = 1; seq <= pointCounter; ++seq) { @@ -569,6 +593,7 @@ Geofence::loadFromFile(const char *filename) if (success) { mission_fence_point.vertex_count = pointCounter; + crc32 = crc32_for_fence_point(mission_fence_point, crc32); _dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } @@ -576,7 +601,7 @@ Geofence::loadFromFile(const char *filename) mission_stats_entry_s stats; stats.num_items = pointCounter; - stats.update_counter = _update_counter + 1; + stats.opaque_id = crc32; bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f75bc2cd78..3f2ecbba61 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -183,8 +183,7 @@ private: MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m] - - uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, polygon data was updated + uint32_t _opaque_id{0}; ///< dataman geofence id: if it does not match, the polygon data was updated bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d7b7405075..f954ef762b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -473,7 +473,7 @@ Mission::save_mission_state() if (success) { /* data read successfully, check dataman ID and items count */ if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count - && mission_state.mission_update_counter == _mission.mission_update_counter) { + && mission_state.mission_id == _mission.mission_id) { /* navigator may modify only sequence, write modified state only if it changed */ if (mission_state.current_seq != _mission.current_seq) { mission_state = _mission; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 662e50f20e..23e4d7f4d3 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -114,7 +114,7 @@ void MissionBase::updateMavlinkMission() if (isMissionValid(new_mission)) { /* Relevant mission items updated externally*/ if (checkMissionDataChanged(new_mission)) { - bool mission_items_changed = (new_mission.mission_update_counter != _mission.mission_update_counter); + bool mission_items_changed = (new_mission.mission_id != _mission.mission_id); if (new_mission.current_seq < 0) { new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast(new_mission.count) - 1), @@ -689,12 +689,12 @@ MissionBase::checkMissionRestart() void MissionBase::check_mission_valid() { - if ((_navigator->get_mission_result()->mission_update_counter != _mission.mission_update_counter) - || (_navigator->get_mission_result()->geofence_update_counter != _mission.geofence_update_counter) + if ((_navigator->get_mission_result()->mission_id != _mission.mission_id) + || (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { - _navigator->get_mission_result()->mission_update_counter = _mission.mission_update_counter; - _navigator->get_mission_result()->geofence_update_counter = _mission.geofence_update_counter; + _navigator->get_mission_result()->mission_id = _mission.mission_id; + _navigator->get_mission_result()->geofence_id = _mission.geofence_id; _navigator->get_mission_result()->home_position_counter = _navigator->get_home_position()->update_count; MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client); @@ -1153,7 +1153,7 @@ void MissionBase::resetMission() new_mission.land_start_index = -1; new_mission.land_index = -1; new_mission.count = 0u; - new_mission.mission_update_counter = _mission.mission_update_counter + 1; + new_mission.mission_id = 0u; new_mission.dataman_id = _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -1355,8 +1355,8 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index) bool MissionBase::checkMissionDataChanged(mission_s new_mission) { - /* count and land_index are the same if the mission_counter did not change. We do not care about changes in geofence or rally counters.*/ + /* count and land_index are the same if the mission_id did not change. We do not care about changes in geofence or rally counters.*/ return ((new_mission.dataman_id != _mission.dataman_id) || - (new_mission.mission_update_counter != _mission.mission_update_counter) || + (new_mission.mission_id != _mission.mission_id) || (new_mission.current_seq != _mission.current_seq)); } diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index aaccaa9fdd..f9c27a7f2c 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -203,8 +203,9 @@ struct mission_item_s { * Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS */ struct mission_stats_entry_s { + uint32_t opaque_id; /**< opaque identifier for current stored mission stats */ 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) */ + uint8_t padding[2]; }; /** @@ -239,6 +240,18 @@ struct DestinationPosition { float yaw; /**< final yaw when landed [rad].*/ }; + +/** + * Crc32 mission item struct. + * Used to pack relevant mission item ifnromation for us in crc32 mission calculation. + */ +typedef struct __attribute__((packed)) CrcMissionItem { + uint8_t frame; + uint16_t command; + uint8_t autocontinue; + float params[7]; +} CrcMissionItem_t; + #if (__GNUC__ >= 5) || __clang__ #pragma GCC diagnostic pop #endif // GCC >= 5 || Clang diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 672c92dc3d..f48cf30bb9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -249,7 +249,7 @@ public: orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - int mission_instance_count() const { return _mission_result.mission_update_counter; } + int mission_instance_count() const { return _mission_result.mission_id; } void set_mission_failure_heading_timeout(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fb1a8892d9..493955a493 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -171,8 +171,8 @@ void Navigator::run() fds[2].fd = _mission_sub; fds[2].events = POLLIN; - uint16_t geofence_update_counter{0}; - uint16_t safe_points_update_counter{0}; + uint32_t geofence_id{0}; + uint32_t safe_points_id{0}; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -201,13 +201,13 @@ void Navigator::run() mission_s mission; orb_copy(ORB_ID(mission), _mission_sub, &mission); - if (mission.geofence_update_counter != geofence_update_counter) { - geofence_update_counter = mission.geofence_update_counter; + if (mission.geofence_id != geofence_id) { + geofence_id = mission.geofence_id; _geofence.updateFence(); } - if (mission.safe_points_update_counter != safe_points_update_counter) { - safe_points_update_counter = mission.safe_points_update_counter; + if (mission.safe_points_id != safe_points_id) { + safe_points_id = mission.safe_points_id; _rtl.updateSafePoints(); } } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 28c669d754..922c02b9d0 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -99,9 +99,9 @@ void RTL::updateDatamanCache() _error_state = DatamanState::ReadWait; _dataman_state = DatamanState::Error; - } else if (_update_counter != _stats.update_counter) { + } else if (_opaque_id != _stats.opaque_id) { - _update_counter = _stats.update_counter; + _opaque_id = _stats.opaque_id; _safe_points_updated = false; _dataman_cache_safepoint.invalidate(); @@ -144,8 +144,8 @@ void RTL::updateDatamanCache() } - if (_mission_counter != _mission_sub.get().mission_update_counter) { - _mission_counter = _mission_sub.get().mission_update_counter; + if (_mission_id != _mission_sub.get().mission_id) { + _mission_id = _mission_sub.get().mission_id; const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); _dataman_cache_landItem.invalidate(); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 53f6f2057c..9ebc7ab4e3 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -190,13 +190,13 @@ private: DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; - uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated + uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache mutable DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4}; DatamanClient &_dataman_client_safepoint = _dataman_cache_safepoint.client(); bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; - int16_t _mission_counter = -1; + uint32_t _mission_id = 0u; mission_stats_entry_s _stats; diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp index dbd1690088..f5cbf25540 100644 --- a/src/systemcmds/tests/test_dataman.cpp +++ b/src/systemcmds/tests/test_dataman.cpp @@ -1096,7 +1096,7 @@ DatamanTest::testResetItems() mission_stats_entry_s stats; stats.num_items = 0; - stats.update_counter = 1; + stats.opaque_id = 0; success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s));