From e512d77b892023874246c39eea0672f7f420e51f Mon Sep 17 00:00:00 2001
From: Thomas Stastny <thomas.stastny@auterion.com>
Date: Thu, 21 Jul 2022 13:56:26 +0200
Subject: [PATCH] RTL: expose RTL state

---
 src/modules/navigator/navigator_main.cpp |  2 +-
 src/modules/navigator/rtl.cpp            |  6 -----
 src/modules/navigator/rtl.h              | 34 ++++++++++++------------
 3 files changed, 18 insertions(+), 24 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 7e375bc57e..8db4482180 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -582,7 +582,7 @@ void Navigator::run()
 				case RTL::RTL_TYPE_MISSION_LANDING:
 				case RTL::RTL_TYPE_CLOSEST:
 
-					if (!rtl_activated && _rtl.getLoiterDone()
+					if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER
 					    && _rtl.getShouldEngageMissionForLanding()) {
 						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index eb787fba88..80c95e219e 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -241,8 +241,6 @@ void RTL::find_RTL_destination()
 
 void RTL::on_activation()
 {
-	setLoiterDone(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)
@@ -291,8 +289,6 @@ void RTL::on_activation()
 		_rtl_state = RTL_STATE_RETURN;
 	}
 
-	setLoiterDone(_rtl_state > RTL_STATE_LOITER);
-
 	// reset cruising speed and throttle to default for RTL
 	_navigator->set_cruising_speed();
 	_navigator->set_cruising_throttle();
@@ -641,8 +637,6 @@ void RTL::advance_rtl()
 
 	case RTL_STATE_LOITER:
 
-		setLoiterDone(true);
-
 		if (vtol_in_fw_mode) {
 			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
 
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 49e2a46e22..d3b0510cf7 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -82,6 +82,19 @@ public:
 		RTL_CURRENT_HEADING,
 	};
 
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
+		RTL_STATE_TRANSITION_TO_MC,
+		RTL_MOVE_TO_LAND_HOVER_VTOL,
+		RTL_STATE_LAND,
+		RTL_STATE_LANDED,
+		RTL_STATE_HEAD_TO_CENTER,
+	};
+
 	void on_inactivation() override;
 	void on_inactive() override;
 	void on_activation() override;
@@ -93,13 +106,12 @@ public:
 
 	int get_rtl_type() const { return _param_rtl_type.get(); }
 
-	void setLoiterDone(bool done) { _loiter_done = done; }
-
-	bool getLoiterDone() { return _loiter_done; }
-
 	void get_rtl_xy_z_speed(float &xy, float &z);
+
 	matrix::Vector2f get_wind();
 
+	RTLState getRTLState() { return _rtl_state; }
+
 	bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; }
 
 private:
@@ -121,18 +133,7 @@ private:
 
 	float getHoverLandSpeed();
 
-	enum RTLState {
-		RTL_STATE_NONE = 0,
-		RTL_STATE_CLIMB,
-		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND,
-		RTL_STATE_LOITER,
-		RTL_STATE_TRANSITION_TO_MC,
-		RTL_MOVE_TO_LAND_HOVER_VTOL,
-		RTL_STATE_LAND,
-		RTL_STATE_LANDED,
-		RTL_STATE_HEAD_TO_CENTER,
-	} _rtl_state{RTL_STATE_NONE};
+	RTLState _rtl_state{RTL_STATE_NONE};
 
 	struct RTLPosition {
 		double lat;
@@ -160,7 +161,6 @@ private:
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
 	float _rtl_loiter_rad{50.0f};		// radius at which a fixed wing would loiter while descending
 
-	bool _loiter_done{false}; // this flag is set to true if RTL is active and we are past the loiter state
 	bool _rtl_alt_min{false};
 	bool _should_engange_mission_for_landing{false};