From aa7d9603a67849a5d9bb7e2c0fdbcf983a67d415 Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Tue, 16 Jul 2019 17:03:43 +0200
Subject: [PATCH 1/7] Added logic to transition to manual after auto landing or
 RTL

---
 src/modules/commander/Commander.cpp |  9 +++++++++
 src/modules/navigator/rtl.cpp       | 11 ++++++++---
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 3d8aa5df6c5d..1382ef079b25 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1841,6 +1841,15 @@ Commander::run()
 						tune_mission_ok(true);
 					}
 				}
+
+				// TODO Transition into the last state, not just directly to Manual
+				const bool in_right_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
+							    internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
+							    internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION;
+
+				if (in_right_state && mission_result.finished && land_detector.landed && !armed.armed) {
+					main_state_transition(status, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
+				}
 			}
 		}
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 2a05d217849f..131100adff75 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -95,9 +95,14 @@ RTL::on_activation()
 void
 RTL::on_active()
 {
-	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
-		advance_rtl();
-		set_rtl_item();
+	if (_rtl_state != RTL_STATE_LANDED) {
+		if (is_mission_item_reached()) {
+			advance_rtl();
+			set_rtl_item();
+		}
+
+	} else {
+		_navigator->get_mission_result()->finished = true;
 	}
 }
 

From b2e4eab9b8a947c8f1328916057c12435c220dd4 Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Wed, 17 Jul 2019 11:21:20 +0200
Subject: [PATCH 2/7] Keep track of last non-auto state and revert to it

---
 src/modules/commander/Commander.cpp            |  9 +++------
 src/modules/commander/state_machine_helper.cpp | 18 ++++++++++++++++++
 src/modules/commander/state_machine_helper.h   |  3 +++
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 1382ef079b25..61eb8341c265 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1843,12 +1843,9 @@ Commander::run()
 				}
 
 				// TODO Transition into the last state, not just directly to Manual
-				const bool in_right_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
-							    internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
-							    internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION;
-
-				if (in_right_state && mission_result.finished && land_detector.landed && !armed.armed) {
-					main_state_transition(status, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
+				if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed
+				    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
+					main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
 				}
 			}
 		}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 06396cb82159..b5d85b7ebf68 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -87,6 +87,8 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 
 static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
 
+main_state_t last_non_auto_state = commander_state_s::MAIN_STATE_MAX;
+
 void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
 			     const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
 			     uint8_t auto_recovery_nav_state);
@@ -375,6 +377,11 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 	if (ret == TRANSITION_CHANGED) {
 		if (internal_state->main_state != new_main_state) {
+			// If transitioning from non-auto to auto state, save the last non-auto state
+			if (!is_auto_state(internal_state->main_state)) {
+				last_non_auto_state = internal_state->main_state;
+			}
+
 			internal_state->main_state = new_main_state;
 			internal_state->timestamp = hrt_absolute_time();
 
@@ -1212,3 +1219,14 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 		break;
 	}
 }
+
+bool is_auto_state(main_state_t state)
+{
+	return state == commander_state_s::MAIN_STATE_AUTO_RTL
+	       || state == commander_state_s::MAIN_STATE_AUTO_LAND
+	       || state == commander_state_s::MAIN_STATE_AUTO_MISSION
+	       || state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
+	       || state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET
+	       || state == commander_state_s::MAIN_STATE_AUTO_LOITER
+	       || state == commander_state_s::MAIN_STATE_AUTO_PRECLAND;
+}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 3ea6711b91a4..31d4d136dfac 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -147,4 +147,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 		      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
 		      const low_battery_action_t low_bat_action);
 
+extern main_state_t last_non_auto_state;
+bool is_auto_state(main_state_t state);
+
 #endif /* STATE_MACHINE_HELPER_H_ */

From 514c8d9f7b45f19d8068a2778bcb0aebdad33dbd Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Thu, 18 Jul 2019 13:30:00 +0200
Subject: [PATCH 3/7] Removed leftover TODO

---
 src/modules/commander/Commander.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 61eb8341c265..475cb0b2a2fa 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1842,7 +1842,6 @@ Commander::run()
 					}
 				}
 
-				// TODO Transition into the last state, not just directly to Manual
 				if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed
 				    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
 					main_state_transition(status, last_non_auto_state, status_flags, &internal_state);

From 6ff6cf932e71c0f5237d905364ed11a54eadcda2 Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Thu, 18 Jul 2019 17:14:12 +0200
Subject: [PATCH 4/7] Removed landing check

---
 src/modules/commander/Commander.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 475cb0b2a2fa..1b83d58a65d7 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1842,7 +1842,11 @@ Commander::run()
 					}
 				}
 
-				if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed
+				// TODO: Determine what to do with `was_armed`. It updates one loop iteration before this code
+				// is reached, so it's not correct here.
+				// One possible solution: Keep track of time of last arm-disarm transition, and see if it's
+				// less than 1 second ago (or less than 0.1 seconds, or whatever time works)
+				if (is_auto_state(internal_state.main_state) && mission_result.finished && !armed.armed // && was_armed
 				    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
 					main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
 				}

From 285dcd77890d0c73d70430fba300b0f2a96cf59f Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Fri, 19 Jul 2019 11:42:37 +0200
Subject: [PATCH 5/7] Added check for recent disarming.

---
 src/modules/commander/Commander.cpp | 24 +++++++++++++++++-------
 src/modules/commander/Commander.hpp |  3 +++
 src/modules/navigator/rtl.cpp       |  1 +
 3 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 1b83d58a65d7..aaa7a087aa90 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -118,6 +118,7 @@ static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms;
 static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s;	/**< wait for hotplug sensors to come online for upto 8 seconds */
 static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms;
 static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms;
+static constexpr uint64_t MISSION_FINISH_DISARM_TIMEOUT = 500_ms;
 
 /* Mavlink log uORB handle */
 static orb_advert_t mavlink_log_pub = nullptr;
@@ -1842,17 +1843,26 @@ Commander::run()
 					}
 				}
 
-				// TODO: Determine what to do with `was_armed`. It updates one loop iteration before this code
-				// is reached, so it's not correct here.
-				// One possible solution: Keep track of time of last arm-disarm transition, and see if it's
-				// less than 1 second ago (or less than 0.1 seconds, or whatever time works)
-				if (is_auto_state(internal_state.main_state) && mission_result.finished && !armed.armed // && was_armed
-				    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
-					main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
+				if (mission_result.finished) {
+					_last_mission_result_finished = mission_result.timestamp;
 				}
 			}
 		}
 
+		// When auto mission/rtl/land is finished, transition back to the most recently-used manual mode.
+		// This check is done using time because we cannot guarantee that the disarm will happen first or
+		// the mission_result message will come in first.
+		const bool disarmed_and_mission_finished = !armed.armed
+				&& hrt_elapsed_time(&last_disarmed_timestamp) < MISSION_FINISH_DISARM_TIMEOUT
+				&& hrt_elapsed_time(&_last_mission_result_finished) < MISSION_FINISH_DISARM_TIMEOUT;
+
+		if (disarmed_and_mission_finished && is_auto_state(internal_state.main_state)
+		    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
+			// This branch will only happen once per mission finish because, after transitioning to the non-auto
+			// state, is_auto_state(internal_state.main_state) will be false.
+			main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
+		}
+
 		/* start geofence result check */
 		geofence_result_sub.update(&geofence_result);
 
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index a32ac7df6e10..f0b7b0fa61c9 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -251,6 +251,9 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
 
+	// Time of the last received `mission_result` message where finished == True
+	hrt_abstime _last_mission_result_finished{0};
+
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 
 	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 131100adff75..334d39a62c05 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -103,6 +103,7 @@ RTL::on_active()
 
 	} else {
 		_navigator->get_mission_result()->finished = true;
+		_navigator->set_mission_result_updated();
 	}
 }
 

From 20ce88c5c332486ec62e2849743fbd03e384a9c4 Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Fri, 18 Oct 2019 10:29:49 +0200
Subject: [PATCH 6/7] Removed timing issue

---
 src/modules/commander/Commander.cpp | 21 ++++++---------------
 src/modules/commander/Commander.hpp |  3 ---
 2 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index aaa7a087aa90..8ddf70b4457f 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1842,24 +1842,15 @@ Commander::run()
 						tune_mission_ok(true);
 					}
 				}
-
-				if (mission_result.finished) {
-					_last_mission_result_finished = mission_result.timestamp;
-				}
 			}
 		}
 
-		// When auto mission/rtl/land is finished, transition back to the most recently-used manual mode.
-		// This check is done using time because we cannot guarantee that the disarm will happen first or
-		// the mission_result message will come in first.
-		const bool disarmed_and_mission_finished = !armed.armed
-				&& hrt_elapsed_time(&last_disarmed_timestamp) < MISSION_FINISH_DISARM_TIMEOUT
-				&& hrt_elapsed_time(&_last_mission_result_finished) < MISSION_FINISH_DISARM_TIMEOUT;
-
-		if (disarmed_and_mission_finished && is_auto_state(internal_state.main_state)
-		    && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) {
-			// This branch will only happen once per mission finish because, after transitioning to the non-auto
-			// state, is_auto_state(internal_state.main_state) will be false.
+		const bool just_disarmed = !armed.armed && was_armed;
+		const bool just_finished_auto_mission = is_auto_state(internal_state.main_state) && _mission_result_sub.get().finished;
+		const bool last_state_valid = last_non_auto_state != commander_state_s::MAIN_STATE_MAX;
+
+		if(just_disarmed && land_detector.landed && just_finished_auto_mission && last_state_valid){
+			PX4_INFO("Just finished auto mission, transitioning back to last manual mode.");
 			main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
 		}
 
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index f0b7b0fa61c9..a32ac7df6e10 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -251,9 +251,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
 
-	// Time of the last received `mission_result` message where finished == True
-	hrt_abstime _last_mission_result_finished{0};
-
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 
 	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};

From c945a025ca06f667f110de7a0ef775ea0f2ddca3 Mon Sep 17 00:00:00 2001
From: Timothy Scott <timothy@auterion.com>
Date: Fri, 25 Oct 2019 09:21:37 +0200
Subject: [PATCH 7/7] Changed to only switch back to manual mode after RTL,
 instead of after any auto mode

---
 src/modules/commander/Commander.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 8ddf70b4457f..55def64b1704 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1846,10 +1846,11 @@ Commander::run()
 		}
 
 		const bool just_disarmed = !armed.armed && was_armed;
-		const bool just_finished_auto_mission = is_auto_state(internal_state.main_state) && _mission_result_sub.get().finished;
+		const bool just_finished_rtl = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL
+				&& _mission_result_sub.get().finished;
 		const bool last_state_valid = last_non_auto_state != commander_state_s::MAIN_STATE_MAX;
 
-		if(just_disarmed && land_detector.landed && just_finished_auto_mission && last_state_valid){
+		if(just_disarmed && land_detector.landed && just_finished_rtl && last_state_valid){
 			PX4_INFO("Just finished auto mission, transitioning back to last manual mode.");
 			main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
 		}