refactor(navigator): cap safe point batch size and better memory handling

Author: jonas

boards/px4/fmu-v6c/visionTargetEstStatic.px4board

@@ -94,4 +94,4 @@ CONFIG_DRIVERS_GNSS_SEPTENTRIO=n
 CONFIG_DRIVERS_IMU_BOSCH_BMI055=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_RTL_MISSION_CACHE_SIZE=300
-CONFIG_RTL_SAFE_POINT_BATCH_SIZE=64
+CONFIG_RTL_SAFE_POINT_BATCH_SIZE=32

docs/en/concept/mission_route_planning.md

@@ -23,7 +23,7 @@ Because the default is `0` on non-testing boards, route planning is disabled unt
 :::
 
 ::: warning
-Rally points are scored in fixed-size batches of `CONFIG_RTL_SAFE_POINT_BATCH_SIZE` (default **1**, **64** on testing builds).
+Rally points are scored in fixed-size batches of `CONFIG_RTL_SAFE_POINT_BATCH_SIZE` (default **1**, **32** on testing builds).
 When more rally points are configured than fit in one batch the planner re-scans the full mission route once per batch, so a mission with many rally points can be walked several times per planning pass. See [Safe-Point Batching](#safe-point-batching).
 :::
 
@@ -229,8 +229,8 @@ If a new Return is requested, the vehicle flies straight to the rally point (`R`
 
 ### Safe-Point Batching {#safe-point-batching}
 
-The planner scores eligible safe points using a single fixed-size projection batch buffer, sized by `CONFIG_RTL_SAFE_POINT_BATCH_SIZE` (default **1**, **64** on testing builds).
-The batch buffer is reused for every planning pass and is allocated in static RAM, costing roughly `sizeof(ProjectionReference) * CONFIG_RTL_SAFE_POINT_BATCH_SIZE` bytes (about 370 bytes per slot).
+The planner scores eligible safe points using a single fixed-size projection batch buffer, sized by `CONFIG_RTL_SAFE_POINT_BATCH_SIZE` (default **1**, **32** on testing builds).
+The batch buffer is reused for every planning pass and is allocated in static RAM, costing roughly `sizeof(ProjectionReference) * CONFIG_RTL_SAFE_POINT_BATCH_SIZE` bytes (about 380 bytes per slot).
 The default of **1** keeps that cost negligible on boards that leave route planning disabled (`CONFIG_RTL_MISSION_CACHE_SIZE=0`); boards that enable route planning should raise it.
 
 
@@ -239,10 +239,10 @@ If the number of eligible rally points exceeds `CONFIG_RTL_SAFE_POINT_BATCH_SIZE
 To keep every rally point in a single mission scan, raise `CONFIG_RTL_SAFE_POINT_BATCH_SIZE` on boards that have the RAM budget for it.
 :::
 
-`CONFIG_RTL_SAFE_POINT_BATCH_SIZE` must stay between `1` and `255` because the batch counters and indices are stored in `uint8_t`.
+`CONFIG_RTL_SAFE_POINT_BATCH_SIZE` must stay between `1` and `32`: the upper bound matches `DM_KEY_SAFE_POINTS_MAX`, the maximum number of storable safe points, so larger batches could never be filled.
 
 ```ini
-CONFIG_RTL_SAFE_POINT_BATCH_SIZE=64
+CONFIG_RTL_SAFE_POINT_BATCH_SIZE=32
 ```
 
 ## Route Cache

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

src/lib/dataman_client/DatamanClient.hpp

@@ -317,6 +317,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{};
 

src/modules/navigator/Kconfig

@@ -43,9 +43,9 @@ config RTL_MISSION_CACHE_SIZE
 
 config RTL_SAFE_POINT_BATCH_SIZE
 	int "Maximum rally points scored per mission-route planning batch"
-	default 64 if BOARD_TESTING
+	default 32 if BOARD_TESTING
 	default 1
-	range 1 255
+	range 1 32
 	depends on MODULES_NAVIGATOR
 	---help---
 		Number of rally (safe) points held in the fixed-size projection batch buffer that the
@@ -58,11 +58,11 @@ config RTL_SAFE_POINT_BATCH_SIZE
 		boards with spare RAM can raise it so all rally points fit in a single pass.
 
 		The projection batch is allocated in static RAM and is the larger hidden cost: it reserves
-		roughly sizeof(ProjectionReference) * CONFIG_RTL_SAFE_POINT_BATCH_SIZE bytes (about 370 bytes
-		per slot, i.e. ~24 KB at a batch size of 64).
+		roughly sizeof(ProjectionReference) * CONFIG_RTL_SAFE_POINT_BATCH_SIZE bytes (about 380 bytes
+		per slot, i.e. ~12 KB at the maximum batch size of 32).
 
 		The default is 1 so that boards which leave route planning disabled (CONFIG_RTL_MISSION_CACHE_SIZE=0)
 		do not pay the full batch RAM cost; boards that enable route planning should raise it.
 
-		The configured batch size must stay <= 255 because the batch counters and
-		indices are stored in uint8_t.
+		The upper bound of 32 comes from DM_KEY_SAFE_POINTS_MAX, the maximum number of storable
+		safe points: batch slots beyond that count can never be filled.

src/modules/navigator/mission_route_cache.cpp

@@ -44,10 +44,17 @@
 
 #include "mission_route_cache.h"
 
+#include "mission_route_types.h"
+
 #include <inttypes.h>
 
 #include <px4_platform_common/log.h>
 
+// The projection batch only ever holds safe points, so a batch larger than the
+// maximum number of storable safe points is wasted RAM.
+static_assert(mission_route::kMaxSafePointBatch <= DM_KEY_SAFE_POINTS_MAX,
+	      "CONFIG_RTL_SAFE_POINT_BATCH_SIZE must not exceed DM_KEY_SAFE_POINTS_MAX");
+
 void MissionRouteCache::update(const mission_s &mission)
 {
 	updateMissionCache(mission);
@@ -75,12 +82,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) {
@@ -223,7 +224,7 @@ bool MissionRouteCache::getMissionLandItem(int32_t &index, mission_item_s &land_
 		return false;
 	}
 
-	if (cached_land_item.nav_cmd != NAV_CMD_LAND && cached_land_item.nav_cmd != NAV_CMD_VTOL_LAND) {
+	if (!mission_route::isLandingCmd(cached_land_item.nav_cmd)) {
 		return false;
 	}
 
@@ -284,6 +285,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);
 

src/modules/navigator/mission_route_planner.cpp

@@ -54,27 +54,22 @@
 using namespace mission_route;
 
 /**
- * The ProjectionReferenceBatch is reused by collectVehicleProjection and selectSafePoint.
- * Defined static so navigator task stack does not scale with the batch size (~370 bytes per kMaxSafePointBatch slot,
- * i.e. ~24 KB at a batch size of 64, tunable via CONFIG_RTL_SAFE_POINT_BATCH_SIZE).
+ * Shared scratch buffer reused by collectVehicleProjection and selectSafePoint, kept off the
+ * navigator task stack because it scales with CONFIG_RTL_SAFE_POINT_BATCH_SIZE (~380 bytes per slot).
  *
- * Because the type defaults some fields to NAN/-1/NAV_CMD_INVALID, raw storage is kept in .bss and constructed
- * at runtime. This preserves the normal default object state without storing the full initialized image
- * in flash.
+ * The buffer must not be a plain static object: ProjectionReferenceBatch has non-zero member
+ * defaults (NAN/-1/NAV_CMD_INVALID), so a static instance would be initialized into
+ * .data and store its full byte image in flash, which does not fit on some boards. Zeroed raw
+ * storage stays in .bss (RAM reserved at link time, no flash image) and the object is
+ * constructed in place on first use. Placement new on static storage cannot fail, so the
+ * returned reference is always valid.
  */
-alignas(ProjectionReferenceBatch)
-static uint8_t _projection_reference_batch_storage[sizeof(ProjectionReferenceBatch)] {};
-
-static ProjectionReferenceBatch *_projection_reference_batch{nullptr};
-
-static ProjectionReferenceBatch *projectionReferenceBatch()
+static ProjectionReferenceBatch &projectionReferenceBatch()
 {
-	if (_projection_reference_batch == nullptr) {
-		_projection_reference_batch =
-			new (_projection_reference_batch_storage) ProjectionReferenceBatch{};
-	}
+	alignas(ProjectionReferenceBatch) static uint8_t storage[sizeof(ProjectionReferenceBatch)];
+	static ProjectionReferenceBatch *batch = new (storage) ProjectionReferenceBatch{};
 
-	return _projection_reference_batch;
+	return *batch;
 }
 
 namespace
@@ -129,47 +124,40 @@ MissionRoutePlanner::MissionRoutePlanner(const Provider &provider) :
 	_goal_selector(provider, _projection),
 	_reference_batch(projectionReferenceBatch())
 {
-	if (_reference_batch == nullptr) {
-		PX4_ERR("RTL route planner scratch init failed");
-	}
 }
 
-VehicleProjectionResult MissionRoutePlanner::collectVehicleProjection(const RoutePlanRequest &request) const
+MissionRoutePlanner::~MissionRoutePlanner()
 {
-	if (_reference_batch == nullptr) {
-		return vehicleProjectionFailure(FailureReason::kInternalError);
-	}
+	perf_free(_collect_vehicle_projection_perf);
+	perf_free(_select_best_goal_perf);
+}
 
-	if (!request.config.parameters.validForVehicleProjection()) {
+VehicleProjectionResult MissionRoutePlanner::collectVehicleProjection(const Position &vehicle_position,
+		int32_t mission_index, const PlannerConfig &config) const
+{
+	if (!config.parameters.validForVehicleProjection()) {
 		return vehicleProjectionFailure(FailureReason::kInvalidRequest);
 	}
 
-	perf_begin(_collect_vehicle_projection_perf.counter);
-	const VehicleProjectionResult result = _projection.collectVehicleProjection(request, *_reference_batch);
-	perf_end(_collect_vehicle_projection_perf.counter);
+	perf_begin(_collect_vehicle_projection_perf);
+	const VehicleProjectionResult result = _projection.collectVehicleProjection(vehicle_position, mission_index,
+					       config, _reference_batch);
+	perf_end(_collect_vehicle_projection_perf);
 	return result;
 }
 
 GoalSelection MissionRoutePlanner::selectSafePoint(const ProjectionContext &projection_context,
 		const PlannerConfig &config) const
 {
-	if (_reference_batch == nullptr) {
-		return {};
-	}
-
-	return _goal_selector.selectSafePoint(projection_context, config, *_reference_batch).selection;
+	return _goal_selector.selectSafePoint(projection_context, config, _reference_batch).selection;
 }
 
 GoalSelection MissionRoutePlanner::selectBestGoal(const ProjectionContext &projection_context,
 		const PlannerConfig &config) const
 {
-	if (_reference_batch == nullptr) {
-		return {};
-	}
-
-	perf_begin(_select_best_goal_perf.counter);
-	const GoalSelectionResult result = _goal_selector.selectBestGoal(projection_context, config, *_reference_batch);
-	perf_end(_select_best_goal_perf.counter);
+	perf_begin(_select_best_goal_perf);
+	const GoalSelectionResult result = _goal_selector.selectBestGoal(projection_context, config, _reference_batch);
+	perf_end(_select_best_goal_perf);
 	return result.selection;
 }
 
@@ -179,13 +167,14 @@ bool MissionRoutePlanner::closeToBranchOffSegment(const Position &position, cons
 	return _goal_selector.closeToBranchOffSegment(position, selection, acceptance_radius, altitude_acceptance_radius);
 }
 
-JoinPlanResult MissionRoutePlanner::planMissionResumeJoin(const RoutePlanRequest &request) const
+JoinPlanResult MissionRoutePlanner::planMissionResumeJoin(const Position &vehicle_position,
+		int32_t mission_index, const PlannerConfig &config) const
 {
-	if (!request.vehicle_position.valid()) {
+	if (!vehicle_position.valid()) {
 		return joinPlanFailure(FailureReason::kNoValidGlobalPos);
 	}
 
-	const VehicleProjectionResult projection = collectVehicleProjection(request);
+	const VehicleProjectionResult projection = collectVehicleProjection(vehicle_position, mission_index, config);
 
 	if (!projection.success) {
 		return joinPlanFailure(projection.failure_reason);
@@ -195,8 +184,8 @@ JoinPlanResult MissionRoutePlanner::planMissionResumeJoin(const RoutePlanRequest
 	plan.projection_context = projection.projection_context;
 
 	plan.path = _goal_selector.findShortestPathAlongRoute(plan.projection_context.route_length,
-			plan.projection_context, request.config, PathDirectionMode::kForceNominal);
-	plan.join_context = _goal_selector.buildJoinContext(request.vehicle_position, plan.projection_context, plan.path);
+			plan.projection_context, config, PathDirectionMode::kForceNominal);
+	plan.join_context = _goal_selector.buildJoinContext(vehicle_position, plan.projection_context, plan.path);
 
 	if (!plan.valid()) {
 		return joinPlanFailure(FailureReason::kNoValidPath);
@@ -205,13 +194,14 @@ JoinPlanResult MissionRoutePlanner::planMissionResumeJoin(const RoutePlanRequest
 	return joinPlanSuccess(plan);
 }
 
-RoutePlanResult MissionRoutePlanner::planRouteToGoal(const RoutePlanRequest &request) const
+RoutePlanResult MissionRoutePlanner::planRouteToGoal(const Position &vehicle_position,
+		int32_t mission_index, const PlannerConfig &config) const
 {
-	if (!request.config.parameters.validForRouteToGoal()) {
+	if (!config.parameters.validForRouteToGoal()) {
 		return routePlanFailure(FailureReason::kInvalidRequest);
 	}
 
-	const VehicleProjectionResult projection = collectVehicleProjection(request);
+	const VehicleProjectionResult projection = collectVehicleProjection(vehicle_position, mission_index, config);
 
 	if (!projection.success) {
 		return routePlanFailure(projection.failure_reason);
@@ -225,20 +215,20 @@ RoutePlanResult MissionRoutePlanner::planRouteToGoal(const RoutePlanRequest &req
 	plan.projection_context.mission_loops_remaining = 0;
 
 	// Find closest safe point, falling back to mission end points if none found
-	perf_begin(_select_best_goal_perf.counter);
-	const GoalSelectionResult selection = _goal_selector.selectBestGoal(plan.projection_context, request.config,
-					      *_reference_batch);
-	perf_end(_select_best_goal_perf.counter);
+	perf_begin(_select_best_goal_perf);
+	const GoalSelectionResult selection = _goal_selector.selectBestGoal(plan.projection_context, config,
+					      _reference_batch);
+	perf_end(_select_best_goal_perf);
 
 	if (!selection.success) {
 		return routePlanFailure(selection.failure_reason);
 	}
 
 	plan.selection = selection.selection;
 	plan.selection.skip_route_to_safe_point =
-		_goal_selector.canSkipRouteFollowToSelectedGoal(request.vehicle_position, plan.selection, request.config);
+		_goal_selector.canSkipRouteFollowToSelectedGoal(vehicle_position, plan.selection, config);
 
-	plan.join_context = _goal_selector.buildJoinContext(request.vehicle_position, plan.projection_context,
+	plan.join_context = _goal_selector.buildJoinContext(vehicle_position, plan.projection_context,
 			    plan.selection.path);
 
 	if (!plan.valid()) {