From 37e28e7fa82a965967abba6650ba24a31aba10d3 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 22 Jan 2014 17:04:07 +0100
Subject: commander: OFFBOARD mode added, WIP

---
 src/modules/commander/commander.cpp            | 80 ++++++++++++++------------
 src/modules/commander/state_machine_helper.cpp |  6 ++
 2 files changed, 49 insertions(+), 37 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e2b6ad1ca..dfcb1cc9f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1429,56 +1429,62 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 	/* evaluate the main state machine */
 	transition_result_t res = TRANSITION_DENIED;
 
-	switch (current_status->mode_switch) {
-	case MODE_SWITCH_MANUAL:
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-		// TRANSITION_DENIED is not possible here
-		break;
+	if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
+		/* offboard switch overrides main switch */
+		res = main_state_transition(current_status, MAIN_STATE_OFFBOARD);
 
-	case MODE_SWITCH_ASSISTED:
-		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-			res = main_state_transition(current_status, MAIN_STATE_EASY);
+	} else {
+		switch (current_status->mode_switch) {
+		case MODE_SWITCH_MANUAL:
+			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+			// TRANSITION_DENIED is not possible here
+			break;
 
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
+		case MODE_SWITCH_ASSISTED:
+			if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
+				res = main_state_transition(current_status, MAIN_STATE_EASY);
 
-			// else fallback to SEATBELT
-			print_reject_mode("EASY");
-		}
+				if (res != TRANSITION_DENIED)
+					break;	// changed successfully or already in this state
 
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+				// else fallback to SEATBELT
+				print_reject_mode("EASY");
+			}
 
-		if (res != TRANSITION_DENIED)
-			break;	// changed successfully or already in this mode
+			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
 
-		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-			print_reject_mode("SEATBELT");
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this mode
 
-		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-		// TRANSITION_DENIED is not possible here
-		break;
+			if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+				print_reject_mode("SEATBELT");
+
+			// else fallback to MANUAL
+			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+			// TRANSITION_DENIED is not possible here
+			break;
 
-	case MODE_SWITCH_AUTO:
-		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+		case MODE_SWITCH_AUTO:
+			res = main_state_transition(current_status, MAIN_STATE_AUTO);
 
-		if (res != TRANSITION_DENIED)
-			break;	// changed successfully or already in this state
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
 
-		// else fallback to SEATBELT (EASY likely will not work too)
-		print_reject_mode("AUTO");
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+			// else fallback to SEATBELT (EASY likely will not work too)
+			print_reject_mode("AUTO");
+			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
 
-		if (res != TRANSITION_DENIED)
-			break;	// changed successfully or already in this state
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
 
-		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-		// TRANSITION_DENIED is not possible here
-		break;
+			// else fallback to MANUAL
+			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+			// TRANSITION_DENIED is not possible here
+			break;
 
-	default:
-		break;
+		default:
+			break;
+		}
 	}
 
 	return res;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 731e0e3ff..e5599e1c3 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -262,6 +262,12 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 				ret = TRANSITION_CHANGED;
 			}
 
+			break;
+
+		case MAIN_STATE_OFFBOARD:
+
+			ret = TRANSITION_CHANGED;
+
 			break;
 		}
 
-- 
cgit v1.2.3