diff --git a/src/modules/navigator/mission_route_planner.cpp b/src/modules/navigator/mission_route_planner.cpp index d815c6a66be..2b0d452f841 100644 --- a/src/modules/navigator/mission_route_planner.cpp +++ b/src/modules/navigator/mission_route_planner.cpp @@ -45,6 +45,8 @@ #include "mission_route_planner.h" +#include + #include #include @@ -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) { diff --git a/src/modules/navigator/mission_route_planner.h b/src/modules/navigator/mission_route_planner.h index e4d1d021659..243344b4479 100644 --- a/src/modules/navigator/mission_route_planner.h +++ b/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"}; }; diff --git a/src/modules/navigator/mission_route_projection.cpp b/src/modules/navigator/mission_route_projection.cpp index d6e59077108..095de610cf6 100644 --- a/src/modules/navigator/mission_route_projection.cpp +++ b/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) { diff --git a/src/modules/navigator/mission_route_projection.h b/src/modules/navigator/mission_route_projection.h index 185781d6a4b..629cf690160 100644 --- a/src/modules/navigator/mission_route_projection.h +++ b/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; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index bad17cee54b..c807cc4ecc8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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(); } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 0a66f8a9122..94a2cda920d 100644 --- a/src/modules/navigator/rtl.h +++ b/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};