From c056410f8439e760905cb50fe08c49c3a0b344e5 Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Thu, 21 Feb 2013 18:10:34 -0800
Subject: Checkpoint: Added arming check function

---
 apps/commander/state_machine_helper.c | 55 +++++++++++++++++++++++++++++++++++
 apps/commander/state_machine_helper.h |  2 ++
 2 files changed, 57 insertions(+)

diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index e6344f1a8..0611bb52c 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -254,6 +254,61 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st
 	return;
 }
 
+int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) {
+
+	int ret = ERROR;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state != current_arming_state) {
+
+		switch (new_arming_state) {
+			case ARMING_STATE_INIT:
+
+				/* allow going back from INIT for calibration */
+				if (current_arming_state == ARMING_STATE_STANDBY) {
+					ret = OK;
+				}
+				break;
+			case ARMING_STATE_STANDBY:
+
+				/* allow coming from INIT and disarming from ARMED */
+				if (current_arming_state == ARMING_STATE_INIT
+				 || current_arming_state == ARMING_STATE_ARMED) {
+					ret = OK;
+				}
+				break;
+			case ARMING_STATE_ARMED:
+
+				/* allow arming from STANDBY and IN-AIR-RESTORE */
+				if (current_arming_state == ARMING_STATE_STANDBY
+				 || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) {
+					ret = OK;
+				}
+				break;
+			case ARMING_STATE_ARMED_ERROR:
+
+				/* an armed error happens when ARMED obviously */
+				if (current_arming_state == ARMING_STATE_ARMED) {
+					ret = OK;
+				}
+				break;
+			case ARMING_STATE_STANDBY_ERROR:
+				/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+				if (current_arming_state == ARMING_STATE_STANDBY
+				 || current_arming_state == ARMING_STATE_INIT
+				 || current_arming_state == ARMING_STATE_ARMED_ERROR) {
+					ret = OK;
+				}
+				break;
+			default:
+				break;
+		}
+	}
+	return ret;
+}
+
+
+
 /*
  * This functions does not evaluate any input flags but only checks if the transitions
  * are valid.
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 1c0564d07..cf1fa80cd 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -54,4 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state);
+int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state);
+
 #endif /* STATE_MACHINE_HELPER_H_ */
-- 
cgit v1.2.3