rtl: integrate dataman cache

src/modules/navigator/mission.cpp

@@ -2023,14 +2023,18 @@ void Mission::publish_navigator_mission_item()
 bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index,
 		unsigned &prev_pos_index) const
 {
-	struct mission_item_s missionitem = {};
 
 	for (int index = inactivation_index; index >= 0; index--) {
-		if (!readMissionItemAtIndex(mission, index, missionitem)) {
+		mission_item_s mission_item;
+		const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
+		bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast<uint8_t *>(&mission_item),
+							sizeof(mission_item), 500_ms);
+
+		if (!success) {
 			break;
 		}
 
-		if (MissionBlock::item_contains_position(missionitem)) {
+		if (MissionBlock::item_contains_position(mission_item)) {
 			prev_pos_index = index;
 			return true;
 		}
@@ -2039,10 +2043,16 @@ bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactiv
 	return false;
 }
 
-bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const
+bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item)
 {
+	const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
+
 	while (start_index < mission.count) {
-		if (readMissionItemAtIndex(mission, start_index, mission_item) && MissionBlock::item_contains_position(mission_item)) {
+		// start_index is expected to be after _current_mission_index, and the item should therefore be cached
+		bool success = _dataman_cache.loadWait(dm_current, start_index, reinterpret_cast<uint8_t *>(&mission_item),
+						       sizeof(mission_item), 500_ms);
+
+		if (success && MissionBlock::item_contains_position(mission_item)) {
 			return true;
 		}
 
@@ -2052,19 +2062,6 @@ bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_ind
 	return false;
 }
 
-bool Mission::readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const
-{
-	bool success = false;
-
-	if (index >= 0 && index < mission.count) {
-		const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
-		const ssize_t len = sizeof(missionitem);
-		success = (dm_read(dm_current, index, &missionitem, len) == len);
-	}
-
-	return success;
-}
-
 void Mission::cacheItem(const mission_item_s &mission_item)
 {
 	switch (mission_item.nav_cmd) {
@@ -2145,9 +2142,12 @@ bool Mission::cameraWasTriggering()
 void Mission::updateCachedItemsUpToIndex(const int end_index)
 {
 	for (int i = 0; i <= end_index; i++) {
-		mission_item_s mission_item = {};
+		mission_item_s mission_item;
+		const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;
+		bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&mission_item),
+							sizeof(mission_item), 500_ms);
 
-		if (readMissionItemAtIndex(_mission, i, mission_item)) {
+		if (success) {
 			cacheItem(mission_item);
 		}
 	}

src/modules/navigator/mission.h

@@ -263,17 +263,7 @@ private:
 	 * @param mission_item The mission item to populate
 	 * @return true if successful
 	 */
-	bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const;
-
-	/**
-	 * @brief Read the mission item at the given index
-	 *
-	 * @param mission The mission to read from
-	 * @param index The index to read
-	 * @param missionitem The mission item to populate
-	 * @return true if successful
-	 */
-	bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const;
+	bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item);
 
 	/**
 	 * @brief Cache the mission items containing gimbal, camera mode and trigger commands

src/modules/navigator/navigator_main.cpp

@@ -919,6 +919,7 @@ void Navigator::run()
 
 		_mission.run();
 		_geofence.run();
+		_rtl.run();
 
 		perf_end(_loop_perf);
 	}

src/modules/navigator/rtl.cpp

@@ -67,6 +67,90 @@ RTL::RTL(Navigator *navigator) :
 	_param_rover_cruise_speed = param_find("GND_SPEED_THR_SC");
 }
 
+void RTL::run()
+{
+	bool success;
+
+	switch (_dataman_state) {
+
+	case DatamanState::UpdateRequestWait:
+
+		if (_initiate_safe_points_updated) {
+			_initiate_safe_points_updated = false;
+			_dataman_state	= DatamanState::Read;
+		}
+
+		break;
+
+	case DatamanState::Read:
+
+		_dataman_state	= DatamanState::ReadWait;
+		success = _dataman_client.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
+						    sizeof(mission_stats_entry_s));
+
+		if (!success) {
+			_error_state = DatamanState::Read;
+			_dataman_state = DatamanState::Error;
+		}
+
+		break;
+
+	case DatamanState::ReadWait:
+
+		_dataman_client.update();
+
+		if (_dataman_client.lastOperationCompleted(success)) {
+
+			if (!success) {
+				_error_state = DatamanState::ReadWait;
+				_dataman_state = DatamanState::Error;
+
+			} else if (_update_counter != _stats.update_counter) {
+
+				_update_counter = _stats.update_counter;
+				_safe_points_updated = false;
+
+				_dataman_cache.invalidate();
+
+				if (_dataman_cache.size() != _stats.num_items) {
+					_dataman_cache.resize(_stats.num_items);
+				}
+
+				for (int index = 1; index <= _dataman_cache.size(); ++index) {
+					_dataman_cache.load(DM_KEY_SAFE_POINTS, index);
+				}
+
+				_dataman_state = DatamanState::Load;
+
+			} else {
+				_dataman_state = DatamanState::UpdateRequestWait;
+			}
+		}
+
+		break;
+
+	case DatamanState::Load:
+
+		_dataman_cache.update();
+
+		if (!_dataman_cache.isLoading()) {
+			_dataman_state = DatamanState::UpdateRequestWait;
+			_safe_points_updated = true;
+		}
+
+		break;
+
+	case DatamanState::Error:
+		PX4_ERR("Safe points update failed! state: %" PRIu8, static_cast<uint8_t>(_error_state));
+		_dataman_state = DatamanState::UpdateRequestWait;
+		break;
+
+	default:
+		break;
+
+	}
+}
+
 void RTL::on_inactivation()
 {
 	if (_navigator->get_precland()->is_activated()) {
@@ -178,40 +262,34 @@ void RTL::find_RTL_destination()
 		return;
 	}
 
-	// compare to safe landing positions
 	mission_safe_point_s closest_safe_point {};
-	mission_stats_entry_s stats;
-	bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
-						sizeof(mission_stats_entry_s));
-	int num_safe_points = 0;
-
-	if (success) {
-		num_safe_points = stats.num_items;
-	}
-
 	// check if a safe point is closer than home or landing
 	int closest_index = 0;
 
-	for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
-		mission_safe_point_s mission_safe_point;
+	if (_safe_points_updated) {
 
-		success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, current_seq, reinterpret_cast<uint8_t *>(&mission_safe_point),
-						   sizeof(mission_safe_point_s));
+		for (int current_seq = 1; current_seq <= _dataman_cache.size(); ++current_seq) {
+			mission_safe_point_s mission_safe_point;
 
-		if (!success) {
-			PX4_ERR("dm_read failed");
-			continue;
-		}
+			bool success = _dataman_cache.loadWait(DM_KEY_SAFE_POINTS, current_seq,
+							       reinterpret_cast<uint8_t *>(&mission_safe_point),
+							       sizeof(mission_safe_point_s));
 
-		// TODO: take altitude into account for distance measurement
-		dlat = mission_safe_point.lat - global_position.lat;
-		dlon = mission_safe_point.lon - global_position.lon;
-		double dist_squared = coord_dist_sq(dlat, dlon);
+			if (!success) {
+				PX4_ERR("dm_read failed");
+				continue;
+			}
 
-		if (dist_squared < min_dist_squared) {
-			closest_index = current_seq;
-			min_dist_squared = dist_squared;
-			closest_safe_point = mission_safe_point;
+			// TODO: take altitude into account for distance measurement
+			dlat = mission_safe_point.lat - global_position.lat;
+			dlon = mission_safe_point.lon - global_position.lon;
+			double dist_squared = coord_dist_sq(dlat, dlon);
+
+			if (dist_squared < min_dist_squared) {
+				closest_index = current_seq;
+				min_dist_squared = dist_squared;
+				closest_safe_point = mission_safe_point;
+			}
 		}
 	}
 

src/modules/navigator/rtl.h

@@ -96,6 +96,11 @@ public:
 		RTL_STATE_HEAD_TO_CENTER,
 	};
 
+	/**
+	 * @brief function to call regularly to do background work
+	 */
+	void run();
+
 	void on_inactivation() override;
 	void on_inactive() override;
 	void on_activation() override;
@@ -138,6 +143,14 @@ private:
 
 	RTLState _rtl_state{RTL_STATE_NONE};
 
+	enum class DatamanState {
+		UpdateRequestWait,
+		Read,
+		ReadWait,
+		Load,
+		Error
+	};
+
 	struct RTLPosition {
 		double lat;
 		double lon;
@@ -157,7 +170,15 @@ private:
 		}
 	};
 
-	DatamanClient _dataman_client{};
+	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
+	bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache
+	DatamanCache _dataman_cache{"rtl_dm_cache_miss", 4};
+	DatamanClient	&_dataman_client = _dataman_cache.client();
+	bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
+
+	mission_stats_entry_s _stats;
 
 	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)