From 7a6506f2dd55503771ad2415f9606d65a7373897 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 22 Jan 2026 14:56:56 +0100 Subject: [PATCH] rtl: redability suggestions from review, remove unused subscription --- src/modules/navigator/rtl.cpp | 15 ++++++--------- src/modules/navigator/rtl.h | 4 ++-- src/modules/navigator/rtl_direct.cpp | 2 +- src/modules/navigator/rtl_direct.h | 6 ++---- .../navigator/rtl_mission_fast_reverse.cpp | 12 +++++++----- 5 files changed, 18 insertions(+), 21 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 1c9794893b..f62b3787a6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -357,12 +357,13 @@ void RTL::setRtlTypeAndDestination() // check the closest allowed destination. findRtlDestination(destination_type, destination, safe_point_index); - switch (destination_type) { - case DestinationType::DESTINATION_TYPE_MISSION_LAND: + if (destination_type == DestinationType::DESTINATION_TYPE_MISSION_LAND) { new_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; - break; - default: + } else { + + new_rtl_type = RtlType::RTL_DIRECT; + land_approaches_s rtl_land_approaches{readVtolLandApproaches(destination)}; // set loiter position to destination initially, overwrite for VTOL if land approaches exist @@ -375,10 +376,6 @@ void RTL::setRtlTypeAndDestination() && rtl_land_approaches.isAnyApproachValid()) { landing_loiter = chooseBestLandingApproach(rtl_land_approaches); } - - new_rtl_type = RtlType::RTL_DIRECT; - - break; } } @@ -406,7 +403,7 @@ 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); - PositionYawSetpoint safe_point{NAN, NAN, NAN, NAN}; + PositionYawSetpoint safe_point{(double)NAN, (double)NAN, NAN, NAN}; if (_safe_points_updated) { _one_rally_point_has_land_approach = false; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index a05156f27a..d6c5a9afb6 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -158,7 +158,7 @@ private: * @brief calculate return altitude from return altitude parameter, current altitude and cone angle * * @param[in] rtl_position landing position of the rtl - * param[in] destination_type type of the rtl destination + * @param[in] destination_type type of the rtl destination * @param[in] cone_half_angle_deg half angle of the cone [deg] * @return return altitude */ @@ -229,7 +229,7 @@ private: mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; uint32_t _mission_id = 0u; uint32_t _safe_points_id = 0u; - PositionYawSetpoint _last_position_before_link_loss{NAN, NAN, NAN, NAN}; + PositionYawSetpoint _last_position_before_link_loss{(double)NAN, (double)NAN, NAN, NAN}; mission_stats_entry_s _stats; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 54c3e82df1..9c53bca44f 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -489,7 +489,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() case RTLState::MOVE_TO_LAND: case RTLState::TRANSITION_TO_MC: case RTLState::MOVE_TO_LAND_HOVER: { - // Add cruise segment to home + // Add cruise segment to destination float move_to_land_dist{0.f}; matrix::Vector2f direction{}; diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 9d9a8bf25c..40fb18e0cc 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -167,10 +166,10 @@ private: bool _force_heading{false}; RtlTimeEstimator _rtl_time_estimator; - PositionYawSetpoint _destination{NAN, NAN, NAN, NAN}; ///< the RTL position to fly to + PositionYawSetpoint _destination{(double)NAN, (double)NAN, NAN, NAN}; ///< the RTL position to fly to loiter_point_s _land_approach; - float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should transit to the destination DEFINE_PARAMETERS( (ParamFloat) _param_rtl_descend_alt, @@ -185,7 +184,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ - uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 02601391af..7f3bb46f9f 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -258,11 +258,10 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { - const bool loiter_indefinitely = _param_rtl_land_delay.get() < -FLT_EPSILON; - _mission_item.lat = _home_pos_sub.get().lat; _mission_item.lon = _home_pos_sub.get().lon; _mission_item.yaw = NAN; + _mission_item.altitude = _global_pos_sub.get().alt; _mission_item.altitude_is_relative = false; // make previous and next setpoints invalid, such that there will be no line following. @@ -274,14 +273,17 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) do_need_move_to_item()) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - _mission_item.altitude = _global_pos_sub.get().alt; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; } else { - _mission_item.altitude = loiter_indefinitely ? _home_pos_sub.get().alt + _param_rtl_descend_alt.get() : _global_pos_sub.get().alt; - _mission_item.nav_cmd = loiter_indefinitely ? NAV_CMD_WAYPOINT : NAV_CMD_LAND; + _mission_item.nav_cmd = NAV_CMD_LAND; + + if (_param_rtl_land_delay.get() < -FLT_EPSILON) { // parameter negative -> loiter indefinitely instead of landing + _mission_item.altitude = _home_pos_sub.get().alt + _param_rtl_descend_alt.get(); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } _mission_item.land_precision = _param_rtl_pld_md.get();