Merge pull request #1078 from PX4/prearm_checks

Prearm checks

3 files changed, 155 insertions, 38 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 02fe4b03d..efa26eb97 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1306,12 +1306,13 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.arming_state == ARMING_STATE_STANDBY &&
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
-						print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
 
-					} else if (status.main_state != MAIN_STATE_MANUAL) {
+					/* we check outside of the transition function here because the requirement
+					 * for being in manual mode only applies to manual arming actions.
+					 * the system can be armed in auto if armed via the GCS.
+					 */
+					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
-
 					} else {
 						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
@@ -1341,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[])
 			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
+				tune_negative(true);
 			}
 
 			/* evaluate the main state machine according to mode switches */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 940a04aa1..80e6861f6 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -209,12 +209,18 @@ int led_init()
 	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
 	(void)ioctl(leds, LED_ON, LED_BLUE);
 
+	/* switch blue off */
+	led_off(LED_BLUE);
+
 	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
 	}
 
+	/* switch amber off */
+	led_off(LED_AMBER);
+
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7a61d481b..244513375 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,20 +46,32 @@
 #include <dirent.h>
 #include <fcntl.h>
 #include <string.h>
+#include <math.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/differential_pressure.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
 // 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
@@ -98,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_INIT == 0);
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
-	/*
-	 * Perform an atomic state update
-	 */
-	irqstate_t flags = irqsave();
-
 	transition_result_t ret = TRANSITION_DENIED;
 
+	arming_state_t current_arming_state = status->arming_state;
+
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == status->arming_state) {
+	if (new_arming_state == current_arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
+
+		/*
+		 * Get sensing state if necessary
+		 */
+		int prearm_ret = OK;
+
+		/* only perform the check if we have to */
+		if (new_arming_state == ARMING_STATE_ARMED) {
+			prearm_ret = prearm_check(status, mavlink_fd);
+		}
+
+		/*
+		 * Perform an atomic state update
+		 */
+		irqstate_t flags = irqsave();
+
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
@@ -124,35 +149,44 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		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
+
+				//      Do not perform pre-arm checks 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, "#audio: NOT ARMING: Press safety switch first.");
-					}
+				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == HIL_STATE_OFF) {
 
-					valid_transition = false;
-				}
+					// Fail transition if pre-arm check fails
+					if (prearm_ret) {
+						valid_transition = false;
 
-				// Perform power checks only if circuit breaker is not
-				// engaged for these checks
-				if (!status->circuit_breaker_engaged_power_check) {
-					// Fail transition if power is not good
-					if (!status->condition_power_input_valid) {
+					// Fail transition if we need safety switch press
+					} else if (safety->safety_switch_available && !safety->safety_off) {
+
+						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
 
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
 						valid_transition = false;
 					}
 
-					// Fail transition if power levels on the avionics rail
-					// are measured but are insufficient
-					if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
-						(status->avionics_power_rail_voltage < 4.9f)) {
+					// Perform power checks only if circuit breaker is not
+					// engaged for these checks
+					if (!status->circuit_breaker_engaged_power_check) {
+						// Fail transition if power is not good
+						if (!status->condition_power_input_valid) {
 
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
-						valid_transition = false;
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+							valid_transition = false;
+						}
+
+						// Fail transition if power levels on the avionics rail
+						// are measured but are insufficient
+						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
+							(status->avionics_power_rail_voltage < 4.9f)) {
+
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
+							valid_transition = false;
+						}
 					}
+
 				}
 
 			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -177,17 +211,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
 		}
-	}
 
-	/* end of atomic state update */
-	irqrestore(flags);
+		/* end of atomic state update */
+		irqrestore(flags);
+	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "Invalid arming transition from %s to %s";
+		static const char *errMsg = "INVAL: %s - %s";
 
-		if (mavlink_fd) {
-			mavlink_log_critical(mavlink_fd, errMsg, 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]);
 	}
@@ -559,3 +591,80 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	return status->nav_state != nav_state_old;
 }
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+	int ret;
+
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
+		ret = fd;
+		goto system_eval;
+	}
+
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+		goto system_eval;
+	}
+
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+		if (accel_scale < 9.78f || accel_scale > 9.83f) {
+			mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
+		}
+
+		if (accel_scale > 30.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+			/* this is frickin' fatal */
+			ret = ERROR;
+			goto system_eval;
+		} else {
+			ret = OK;
+		}
+	} else {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		ret = ERROR;
+		goto system_eval;
+	}
+
+	if (!status->is_rotary_wing) {
+		int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+		if (fd < 0) {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
+			ret = fd;
+			goto system_eval;
+		}
+
+		struct differential_pressure_s diff_pres;
+
+		ret = read(fd, &diff_pres, sizeof(diff_pres));
+
+		if (ret == sizeof(diff_pres)) {
+			if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
+				mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
+				// XXX do not make this fatal yet
+				ret = OK;
+			}
+		} else {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
+			/* this is frickin' fatal */
+			ret = ERROR;
+			goto system_eval;
+		}
+	}
+
+system_eval:
+	close(fd);
+	return ret;
+}