State machine rewritten, compiles, WIP

1 files changed, 15 insertions, 38 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index ae8e168e1..9132d1b49 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 	}
 
 	/* autonomous mode */
-	if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+	if (v_status.main_state == MAIN_STATE_AUTO) {
 		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
 	}
 
@@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
-			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
+	if (v_status.main_state == MAIN_STATE_MANUAL
+			|| v_status.main_state == MAIN_STATE_SEATBELT
+			|| v_status.main_state == MAIN_STATE_EASY) {
 
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
-		|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
-		|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
+	if (v_status.navigation_state == MAIN_STATE_EASY) {
 
 		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
 	}
@@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 
 	/* set calibration state */
 	if (v_status.preflight_calibration) {
-
 		*mavlink_state = MAV_STATE_CALIBRATING;
-
 	} else if (v_status.system_emergency) {
-
 		*mavlink_state = MAV_STATE_EMERGENCY;
-
-	// XXX difference between active and armed? is AUTO_READY active?
-	} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
-			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
-			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
-			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
-
+	} else if (v_status.arming_state == ARMING_STATE_INIT
+			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
-
-	} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
-			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
-			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
-			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
-
+	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
 		*mavlink_state = MAV_STATE_STANDBY;
-
-	} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
-
-		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
+		*mavlink_state = MAV_STATE_POWEROFF;
 	} else {
-
 		warnx("Unknown mavlink state");
 		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }