From 1ec62c4063140310ecd48c023402a6246423769a Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 19 Jul 2022 17:30:47 +0200 Subject: [PATCH] rtl: let fixed-wing RTL all the way to the loiter/delay state also fix home vs destination alt discrepancy on RTL --- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/rtl.cpp | 22 +++++++++++----------- src/modules/navigator/rtl.h | 6 +++--- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 225c52d004..7e375bc57e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -582,7 +582,7 @@ void Navigator::run() case RTL::RTL_TYPE_MISSION_LANDING: case RTL::RTL_TYPE_CLOSEST: - if (!rtl_activated && _rtl.getClimbAndReturnDone() + if (!rtl_activated && _rtl.getLoiterDone() && _rtl.getShouldEngageMissionForLanding()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 5b40b01ca0..8485e7bec6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -235,14 +235,13 @@ void RTL::find_RTL_destination() _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); } else { - _rtl_alt = max(global_position.alt, max(_destination.alt, - _navigator->get_home_position()->alt + _param_rtl_return_alt.get())); + _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); } } void RTL::on_activation() { - setClimbAndReturnDone(false); + setLoiterDone(false); // if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission @@ -276,9 +275,9 @@ void RTL::on_activation() _rtl_state = RTL_STATE_LANDED; } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { - // we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode, + // we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode, // which will continue executing the landing - _rtl_state = RTL_STATE_DESCEND; + _rtl_state = RTL_STATE_LAND; } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { @@ -292,7 +291,7 @@ void RTL::on_activation() _rtl_state = RTL_STATE_RETURN; } - setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN); + setLoiterDone(_rtl_state > RTL_STATE_LOITER); // reset cruising speed and throttle to default for RTL _navigator->set_cruising_speed(); @@ -332,8 +331,7 @@ void RTL::set_rtl_item() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); - const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt); - const float loiter_altitude = min(descend_altitude_target, _rtl_alt); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); @@ -430,7 +428,8 @@ void RTL::set_rtl_item() _mission_item.yaw = _destination.yaw; } - if (_navigator->get_vstatus()->is_vtol) { + 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; } @@ -617,8 +616,6 @@ void RTL::advance_rtl() break; case RTL_STATE_RETURN: - setClimbAndReturnDone(true); - if (vtol_in_fw_mode || descend_and_loiter) { _rtl_state = RTL_STATE_DESCEND; @@ -643,6 +640,9 @@ void RTL::advance_rtl() break; case RTL_STATE_LOITER: + + setLoiterDone(true); + if (vtol_in_fw_mode) { _rtl_state = RTL_STATE_TRANSITION_TO_MC; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 42f8f320f5..49e2a46e22 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -93,9 +93,9 @@ public: int get_rtl_type() const { return _param_rtl_type.get(); } - void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; } + void setLoiterDone(bool done) { _loiter_done = done; } - bool getClimbAndReturnDone() { return _climb_and_return_done; } + bool getLoiterDone() { return _loiter_done; } void get_rtl_xy_z_speed(float &xy, float &z); matrix::Vector2f get_wind(); @@ -160,7 +160,7 @@ private: 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 _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state + bool _loiter_done{false}; // this flag is set to true if RTL is active and we are past the loiter state bool _rtl_alt_min{false}; bool _should_engange_mission_for_landing{false};