diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b5caaef4b2..d180002607 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -468,7 +468,9 @@ Mission::land_start() { // if not currently landing, jump to do_land_start if (_land_start_available) { - if (landing()) { + // check if we're currently already in mission mode and on landing part, then simply return true. + // note: it's not enough to check landing(), as that is not reset until set_current_mission_index(get_land_start_index()) + if (_navigator->on_mission_landing()) { return true; } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9a08ed946d..b62bffe334 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -261,7 +261,7 @@ public: bool is_planned_mission() const { return _navigation_mode == &_mission; } - bool on_mission_landing() { return _mission.landing(); } + bool on_mission_landing() { return (_mission.landing() && _navigation_mode == &_mission); } bool start_mission_landing() { return _mission.land_start(); } @@ -365,6 +365,8 @@ private: bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ + bool _shouldEngageMissionForLanding{false}; + Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e1187cf27..317a15f93b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -668,23 +668,37 @@ void Navigator::run() switch (_rtl.get_rtl_type()) { case RTL::RTL_TYPE_MISSION_LANDING: - case RTL::RTL_TYPE_CLOSEST: - if (on_mission_landing() && _rtl.getShouldEngageMissionForLanding()) { - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - - navigation_mode_new = &_mission; - + case RTL::RTL_TYPE_CLOSEST: { + // 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. if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); - events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, - "RTL to Mission landing, continue landing"); + _shouldEngageMissionForLanding = _rtl.getRTLDestinationTypeMission() + && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; } - } else { - navigation_mode_new = &_rtl; - } + if (_shouldEngageMissionForLanding && (on_mission_landing() || _rtl.getRTLState() > RTL::RTL_STATE_CLIMB)) { - break; + // already in a mission landing, we just need to inform the user and stay in mission + if (rtl_activated_now) { + mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); + events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, + "RTL to Mission landing, continue landing"); + } + + if (_navigation_mode != &_mission) { + // the first time we're here start the mission landig + start_mission_landing(); + } + + _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); + navigation_mode_new = &_mission; + + } else { + navigation_mode_new = &_rtl; + } + + break; + } case RTL::RTL_TYPE_MISSION_LANDING_REVERSED: if (_mission.get_land_start_available() && !get_land_detected()->landed) { @@ -801,6 +815,7 @@ void Navigator::run() if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { _rtl_activated = false; + _rtl.resetRtlState(); } // Do not execute any state machine while we are disarmed diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b9ac759cb8..34ddf90591 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -253,11 +253,6 @@ void RTL::on_activation() { _rtl_state = RTL_STATE_NONE; - // 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: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 159addd71f..3c6dea1a2b 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -112,7 +112,9 @@ public: RTLState getRTLState() { return _rtl_state; } - bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; } + bool getRTLDestinationTypeMission() { return _destination.type == RTLDestinationType::RTL_DESTINATION_MISSION_LANDING; } + + void resetRtlState() { _rtl_state = RTL_STATE_NONE; } private: @@ -161,7 +163,6 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; - bool _should_engange_mission_for_landing{false}; DEFINE_PARAMETERS( (ParamFloat) _param_rtl_return_alt,