diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3afc3de1fe..3ee1620303 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 df1d2a9613..54978d8a1f 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 02570dca84..42f8f320f5 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -100,7 +100,7 @@ public: 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 @@ private: 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,