fix(navigator): move ProjectionReferenceBatch to .bss (AXI_SRAM)

src/modules/navigator/mission_route_planner.cpp

@@ -45,6 +45,8 @@
 
 #include "mission_route_planner.h"
 
+#include <new>
+
 #include <lib/perf/perf_counter.h>
 
 #include <px4_platform_common/log.h>
@@ -52,16 +54,28 @@
 using namespace mission_route;
 
 /**
- * TODO: FIX: for now this does not work because some variables are init to NAN.
- * Therefore, the large struct lives in .data instead of .bss
+ * 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).
  *
- * The ProjectionReferenceBatch (~370 bytes per kMaxSafePointBatch slot, i.e. ~24 KB at a batch
- * size of 64, board-tunable via CONFIG_RTL_SAFE_POINT_BATCH_SIZE) is reused by
- * collectVehicleProjection and selectSafePoint. It is file-static rather than stack-allocated so the
- * navigator task stack does not scale with the batch size. There is no thread concern, we just need to
- * find a way to define it in the .bss
+ * 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.
  */
-static ProjectionReferenceBatch _projection_reference_batch;
+alignas(ProjectionReferenceBatch)
+static uint8_t _projection_reference_batch_storage[sizeof(ProjectionReferenceBatch)] {};
+
+static ProjectionReferenceBatch *_projection_reference_batch{nullptr};
+
+static ProjectionReferenceBatch *projectionReferenceBatch()
+{
+	if (_projection_reference_batch == nullptr) {
+		_projection_reference_batch =
+			new (_projection_reference_batch_storage) ProjectionReferenceBatch{};
+	}
+
+	return _projection_reference_batch;
+}
 
 namespace
 {
@@ -110,14 +124,28 @@ RoutePlanResult routePlanSuccess(const RoutePlan &plan)
 
 } // namespace
 
+MissionRoutePlanner::MissionRoutePlanner(const Provider &provider) :
+	_projection(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
 {
+	if (_reference_batch == nullptr) {
+		return vehicleProjectionFailure(FailureReason::kInternalError);
+	}
+
 	if (!request.config.parameters.validForVehicleProjection()) {
 		return vehicleProjectionFailure(FailureReason::kInvalidRequest);
 	}
 
 	perf_begin(_collect_vehicle_projection_perf.counter);
-	const VehicleProjectionResult result = _projection.collectVehicleProjection(request, _projection_reference_batch);
+	const VehicleProjectionResult result = _projection.collectVehicleProjection(request, *_reference_batch);
 	perf_end(_collect_vehicle_projection_perf.counter);
 	return result;
 }
@@ -125,14 +153,22 @@ VehicleProjectionResult MissionRoutePlanner::collectVehicleProjection(const Rout
 GoalSelection MissionRoutePlanner::selectSafePoint(const ProjectionContext &projection_context,
 		const PlannerConfig &config) const
 {
-	return _goal_selector.selectSafePoint(projection_context, config, _projection_reference_batch).selection;
+	if (_reference_batch == nullptr) {
+		return {};
+	}
+
+	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, _projection_reference_batch);
+	const GoalSelectionResult result = _goal_selector.selectBestGoal(projection_context, config, *_reference_batch);
 	perf_end(_select_best_goal_perf.counter);
 	return result.selection;
 }
@@ -191,7 +227,7 @@ RoutePlanResult MissionRoutePlanner::planRouteToGoal(const RoutePlanRequest &req
 	// 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,
-					      _projection_reference_batch);
+					      *_reference_batch);
 	perf_end(_select_best_goal_perf.counter);
 
 	if (!selection.success) {

src/modules/navigator/mission_route_planner.h

@@ -55,8 +55,7 @@
 class MissionRoutePlanner
 {
 public:
-	explicit MissionRoutePlanner(const mission_route::Provider &provider) :
-		_projection(provider), _goal_selector(provider, _projection) {}
+	explicit MissionRoutePlanner(const mission_route::Provider &provider);
 	MissionRoutePlanner(const MissionRoutePlanner &) = delete;
 	MissionRoutePlanner &operator=(const MissionRoutePlanner &) = delete;
 	MissionRoutePlanner(MissionRoutePlanner &&) = delete;
@@ -101,6 +100,7 @@ private:
 
 	mission_route::MissionRouteProjection _projection;
 	mission_route::MissionRouteGoalSelector _goal_selector;
+	mission_route::ProjectionReferenceBatch *_reference_batch{nullptr};
 	PerfCounterGuard _collect_vehicle_projection_perf{"rtl_route_collect_vehicle_proj"};
 	PerfCounterGuard _select_best_goal_perf{"rtl_route_select_best_goal"};
 };

src/modules/navigator/mission_route_projection.cpp

@@ -53,6 +53,9 @@ using namespace math;
 
 namespace mission_route
 {
+
+MissionRouteProjection::BatchSearchState MissionRouteProjection::_batch_search_state{};
+
 namespace
 {
 
@@ -561,7 +564,12 @@ ProjectionScanResult MissionRouteProjection::findProjectionCandidates(const Proj
 	SegmentPositions segment_positions{};
 	bool have_previous = false;
 	float total_dist = 0.f;
-	BatchSearchState batch_state{};
+	BatchSearchState &batch_state = _batch_search_state;
+	batch_state.stats = {};
+
+	for (uint8_t i = 0; i < batch.count; ++i) {
+		batch_state.candidate_states[i].reset();
+	}
 
 	// Scan the full mission once, evaluating segments against every batch item.
 	for (int32_t index = first_position_index; index < _provider.missionCount(); ++index) {

src/modules/navigator/mission_route_projection.h

@@ -120,10 +120,18 @@ private:
 	struct BranchInSelectionResult;
 
 	struct CandidateSearchState {
-		bool prev_projection_on_end{true};
+		bool prev_projection_on_end{false};
 		bool projection_on_end_for_segment{false};
-		float min_xtrack{FLT_MAX};
-		float xtrack_limit{FLT_MAX};
+		float min_xtrack{0.f};
+		float xtrack_limit{0.f};
+
+		void reset()
+		{
+			prev_projection_on_end = true;
+			projection_on_end_for_segment = false;
+			min_xtrack = FLT_MAX;
+			xtrack_limit = FLT_MAX;
+		}
 	};
 
 	struct RouteSegmentView {
@@ -149,6 +157,8 @@ private:
 		ProjectionScanStats stats{};
 	};
 
+	static BatchSearchState _batch_search_state;
+
 	PositionLookupStatus findNextValidPositionIndex(int32_t start_index, float home_altitude_amsl,
 			int32_t &next_position_index) const;
 

src/modules/navigator/rtl.cpp

@@ -69,7 +69,8 @@ static_assert(DM_KEY_SAFE_POINTS_MAX < RTL_STATUS_NO_SAFE_POINT,
 RTL::RTL(Navigator *navigator) :
 	NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL),
 	ModuleParams(navigator),
-	_rtl_direct(navigator)
+	_rtl_direct(navigator),
+	_mission_route_planner(*navigator->get_mission_route_cache())
 {
 	_rtl_direct.initialize();
 }

src/modules/navigator/rtl.h

@@ -45,6 +45,7 @@
 
 #include "navigator_mode.h"
 #include "navigation.h"
+#include "mission_route_planner.h"
 #include "rtl_base.h"
 #include "rtl_direct.h"
 #include "rtl_direct_mission_land.h"
@@ -184,6 +185,7 @@ private:
 	bool _one_rally_point_has_land_approach{false}; ///< Flag if a rally point has a land approach defined
 
 	RtlDirect _rtl_direct;
+	MissionRoutePlanner _mission_route_planner;
 
 	bool _enforce_rtl_alt{false};