Merge branch 'master' of github.com:PX4/Firmware into battload

2 files changed, 31 insertions, 24 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 562e9a1b6..a0c896178 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-#if 0
 	/* Flight termination */
-	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
-
-			//XXX: to enable the parachute, a param needs to be set
-			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
-				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
+			if (cmd->param1 > 0.5f) {
+				//XXX update state machine?
+				armed_local->force_failsafe = true;
+				warnx("forcing failsafe");
 			} else {
-				/* reject parachute depoyment not armed */
-				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				armed_local->force_failsafe = false;
+				warnx("disabling failsafe");
 			}
-
+			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
-#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -1421,8 +1416,12 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 
 			} else if (arming_ret == TRANSITION_DENIED) {
-				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				/*
+				 * the arming transition can be denied to a number of reasons:
+				 *  - pre-flight check failed (sensors not ok or not calibrated)
+				 *  - safety not disabled
+				 *  - system not in manual mode
+				 */
 				tune_negative(true);
 			}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 372ba9d7d..7b26e3e8c 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
-
 	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
 
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
 						valid_transition = false;
 
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
-
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
 						valid_transition = false;
 					}
 
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						if (!status->condition_power_input_valid) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
 							valid_transition = false;
 						}
 
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 							(status->avionics_power_rail_voltage < 4.9f))) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							feedback_provided = true;
 							valid_transition = false;
 						}
 					}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
 			valid_transition = false;
 		}
 
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "INVAL: %s - %s";
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
 
-		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]);
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
 	}
 
 	return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
-			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
 			/* this is frickin' fatal */
 			failed = true;
 			goto system_eval;