Merge branch 'ArmDisarm' of github.com:DonLakeFlyer/Firmware into ekf_params

6 files changed, 386 insertions, 279 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7ac7aff0f..e39cc5fe7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -137,7 +137,7 @@ enum MAV_MODE_FLAG {
 };
 
 /* Mavlink file descriptors */
-static int mavlink_fd;
+static int mavlink_fd = 0;
 
 /* flags */
 static bool commander_initialized = false;
@@ -217,11 +217,10 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-int arm();
-int disarm();
-
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
+
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm();
+		arm_disarm(true, mavlink_fd, "command line");
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "disarm")) {
-		disarm();
+	if (!strcmp(argv[1], "2")) {
+		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
 
@@ -363,30 +362,20 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-int arm()
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
 {
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-int disarm()
-{
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
+    transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+    
+    // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+    // output appropriate error messages if the state cannot transition.
+    arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+    if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+        mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+    } else if (arming_res == TRANSITION_DENIED) {
+        tune_negative(true);
+    }
+    
+    return arming_res;
 }
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
@@ -429,37 +418,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (hil_ret == OK)
 				ret = true;
 
-			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
-			/* set arming state */
-			arming_res = TRANSITION_NOT_CHANGED;
-
-			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
-				}
-
-			} else {
-				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
-
-					if (arming_res == TRANSITION_CHANGED) {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
-					}
-
-				} else {
-					arming_res = TRANSITION_NOT_CHANGED;
-				}
-			}
+            // Transition the arming state
+            arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (arming_res == TRANSITION_CHANGED)
 				ret = true;
@@ -518,27 +478,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
-			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-					ret = true;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
-			}
+            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+            // We use an float epsilon delta to test float equality.
+            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
+            } else {
+                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+                if (arming_res == TRANSITION_DENIED) {
+                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+                } else {
+                    result = VEHICLE_CMD_RESULT_ACCEPTED;
+                }
+            }
 		}
 		break;
 
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 6e72cf0d9..0abb84a82 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	state_machine_helper_test();
-	//state_machine_test();
+	stateMachineHelperTest();
 
 	return 0;
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 40bedd9f3..8a946543d 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,13 +49,12 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual const char*	run_tests();
+	virtual void runTests(void);
 
 private:
-	const char*	arming_state_transition_test();
-	const char*	arming_state_transition_arm_disarm_test();
-	const char*	main_state_transition_test();
-	const char*	is_safe_test();
+	bool armingStateTransitionTest();
+	bool mainStateTransitionTest();
+	bool isSafeTest();
 };
 
 StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() {
 StateMachineHelperTest::~StateMachineHelperTest() {
 }
 
-const char*
-StateMachineHelperTest::arming_state_transition_test()
+bool StateMachineHelperTest::armingStateTransitionTest(void)
 {
+    // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
+    // to simulate machine state prior to testing an arming state transition. This structure is also
+    // use to represent the expected machine state after the transition has been requested.
+    typedef struct {
+        arming_state_t  arming_state;   // vehicle_status_s.arming_state
+        bool            armed;          // actuator_armed_s.armed
+        bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
+    } ArmingTransitionVolatileState_t;
+    
+    // This structure represents a test case for arming_state_transition. It contains the machine
+    // state prior to transtion, the requested state to transition to and finally the expected
+    // machine state after transition.
+    typedef struct {
+        const char*                     assertMsg;                              // Text to show when test case fails
+        ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transtion
+        hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
+        bool                            condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
+        bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+        bool                            safety_off;                             // Current safety_s.safety_off
+        arming_state_t                  requested_state;                        // Requested arming state to transition to
+        ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
+        transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
+    } ArmingTransitionTest_t;
+    
+    // We use these defines so that our test cases are more readable
+    #define ATT_ARMED true
+    #define ATT_DISARMED false
+    #define ATT_READY_TO_ARM true
+    #define ATT_NOT_READY_TO_ARM false
+    #define ATT_SENSORS_INITIALIZED true
+    #define ATT_SENSORS_NOT_INITIALIZED false
+    #define ATT_SAFETY_AVAILABLE true
+    #define ATT_SAFETY_NOT_AVAILABLE true
+    #define ATT_SAFETY_OFF true
+    #define ATT_SAFETY_ON false
+    
+    // These are test cases for arming_state_transition
+    static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
+        // TRANSITION_NOT_CHANGED tests
+        
+        { "no transition: identical states",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+        
+        // TRANSITION_CHANGED tests
+        
+        // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "transition: init to standby",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby error",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to reboot",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to init",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to standby error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to reboot",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to standby",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to armed error",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed error to standby error",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby error to reboot",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to armed",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to reboot",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // hil on tests, standby error to standby not normally allowed
+        
+        { "transition: standby error to standby, hil on",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // Safety switch arming tests
+        
+        { "transition: init to standby, no safety switch",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby, safety switch off",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // standby error
+        { "transition: armed error to standby error requested standby",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // TRANSITION_DENIED tests
+        
+        // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "no transition: init to armed",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby to armed error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to init",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to reboot",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to armed",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to reboot",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to armed",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to standby",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: reboot to armed",
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: in air restore to standby",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Sensor tests
+        
+        { "no transition: init to standby - sensors not initialized",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Safety switch arming tests
+        
+        { "no transition: init to standby, safety switch on",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+    };
+    
 	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
-	struct actuator_armed_s armed;
-
-	// Identical states.
-	status.arming_state = ARMING_STATE_INIT;
-	new_arming_state = ARMING_STATE_INIT;
-	mu_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-
-	// INIT to STANDBY.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = true;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("transition: init to standby",
-		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("ready to arm", armed.ready_to_arm);
-
-	// INIT to STANDBY, sensors not initialized.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = false;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("no transition: sensors not initialized",
-		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("not ready to arm", !armed.ready_to_arm);
-
-	return 0;
-}
-
-const char*
-StateMachineHelperTest::arming_state_transition_arm_disarm_test()
-{
-	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
+	struct safety_s         safety;
 	struct actuator_armed_s armed;
-
-	// TODO(sjwilks): ARM then DISARM.
-	return 0;
+    
+    size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
+    for (size_t i=0; i<cArmingTransitionTests; i++) {
+        const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
+        
+        // Setup initial machine state
+        status.arming_state = test->current_state.arming_state;
+        status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+        status.hil_state = test->hil_state;
+        safety.safety_switch_available = test->safety_switch_available;
+        safety.safety_off = test->safety_off;
+        armed.armed = test->current_state.armed;
+        armed.ready_to_arm = test->current_state.ready_to_arm;
+        
+        // Attempt transition
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        
+        // Validate result of transition
+        ut_assert(test->assertMsg, test->expected_transition_result == result);
+        ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
+        ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
+        ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
+    }
+
+	return true;
 }
 
-const char*
-StateMachineHelperTest::main_state_transition_test()
+bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
 	struct vehicle_status_s current_state;
 	main_state_t new_main_state;
@@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test()
 	// Identical states.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("no transition: identical states",
+	ut_assert("no transition: identical states",
 		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
 
 	// AUTO to MANUAL.
 	current_state.main_state = MAIN_STATE_AUTO;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("transition changed: auto to manual",
+	ut_assert("transition changed: auto to manual",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to SEATBELT.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = true;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("tranisition: manual to seatbelt", 
+	ut_assert("tranisition: manual to seatbelt", 
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+	ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
 
 	// MANUAL to SEATBELT, invalid local altitude.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = false;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("no transition: invalid local altitude",
+	ut_assert("no transition: invalid local altitude",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to EASY.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = true;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("transition: manual to easy",
+	ut_assert("transition: manual to easy",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+	ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
 
 	// MANUAL to EASY, invalid local position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = false;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("no transition: invalid position",
+	ut_assert("no transition: invalid position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to AUTO.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = true;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("transition: manual to auto",
+	ut_assert("transition: manual to auto",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
 
 	// MANUAL to AUTO, invalid global position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = false;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("no transition: invalid global position",
+	ut_assert("no transition: invalid global position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::is_safe_test()
+bool StateMachineHelperTest::isSafeTest(void)
 {
 	struct vehicle_status_s current_state;
 	struct safety_s safety;
@@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test()
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = false;
 	armed.lockdown = true;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
-	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::run_tests()
+void StateMachineHelperTest::runTests(void)
 {
-	mu_run_test(arming_state_transition_test);
-	mu_run_test(arming_state_transition_arm_disarm_test);
-	mu_run_test(main_state_transition_test);
-	mu_run_test(is_safe_test);
-
-	return 0;
+	ut_run_test(armingStateTransitionTest);
+	ut_run_test(mainStateTransitionTest);
+	ut_run_test(isSafeTest);
 }
 
-void
-state_machine_helper_test()
+void stateMachineHelperTest(void)
 {
 	StateMachineHelperTest* test = new StateMachineHelperTest();
-	test->UnitTest::print_results(test->run_tests());
+    test->runTests();
+	test->printResults();
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index 10a68e602..bbf66255e 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void state_machine_helper_test();
+void stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e6f245cf9..f09d586c7 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -69,10 +69,44 @@ static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool failsafe_state_changed = true;
 
+// This array defines the arming state transitions. The rows are the new state, and the columns
+// are the current state. Using new state and current  state you can index into the array which
+// will be true for a valid transition or false for a invalid transition. In some cases even
+// though the transition is marked as true additional checks must be made. See arming_state_transition
+// code for those checks.
+static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
+    //                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
+    { /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
+    { /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
+    { /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
+    { /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
+    { /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
+    { /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
+    { /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+};
+
+// You can index into the array with an arming_state_t in order to get it's textual representation
+static const char* state_names[ARMING_STATE_MAX] = {
+    "ARMING_STATE_INIT",
+    "ARMING_STATE_STANDBY",
+    "ARMING_STATE_ARMED",
+    "ARMING_STATE_ARMED_ERROR",
+    "ARMING_STATE_STANDBY_ERROR",
+    "ARMING_STATE_REBOOT",
+    "ARMING_STATE_IN_AIR_RESTORE",
+};
+
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-			arming_state_t new_arming_state, struct actuator_armed_s *armed)
+arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
+                        const struct safety_s   *safety,            /// current safety settings
+                        arming_state_t          new_arming_state,   /// arming state requested
+                        struct actuator_armed_s *armed,             /// current armed status
+                        const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
+    // Double check that our static arrays are still valid
+    ASSERT(ARMING_STATE_INIT == 0);
+    ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+    
 	/*
 	 * Perform an atomic state update
 	 */
@@ -83,116 +117,63 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == status->arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
-
 	} else {
-
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
-
 		} else {
 			armed->lockdown = false;
 		}
-
-		switch (new_arming_state) {
-		case ARMING_STATE_INIT:
-
-			/* allow going back from INIT for calibration */
-			if (status->arming_state == ARMING_STATE_STANDBY) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY:
-
-			/* allow coming from INIT and disarming from ARMED */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED
-			    || status->hil_state == HIL_STATE_ON) {
-
-				/* sensors need to be initialized for STANDBY state */
-				if (status->condition_system_sensors_initialized) {
-					ret = TRANSITION_CHANGED;
-					armed->armed = false;
-					armed->ready_to_arm = true;
-				}
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED:
-
-			/* allow arming from STANDBY and IN-AIR-RESTORE */
-			if ((status->arming_state == ARMING_STATE_STANDBY
-			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = true;
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED_ERROR:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_ARMED) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY_ERROR:
-
-			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
-			if (status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_REBOOT:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_IN_AIR_RESTORE:
-
-			/* XXX implement */
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->arming_state = new_arming_state;
-			arming_state_changed = true;
-		}
-	}
-
+        
+        // Check that we have a valid state transition
+        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+        if (valid_transition) {
+            // We have a good transition. Now perform any secondary validation.
+            if (new_arming_state == ARMING_STATE_ARMED) {
+                // Fail transition if we need safety switch press
+                //      Allow if coming from in air restore
+                //      Allow if HIL_STATE_ON
+                if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
+                    if (mavlink_fd) {
+                        mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+                    }
+                    valid_transition = false;
+                }
+            } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+                new_arming_state = ARMING_STATE_STANDBY_ERROR;
+            }
+        }
+        
+        // HIL can always go to standby
+        if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+            valid_transition = true;
+        }
+        
+        /* Sensors need to be initialized for STANDBY state */
+        if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+            valid_transition = false;
+        }
+
+        // Finish up the state transition
+        if (valid_transition) {
+            armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+            armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+            ret = TRANSITION_CHANGED;
+            status->arming_state = new_arming_state;
+            arming_state_changed = true;
+        }
+    }
+    
 	/* end of atomic state update */
 	irqrestore(flags);
 
-	if (ret == TRANSITION_DENIED)
-		warnx("arming transition rejected");
+    if (ret == TRANSITION_DENIED) {
+        static const char* errMsg = "Invalid arming transition from %s to %s";
+        if (mavlink_fd) {
+            mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+        }
+        warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+    }
 
 	return ret;
 }
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index f04879ff9..0ddd4f05a 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);