geofence: lock geofence items during a write transfers

- avoids race conditions when geofence data is updated in flight. During
  a transfer, the geofence module will not check for violations, which
  is done with the new dm_trylock method.
- there is an update counter stored in dataman, and for each write
  transaction this is increased, so that the geofence module can reload
  the data upon data change (after it's unlocked).
- single dm item updates are atomic already, so resetting the polygons
  to 0 does not need locking.

src/modules/mavlink/mavlink_mission.cpp

@@ -61,6 +61,7 @@ int MavlinkMissionManager::_current_seq = 0;
 int MavlinkMissionManager::_last_reached = -1;
 bool MavlinkMissionManager::_transfer_in_progress = false;
 constexpr unsigned MavlinkMissionManager::MAX_COUNT[];
+uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
 
 #define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
 		((_msg.target_component == mavlink_system.compid) || \
@@ -87,6 +88,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
 	_offboard_mission_pub(nullptr),
+	_geofence_locked(false),
 	_slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz
 	_verbose(mavlink->verbose()),
 	_mavlink(mavlink)
@@ -138,6 +140,7 @@ MavlinkMissionManager::load_geofence_stats()
 
 	if (ret == sizeof(mission_stats_entry_s)) {
 		_count[(uint8_t)MAV_MISSION_TYPE_FENCE] = stats.num_items;
+		_geofence_update_counter = stats.update_counter;
 	}
 
 	return ret;
@@ -204,6 +207,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
 {
 	mission_stats_entry_s stats;
 	stats.num_items = count;
+	stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
 
 	/* update stats in dataman */
 	int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
@@ -546,7 +550,7 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		if (_verbose) { PX4_INFO("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
 
-		_state = MAVLINK_WPM_STATE_IDLE;
+		switch_to_idle_state();
 
 		// since we are giving up, reset this state also, so another request can be started.
 		_transfer_in_progress = false;
@@ -625,7 +629,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
 					if (_verbose) { PX4_ERR("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
 				}
 
-				_state = MAVLINK_WPM_STATE_IDLE;
+				switch_to_idle_state();
 
 			} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
 
@@ -818,7 +822,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
 						if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); }
 					}
 
-					_state = MAVLINK_WPM_STATE_IDLE;
+					switch_to_idle_state();
 
 					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
 					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
@@ -832,7 +836,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
 				} else {
 					if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); }
 
-					_state = MAVLINK_WPM_STATE_IDLE;
+					switch_to_idle_state();
 
 					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
 					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
@@ -922,6 +926,21 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 			_transfer_dataman_id = _dataman_id == 0 ? 1 : 0;	// use inactive storage for transmission
 			_transfer_current_seq = -1;
 
+			if (_mission_type == MAV_MISSION_TYPE_FENCE) {
+				// We're about to write new geofence items, so take the lock. It will be released when
+				// switching back to idle
+				if (_verbose) { PX4_INFO("locking fence dataman items"); }
+
+				int ret = dm_lock(DM_KEY_FENCE_POINTS);
+
+				if (ret == 0) {
+					_geofence_locked = true;
+
+				} else {
+					PX4_ERR("locking failed (%i)", errno);
+				}
+			}
+
 		} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -947,6 +966,21 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkMissionManager::switch_to_idle_state()
+{
+	// when switching to idle, we *always* check if the lock was held and release it.
+	// This is to ensure we don't end up in a state where we forget to release it.
+	if (_geofence_locked) {
+		dm_unlock(DM_KEY_FENCE_POINTS);
+		_geofence_locked = false;
+
+		if (_verbose) { PX4_INFO("unlocking geofence"); }
+	}
+
+	_state = MAVLINK_WPM_STATE_IDLE;
+}
+
 
 void
 MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
@@ -1020,7 +1054,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
 
 			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
-			_state = MAVLINK_WPM_STATE_IDLE;
+			switch_to_idle_state();
 			_transfer_in_progress = false;
 			return;
 		}
@@ -1101,7 +1135,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 				_mavlink->send_statustext_critical("Unable to write on micro SD");
 			}
 
-			_state = MAVLINK_WPM_STATE_IDLE;
+			switch_to_idle_state();
 			_transfer_in_progress = false;
 			return;
 		}
@@ -1119,8 +1153,6 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			/* got all new mission items successfully */
 			if (_verbose) { PX4_INFO("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
 
-			_state = MAVLINK_WPM_STATE_IDLE;
-
 			ret = 0;
 
 			switch (_mission_type) {
@@ -1141,6 +1173,10 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 				break;
 			}
 
+			// Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order
+			switch_to_idle_state();
+
+
 			if (ret == PX4_OK) {
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
 

src/modules/mavlink/mavlink_mission.h

@@ -127,6 +127,9 @@ private:
 	int			_mission_result_sub;
 	orb_advert_t		_offboard_mission_pub;
 
+	static uint16_t _geofence_update_counter;
+	bool		_geofence_locked; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
+
 	MavlinkRateLimiter	_slow_rate_limiter;
 
 	bool _verbose;
@@ -236,4 +239,8 @@ private:
 	int format_mavlink_mission_item(const struct mission_item_s *mission_item,
 					mavlink_mission_item_t *mavlink_mission_item);
 
+	/**
+	 * set _state to idle (and do necessary cleanup)
+	 */
+	void switch_to_idle_state();
 };

src/modules/navigator/geofence.cpp

@@ -64,7 +64,9 @@ Geofence::Geofence(Navigator *navigator) :
 	_param_max_ver_distance(this, "GF_MAX_VER_DIST", false)
 {
 	updateParams();
-	updateFence();
+
+	// we assume there's no concurrent fence update on startup
+	_updateFence();
 }
 
 Geofence::~Geofence()
@@ -76,6 +78,22 @@ Geofence::~Geofence()
 
 void Geofence::updateFence()
 {
+	// Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer.
+	// However this is currently not used
+	int ret = dm_lock(DM_KEY_FENCE_POINTS);
+
+	if (ret != 0) {
+		PX4_ERR("lock failed");
+		return;
+	}
+
+	_updateFence();
+	dm_unlock(DM_KEY_FENCE_POINTS);
+}
+
+void Geofence::_updateFence()
+{
+
 	// initialize fence points count
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
@@ -83,6 +101,7 @@ void Geofence::updateFence()
 
 	if (ret == sizeof(mission_stats_entry_s)) {
 		num_fence_items = stats.num_items;
+		_update_counter = stats.update_counter;
 	}
 
 	// iterate over all polygons and store their starting vertices
@@ -268,7 +287,23 @@ bool Geofence::checkAll(double lat, double lon, float altitude)
 
 bool Geofence::checkPolygons(double lat, double lon, float altitude)
 {
+	// the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means
+	// the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now
+	if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) {
+		return true;
+	}
+
+	// we got the lock, now check if the fence data got updated
+	mission_stats_entry_s stats;
+	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+
+	if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) {
+		PX4_INFO("reloading geofence");
+		_updateFence();
+	}
+
 	if (isEmpty()) {
+		dm_unlock(DM_KEY_FENCE_POINTS);
 		/* Empty fence -> accept all points */
 		return true;
 	}
@@ -276,10 +311,12 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
 	/* Vertical check */
 	if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly
 		if (altitude > _altitude_max || altitude < _altitude_min) {
+			dm_unlock(DM_KEY_FENCE_POINTS);
 			return false;
 		}
 	}
 
+
 	/* Horizontal check: iterate all polygons & circles */
 	bool outside_exclusion = true;
 	bool inside_inclusion = false;
@@ -320,6 +357,8 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
 		}
 	}
 
+	dm_unlock(DM_KEY_FENCE_POINTS);
+
 	return (!had_inclusion_areas || inside_inclusion) && outside_exclusion;
 }
 

src/modules/navigator/geofence.h

@@ -78,7 +78,8 @@ public:
 	};
 
 	/**
-	 * update the geofence from dataman
+	 * update the geofence from dataman.
+	 * It's generally not necessary to call this as it will automatically update when the data is changed.
 	 */
 	void updateFence();
 
@@ -167,6 +168,12 @@ private:
 	control::BlockParamFloat _param_max_ver_distance;
 
 	int _outside_counter{0};
+	uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
+
+	/**
+	 * implementation of updateFence(), but without locking
+	 */
+	void _updateFence();
 
 	/**
 	 * Check if a point passes the Geofence test.

src/modules/navigator/navigation.h

@@ -153,6 +153,7 @@ 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) */
 };
 
 /**