Commander: start pulling arming related parts into separate folder

* PreFlightCheck: remove unused reportFailures flag
* Commander: pull all pre flight checks together on the PreFlightCheck class
* PreFlightCheck: separate checks into their own files

src/drivers/drv_rc_input.h

@@ -63,21 +63,6 @@
  */
 #define RC_INPUT_RSSI_MAX	100
 
-/**
- * Minimum value
- */
-#define RC_INPUT_LOWEST_MIN_US	500
-
-/**
- * Maximum value
- */
-#define RC_INPUT_HIGHEST_MAX_US	2500
-
-/**
- * Maximum deadzone value
- */
-#define RC_INPUT_MAX_DEADZONE_US	500
-
 /**
  * Input signal type, value is a control position from zero to 100
  * percent.

src/lib/systemlib/mavlink_log.h

@@ -41,6 +41,7 @@
 
 #pragma once
 
+#include <px4_log.h>
 #include <uORB/uORB.h>
 
 /**

src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp

@@ -30,7 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-#include "arm_auth.h"
+#include "ArmAuthorization.h"
 
 #include <string.h>
 #include <unistd.h>

src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2019 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(ArmAuthorization
+	ArmAuthorization.cpp
+)
+target_include_directories(ArmAuthorization
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)

src/modules/commander/Arming/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2019 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_subdirectory(PreFlightCheck)
+add_subdirectory(ArmAuthorization)
+add_subdirectory(HealthFlags)

src/modules/commander/Arming/HealthFlags/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2019 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(HealthFlags
+	HealthFlags.cpp
+)
+target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/commander/Arming/HealthFlags/HealthFlags.cpp

@@ -32,14 +32,14 @@
  ****************************************************************************/
 
 /**
- * @file health_flag_helper.cpp
+ * @file HealthFlags.cpp
  *
  * Contains helper functions to efficiently set the system health flags from commander and preflight check.
  *
  * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
  */
 
-#include "health_flag_helper.h"
+#include "HealthFlags.h"
 
 void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
 {

src/modules/commander/Arming/HealthFlags/HealthFlags.h

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file health_flag_helper.h
+ * @file HealthFlags.h
  *
  * Contains helper functions to efficiently set the system health flags from commander and preflight check.
  *

src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2019 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(PreFlightCheck
+	PreFlightCheck.cpp
+	checks/preArmCheck.cpp
+	checks/magnetometerCheck.cpp
+	checks/magConsistencyCheck.cpp
+	checks/accelerometerCheck.cpp
+	checks/gyroCheck.cpp
+	checks/baroCheck.cpp
+	checks/imuConsistencyCheck.cpp
+	checks/airspeedCheck.cpp
+	checks/rcCalibrationCheck.cpp
+	checks/powerCheck.cpp
+	checks/ekf2Check.cpp
+	checks/failureDetectorCheck.cpp
+)
+target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags)

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -0,0 +1,373 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightCheck.cpp
+ */
+
+#include "PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+static constexpr unsigned max_mandatory_gyro_count = 1;
+static constexpr unsigned max_optional_gyro_count = 3;
+static constexpr unsigned max_mandatory_accel_count = 1;
+static constexpr unsigned max_optional_accel_count = 3;
+static constexpr unsigned max_mandatory_mag_count = 1;
+static constexpr unsigned max_optional_mag_count = 4;
+static constexpr unsigned max_mandatory_baro_count = 1;
+static constexpr unsigned max_optional_baro_count = 1;
+
+bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+				    vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
+				    const hrt_abstime &time_since_boot)
+{
+	if (time_since_boot < 2_s) {
+		// the airspeed driver filter doesn't deliver the actual value yet
+		reportFailures = false;
+	}
+
+	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+	bool checkSensors = !hil_enabled;
+	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
+	const bool checkDynamic = !hil_enabled;
+	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
+	const bool checkFailureDetector = true;
+
+	bool checkAirspeed = false;
+
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status_flags.circuit_breaker_engaged_airspd_check &&
+	    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
+		checkAirspeed = true;
+	}
+
+	reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
+			  && !status_flags.condition_calibration_enabled);
+
+	bool failed = false;
+
+	/* ---- MAG ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
+		int32_t sys_has_mag = 1;
+		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
+
+		bool mag_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_mag_count; i++) {
+			const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
+			const bool report_fail = (reportFailures && !failed && !mag_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					mag_fail_reported = true;
+				}
+			}
+		}
+
+		if (sys_has_mag == 1) {
+			/* check if the primary device is present */
+			if (!prime_found) {
+				if (reportFailures && !failed) {
+					mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
+				}
+
+				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
+				failed = true;
+			}
+
+			/* mag consistency checks (need to be performed after the individual checks) */
+			if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- ACCEL ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
+		bool accel_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_accel_count; i++) {
+			const bool required = (i < max_mandatory_accel_count);
+			const bool report_fail = (reportFailures && !failed && !accel_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					accel_fail_reported = true;
+				}
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			if (reportFailures && !failed) {
+				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
+			}
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
+			failed = true;
+		}
+	}
+
+	/* ---- GYRO ---- */
+	if (checkSensors) {
+		bool prime_found = false;
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
+		bool gyro_fail_reported = false;
+
+		/* check all sensors individually, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
+			const bool required = (i < max_mandatory_gyro_count);
+			const bool report_fail = (reportFailures && !failed && !gyro_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					gyro_fail_reported = true;
+				}
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			if (reportFailures && !failed) {
+				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
+			}
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
+			failed = true;
+		}
+	}
+
+	/* ---- BARO ---- */
+	if (checkSensors) {
+		//bool prime_found = false;
+
+		int32_t prime_id = -1;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
+		int32_t sys_has_baro = 1;
+		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
+
+		bool baro_fail_reported = false;
+
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_baro_count; i++) {
+			const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
+			const bool report_fail = (reportFailures && !failed && !baro_fail_reported);
+
+			int32_t device_id = -1;
+
+			if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if ((prime_id > 0) && (device_id == prime_id)) {
+					//prime_found = true;
+				}
+
+			} else {
+				if (required) {
+					failed = true;
+					baro_fail_reported = true;
+				}
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present
+		// if (false) {
+		// 	if (reportFailures && !failed) {
+		// 		mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
+		// 	}
+
+		// 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
+		// 	failed = true;
+		// }
+	}
+
+	/* ---- IMU CONSISTENCY ---- */
+	// To be performed after the individual sensor checks have completed
+	if (checkSensors) {
+		if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
+			failed = true;
+		}
+	}
+
+	/* ---- AIRSPEED ---- */
+	if (checkAirspeed) {
+		int32_t optional = 0;
+		param_get(param_find("FW_ARSP_MODE"), &optional);
+
+		if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
+	if (checkRC) {
+		if (rcCalibrationCheck(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) {
+			if (reportFailures) {
+				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
+			}
+
+			failed = true;
+
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
+			status_flags.rc_calibration_valid = false;
+
+		} else {
+			// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
+			status_flags.rc_calibration_valid = true;
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
+					 !status.rc_signal_lost, status);
+		}
+	}
+
+	/* ---- SYSTEM POWER ---- */
+	if (checkPower) {
+		if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
+			failed = true;
+		}
+	}
+
+	/* ---- Navigation EKF ---- */
+	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
+	int32_t estimator_type = -1;
+
+	if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
+		param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
+
+	} else {
+		// EKF2 is currently the only supported option for FW & VTOL
+		estimator_type = 2;
+	}
+
+	if (estimator_type == 2) {
+		// don't report ekf failures for the first 10 seconds to allow time for the filter to start
+		bool report_ekf_fail = (time_since_boot > 10_s);
+
+		if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
+			failed = true;
+		}
+	}
+
+	/* ---- Failure Detector ---- */
+	if (checkFailureDetector) {
+		if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
+			failed = true;
+		}
+	}
+
+	/* Report status */
+	return !failed;
+}
+
+bool PreFlightCheck::check_calibration(const char *param_template, const int32_t device_id)
+{
+	bool calibration_found = false;
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		const param_t parm = param_find_no_notification(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int32_t calibration_devid = -1;
+
+		if (param_get(parm, &calibration_devid) == PX4_OK) {
+
+			/* if the devid matches, exit early */
+			if (device_id == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+
+		instance++;
+	}
+
+	return calibration_found;
+}

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightCheck.hpp
+ *
+ * Check if flight is safely possible
+ * if not prevent it and inform the user.
+ */
+
+#pragma once
+
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+#include <uORB/topics/vehicle_status.h>
+#include <drivers/drv_hrt.h>
+
+class PreFlightCheck
+{
+public:
+	PreFlightCheck() = default;
+	~PreFlightCheck() = default;
+
+	/**
+	* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
+	*
+	* The function won't fail the test if optional sensors are not found, however,
+	* it will fail the test if optional sensors are found but not in working condition.
+	*
+	* @param mavlink_log_pub
+	*   Mavlink output orb handle reference for feedback when a sensor fails
+	* @param checkMag
+	*   true if the magneteometer should be checked
+	* @param checkAcc
+	*   true if the accelerometers should be checked
+	* @param checkGyro
+	*   true if the gyroscopes should be checked
+	* @param checkBaro
+	*   true if the barometer should be checked
+	* @param checkAirspeed
+	*   true if the airspeed sensor should be checked
+	* @param checkRC
+	*   true if the Remote Controller should be checked
+	* @param checkGNSS
+	*   true if the GNSS receiver should be checked
+	* @param checkPower
+	*   true if the system power should be checked
+	**/
+	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+				   vehicle_status_flags_s &status_flags,
+				   const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot);
+
+	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				const safety_s &safety, const uint8_t arm_requirements);
+
+	typedef enum {
+		ARM_REQ_NONE = 0,
+		ARM_REQ_MISSION_BIT = (1 << 0),
+		ARM_REQ_ARM_AUTH_BIT = (1 << 1),
+		ARM_REQ_GPS_BIT = (1 << 2),
+		ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
+	} arm_requirements_t;
+
+private:
+	static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				    const bool optional, int32_t &device_id, const bool report_fail);
+	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
+	static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				       const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail);
+	static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
+	static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
+				  const bool report_fail, const bool prearm);
+	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
+	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+			       const bool prearm);
+	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			      const bool report_fail, const bool enforce_gps_required);
+	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+					 const bool prearm);
+	static bool check_calibration(const char *param_template, const int32_t device_id);
+};

src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <math.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+					const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool accel_valid = true;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
+
+		accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
+
+		if (!accel_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance);
+			}
+		}
+
+		device_id = accel.get().device_id;
+
+		calibration_valid = check_calibration("CAL_ACC%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance);
+			}
+
+		} else {
+
+			if (dynamic) {
+				const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+								    + accel.get().y * accel.get().y
+								    + accel.get().z * accel.get().z);
+
+				if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+					if (report_fail) {
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
+					}
+
+					/* this is frickin' fatal */
+					accel_valid = false;
+				}
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance);
+		}
+	}
+
+	const bool success = calibration_valid && accel_valid;
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status);
+	}
+
+	return success;
+}

src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
+				   const bool report_fail, const bool prearm)
+{
+	bool present = true;
+	bool success = true;
+
+	uORB::SubscriptionData<airspeed_s> airspeed_sub{ORB_ID(airspeed)};
+	airspeed_sub.update();
+	const airspeed_s &airspeed = airspeed_sub.get();
+
+	if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) {
+		if (report_fail && !optional) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing");
+		}
+
+		present = false;
+		success = false;
+		goto out;
+	}
+
+	/*
+	 * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise
+	 * on the bench and trigger false positives of the voter. Therefore only fail this
+	 * for a pre-arm check, as then the cover is off and the natural airflow in the field
+	 * will ensure there is not zero noise.
+	 */
+	if (prearm && fabsf(airspeed.confidence) < 0.95f) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+	/**
+	 * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying
+	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
+	 * might have been removed.
+	 */
+	if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot");
+		}
+
+		present = true;
+		success = false;
+		goto out;
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
+
+	return success;
+}

src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			       const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
+	bool baro_valid = false;
+
+	if (exists) {
+		uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
+
+		baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
+
+		if (!baro_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance);
+			}
+		}
+
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance);
+		}
+	}
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status);
+	}
+
+	return baro_valid;
+}

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <math.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/subsystem_info.h>
+
+bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
+			       const bool report_fail, const bool enforce_gps_required)
+{
+	bool success = true; // start with a pass and change to a fail if any test fails
+	bool present = true;
+	float test_limit = 1.0f; // pass limit re-used for each test
+
+	bool gps_success = true;
+	bool gps_present = true;
+
+	// Get estimator status data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
+	status_sub.update();
+	const estimator_status_s &status = status_sub.get();
+
+	if (status.timestamp == 0) {
+		present = false;
+		goto out;
+	}
+
+	// Check if preflight check performed by estimator has failed
+	if (status.pre_flt_fail_innov_heading ||
+	    status.pre_flt_fail_innov_vel_horiz ||
+	    status.pre_flt_fail_innov_vel_vert ||
+	    status.pre_flt_fail_innov_height) {
+		if (report_fail) {
+			if (status.pre_flt_fail_innov_heading) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_height) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
+			}
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check vertical position innovation test ratio
+	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
+
+	if (status.hgt_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check velocity innovation test ratio
+	param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
+
+	if (status.vel_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check horizontal position innovation test ratio
+	param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
+
+	if (status.pos_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check magnetometer innovation test ratio
+	param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
+
+	if (status.mag_test_ratio > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// check accelerometer delta velocity bias estimates
+	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
+
+	for (uint8_t index = 13; index < 16; index++) {
+		// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+		// adjust test threshold by 3-sigma
+		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
+
+		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+			}
+
+			success = false;
+			goto out;
+		}
+	}
+
+	// check gyro delta angle bias estimates
+	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
+
+	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
+	    || fabsf(status.states[12]) > test_limit) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+		}
+
+		success = false;
+		goto out;
+	}
+
+	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+	if (enforce_gps_required || report_fail) {
+		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+		const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
+
+		gps_success = ekf_gps_fusion; // default to success if gps data is fused
+
+		if (ekf_gps_check_fail) {
+			if (report_fail) {
+				// Only report the first failure to avoid spamming
+				const char *message = nullptr;
+
+				if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+					message = "Preflight%s: GPS fix too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+					message = "Preflight%s: not enough GPS Satellites";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
+					message = "Preflight%s: GPS GDoP too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+					message = "Preflight%s: GPS Horizontal Pos Error too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+					message = "Preflight%s: GPS Vertical Pos Error too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+					message = "Preflight%s: GPS Speed Accuracy too low";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+					message = "Preflight%s: GPS Horizontal Pos Drift too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+					message = "Preflight%s: GPS Vertical Pos Drift too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
+					message = "Preflight%s: GPS Hor Speed Drift too high";
+
+				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
+					message = "Preflight%s: GPS Vert Speed Drift too high";
+
+				} else {
+					if (!ekf_gps_fusion) {
+						// Likely cause unknown
+						message = "Preflight%s: Estimator not using GPS";
+						gps_present = false;
+
+					} else {
+						// if we land here there was a new flag added and the code not updated. Show a generic message.
+						message = "Preflight%s: Poor GPS Quality";
+					}
+				}
+
+				if (message) {
+					if (enforce_gps_required) {
+						mavlink_log_critical(mavlink_log_pub, message, " Fail");
+
+					} else {
+						mavlink_log_warning(mavlink_log_pub, message, "");
+					}
+				}
+			}
+
+			gps_success = false;
+
+			if (enforce_gps_required) {
+				success = false;
+				goto out;
+			}
+		}
+	}
+
+out:
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status);
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status);
+
+	return success;
+}

src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -31,19 +31,35 @@
  *
  ****************************************************************************/
 
-/**
+#include "../PreFlightCheck.hpp"
- * @file rc_check.h
- *
- * RC calibration check
- */
-#include <uORB/uORB.h>
 
-#pragma once
+#include <systemlib/mavlink_log.h>
 
-/**
+bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
- * Check the RC calibration
+		const bool report_fail, const bool prearm)
- *
+{
- * @return			0 / OK if RC calibration is ok, index + 1 of the first
+	// Ignore failure detector check after arming
- *				channel that failed else (so 1 == first channel failed)
+	if (!prearm) {
- */
+		return true;
-int	rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
+	}
+
+	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+		if (report_fail) {
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
+			}
+		}
+
+		return false;
+	}
+
+	return true;
+}

src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+			       const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool gyro_valid = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
+
+		gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
+
+		if (!gyro_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance);
+			}
+		}
+
+		device_id = gyro.get().device_id;
+
+		calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance);
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor #%u missing", instance);
+		}
+	}
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status);
+	}
+
+	return calibration_valid && gyro_valid;
+}

src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/subsystem_info.h>
+
+bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+		const bool report_status)
+{
+	float test_limit = 1.0f; // pass limit re-used for each test
+
+	// Get sensor_preflight data if available and exit with a fail recorded if not
+	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	sensors_sub.update();
+	const sensor_preflight_s &sensors = sensors_sub.get();
+
+	// Use the difference between IMU's to detect a bad calibration.
+	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
+	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
+
+	if (sensors.accel_inconsistency_m_s_s > test_limit) {
+		if (report_status) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal");
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
+		}
+
+		return false;
+
+	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
+		if (report_status) {
+			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal");
+		}
+	}
+
+	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
+	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
+
+	if (sensors.gyro_inconsistency_rad_s > test_limit) {
+		if (report_status) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal");
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
+			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
+		}
+
+		return false;
+
+	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
+		if (report_status) {
+			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal");
+		}
+	}
+
+	return true;
+}

src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <HealthFlags.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/sensor_preflight.h>
+
+// return false if the magnetomer measurements are inconsistent
+bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
+		const bool report_status)
+{
+	bool pass = false; // flag for result of checks
+
+	// get the sensor preflight data
+	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	sensors_sub.update();
+	const sensor_preflight_s &sensors = sensors_sub.get();
+
+	if (sensors.timestamp == 0) {
+		// can happen if not advertised (yet)
+		pass = true;
+	}
+
+	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
+	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
+	int32_t angle_difference_limit_deg;
+	param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg);
+
+	pass = pass || angle_difference_limit_deg < 0; // disabled, pass check
+	pass = pass || sensors.mag_inconsistency_angle < math::radians<float>(angle_difference_limit_deg);
+
+	if (!pass && report_status) {
+		mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent",
+				     static_cast<int>(math::degrees<float>(sensors.mag_inconsistency_angle)));
+		mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate");
+		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
+		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
+	}
+
+	return pass;
+}

src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <HealthFlags.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/subsystem_info.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				     const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
+	bool calibration_valid = false;
+	bool mag_valid = false;
+
+	if (exists) {
+
+		uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
+
+		mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s);
+
+		if (!mag_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance);
+			}
+		}
+
+		device_id = magnetometer.get().device_id;
+
+		calibration_valid = check_calibration("CAL_MAG%u_ID", device_id);
+
+		if (!calibration_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance);
+		}
+	}
+
+	const bool success = calibration_valid && mag_valid;
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
+	}
+
+	return success;
+}

src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/system_power.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
+				const bool prearm)
+{
+	bool success = true;
+
+	if (!prearm) {
+		// Ignore power check after arming.
+		return true;
+
+	} else {
+		uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
+		system_power_sub.update();
+		const system_power_s &system_power = system_power_sub.get();
+
+		if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
+
+			/* copy avionics voltage */
+			float avionics_power_rail_voltage = system_power.voltage5v_v;
+
+			// avionics rail
+			// Check avionics rail voltages
+			if (avionics_power_rail_voltage < 4.5f) {
+				success = false;
+
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
+							     (double)avionics_power_rail_voltage);
+				}
+
+			} else if (avionics_power_rail_voltage < 4.9f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
+
+			} else if (avionics_power_rail_voltage > 5.4f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
+			}
+		}
+	}
+
+	return success;
+}

src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <ArmAuthorization.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
+				 const safety_s &safety, const uint8_t arm_requirements)
+{
+	bool prearm_ok = true;
+
+	// USB not connected
+	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
+		mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe");
+
+		prearm_ok = false;
+	}
+
+	// battery and system power status
+	if (!status_flags.circuit_breaker_engaged_power_check) {
+
+		// Fail transition if power is not good
+		if (!status_flags.condition_power_input_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module");
+
+			prearm_ok = false;
+		}
+
+		// main battery level
+		if (!status_flags.condition_battery_healthy) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: mission
+	if (arm_requirements & ARM_REQ_MISSION_BIT) {
+
+		if (!status_flags.condition_auto_mission_available) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission");
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: global position
+	if (arm_requirements & ARM_REQ_GPS_BIT) {
+
+		if (!status_flags.condition_global_position_valid) {
+			if (prearm_ok) {
+				mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required");
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// safety button
+	if (safety.safety_switch_available && !safety.safety_off) {
+		// Fail transition if we need safety switch press
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first");
+		}
+
+		prearm_ok = false;
+	}
+
+	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready");
+		}
+
+		prearm_ok = false;
+
+	}
+
+	if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
+		if (prearm_ok) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: authorization
+	// check last, and only if everything else has passed
+	if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) {
+		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+			// feedback provided in arm_auth_check
+			prearm_ok = false;
+		}
+	}
+
+
+	return prearm_ok;
+}

src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -31,30 +31,28 @@
  *
  ****************************************************************************/
 
-/**
+#include "../PreFlightCheck.hpp"
- * @file rc_check.c
- *
- * RC calibration check
- */
-
-#include "rc_check.h"
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/time.h>
-
-#include <stdio.h>
-#include <unistd.h>
-#include <fcntl.h>
-
-#include <systemlib/err.h>
 
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
-#include <drivers/drv_rc_input.h>
+#include <uORB/topics/input_rc.h>
 
-#define RC_INPUT_MAP_UNMAPPED 0
+/**
+ * Maximum deadzone value
+ */
+#define RC_INPUT_MAX_DEADZONE_US	500
 
-int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
+/**
+ * Minimum value
+ */
+#define RC_INPUT_LOWEST_MIN_US	500
+
+/**
+ * Maximum value
+ */
+#define RC_INPUT_HIGHEST_MAX_US	2500
+
+int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
 {
 	char nbuf[20];
 	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
@@ -92,7 +90,6 @@ int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool i
 		}
 	}
 
-
 	/* first check channel mappings */
 	while (rc_map_mandatory[j] != nullptr) {
 

src/modules/commander/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(failure_detector)
+add_subdirectory(Arming)
 
 px4_add_module(
 	MODULE modules__commander
@@ -40,18 +41,14 @@ px4_add_module(
 	SRCS
 		accelerometer_calibration.cpp
 		airspeed_calibration.cpp
-		arm_auth.cpp
 		baro_calibration.cpp
 		calibration_routines.cpp
 		Commander.cpp
 		commander_helper.cpp
 		esc_calibration.cpp
 		gyro_calibration.cpp
-		health_flag_helper.cpp
 		mag_calibration.cpp
-		PreflightCheck.cpp
 		rc_calibration.cpp
-		rc_check.cpp
 		state_machine_helper.cpp
 	DEPENDS
 		circuit_breaker
@@ -60,6 +57,9 @@ px4_add_module(
 		git_ecl
 		ecl_geo
 		hysteresis
+		PreFlightCheck
+		ArmAuthorization
+		HealthFlags
 	)
 
 if(PX4_TESTING)

src/modules/commander/Commander.cpp

@@ -45,20 +45,20 @@
 #include "Commander.hpp"
 
 /* commander module headers */
+#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
+#include "Arming/ArmAuthorization/ArmAuthorization.h"
+#include "Arming/HealthFlags/HealthFlags.h"
 #include "accelerometer_calibration.h"
 #include "airspeed_calibration.h"
-#include "arm_auth.h"
 #include "baro_calibration.h"
 #include "calibration_routines.h"
 #include "commander_helper.h"
 #include "esc_calibration.h"
 #include "gyro_calibration.h"
 #include "mag_calibration.h"
-#include "PreflightCheck.h"
 #include "px4_custom_mode.h"
 #include "rc_calibration.h"
 #include "state_machine_helper.h"
-#include "health_flag_helper.h"
 
 /* PX4 headers */
 #include <dataman/dataman.h>
@@ -156,7 +156,7 @@ static struct vehicle_status_flags_s status_flags = {};
 
 static uint64_t rc_signal_lost_timestamp;		// Time at which the RC reception was lost
 
-static uint8_t arm_requirements = ARM_REQ_NONE;
+static uint8_t arm_requirements = PreFlightCheck::ARM_REQ_NONE;
 
 static bool _last_condition_local_altitude_valid = false;
 static bool _last_condition_local_position_valid = false;
@@ -374,7 +374,7 @@ int commander_main(int argc, char *argv[])
 		bool preflight_check_res = Commander::preflight_check(true);
 		PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
 
-		bool prearm_check_res = prearm_check(&mavlink_log_pub, status_flags, safety, arm_requirements);
+		bool prearm_check_res = PreFlightCheck::preArmCheck(&mavlink_log_pub, status_flags, safety, arm_requirements);
 		PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
 
 		return 0;
@@ -1392,15 +1392,17 @@ Commander::run()
 
 	int32_t arm_without_gps_param = 0;
 	param_get(_param_arm_without_gps, &arm_without_gps_param);
-	arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
+	arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT;
 
 	int32_t arm_mission_required_param = 0;
 	param_get(_param_arm_mission_required, &arm_mission_required_param);
-	arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
+	arm_requirements |= (arm_mission_required_param &
+			     (PreFlightCheck::ARM_REQ_MISSION_BIT | PreFlightCheck::ARM_REQ_ARM_AUTH_BIT));
 
 	int32_t arm_escs_checks_required_param = 0;
 	param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
-	arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
+	arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
+			    PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT;
 
 	status.rc_input_mode = rc_in_off;
 
@@ -1528,11 +1530,13 @@ Commander::run()
 			param_get(_param_arm_switch_is_button, &arm_switch_is_button);
 
 			param_get(_param_arm_without_gps, &arm_without_gps_param);
-			arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
+			arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT;
 			param_get(_param_arm_mission_required, &arm_mission_required_param);
-			arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
+			arm_requirements |= (arm_mission_required_param &
+					     (PreFlightCheck::ARM_REQ_MISSION_BIT | PreFlightCheck::ARM_REQ_ARM_AUTH_BIT));
 			param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
-			arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
+			arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
+					    PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT;
 
 
 			/* flight mode slots */
@@ -3755,9 +3759,9 @@ void Commander::mission_init()
 
 bool Commander::preflight_check(bool report)
 {
-	const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
+	const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT);
 
-	const bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false,
+	const bool success = PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false,
 			     hrt_elapsed_time(&commander_boot_timestamp));
 
 	if (success) {

src/modules/commander/PreflightCheck.cpp

@@ -1,990 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2012-2017 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
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
-* @file PreflightCheck.cpp
-*
-* Preflight check for main system components
-*
-* @author Lorenz Meier <lorenz@px4.io>
-* @author Johan Jansen <jnsn.johan@gmail.com>
-*/
-
-#include "PreflightCheck.h"
-#include "health_flag_helper.h"
-#include "rc_check.h"
-
-#include <math.h>
-#include <mathlib/mathlib.h>
-
-#include <parameters/param.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_mag.h>
-#include <uORB/topics/sensor_preflight.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/system_power.h>
-
-using namespace time_literals;
-
-namespace Preflight
-{
-
-static bool check_calibration(const char *param_template, const int32_t device_id)
-{
-	bool calibration_found = false;
-
-	char s[20];
-	int instance = 0;
-
-	/* old style transition: check param values */
-	while (!calibration_found) {
-		sprintf(s, param_template, instance);
-		const param_t parm = param_find_no_notification(s);
-
-		/* if the calibration param is not present, abort */
-		if (parm == PARAM_INVALID) {
-			break;
-		}
-
-		/* if param get succeeds */
-		int32_t calibration_devid = -1;
-
-		if (param_get(parm, &calibration_devid) == PX4_OK) {
-
-			/* if the devid matches, exit early */
-			if (device_id == calibration_devid) {
-				calibration_found = true;
-				break;
-			}
-		}
-
-		instance++;
-	}
-
-	return calibration_found;
-}
-
-static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			    const bool optional, int32_t &device_id, const bool report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
-	bool calibration_valid = false;
-	bool mag_valid = false;
-
-	if (exists) {
-
-		uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
-
-		mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s);
-
-		if (!mag_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance);
-			}
-		}
-
-		device_id = magnetometer.get().device_id;
-
-		calibration_valid = check_calibration("CAL_MAG%u_ID", device_id);
-
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
-			}
-		}
-
-	} else {
-		if (!optional && report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance);
-		}
-	}
-
-	const bool success = calibration_valid && mag_valid;
-
-	if (instance == 0) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
-
-	} else if (instance == 1) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
-	}
-
-	return success;
-}
-
-static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status)
-{
-	float test_limit = 1.0f; // pass limit re-used for each test
-
-	// Get sensor_preflight data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
-	sensors_sub.update();
-	const sensor_preflight_s &sensors = sensors_sub.get();
-
-	// Use the difference between IMU's to detect a bad calibration.
-	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
-	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
-
-	if (sensors.accel_inconsistency_m_s_s > test_limit) {
-		if (report_status) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal");
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
-		}
-
-		return false;
-
-	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
-		if (report_status) {
-			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal");
-		}
-	}
-
-	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
-	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
-
-	if (sensors.gyro_inconsistency_rad_s > test_limit) {
-		if (report_status) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal");
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
-		}
-
-		return false;
-
-	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
-		if (report_status) {
-			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal");
-		}
-	}
-
-	return true;
-}
-
-// return false if the magnetomer measurements are inconsistent
-static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status)
-{
-	bool pass = false; // flag for result of checks
-
-	// get the sensor preflight data
-	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
-	sensors_sub.update();
-	const sensor_preflight_s &sensors = sensors_sub.get();
-
-	if (sensors.timestamp == 0) {
-		// can happen if not advertised (yet)
-		pass = true;
-	}
-
-	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
-	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
-	int32_t angle_difference_limit_deg;
-	param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg);
-
-	pass = pass || angle_difference_limit_deg < 0; // disabled, pass check
-	pass = pass || sensors.mag_inconsistency_angle < math::radians<float>(angle_difference_limit_deg);
-
-	if (!pass && report_status) {
-		mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent",
-				     static_cast<int>(math::degrees<float>(sensors.mag_inconsistency_angle)));
-		mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate");
-		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
-		set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
-	}
-
-	return pass;
-}
-
-static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			       const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
-	bool calibration_valid = false;
-	bool accel_valid = true;
-
-	if (exists) {
-
-		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
-
-		accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
-
-		if (!accel_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance);
-			}
-		}
-
-		device_id = accel.get().device_id;
-
-		calibration_valid = check_calibration("CAL_ACC%u_ID", device_id);
-
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance);
-			}
-
-		} else {
-
-			if (dynamic) {
-				const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
-								    + accel.get().y * accel.get().y
-								    + accel.get().z * accel.get().z);
-
-				if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-					if (report_fail) {
-						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
-					}
-
-					/* this is frickin' fatal */
-					accel_valid = false;
-				}
-			}
-		}
-
-	} else {
-		if (!optional && report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance);
-		}
-	}
-
-	const bool success = calibration_valid && accel_valid;
-
-	if (instance == 0) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status);
-
-	} else if (instance == 1) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status);
-	}
-
-	return success;
-}
-
-static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-		      const bool optional, int32_t &device_id, const bool report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
-	bool calibration_valid = false;
-	bool gyro_valid = false;
-
-	if (exists) {
-
-		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
-
-		gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
-
-		if (!gyro_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance);
-			}
-		}
-
-		device_id = gyro.get().device_id;
-
-		calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id);
-
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance);
-			}
-		}
-
-	} else {
-		if (!optional && report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor #%u missing", instance);
-		}
-	}
-
-	if (instance == 0) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status);
-
-	} else if (instance == 1) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status);
-	}
-
-	return calibration_valid && gyro_valid;
-}
-
-static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-		      const bool optional, int32_t &device_id, const bool report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
-	bool baro_valid = false;
-
-	if (exists) {
-		uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
-
-		baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
-
-		if (!baro_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance);
-			}
-		}
-
-
-	} else {
-		if (!optional && report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance);
-		}
-	}
-
-	if (instance == 0) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status);
-	}
-
-	return baro_valid;
-}
-
-static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-			  const bool report_fail, const bool prearm)
-{
-	bool present = true;
-	bool success = true;
-
-	uORB::SubscriptionData<airspeed_s> airspeed_sub{ORB_ID(airspeed)};
-	airspeed_sub.update();
-	const airspeed_s &airspeed = airspeed_sub.get();
-
-	if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) {
-		if (report_fail && !optional) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing");
-		}
-
-		present = false;
-		success = false;
-		goto out;
-	}
-
-	/*
-	 * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise
-	 * on the bench and trigger false positives of the voter. Therefore only fail this
-	 * for a pre-arm check, as then the cover is off and the natural airflow in the field
-	 * will ensure there is not zero noise.
-	 */
-	if (prearm && fabsf(airspeed.confidence) < 0.95f) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck");
-		}
-
-		present = true;
-		success = false;
-		goto out;
-	}
-
-	/**
-	 * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying
-	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
-	 * might have been removed.
-	 */
-	if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot");
-		}
-
-		present = true;
-		success = false;
-		goto out;
-	}
-
-out:
-	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
-
-	return success;
-}
-
-static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
-		       const bool prearm)
-{
-	bool success = true;
-
-	if (!prearm) {
-		// Ignore power check after arming.
-		return true;
-
-	} else {
-		uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
-		system_power_sub.update();
-		const system_power_s &system_power = system_power_sub.get();
-
-		if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
-
-			/* copy avionics voltage */
-			float avionics_power_rail_voltage = system_power.voltage5v_v;
-
-			// avionics rail
-			// Check avionics rail voltages
-			if (avionics_power_rail_voltage < 4.5f) {
-				success = false;
-
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
-							     (double)avionics_power_rail_voltage);
-				}
-
-			} else if (avionics_power_rail_voltage < 4.9f) {
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
-				}
-
-			} else if (avionics_power_rail_voltage > 5.4f) {
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
-				}
-			}
-		}
-	}
-
-	return success;
-}
-
-static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
-		      const bool report_fail, const bool enforce_gps_required)
-{
-	bool success = true; // start with a pass and change to a fail if any test fails
-	bool present = true;
-	float test_limit = 1.0f; // pass limit re-used for each test
-
-	bool gps_success = true;
-	bool gps_present = true;
-
-	// Get estimator status data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
-	status_sub.update();
-	const estimator_status_s &status = status_sub.get();
-
-	if (status.timestamp == 0) {
-		present = false;
-		goto out;
-	}
-
-	// Check if preflight check performed by estimator has failed
-	if (status.pre_flt_fail_innov_heading ||
-	    status.pre_flt_fail_innov_vel_horiz ||
-	    status.pre_flt_fail_innov_vel_vert ||
-	    status.pre_flt_fail_innov_height) {
-		if (report_fail) {
-			if (status.pre_flt_fail_innov_heading) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_vel_horiz) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_vel_horiz) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_height) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
-			}
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check vertical position innovation test ratio
-	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
-
-	if (status.hgt_test_ratio > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check velocity innovation test ratio
-	param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
-
-	if (status.vel_test_ratio > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check horizontal position innovation test ratio
-	param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
-
-	if (status.pos_test_ratio > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check magnetometer innovation test ratio
-	param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
-
-	if (status.mag_test_ratio > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check accelerometer delta velocity bias estimates
-	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
-
-	for (uint8_t index = 13; index < 16; index++) {
-		// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
-		// adjust test threshold by 3-sigma
-		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
-
-		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
-			}
-
-			success = false;
-			goto out;
-		}
-	}
-
-	// check gyro delta angle bias estimates
-	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
-
-	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
-	    || fabsf(status.states[12]) > test_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (enforce_gps_required || report_fail) {
-		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
-		const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
-
-		gps_success = ekf_gps_fusion; // default to success if gps data is fused
-
-		if (ekf_gps_check_fail) {
-			if (report_fail) {
-				// Only report the first failure to avoid spamming
-				const char *message = nullptr;
-
-				if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
-					message = "Preflight%s: GPS fix too low";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
-					message = "Preflight%s: not enough GPS Satellites";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
-					message = "Preflight%s: GPS GDoP too low";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
-					message = "Preflight%s: GPS Horizontal Pos Error too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
-					message = "Preflight%s: GPS Vertical Pos Error too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
-					message = "Preflight%s: GPS Speed Accuracy too low";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
-					message = "Preflight%s: GPS Horizontal Pos Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
-					message = "Preflight%s: GPS Vertical Pos Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
-					message = "Preflight%s: GPS Hor Speed Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
-					message = "Preflight%s: GPS Vert Speed Drift too high";
-
-				} else {
-					if (!ekf_gps_fusion) {
-						// Likely cause unknown
-						message = "Preflight%s: Estimator not using GPS";
-						gps_present = false;
-
-					} else {
-						// if we land here there was a new flag added and the code not updated. Show a generic message.
-						message = "Preflight%s: Poor GPS Quality";
-					}
-				}
-
-				if (message) {
-					if (enforce_gps_required) {
-						mavlink_log_critical(mavlink_log_pub, message, " Fail");
-
-					} else {
-						mavlink_log_warning(mavlink_log_pub, message, "");
-					}
-				}
-			}
-
-			gps_success = false;
-
-			if (enforce_gps_required) {
-				success = false;
-				goto out;
-			}
-		}
-	}
-
-out:
-	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status);
-	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status);
-
-	return success;
-}
-
-static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
-				 const bool prearm)
-{
-	// Ignore failure detector check after arming
-	if (!prearm) {
-		return true;
-	}
-
-	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
-		if (report_fail) {
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
-			}
-		}
-
-		return false;
-	}
-
-	return true;
-}
-
-bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags,
-		    const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot)
-{
-	if (time_since_boot < 2_s) {
-		// the airspeed driver filter doesn't deliver the actual value yet
-		reportFailures = false;
-	}
-
-	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
-
-	bool checkSensors = !hil_enabled;
-	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
-	const bool checkDynamic = !hil_enabled;
-	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
-	const bool checkFailureDetector = true;
-
-	bool checkAirspeed = false;
-
-	/* Perform airspeed check only if circuit breaker is not
-	 * engaged and it's not a rotary wing */
-	if (!status_flags.circuit_breaker_engaged_airspd_check &&
-	    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
-		checkAirspeed = true;
-	}
-
-	reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
-			  && !status_flags.condition_calibration_enabled);
-
-	bool failed = false;
-
-	/* ---- MAG ---- */
-	if (checkSensors) {
-		bool prime_found = false;
-
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
-
-		int32_t sys_has_mag = 1;
-		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
-
-		bool mag_fail_reported = false;
-
-		/* check all sensors individually, but fail only for mandatory ones */
-		for (unsigned i = 0; i < max_optional_mag_count; i++) {
-			const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
-			const bool report_fail = (reportFailures && !failed && !mag_fail_reported);
-
-			int32_t device_id = -1;
-
-			if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					prime_found = true;
-				}
-
-			} else {
-				if (required) {
-					failed = true;
-					mag_fail_reported = true;
-				}
-			}
-		}
-
-		if (sys_has_mag == 1) {
-			/* check if the primary device is present */
-			if (!prime_found) {
-				if (reportFailures && !failed) {
-					mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
-				}
-
-				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
-				failed = true;
-			}
-
-			/* mag consistency checks (need to be performed after the individual checks) */
-			if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
-				failed = true;
-			}
-		}
-	}
-
-	/* ---- ACCEL ---- */
-	if (checkSensors) {
-		bool prime_found = false;
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
-
-		bool accel_fail_reported = false;
-
-		/* check all sensors individually, but fail only for mandatory ones */
-		for (unsigned i = 0; i < max_optional_accel_count; i++) {
-			const bool required = (i < max_mandatory_accel_count);
-			const bool report_fail = (reportFailures && !failed && !accel_fail_reported);
-
-			int32_t device_id = -1;
-
-			if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
-
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					prime_found = true;
-				}
-
-			} else {
-				if (required) {
-					failed = true;
-					accel_fail_reported = true;
-				}
-			}
-		}
-
-		/* check if the primary device is present */
-		if (!prime_found) {
-			if (reportFailures && !failed) {
-				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
-			}
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
-			failed = true;
-		}
-	}
-
-	/* ---- GYRO ---- */
-	if (checkSensors) {
-		bool prime_found = false;
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
-
-		bool gyro_fail_reported = false;
-
-		/* check all sensors individually, but fail only for mandatory ones */
-		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
-			const bool required = (i < max_mandatory_gyro_count);
-			const bool report_fail = (reportFailures && !failed && !gyro_fail_reported);
-
-			int32_t device_id = -1;
-
-			if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					prime_found = true;
-				}
-
-			} else {
-				if (required) {
-					failed = true;
-					gyro_fail_reported = true;
-				}
-			}
-		}
-
-		/* check if the primary device is present */
-		if (!prime_found) {
-			if (reportFailures && !failed) {
-				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
-			}
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
-			failed = true;
-		}
-	}
-
-	/* ---- BARO ---- */
-	if (checkSensors) {
-		//bool prime_found = false;
-
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
-
-		int32_t sys_has_baro = 1;
-		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
-
-		bool baro_fail_reported = false;
-
-		/* check all sensors, but fail only for mandatory ones */
-		for (unsigned i = 0; i < max_optional_baro_count; i++) {
-			const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
-			const bool report_fail = (reportFailures && !failed && !baro_fail_reported);
-
-			int32_t device_id = -1;
-
-			if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					//prime_found = true;
-				}
-
-			} else {
-				if (required) {
-					failed = true;
-					baro_fail_reported = true;
-				}
-			}
-		}
-
-		// TODO there is no logic in place to calibrate the primary baro yet
-		// // check if the primary device is present
-		// if (false) {
-		// 	if (reportFailures && !failed) {
-		// 		mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
-		// 	}
-
-		// 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
-		// 	failed = true;
-		// }
-	}
-
-	/* ---- IMU CONSISTENCY ---- */
-	// To be performed after the individual sensor checks have completed
-	if (checkSensors) {
-		if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
-			failed = true;
-		}
-	}
-
-	/* ---- AIRSPEED ---- */
-	if (checkAirspeed) {
-		int32_t optional = 0;
-		param_get(param_find("FW_ARSP_MODE"), &optional);
-
-		if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
-			failed = true;
-		}
-	}
-
-	/* ---- RC CALIBRATION ---- */
-	if (checkRC) {
-		if (rc_calibration_check(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) {
-			if (reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
-			}
-
-			failed = true;
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
-			status_flags.rc_calibration_valid = false;
-
-		} else {
-			// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
-			status_flags.rc_calibration_valid = true;
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
-					 !status.rc_signal_lost, status);
-		}
-	}
-
-	/* ---- SYSTEM POWER ---- */
-	if (checkPower) {
-		if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
-			failed = true;
-		}
-	}
-
-	/* ---- Navigation EKF ---- */
-	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
-	int32_t estimator_type = -1;
-
-	if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
-		param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
-
-	} else {
-		// EKF2 is currently the only supported option for FW & VTOL
-		estimator_type = 2;
-	}
-
-	if (estimator_type == 2) {
-		// don't report ekf failures for the first 10 seconds to allow time for the filter to start
-		bool report_ekf_fail = (time_since_boot > 10_s);
-
-		if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
-			failed = true;
-		}
-	}
-
-	/* ---- Failure Detector ---- */
-	if (checkFailureDetector) {
-		if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
-			failed = true;
-		}
-	}
-
-	/* Report status */
-	return !failed;
-}
-
-}

src/modules/commander/PreflightCheck.h

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2015 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file PreflightCheck.h
- *
- * Preflight check for main system components
- *
- * @author Johan Jansen <jnsn.johan@gmail.com>
- */
-
-#include <drivers/drv_hrt.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_status_flags.h>
-
-#pragma once
-
-namespace Preflight
-{
-/**
-* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
-*
-* The function won't fail the test if optional sensors are not found, however,
-* it will fail the test if optional sensors are found but not in working condition.
-*
-* @param mavlink_log_pub
-*   Mavlink output orb handle reference for feedback when a sensor fails
-* @param checkMag
-*   true if the magneteometer should be checked
-* @param checkAcc
-*   true if the accelerometers should be checked
-* @param checkGyro
-*   true if the gyroscopes should be checked
-* @param checkBaro
-*   true if the barometer should be checked
-* @param checkAirspeed
-*   true if the airspeed sensor should be checked
-* @param checkRC
-*   true if the Remote Controller should be checked
-* @param checkGNSS
-*   true if the GNSS receiver should be checked
-* @param checkPower
-*   true if the system power should be checked
-**/
-bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
-		    vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
-		    const hrt_abstime &time_since_boot);
-
-static constexpr unsigned max_mandatory_gyro_count = 1;
-static constexpr unsigned max_optional_gyro_count = 3;
-
-static constexpr unsigned max_mandatory_accel_count = 1;
-static constexpr unsigned max_optional_accel_count = 3;
-
-static constexpr unsigned max_mandatory_mag_count = 1;
-static constexpr unsigned max_optional_mag_count = 4;
-
-static constexpr unsigned max_mandatory_baro_count = 1;
-static constexpr unsigned max_optional_baro_count = 1;
-
-}

src/modules/commander/commander_tests/CMakeLists.txt

@@ -37,6 +37,5 @@ px4_add_module(
 		commander_tests.cpp
 		state_machine_helper_test.cpp
 		../state_machine_helper.cpp
-		../PreflightCheck.cpp
 	DEPENDS
 	)

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -42,6 +42,7 @@
 
 #include "../state_machine_helper.h"
 #include <unit_test.h>
+#include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
 
 class StateMachineHelperTest : public UnitTest
 {
@@ -304,7 +305,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
 					     true /* enable pre-arm checks */,
 					     nullptr /* no mavlink_log_pub */,
 					     &status_flags,
-					     (check_gps ? ARM_REQ_GPS_BIT : 0),
+					     (check_gps ? PreFlightCheck::ARM_REQ_GPS_BIT : 0),
 					     2e6 /* 2 seconds after boot, everything should be checked */
 								    );
 

src/modules/commander/state_machine_helper.cpp

@@ -46,11 +46,9 @@
 #include <systemlib/mavlink_log.h>
 #include <drivers/drv_hrt.h>
 
-
+#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "state_machine_helper.h"
 #include "commander_helper.h"
-#include "PreflightCheck.h"
-#include "arm_auth.h"
 
 static constexpr const char reason_no_rc[] = "no RC";
 static constexpr const char reason_no_offboard[] = "no offboard";
@@ -132,13 +130,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 		 */
 		bool preflight_check_ret = true;
 
-		const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
+		const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT);
 
 		/* only perform the pre-arm check if we have to */
 		if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
 		    && !hil_enabled) {
 
-			preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true,
+			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true,
 					      time_since_boot);
 
 			if (preflight_check_ret) {
@@ -157,7 +155,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 
 			if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
 
-				status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags,
+				status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status,
+						*status_flags,
 						checkGNSS, false, false, time_since_boot);
 
 				last_preflight_check = hrt_absolute_time();
@@ -179,7 +178,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 
 					if (fRunPreArmChecks && preflight_check_ret) {
 						// only bother running prearm if preflight was successful
-						prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, safety, arm_requirements);
+						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, *status_flags, safety, arm_requirements);
 					}
 
 					if (!preflight_check_ret || !prearm_check_ret) {
@@ -1009,115 +1008,8 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf
 	}
 }
 
-bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety,
-		  const uint8_t arm_requirements)
-{
-	bool reportFailures = true;
-	bool prearm_ok = true;
 
 
-	// USB not connected
-	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
-		if (reportFailures) {
-			mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe");
-		}
-
-		prearm_ok = false;
-	}
-
-	// battery and system power status
-	if (!status_flags.circuit_breaker_engaged_power_check) {
-
-		// Fail transition if power is not good
-		if (!status_flags.condition_power_input_valid) {
-			if (reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module");
-			}
-
-			prearm_ok = false;
-		}
-
-		// main battery level
-		if (!status_flags.condition_battery_healthy) {
-			if (prearm_ok && reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery");
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// Arm Requirements: mission
-	if (arm_requirements & ARM_REQ_MISSION_BIT) {
-
-		if (!status_flags.condition_auto_mission_available) {
-			if (prearm_ok && reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission");
-			}
-
-			prearm_ok = false;
-		}
-
-		if (!status_flags.condition_global_position_valid) {
-			if (prearm_ok && reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position");
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// Arm Requirements: global position
-	if (arm_requirements & ARM_REQ_GPS_BIT) {
-
-		if (!status_flags.condition_global_position_valid) {
-			if (prearm_ok && reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required");
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// safety button
-	if (safety.safety_switch_available && !safety.safety_off) {
-		// Fail transition if we need safety switch press
-		if (prearm_ok && reportFailures) {
-			mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first");
-		}
-
-		prearm_ok = false;
-	}
-
-	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
-		if (prearm_ok && reportFailures) {
-			mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready");
-		}
-
-		prearm_ok = false;
-
-	}
-
-	if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
-		if (prearm_ok && reportFailures) {
-			mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
-			prearm_ok = false;
-		}
-	}
-
-	// Arm Requirements: authorization
-	// check last, and only if everything else has passed
-	if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) {
-		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
-			// feedback provided in arm_auth_check
-			prearm_ok = false;
-		}
-	}
-
-
-	return prearm_ok;
-}
-
 
 void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
 		      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,

src/modules/commander/state_machine_helper.h

@@ -94,14 +94,6 @@ enum class position_nav_loss_actions_t {
 	LAND_TERMINATE = 1,	// Land/Terminate.  Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
 };
 
-typedef enum {
-	ARM_REQ_NONE = 0,
-	ARM_REQ_MISSION_BIT = (1 << 0),
-	ARM_REQ_ARM_AUTH_BIT = (1 << 1),
-	ARM_REQ_GPS_BIT = (1 << 2),
-	ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
-} arm_requirements_t;
-
 extern const char *const arming_state_names[];
 
 bool is_safe(const safety_s &safety, const actuator_armed_s &armed);
@@ -125,15 +117,12 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 		   const position_nav_loss_actions_t posctl_nav_loss_act);
 
 /*
- * Checks the validty of position data aaainst the requirements of the current navigation
+ * Checks the validty of position data against the requirements of the current navigation
  * mode and switches mode if position data required is not available.
  */
 bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
 				 const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
 
-bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety,
-		  const uint8_t arm_requirements);
-
 
 // COM_LOW_BAT_ACT parameter values
 typedef enum LOW_BAT_ACTION {