Fix mainStateTransitionTest

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         
         // Safety switch arming tests
         
-        { "transition: init to standby, no safety switch",
+        { "transition: standby to armed, 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",
+        { "transition: standby to armed, 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 },
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         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);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
 bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
-	struct vehicle_status_s current_state;
+	// This structure represent a single test case for testing Main State transitions.
-	main_state_t new_main_state;
+	typedef struct {
+		const char*     assertMsg;				// Text to show when test case fails
+		uint8_t		condition_bits;				// Bits for various condition_* values
+		main_state_t	from_state;				// State prior to transition request
+		main_state_t	to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
 	
-	// Identical states.
+	// Bits for condition_bits
-	current_state.main_state = MAIN_STATE_MANUAL;
+	#define MTT_ALL_NOT_VALID		0
-	new_main_state = MAIN_STATE_MANUAL;
+	#define MTT_ROTARY_WING			1 << 0
-	ut_assert("no transition: identical states",
+	#define MTT_LOC_ALT_VALID		1 << 1
-		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
+	#define MTT_LOC_POS_VALID		1 << 2
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+	#define MTT_HOME_POS_VALID		1 << 3
+	#define MTT_GLOBAL_POS_VALID		1 << 4
+	
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+		
+		// TRANSITION_NOT_CHANGED tests
+		
+		{ "no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+				
+		// TRANSITION_CHANGED tests
+		
+		{ "transition: MANUAL to ACRO",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
-	// AUTO to MANUAL.
+		{ "transition: ACRO to MANUAL",
-	current_state.main_state = MAIN_STATE_AUTO;
+			MTT_ALL_NOT_VALID,
-	new_main_state = MAIN_STATE_MANUAL;
+			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
-	ut_assert("transition changed: auto to manual",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	// MANUAL to ALTCTRL.
+		{ "transition: MANUAL to AUTO_MISSION - global position valid",
-	current_state.main_state = MAIN_STATE_MANUAL;
+			MTT_GLOBAL_POS_VALID,
-	current_state.condition_local_altitude_valid = true;
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("tranisition: manual to altctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
 
-	// MANUAL to ALTCTRL, invalid local altitude.
+		{ "transition: AUTO_MISSION to MANUAL - global position valid",
-	current_state.main_state = MAIN_STATE_MANUAL;
+			MTT_GLOBAL_POS_VALID,
-	current_state.condition_local_altitude_valid = false;
+			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("no transition: invalid local altitude",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	// MANUAL to POSCTRL.
+		{ "transition: MANUAL to AUTO_LOITER - global position valid",
-	current_state.main_state = MAIN_STATE_MANUAL;
+			MTT_GLOBAL_POS_VALID,
-	current_state.condition_local_position_valid = true;
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("transition: manual to posctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
 
-	// MANUAL to POSCTRL, invalid local position.
+		{ "transition: AUTO_LOITER to MANUAL - global position valid",
-	current_state.main_state = MAIN_STATE_MANUAL;
+			MTT_GLOBAL_POS_VALID,
-	current_state.condition_local_position_valid = false;
+			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("no transition: invalid position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	// MANUAL to AUTO.
+		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
-	current_state.main_state = MAIN_STATE_MANUAL;
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-	current_state.condition_global_position_valid = true;
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
-	new_main_state = MAIN_STATE_AUTO;
+
-	ut_assert("transition: manual to auto",
+		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		// TRANSITION_DENIED tests
+
+		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
+		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+	};
+	
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+	for (size_t i=0; i<cMainTransitionTests; i++) {
+		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_state;
+		current_state.main_state = test->from_state;
+		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		
+		// Attempt transition
+		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		
+		// Validate result of transition
+		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+			} else {
+				ut_assert(test->assertMsg, test->from_state == 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;
-	ut_assert("no transition: invalid global position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	return true;
 }