refactor(navigator): better memory handling

jonas · jonas · commit ff46fd6dc93b · 2026-07-10T08:26:44.000+02:00
diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp
@@ -600,13 +600,17 @@ bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uin
 	bool item_found = false;
 
 	if (_items) {
-		for (uint32_t i = 0; i < _num_items; ++i) {
+		// Start scanning at the last hit
+		for (uint32_t n = 0; n < _num_items; ++n) {
+			const uint32_t i = (_search_hint + n) % _num_items;
+
 			if ((_items[i].response.item == item) &&
 			    (_items[i].response.index == index)) {
 				item_found = true;
 
 				if (_items[i].cache_state == State::ResponseReceived) {
 					memcpy(buffer, _items[i].response.data, length);
+					_search_hint = i;
 					success = true;
 					break;
 				}
@@ -672,17 +676,14 @@ bool DatamanCache::updateCachedItem(dm_item_t item, uint32_t index, const uint8_
 		return false;
 	}
 
+	// Only patch data that is fully received and stable. Slots in other states are skipped:
+	// an in-flight async read will fetch the new SD card data shortly anyway, and invalidated
+	// slots can still carry a stale key match for the same item/index.
 	for (uint32_t i = 0; i < _num_items; ++i) {
-		if ((_items[i].response.item == item) && (_items[i].response.index == index)) {
-
-			// Only patch data that is fully received and stable. If it is RequestPrepared,
-			// an async read is flying and will fetch the new SD card data shortly anyway.
-			if (_items[i].cache_state == State::ResponseReceived) {
-				memcpy(_items[i].response.data, buffer, length);
-				return true;
-			}
-
-			return false; // Item exists but is not in a safe state to patch
+		if ((_items[i].response.item == item) && (_items[i].response.index == index)
+		    && (_items[i].cache_state == State::ResponseReceived)) {
+			memcpy(_items[i].response.data, buffer, length);
+			return true;
 		}
 	}
 
@@ -760,6 +761,7 @@ void DatamanCache::resetCacheState()
 	_update_index = 0;
 	_item_counter = 0;
 	_load_index = 0;
+	_search_hint = 0;
 }
 
 void DatamanCache::invalidate()
diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp
@@ -318,6 +318,7 @@ class DatamanCache
 	uint32_t _update_index{0};	///< index for tracking last index used by update function
 	uint32_t _item_counter{0};	///< number of items to process with update function
 	uint32_t _num_items{0};		///< number of items that cache can store
+	uint32_t _search_hint{0};	///< slot of the last loadWait hit, speeds up sequential lookups
 
 	DatamanClient _client{};
 
diff --git a/src/modules/navigator/mission_route_cache.cpp b/src/modules/navigator/mission_route_cache.cpp
@@ -44,6 +44,8 @@
 
 #include "mission_route_cache.h"
 
+#include "mission_route_types.h"
+
 #include <inttypes.h>
 
 #include <px4_platform_common/log.h>
@@ -75,12 +77,6 @@ bool MissionRouteCache::queueMissionCacheLoads(const mission_s &mission)
 	// Rebuild the mission cache from scratch, callers retry the full queue on failure.
 	_dataman_cache_mission.invalidate();
 
-	if (static_cast<int32_t>(_dataman_cache_mission.size()) < mission.count) {
-		PX4_ERR("Mission cache capacity too small! requested: %d, capacity: %d",
-			static_cast<int>(mission.count), static_cast<int>(_dataman_cache_mission.size()));
-		return false;
-	}
-
 	const dm_item_t mission_dataman_id = static_cast<dm_item_t>(mission.mission_dataman_id);
 
 	for (int32_t index = 0; index < mission.count; ++index) {
@@ -134,7 +130,7 @@ bool MissionRouteCache::missionLandItemCacheFullyLoaded() const
 		return false;
 	}
 
-	return isMissionLandCommand(land_item.nav_cmd);
+	return mission_route::isLandingCmd(land_item.nav_cmd);
 }
 
 bool MissionRouteCache::safePointCacheFullyLoaded() const
@@ -242,7 +238,7 @@ bool MissionRouteCache::getMissionLandItem(int32_t &index, mission_item_s &land_
 		return false;
 	}
 
-	if (!isMissionLandCommand(cached_land_item.nav_cmd)) {
+	if (!mission_route::isLandingCmd(cached_land_item.nav_cmd)) {
 		return false;
 	}
 
@@ -288,7 +284,7 @@ bool MissionRouteCache::syncMissionItem(const mission_s &mission, int32_t index,
 				return false;
 			}
 
-			if (!isMissionLandCommand(mission_item.nav_cmd)) {
+			if (!mission_route::isLandingCmd(mission_item.nav_cmd)) {
 				_dataman_cache_land_item.invalidate();
 				_mission_land.ready = false;
 				_mission_land.validation_pending = false;
@@ -300,7 +296,7 @@ bool MissionRouteCache::syncMissionItem(const mission_s &mission, int32_t index,
 			_mission_land.validation_pending = false;
 			_mission_land.retry.clear();
 
-			if (isMissionLandCommand(mission_item.nav_cmd)) {
+			if (mission_route::isLandingCmd(mission_item.nav_cmd)) {
 				_mission_land.validation_pending = queueMissionLandItem();
 
 				if (!_mission_land.validation_pending) {
@@ -332,6 +328,13 @@ void MissionRouteCache::updateMissionCache(const mission_s &mission)
 		state.ready = mission.count == 0;
 		state.too_large = missionExceedsCacheLimit(mission);
 
+		// If mission exceed the capacity (e.g. the boot-time cache allocation failed) no recovery is possible.
+		if (!state.too_large && static_cast<int32_t>(_dataman_cache_mission.size()) < mission.count) {
+			PX4_ERR("Mission cache capacity too small, cache disabled! requested: %d, capacity: %d",
+				static_cast<int>(mission.count), static_cast<int>(_dataman_cache_mission.size()));
+			state.too_large = true;
+		}
+
 		if (!state.too_large && mission.count > 0) {
 			state.validation_pending = queueMissionCacheLoads(mission);
 
diff --git a/src/modules/navigator/mission_route_cache.h b/src/modules/navigator/mission_route_cache.h
@@ -94,7 +94,6 @@ class MissionRouteCache : public mission_route::Provider
 		       && (_mission_land.validation_pending || _mission_land.retry.retry_at != 0);
 	}
 
-	inline bool isMissionLandCommand(uint16_t nav_cmd) const { return nav_cmd == NAV_CMD_LAND || nav_cmd == NAV_CMD_VTOL_LAND;}
 	int missionCount() const override;
 	bool loadMissionItem(int index, mission_item_s &mission_item) const override;
 	int safePointCount() const override;
diff --git a/src/modules/navigator/mission_route_types.cpp b/src/modules/navigator/mission_route_types.cpp
@@ -54,6 +54,11 @@ bool Position::valid() const
 	       && (fabs(lat) <= 90.0) && (fabs(lon) <= 180.0);
 }
 
+bool isLandingCmd(uint16_t nav_cmd)
+{
+	return nav_cmd == NAV_CMD_LAND || nav_cmd == NAV_CMD_VTOL_LAND;
+}
+
 float getAbsoluteAltitudeForMissionItem(const mission_item_s &mission_item, float home_altitude_amsl)
 {
 	if (mission_item.altitude_is_relative) {
diff --git a/src/modules/navigator/mission_route_types.h b/src/modules/navigator/mission_route_types.h
@@ -64,6 +64,8 @@ struct Position {
 	bool valid() const;
 };
 
+bool isLandingCmd(uint16_t nav_cmd);
+
 float getAbsoluteAltitudeForMissionItem(const mission_item_s &mission_item, float home_altitude_amsl);
 
 /** @brief Extract the valid position from a rally-point safe-point item. */
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
@@ -181,7 +181,7 @@ class Navigator : public ModuleBase, public ModuleParams
 	uint8_t                      get_takeoff_state() { return _takeoff_status_sub.get().takeoff_state; }
 	vehicle_local_position_s    *get_local_position() { return &_local_pos; }
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
-	MissionRouteCache           *get_mission_route_cache() { return &_mission_route_cache; }
+	MissionRouteCache           &get_mission_route_cache() { return _mission_route_cache; }
 
 	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
 	Course *get_course() { return &_course; }
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
@@ -76,11 +76,7 @@ RTL::RTL(Navigator *navigator) :
 
 void RTL::updateDatamanCache()
 {
-	MissionRouteCache *mission_route_cache = _navigator->get_mission_route_cache();
-
-	if (mission_route_cache != nullptr) {
-		mission_route_cache->update(_mission_sub.get());
-	}
+	_navigator->get_mission_route_cache().update(_mission_sub.get());
 }
 
 void RTL::on_inactive()
@@ -265,7 +261,7 @@ void RTL::setRtlTypeAndDestination()
 {
 	uint8_t safe_point_index = RTL_STATUS_NO_SAFE_POINT;
 	RtlType new_rtl_type{RtlType::RTL_DIRECT};
-	MissionRouteCache *mission_route_cache = _navigator->get_mission_route_cache();
+	const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache();
 
 	// init destination with Home (used also with Type 2 and 4 as backup)
 	DestinationType destination_type = DestinationType::DESTINATION_TYPE_HOME;
@@ -372,10 +368,9 @@ void RTL::setRtlTypeAndDestination()
 
 	// Publish rtl status
 	rtl_status_s rtl_status{};
-	rtl_status.safe_points_id = mission_route_cache != nullptr ? mission_route_cache->safePointsId() : 0;
-	rtl_status.is_evaluation_pending = mission_route_cache != nullptr
-					   && (mission_route_cache->safePointUpdatePending()
-					       || mission_route_cache->missionLandItemUpdatePending());
+	rtl_status.safe_points_id = mission_route_cache.safePointsId();
+	rtl_status.is_evaluation_pending = mission_route_cache.safePointUpdatePending()
+					   || mission_route_cache.missionLandItemUpdatePending();
 	rtl_status.has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
 	rtl_status.rtl_type = static_cast<uint8_t>(_rtl_type);
 	rtl_status.safe_point_index = safe_point_index;
@@ -387,19 +382,19 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
 {
 	const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
 				     && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	MissionRouteCache *mission_route_cache = _navigator->get_mission_route_cache();
+	const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache();
 
 	PositionYawSetpoint safe_point{static_cast<double>(NAN), static_cast<double>(NAN), NAN, NAN};
 	_one_rally_point_has_land_approach = false;
 
-	if (mission_route_cache == nullptr || !mission_route_cache->safePointsReady()) {
+	if (!mission_route_cache.safePointsReady()) {
 		return safe_point;
 	}
 
-	for (int current_seq = 0; current_seq < mission_route_cache->safePointCount(); ++current_seq) {
+	for (int current_seq = 0; current_seq < mission_route_cache.safePointCount(); ++current_seq) {
 		mission_item_s mission_safe_point;
 
-		if (!mission_route_cache->loadSafePointItem(current_seq, mission_safe_point)) {
+		if (!mission_route_cache.loadSafePointItem(current_seq, mission_safe_point)) {
 			PX4_ERR("dm_read failed");
 			continue;
 		}
@@ -430,7 +425,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
 					 safepoint_position.lat, safepoint_position.lon)};
 
 			const bool current_safe_point_has_approaches{
-				mission_route_cache->hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)
+				mission_route_cache.hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)
 			};
 
 			_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
@@ -460,12 +455,11 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 	const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
 				     && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
 
-	MissionRouteCache *mission_route_cache = _navigator->get_mission_route_cache();
+	const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache();
 	float min_dist = FLT_MAX;
 
 	if (_param_rtl_type.get() != RTL_TYPE_SAFE_POINT_DIRECT) {
-		_home_has_land_approach = mission_route_cache != nullptr
-					  && mission_route_cache->hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
+		_home_has_land_approach = mission_route_cache.hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
 
 		const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
 		const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach);
@@ -483,7 +477,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 		     || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON)) && hasMissionLandStart()) {
 			mission_item_s land_mission_item{};
 			int32_t land_index = -1;
-			const bool success = mission_route_cache != nullptr && mission_route_cache->getMissionLandItem(land_index, land_mission_item);
+			const bool success = mission_route_cache.getMissionLandItem(land_index, land_mission_item);
 
 			if (!success) {
 
@@ -658,14 +652,8 @@ loiter_point_s RTL::selectLandingApproach(const PositionYawSetpoint &destination
 		return landing_approach;
 	}
 
-	const MissionRouteCache *mission_route_cache = _navigator->get_mission_route_cache();
-
-	if (mission_route_cache == nullptr) {
-		return landing_approach;
-	}
-
 	const land_approaches_s vtol_land_approaches =
-		mission_route_cache->getVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
+		_navigator->get_mission_route_cache().getVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
 
 	if (vtol_land_approaches.isAnyApproachValid()) {
 		landing_approach = chooseBestLandingApproach(vtol_land_approaches);
diff --git a/src/modules/navigator/test/test_RTL.cpp b/src/modules/navigator/test/test_RTL.cpp
@@ -218,7 +218,7 @@ class RTLTest : public NavigatorDatamanTestBase
 		ASSERT_TRUE(_dataman_client.writeSync(DM_KEY_SAFE_POINTS_STATE, 0,
 						      reinterpret_cast<uint8_t *>(&empty_stats), sizeof(empty_stats)));
 
-		_navigator.get_mission_route_cache()->invalidate();
+		_navigator.get_mission_route_cache().invalidate();
 
 		publishHomePosition(makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt));
 		publishVehicleStatus(false, vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
@@ -227,7 +227,7 @@ class RTLTest : public NavigatorDatamanTestBase
 
 	void TearDown() override
 	{
-		_navigator.get_mission_route_cache()->invalidate();
+		_navigator.get_mission_route_cache().invalidate();
 
 		if (_home_pub != nullptr) {
 			orb_unadvertise(_home_pub);
@@ -267,10 +267,10 @@ class RTLTest : public NavigatorDatamanTestBase
 		mission.safe_points_id = ++_safe_points_id;
 		mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_0;
 
-		MissionRouteCache *mission_route_cache = _navigator.get_mission_route_cache();
-		mission_route_cache->invalidate();
-		ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(*mission_route_cache, mission,
-				[&] { return mission_route_cache->safePointsReady(); }))
+		MissionRouteCache &mission_route_cache = _navigator.get_mission_route_cache();
+		mission_route_cache.invalidate();
+		ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(mission_route_cache, mission,
+				[&] { return mission_route_cache.safePointsReady(); }))
 				<< "test safe points did not load";
 	}
 

Original file line number	Diff line number	Diff line change
`@@ -94,7 +94,6 @@ class MissionRouteCache : public mission_route::Provider`
`94`	`94`	`&& (_mission_land.validation_pending \|\| _mission_land.retry.retry_at != 0);`
`95`	`95`	`}`
`96`	`96`
`97`		`- inline bool isMissionLandCommand(uint16_t nav_cmd) const { return nav_cmd == NAV_CMD_LAND \|\| nav_cmd == NAV_CMD_VTOL_LAND;}`
`98`	`97`	`int missionCount() const override;`
`99`	`98`	`bool loadMissionItem(int index, mission_item_s &mission_item) const override;`
`100`	`99`	`int safePointCount() const override;`
Original file line number	Diff line number	Diff line change
`@@ -54,6 +54,11 @@ bool Position::valid() const`
`54`	`54`	`&& (fabs(lat) <= 90.0) && (fabs(lon) <= 180.0);`
`55`	`55`	`}`
`56`	`56`
	`57`	`+bool isLandingCmd(uint16_t nav_cmd)`
	`58`	`+{`
	`59`	`+ return nav_cmd == NAV_CMD_LAND \|\| nav_cmd == NAV_CMD_VTOL_LAND;`
	`60`	`+}`
	`61`	`+`
`57`	`62`	`float getAbsoluteAltitudeForMissionItem(const mission_item_s &mission_item, float home_altitude_amsl)`
`58`	`63`	`{`
`59`	`64`	`if (mission_item.altitude_is_relative) {`