diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 618cd2f58b..8409272daa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -442,6 +442,7 @@ Mission::find_mission_land_start() _landing_start_lon = missionitem.lon; _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : missionitem.altitude; + _landing_loiter_radius = missionitem.loiter_radius; _land_start_available = true; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index b025179de6..1215dbe8c6 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -96,6 +96,7 @@ public: double get_landing_lat() { return _landing_lat; } double get_landing_lon() { return _landing_lon; } float get_landing_alt() { return _landing_alt; } + float get_landing_loiter_rad() { return _landing_loiter_radius; } void set_closest_item_as_current(); @@ -263,6 +264,8 @@ private: double _landing_lon{0.0}; float _landing_alt{0.0f}; + float _landing_loiter_radius{0.f}; + bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ hrt_abstime _time_mission_deactivated{0}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ddeda2d971..31fd91e1e0 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -301,6 +301,8 @@ public: double get_mission_landing_lon() { return _mission.get_landing_lon(); } float get_mission_landing_alt() { return _mission.get_landing_alt(); } + float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); } + // RTL bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d2e9875ef5..3af04b33fa 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -265,8 +265,6 @@ void RTL::on_activation() const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - _rtl_loiter_rad = _param_rtl_loiter_rad.get(); - if (_navigator->get_land_detected()->landed) { // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; @@ -328,6 +326,10 @@ void RTL::set_rtl_item() const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + // if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission + const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ? + _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get(); + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); switch (_rtl_state) { @@ -423,9 +425,8 @@ void RTL::set_rtl_item() _mission_item.yaw = _destination.yaw; } - if (_navigator->get_vstatus()->is_vtol - || _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.loiter_radius = _rtl_loiter_rad; + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.loiter_radius = landing_loiter_radius; } _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -471,7 +472,10 @@ void RTL::set_rtl_item() _mission_item.yaw = _destination.yaw; } - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.loiter_radius = landing_loiter_radius; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); _mission_item.autocontinue = autocontinue; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3350e0ff57..7b9320a87a 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -159,7 +159,6 @@ private: hrt_abstime _destination_check_time{0}; float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position - float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending bool _rtl_alt_min{false}; bool _should_engange_mission_for_landing{false};