From 1b7d3883a4f4d2997cf484eae01673bccd3cfc32 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Thu, 30 Nov 2017 12:12:08 +0000
Subject: [PATCH] Commander: Obey override / manual reversion mode from
 external override device

This change will force commander into manual reversion mode when an external override device (like PX4IO) overrides the system externally. This is not a functional change on the outputs, as they were in override mode even without this patch. However, this change ensures that the system state is consistent with the output state and also ensures that the pilot and operator has better situational awareness when he / she triggers the manual reversion without realizing it.
---
 src/modules/commander/commander.cpp | 34 ++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 429e0c1af0..73161acfd4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -295,6 +295,19 @@ void get_circuit_breaker_params();
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
+/**
+ * Set the main system state based on RC and override device inputs
+ */
+transition_result_t set_main_state(struct vehicle_status_s *status, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed);
+
+/**
+ * Enable override (manual reversion mode) on the system
+ */
+transition_result_t set_main_state_override_on(struct vehicle_status_s *status_local, bool *changed);
+
+/**
+ * Set the system main state based on the current RC inputs
+ */
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed);
 
 void reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed);
@@ -2784,7 +2797,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* evaluate the main state machine according to mode switches */
 			bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);
-			transition_result_t main_res = set_main_state_rc(&status, &global_position, &local_position, &status_changed);
+			transition_result_t main_res = set_main_state(&status, &global_position, &local_position, &status_changed);
 
 			/* store last position lock state */
 			_last_condition_global_position_valid = status_flags.condition_global_position_valid;
@@ -3374,6 +3387,25 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 	leds_counter++;
 }
 
+transition_result_t
+set_main_state(struct vehicle_status_s *status_local, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed)
+{
+	if (safety.override_available && safety.override_enabled) {
+		return set_main_state_override_on(status_local, changed);
+	} else {
+		return set_main_state_rc(status_local, global_position, local_position, changed);
+	}
+}
+
+transition_result_t
+set_main_state_override_on(struct vehicle_status_s *status_local, bool *changed)
+{
+	transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
+	*changed = (res == TRANSITION_CHANGED);
+
+	return res;
+}
+
 transition_result_t
 set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed)
 {