RTL: fix Mission RTL vs normal RTL (#21464)

* Navigator: on_mission_landing() only can return true if currently in mission mode

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* RTL: reset RTL state when not in RTL nav_state

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Navigator: fix mission vs. normal RTL

- remove extra state _should_engange_mission_for_landing from rtl and have
this logic outside of RTL where Navigator decides on running mission RTL or normal RTL
- fix logic in Navigator to decide mission RTL vs normal RTL

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Mission: land_start(): fix decision if already on mission landing

Simply checking landing() is not enough, as that is not reset until
set_current_mission_index(get_land_start_index()) later in the function.
Instead ask Naviator about it (on_mission_landing()).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Navigator: only update _shouldEngangeMissionForLanding once, to not set it to false after VTOL backtansition

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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 {

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 */

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

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:

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<px4::params::RTL_RETURN_ALT>)  _param_rtl_return_alt,