Harmonize preflight and prearm checks, run same code except for dynamic range check only on arming

4 files changed, 112 insertions, 76 deletions

diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 70015cddd..6bb7e5c24 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -58,6 +58,9 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_airspeed.h>
+
+#include <uORB/topics/airspeed.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -109,7 +112,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
 {
 	bool success = true;
 
@@ -148,6 +151,29 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		goto out;
 	}
 
+	if (dynamic) {
+		/* check measurement result range */
+		struct accel_report acc;
+		ret = read(fd, &acc, sizeof(acc));
+
+		if (ret == sizeof(acc)) {
+			/* evaluate values */
+			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_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
+				/* this is frickin' fatal */
+				success = false;
+				goto out;
+			}
+		} else {
+			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			/* this is frickin' fatal */
+			success = false;
+			goto out;
+		}
+	}
+
 out:
 	close(fd);
 	return success;
@@ -218,11 +244,37 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 	return success;
 }
 
-bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC)
+static bool airspeedCheck(int mavlink_fd, bool optional)
+{
+	bool success = true;
+	int ret;
+	int fd = orb_subscribe(ORB_ID(airspeed));
+
+	struct airspeed_s airspeed;
+
+	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
+		mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		success = false;
+		goto out;
+	}
+
+	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+		mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		// XXX do not make this fatal yet
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
 {
 	bool failed = false;
 
-//Magnetometer
+	/* ---- MAG ---- */
 	if (checkMag) {
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_mag_count; i++) {
@@ -234,19 +286,19 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
-//Accelerometer
+	/* ---- ACCEL ---- */
 	if (checkAcc) {
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			bool required = (i < max_mandatory_accel_count);
 
-			if (!accelerometerCheck(mavlink_fd, i, !required) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
 				failed = true;
 			}
 		}
 	}
 
-// ---- GYRO ----
+	/* ---- GYRO ---- */
 	if (checkGyro) {
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
@@ -258,7 +310,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
-// ---- BARO ----
+	/* ---- BARO ---- */
 	if (checkBaro) {
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_baro_count; i++) {
@@ -270,14 +322,22 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
-// ---- RC CALIBRATION ----
+	/* ---- AIRSPEED ---- */
+	if (checkAirspeed) {
+		if (!airspeedCheck(mavlink_fd, true)) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd) != OK) {
 			failed = true;
 		}
 	}
 
-// Report status
+	/* Report status */
 	return !failed;
 }
+
 }
diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h
index 935f69969..66947a347 100644
--- a/src/modules/commander/PreflightCheck.h
+++ b/src/modules/commander/PreflightCheck.h
@@ -62,7 +62,8 @@ namespace Commander
 * @param checkRC
 *   true if the Remote Controller should be checked
 **/
-bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC);
+bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
+	bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c512c15c..bb1ed7f5d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1125,8 +1125,15 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = true;
 	thread_running = true;
 
+	bool checkAirspeed = false;
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+		checkAirspeed = true;
+	}
+
 	//Run preflight check
-	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
+	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
 	if(!status.condition_system_sensors_initialized) {
 		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
 	}
@@ -1302,8 +1309,15 @@ int commander_thread_main(int argc, char *argv[])
 				    telemetry.heartbeat_time > 0 &&
 				    hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
 
+				    	bool chAirspeed = false;
+					/* Perform airspeed check only if circuit breaker is not
+					 * engaged and it's not a rotary wing */
+					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+						chAirspeed = true;
+					}
+
 					/* provide RC and sensor status feedback to the user */
-					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
+					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
 				}
 
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -2711,7 +2725,15 @@ void *commander_low_prio_loop(void *arg)
 					// we do not set the calibration return value based on it because the calibration
 					// might have worked just fine, but the preflight check fails for a different reason,
 					// so this would be prone to false negatives.
-					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
+
+					bool checkAirspeed = false;
+					/* Perform airspeed check only if circuit breaker is not
+					 * engaged and it's not a rotary wing */
+					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+						checkAirspeed = true;
+					}
+
+					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
 
 					arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
@@ -2719,8 +2741,16 @@ void *commander_low_prio_loop(void *arg)
 				}
 
 			case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+
+				bool checkAirspeed = false;
+				/* Perform airspeed check only if circuit breaker is not
+				 * engaged and it's not a rotary wing */
+				if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+					checkAirspeed = true;
+				}
+
 				// Update preflight check status
-				status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
+				status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
 
 				arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3be329500..ccfc7a986 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -52,18 +52,17 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/airspeed.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"
+#include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -650,69 +649,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 {
-	int ret;
-	bool failed = false;
-
-	int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
-		failed = true;
-		goto system_eval;
-	}
-
-	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
-		failed = true;
-		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_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, hold still");
-			/* this is frickin' fatal */
-			failed = true;
-			goto system_eval;
-		}
-	} else {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
-		/* this is frickin' fatal */
-		failed = true;
-		goto system_eval;
-	}
-
+	/* at this point this equals the preflight check, but might add additional
+	 * quantities later.
+	 */
+	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
 	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
-		/* accel done, close it */
-		close(fd);
-		fd = orb_subscribe(ORB_ID(airspeed));
-
-		struct airspeed_s airspeed;
-
-		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
-			(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
-			failed = true;
-			goto system_eval;
-		}
-
-		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
-			// XXX do not make this fatal yet
-		}
+		checkAirspeed = true;
 	}
 
-system_eval:
-	close(fd);
-	return (failed);
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
 }