mavlink-mission: Add support for opaque ids and replace update counter with it

src/modules/navigator/geofence.cpp

@@ -40,8 +40,10 @@
  */
 #include "geofence.h"
 #include "navigator.h"
+#include "navigation.h"
 
 #include <ctype.h>
+#include <crc32.h>
 
 #include <dataman_client/DatamanClient.hpp>
 #include <drivers/drv_hrt.h>
@@ -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<float>(fence_point.lat);
+	u.item.params[5] = static_cast<float>(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<uint8_t *>(&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<uint8_t *>(&stats),
 				sizeof(mission_stats_entry_s));

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
 

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;

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<int32_t>(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));
 }

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

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();
 

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();
 			}
 		}

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<dm_item_t>(_mission_sub.get().dataman_id);
 		_dataman_cache_landItem.invalidate();
 

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;