Merge remote-tracking branch 'origin/master' into obcfailsafe

4 files changed, 22 insertions, 21 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4e14c3ae3..c6f56d5e3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -392,7 +392,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// 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_local);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
 		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1092,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -1319,14 +1319,14 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
@@ -1340,7 +1340,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -1399,7 +1399,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
 					}
@@ -1425,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[])
 					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
@@ -2195,7 +2195,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2258,7 +2258,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}
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 2e18c4284..08dda2fab 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -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, 0 /* no mavlink_fd */);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			MTT_ALL_NOT_VALID,
 			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-		{ "transition: MANUAL to AUTO_MISSION - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
-		{ "transition: AUTO_MISSION to MANUAL - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a3265142d..ecfe62e03 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
 };
 
 transition_result_t
-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
+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
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			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);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 4285d2977..61d0f29d0 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 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, const int mavlink_fd);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);