mission: be more intelligent about saying that we are on a mission landing

- previously the decision of being on a landing pattern was taken by just
looking at the current mission index. However, even if the current mission
index indicates a landing pattern the vehicle could be at an arbitrary location, far
from being established on the pattern.

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/mission.cpp

@@ -58,6 +58,9 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
 
 Mission::Mission(Navigator *navigator) :
 	MissionBlock(navigator),
@@ -73,6 +76,12 @@ Mission::on_inactive()
 	 * is used for missions such as RTL. */
 	_navigator->set_cruising_speed();
 
+	// if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid
+	// this prevents RTL to just continue at the current mission index
+	if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) {
+		_navigator->setMissionLandingInProgress(false);
+	}
+
 	/* Without home a mission can't be valid yet anyway, let's wait. */
 	if (!_navigator->home_position_valid()) {
 		return;
@@ -147,6 +156,8 @@ Mission::on_inactivation()
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
 	}
+
+	_time_mission_deactivated = hrt_absolute_time();
 }
 
 void
@@ -421,12 +432,15 @@ Mission::land_start()
 {
 	// if not currently landing, jump to do_land_start
 	if (_land_start_available) {
-		if (landing()) {
+		if (_navigator->getMissionLandingInProgress()) {
 			return true;
 
 		} else {
 			set_current_mission_index(get_land_start_index());
-			return landing();
+
+			const bool can_land_now = landing();
+			_navigator->setMissionLandingInProgress(can_land_now);
+			return can_land_now;
 		}
 	}
 
@@ -1626,6 +1640,11 @@ Mission::set_mission_item_reached()
 	_navigator->get_mission_result()->seq_reached = _current_mission_index;
 	_navigator->set_mission_result_updated();
 
+	// let the navigator know that we are currently executing the mission landing.
+	// Using the method landing() itself is not accurate as it only give information about the mission index
+	// but the vehicle could still be very far from the actual landing items
+	_navigator->setMissionLandingInProgress(landing());
+
 	reset_mission_item_reached();
 }
 

src/modules/navigator/mission.h

@@ -255,6 +255,8 @@ private:
 
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
+	hrt_abstime _time_mission_deactivated{0};
+
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_MISSION

src/modules/navigator/navigator.h

@@ -269,7 +269,10 @@ public:
 
 	void 		set_mission_failure(const char *reason);
 
-	// MISSION
+	void 		setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; }
+
+	bool 		getMissionLandingInProgress() { return _mission_landing_in_progress; }
+
 	bool		is_planned_mission() const { return _navigation_mode == &_mission; }
 	bool		on_mission_landing() { return _mission.landing(); }
 	bool		start_mission_landing() { return _mission.land_start(); }
@@ -396,6 +399,9 @@ private:
 	float _mission_cruising_speed_fw{-1.0f};
 	float _mission_throttle{NAN};
 
+	bool _mission_landing_in_progress{false};	// this flag gets set if the mission is currently executing on a landing pattern
+	// if mission mode is inactive, this flag will be cleared after 2 seconds
+
 	// update subscriptions
 	void		params_update();
 

src/modules/navigator/navigator_main.cpp

@@ -563,9 +563,14 @@ Navigator::run()
 
 					}
 
-					// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly
-					if (on_mission_landing() && !get_land_detected()->landed) {
+
+					if (!rtl_activated && _rtl.initialClimbDone() && get_mission_start_land_available()) {
 						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
+
+						if (!getMissionLandingInProgress()) {
+							start_mission_landing();
+						}
+
 						navigation_mode_new = &_mission;
 
 					} else {

src/modules/navigator/rtl.cpp

@@ -225,8 +225,10 @@ void RTL::on_activation()
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
 
-	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->on_mission_landing()) {
+	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
 		// RTL straight to RETURN state, but mission will takeover for landing.
+		_rtl_state = RTL_STATE_RETURN;
+
 
 	} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
 
@@ -239,7 +241,10 @@ void RTL::on_activation()
 		_rtl_state = RTL_STATE_RETURN;
 	}
 
+	setInitialClimbDone(_rtl_state != RTL_STATE_CLIMB);
+
 	set_rtl_item();
+
 }
 
 void RTL::on_active()
@@ -476,6 +481,7 @@ void RTL::advance_rtl()
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB:
+		setInitialClimbDone(true);
 		_rtl_state = RTL_STATE_RETURN;
 		break;
 

src/modules/navigator/rtl.h

@@ -83,6 +83,10 @@ public:
 
 	int rtl_destination();
 
+	void setInitialClimbDone(bool done) { _initial_climb_done = done; }
+
+	bool initialClimbDone() { return _initial_climb_done; }
+
 private:
 	/**
 	 * Set the RTL item
@@ -134,6 +138,7 @@ private:
 
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
 	bool _rtl_alt_min{false};
+	bool _initial_climb_done{false};	// this flag is set to true if RTL is active and we are past the climb state
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,