navigator: use mission topic to notify about geofence & safe point changes

This avoids the need to regularly access dataman for checking.

msg/Mission.msg

@@ -3,3 +3,6 @@ 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
+
+int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
+int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased

src/modules/mavlink/mavlink_mission.cpp

@@ -146,6 +146,8 @@ 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.geofence_update_counter = _geofence_update_counter;
+	mission.safe_points_update_counter = _safepoint_update_counter;
 
 	/* update active mission state */
 	_dataman_id = dataman_id;
@@ -161,7 +163,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
+	stats.update_counter = ++_geofence_update_counter;
 
 	/* update stats in dataman */
 	bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
@@ -181,6 +183,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
 		return PX4_ERROR;
 	}
 
+	update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq);
 	return PX4_OK;
 }
 
@@ -209,6 +212,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
 		return PX4_ERROR;
 	}
 
+	update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq);
 	return PX4_OK;
 }
 

src/modules/navigator/navigator_main.cpp

@@ -168,6 +168,9 @@ void Navigator::run()
 	fds[2].fd = _mission_sub;
 	fds[2].events = POLLIN;
 
+	int16_t geofence_update_counter{0};
+	int16_t safe_points_update_counter{0};
+
 	/* rate-limit position subscription to 20 Hz / 50 ms */
 	orb_set_interval(_local_pos_sub, 50);
 
@@ -192,9 +195,18 @@ void Navigator::run()
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus);
 
 		if (fds[2].revents & POLLIN) {
-			// copy mission to clear any update
 			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;
+				_geofence.updateFence();
+			}
+
+			if (mission.safe_points_update_counter != safe_points_update_counter) {
+				safe_points_update_counter = mission.safe_points_update_counter;
+				_rtl.updateSafePoints();
+			}
 		}
 
 		/* gps updated */

src/modules/navigator/rtl.h

@@ -122,6 +122,8 @@ public:
 
 	void resetRtlState() { _rtl_state = RTL_STATE_NONE; }
 
+	void updateSafePoints() { _initiate_safe_points_updated = true; }
+
 private:
 
 	void set_rtl_item();