From 0e9d76a6550e2cca57746508dfc74c005b79a34c Mon Sep 17 00:00:00 2001 From: jonas Date: Fri, 24 Jul 2026 10:09:12 +0200 Subject: [PATCH] fix(navigator): reduce flash on non-VTOL targets --- src/modules/navigator/mission_route_cache.cpp | 56 ++++--------------- src/modules/navigator/mission_route_cache.h | 1 - .../navigator/mission_route_provider.h | 8 +-- src/modules/navigator/mission_route_types.cpp | 2 +- src/modules/navigator/rtl.cpp | 26 +++++++-- 5 files changed, 37 insertions(+), 56 deletions(-) diff --git a/src/modules/navigator/mission_route_cache.cpp b/src/modules/navigator/mission_route_cache.cpp index 30f3cdabbce..d589d46d6f7 100644 --- a/src/modules/navigator/mission_route_cache.cpp +++ b/src/modules/navigator/mission_route_cache.cpp @@ -45,8 +45,6 @@ #include "mission_route_types.h" -#include - #include void MissionRouteCache::update(const mission_s &mission) @@ -196,7 +194,7 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission) _dataman_cache_land_item.invalidate(); if (valid_land_index && !valid_dataman_id) { - PX4_ERR("Mission land cache rejected invalid dataman id: %" PRIu8, mission.mission_dataman_id); + PX4_ERR("Mission land cache: invalid dataman id"); } if (state.index >= 0) { @@ -226,8 +224,7 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission) state.retry.clear(); } else { - PX4_WARN("Mission land cache invalid or incomplete, retrying mission_id=%" PRIu32 ", index=%" PRIi32, - state.mission_id, state.index); + PX4_WARN("Mission land cache retry"); _dataman_cache_land_item.invalidate(); state.retry.scheduleRetry(now); } @@ -258,7 +255,7 @@ bool MissionRouteCache::queueMissionLandItem() return true; } - PX4_WARN("Mission land cache queue failed, retrying! item=%" PRIu8 ", index=%" PRIi32, state.dataman_id, state.index); + PX4_WARN("Mission land cache retry"); return false; } @@ -290,7 +287,6 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission) reinterpret_cast(&state.read_stats), sizeof(mission_stats_entry_s)); if (!success) { - state.error_state = SafePointDatamanState::kRead; state.dataman_state = SafePointDatamanState::kError; } @@ -304,38 +300,17 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission) } if (!success) { - state.error_state = SafePointDatamanState::kReadWait; state.dataman_state = SafePointDatamanState::kError; break; } - if (state.read_stats.num_items > DM_KEY_SAFE_POINTS_MAX) { - PX4_ERR("Safe points update failed! invalid count: %" PRIu16, state.read_stats.num_items); - state.error_state = SafePointDatamanState::kReadWait; - state.dataman_state = SafePointDatamanState::kError; - break; - } - - if (state.read_stats.opaque_id != state.source_id) { - PX4_WARN("Safe point state id mismatch, expected %" PRIu32 ", got %" PRIu32, - state.source_id, state.read_stats.opaque_id); - state.error_state = SafePointDatamanState::kReadWait; - state.dataman_state = SafePointDatamanState::kError; - break; - } - - if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != state.source_dataman_id) { - PX4_WARN("Safe point dataman id mismatch, expected %" PRIu8 ", got %" PRIu8, - state.source_dataman_id, state.read_stats.dataman_id); - state.error_state = SafePointDatamanState::kReadWait; - state.dataman_state = SafePointDatamanState::kError; - break; - } - - if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_0 - && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_1) { - PX4_ERR("Safe points update failed! invalid dataman id: %" PRIu8, state.read_stats.dataman_id); - state.error_state = SafePointDatamanState::kReadWait; + if (!(state.read_stats.num_items <= DM_KEY_SAFE_POINTS_MAX + && state.read_stats.opaque_id == state.source_id + && (state.read_stats.num_items == 0 + || (state.read_stats.dataman_id == state.source_dataman_id + && (state.read_stats.dataman_id == DM_KEY_SAFE_POINTS_0 + || state.read_stats.dataman_id == DM_KEY_SAFE_POINTS_1))))) { + PX4_ERR("Safe point cache metadata invalid"); state.dataman_state = SafePointDatamanState::kError; break; } @@ -358,18 +333,12 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission) } if (_dataman_cache_safepoint.size() != state.read_stats.num_items) { - PX4_WARN("Safe point cache resize failed, retrying! requested: %" PRIu16 ", actual: %d", - state.read_stats.num_items, _dataman_cache_safepoint.size()); - state.error_state = SafePointDatamanState::kReadWait; state.dataman_state = SafePointDatamanState::kError; break; } for (int index = 0; index < state.read_stats.num_items; ++index) { if (!_dataman_cache_safepoint.load(static_cast(state.read_stats.dataman_id), index)) { - PX4_WARN("Safe point cache queue failed, retrying! item=%" PRIu8 ", index=%d", - state.read_stats.dataman_id, index); - state.error_state = SafePointDatamanState::kReadWait; state.dataman_state = SafePointDatamanState::kError; break; } @@ -406,7 +375,6 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission) state.retry.clear(); } else { - state.error_state = SafePointDatamanState::kLoad; state.dataman_state = SafePointDatamanState::kError; } @@ -418,10 +386,10 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission) const RetryBackoff retry = state.retry; if (state.dataman_state == SafePointDatamanState::kError) { - PX4_WARN("Safe points update failed, retrying! error state: %" PRIu8, static_cast(state.error_state)); + PX4_WARN("Safe point cache retry"); } else { - PX4_ERR("Safe points update failed! invalid dataman state: %" PRIu8, static_cast(state.dataman_state)); + PX4_ERR("Safe point cache state invalid"); } resetSafePointCacheState(false); diff --git a/src/modules/navigator/mission_route_cache.h b/src/modules/navigator/mission_route_cache.h index d417cc2a718..dd1dd89a78e 100644 --- a/src/modules/navigator/mission_route_cache.h +++ b/src/modules/navigator/mission_route_cache.h @@ -155,7 +155,6 @@ private: struct SafePointState { SafePointDatamanState dataman_state{SafePointDatamanState::kUpdateRequestWait}; - SafePointDatamanState error_state{SafePointDatamanState::kUpdateRequestWait}; mission_stats_entry_s read_stats{}; ///< Async DM_KEY_SAFE_POINTS_STATE read buffer. mission_stats_entry_s cached_stats{}; ///< Identity of the safe-point set currently in the RAM cache. uint32_t source_id{0}; ///< Active mission.safe_points_id being tracked. diff --git a/src/modules/navigator/mission_route_provider.h b/src/modules/navigator/mission_route_provider.h index 8505be2bde4..a5f8ed4470d 100644 --- a/src/modules/navigator/mission_route_provider.h +++ b/src/modules/navigator/mission_route_provider.h @@ -70,11 +70,11 @@ public: * items that follow it. The next rally point starts a new block. * Invalid rally points are skipped so a later nearby valid rally point can still be considered. */ - virtual land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, + land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const; - virtual bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, - float home_altitude_amsl) const; - virtual bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const; + bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, + float home_altitude_amsl) const; + bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const; protected: /** diff --git a/src/modules/navigator/mission_route_types.cpp b/src/modules/navigator/mission_route_types.cpp index 0c99c4ce6d4..7344828d3fb 100644 --- a/src/modules/navigator/mission_route_types.cpp +++ b/src/modules/navigator/mission_route_types.cpp @@ -91,7 +91,7 @@ bool extractSafePointPosition(const mission_item_s &safe_point_item, float home_ break; default: - PX4_WARN("RTL safe point frame %u unsupported", static_cast(safe_point_item.frame)); + PX4_WARN("RTL: unsupported rally frame"); return false; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 5c5a6838cf1..adeac909167 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -303,7 +303,9 @@ void RTL::setRtlTypeAndDestination() } if (new_rtl_type == RtlType::RTL_DIRECT) { +#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL landing_loiter = selectLandingApproach(destination); +#endif if (!landing_loiter.isValid()) { landing_loiter.lat = destination.lat; @@ -369,8 +371,10 @@ void RTL::setRtlTypeAndDestination() PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_point_index) { +#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL 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); +#endif const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache(); PositionYawSetpoint closest_safe_point{static_cast(NAN), static_cast(NAN), NAN, NAN}; @@ -414,14 +418,19 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, candidate_setpoint.lat, candidate_setpoint.lon)}; - const bool current_safe_point_has_approaches{ +#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL + const bool current_safe_point_has_approaches { mission_route_cache.hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt) }; _one_rally_point_has_land_approach |= current_safe_point_has_approaches; + const bool approach_requirement_satisfied = !vtol_in_fw_mode || (_param_rtl_appr_force.get() == 0) + || current_safe_point_has_approaches; +#else + constexpr bool approach_requirement_satisfied = true; +#endif - if (((dist + MIN_DIST_THRESHOLD) < min_dist) - && (!vtol_in_fw_mode || (_param_rtl_appr_force.get() == 0) || current_safe_point_has_approaches)) { + if (((dist + MIN_DIST_THRESHOLD) < min_dist) && approach_requirement_satisfied) { min_dist = dist; closest_safe_point = candidate_setpoint; safe_point_index = static_cast(current_seq); @@ -437,17 +446,22 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); - 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); - 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) { +#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL + 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); _home_has_land_approach = mission_route_cache.hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt); +#endif const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode); +#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach); +#else + constexpr bool required_approach_missing_for_home = false; +#endif // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we force approach landing for vtol in fw and it is not defined for home. const bool deprioritize_home = prioritize_safe_points_over_home || required_approach_missing_for_home;