From 58a4c38519498888679e3d81c7e6c85ae860dfa8 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 8 Mar 2022 20:16:05 +0300 Subject: [PATCH] rtl: don't fly mission landing if we trigger rtl in hover Signed-off-by: RomanBapst --- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/rtl.cpp | 26 +++++++----------------- src/modules/navigator/rtl.h | 3 ++- 3 files changed, 10 insertions(+), 21 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3afc3de1fe5c..3ee1620303a7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -592,7 +592,7 @@ void Navigator::run() case RTL::RTL_TYPE_CLOSEST: if (!rtl_activated && _rtl.getClimbAndReturnDone() - && _rtl.getDestinationTypeMissionLanding()) { + && _rtl.getShouldEngageMissionForLanding()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index df1d2a9613f4..54978d8a1fc6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -242,6 +242,13 @@ void RTL::find_RTL_destination() void RTL::on_activation() { + setClimbAndReturnDone(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 + _should_engange_mission_for_landing = (_destination.type == RTL_DESTINATION_MISSION_LANDING) + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + // output the correct message, depending on where the RTL destination is switch (_destination.type) { case RTL_DESTINATION_HOME: @@ -318,25 +325,6 @@ void RTL::on_active() void RTL::set_rtl_item() { - // RTL_TYPE: mission landing. - // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination. - // After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing. - if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { - if (_rtl_state > RTL_STATE_RETURN) { - if (_navigator->start_mission_landing()) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing\t"); - events::send(events::ID("rtl_using_mission_landing"), events::Log::Info, "RTL: using mission landing"); - return; - - } else { - // Otherwise use regular RTL. - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing\t"); - events::send(events::ID("rtl_not_using_mission_landing"), events::Log::Error, - "RTL: unable to use mission landing, doing regular RTL"); - } - } - } - _navigator->set_can_loiter_at_sp(false); const vehicle_global_position_s &gpos = *_navigator->get_global_position(); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 02570dca840a..42f8f320f5c1 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -100,7 +100,7 @@ class RTL : public MissionBlock, public ModuleParams void get_rtl_xy_z_speed(float &xy, float &z); matrix::Vector2f get_wind(); - bool getDestinationTypeMissionLanding() { return _destination.type == RTL_DESTINATION_MISSION_LANDING; } + bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; } private: @@ -162,6 +162,7 @@ class RTL : public MissionBlock, public ModuleParams 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 _rtl_alt_min{false}; + bool _should_engange_mission_for_landing{false}; DEFINE_PARAMETERS( (ParamFloat) _param_rtl_return_alt,