commander: rework arming checks to use the events interface

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -34,7 +34,26 @@
 px4_add_library(health_and_arming_checks
 	Common.cpp
 	HealthAndArmingChecks.cpp
+	checks/systemCheck.cpp
+	checks/magnetometerCheck.cpp
+	checks/accelerometerCheck.cpp
+	checks/gyroCheck.cpp
+	checks/baroCheck.cpp
+	checks/imuConsistencyCheck.cpp
+	checks/airspeedCheck.cpp
+	checks/distanceSensorChecks.cpp
+	checks/rcCalibrationCheck.cpp
+	checks/powerCheck.cpp
+	checks/estimatorCheck.cpp
+	checks/failureDetectorCheck.cpp
+	checks/manualControlCheck.cpp
+	checks/modeCheck.cpp
+	checks/cpuResourceCheck.cpp
+	checks/sdcardCheck.cpp
+	checks/parachuteCheck.cpp
+	checks/batteryCheck.cpp
 )
+add_dependencies(health_and_arming_checks mode_util)
 
 px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
 	LINKLIBS health_and_arming_checks mode_util

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "Common.hpp"
+#include "../ModeUtil/mode_requirements.hpp"
 
 void Report::getHealthReport(health_report_s &report) const
 {
@@ -157,7 +158,12 @@ void Report::reset()
 	_results[_current_result].reset();
 	_next_buffer_idx = 0;
 	_buffer_overflowed = false;
-	_failing_checks_prevent_mode_switch = 0;
+}
+
+void Report::prepare(uint8_t vehicle_type)
+{
+	// Get mode requirements before running any checks (in particular the mode checks require them)
+	getModeRequirements(vehicle_type, _status_flags);
 }
 
 NavModes Report::getModeGroup(uint8_t nav_state) const

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -316,6 +316,7 @@ private:
 	 * - report() (which can be called independently as well)
 	 */
 	void reset();
+	void prepare(uint8_t vehicle_type);
 	void finalize();
 
 	bool report(bool is_armed, bool force);

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -45,6 +45,8 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 {
 	_reporter.reset();
 
+	_reporter.prepare(_context.status().vehicle_type);
+
 	for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
 		if (!_checks[i]) {
 			break;
@@ -64,6 +66,8 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_reporter._mavlink_log_pub = &_mavlink_log_pub;
 		_reporter.reset();
 
+		_reporter.prepare(_context.status().vehicle_type);
+
 		for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
 			if (!_checks[i]) {
 				break;

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -39,6 +39,26 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/health_report.h>
 
+#include "checks/accelerometerCheck.hpp"
+#include "checks/airspeedCheck.hpp"
+#include "checks/baroCheck.hpp"
+#include "checks/cpuResourceCheck.hpp"
+#include "checks/distanceSensorChecks.hpp"
+#include "checks/estimatorCheck.hpp"
+#include "checks/failureDetectorCheck.hpp"
+#include "checks/gyroCheck.hpp"
+#include "checks/imuConsistencyCheck.hpp"
+#include "checks/magnetometerCheck.hpp"
+#include "checks/manualControlCheck.hpp"
+#include "checks/modeCheck.hpp"
+#include "checks/parachuteCheck.hpp"
+#include "checks/powerCheck.hpp"
+#include "checks/rcCalibrationCheck.hpp"
+#include "checks/sdcardCheck.hpp"
+#include "checks/systemCheck.hpp"
+#include "checks/batteryCheck.hpp"
+
+
 class HealthAndArmingChecks : public ModuleParams
 {
 public:
@@ -58,6 +78,11 @@ public:
 	 */
 	bool canArm(uint8_t nav_state) const { return _reporter.canArm(nav_state); }
 
+	/**
+	 * Whether switching into a given navigation mode is possible
+	 */
+	bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); }
+
 protected:
 	void updateParams() override;
 private:
@@ -68,7 +93,44 @@ private:
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
 
 	// all checks
-	HealthAndArmingCheckBase *_checks[10] = {
+	AccelerometerChecks _accelerometer_checks;
+	AirspeedChecks _airspeed_checks;
+	BaroChecks _baro_checks;
+	CpuResourceChecks _cpu_resource_checks;
+	DistanceSensorChecks _distance_sensor_checks;
+	EstimatorChecks _estimator_checks;
+	FailureDetectorChecks _failure_detector_checks;
+	GyroChecks _gyro_checks;
+	ImuConsistencyChecks _imu_consistency_checks;
+	MagnetometerChecks _magnetometer_checks;
+	ManualControlChecks _manual_control_checks;
+	ModeChecks _mode_checks;
+	ParachuteChecks _parachute_checks;
+	PowerChecks _power_checks;
+	RcCalibrationChecks _rc_calibration_checks;
+	SdCardChecks _sd_card_checks;
+	SystemChecks _system_checks;
+	BatteryChecks _battery_checks;
+
+	HealthAndArmingCheckBase *_checks[30] = {
+		&_accelerometer_checks,
+		&_airspeed_checks,
+		&_baro_checks,
+		&_cpu_resource_checks,
+		&_distance_sensor_checks,
+		&_estimator_checks,
+		&_failure_detector_checks,
+		&_gyro_checks,
+		&_imu_consistency_checks,
+		&_magnetometer_checks,
+		&_manual_control_checks,
+		&_mode_checks,
+		&_parachute_checks,
+		&_power_checks,
+		&_rc_calibration_checks,
+		&_sd_card_checks,
+		&_system_checks,
+		&_battery_checks,
 	};
 };
 

src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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 "accelerometerCheck.hpp"
+
+#include <lib/sensor_calibration/Utilities.hpp>
+
+using namespace time_literals;
+
+void AccelerometerChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	for (int instance = 0; instance < _sensor_accel_sub.size(); instance++) {
+		const bool is_required = instance == 0 || isAccelRequired(instance);
+
+		if (!is_required) {
+			continue;
+		}
+
+		const bool exists = _sensor_accel_sub[instance].advertised();
+		bool is_valid = false;
+		bool is_calibration_valid = false;
+
+		if (exists) {
+			sensor_accel_s accel_data;
+			is_valid = _sensor_accel_sub[instance].copy(&accel_data) && (accel_data.device_id != 0) && (accel_data.timestamp != 0)
+				   && (hrt_elapsed_time(&accel_data.timestamp) < 1_s);
+
+			if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
+				is_calibration_valid = true;
+
+			} else {
+				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
+			}
+
+			reporter.setIsPresent(health_component_t::gyro);
+		}
+
+		const bool is_sensor_ok = is_valid && is_calibration_valid;
+
+		if (!is_sensor_ok) {
+			if (!exists) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_missing"),
+								events::Log::Error, "Accelerometer sensor {1} missing", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel Sensor %u missing", instance);
+				}
+
+			} else if (!is_valid) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_no_data"),
+								events::Log::Error, "No valid data from Accelerometer {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Accel %u", instance);
+				}
+
+			} else if (!is_calibration_valid) {
+				/* EVENT
+				 */
+				reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_not_calibrated"),
+								     events::Log::Error, "Accelerometer {1} uncalibrated", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u uncalibrated", instance);
+				}
+			}
+		}
+	}
+}
+
+bool AccelerometerChecks::isAccelRequired(int instance)
+{
+	sensor_accel_s sensor_accel;
+
+	if (!_sensor_accel_sub[instance].copy(&sensor_accel)) {
+		return false;
+	}
+
+	const uint32_t device_id = static_cast<uint32_t>(sensor_accel.device_id);
+
+	if (device_id == 0) {
+		return false;
+	}
+
+	bool is_used_by_nav = false;
+
+	for (int i = 0; i < _estimator_status_sub.size(); i++) {
+		estimator_status_s estimator_status;
+
+		if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.accel_device_id == device_id) {
+			is_used_by_nav = true;
+			break;
+		}
+	}
+
+	return is_used_by_nav;
+}

src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_accel.h>
+#include <lib/sensor_calibration/Accelerometer.hpp>
+
+class AccelerometerChecks : public HealthAndArmingCheckBase
+{
+public:
+	AccelerometerChecks() = default;
+	~AccelerometerChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	bool isAccelRequired(int instance);
+
+	uORB::SubscriptionMultiArray<sensor_accel_s, calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
+	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
+};

src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   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 "airspeedCheck.hpp"
+
+using namespace time_literals;
+
+AirspeedChecks::AirspeedChecks()
+	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_trim_handle(param_find("FW_AIRSPD_TRIM"))
+{
+}
+
+void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (context.status().circuit_breaker_engaged_airspd_check ||
+	    (context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) {
+		return;
+	}
+
+	int32_t airspeed_mode = 0;
+	param_get(_param_fw_arsp_mode_handle, &airspeed_mode);
+	const bool optional = (airspeed_mode == 1);
+
+	airspeed_validated_s airspeed_validated;
+
+	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) {
+
+		reporter.setIsPresent(health_component_t::differential_pressure);
+
+		float airspeed_trim = 10.0f;
+		param_get(_param_fw_airspd_trim_handle, &airspeed_trim);
+
+		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
+
+		/*
+		 * Check if airspeed is declared valid or not by airspeed selector.
+		 */
+		if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
+
+			/* EVENT
+			 */
+			reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
+					       events::ID("check_airspeed_invalid"),
+					       events::Log::Error, "Airspeed invalid");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed invalid");
+			}
+		}
+
+		/*
+		 * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
+		 * Negative and positive offsets are considered. This check is optional, because the pitot cover
+		 * might have been removed before arming.
+		 */
+		if (!context.isArmed() && _param_com_arm_arsp_en.get()
+		    && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) {
+			/* EVENT
+			 * @description
+			 * Check the airspeed calibration and the pitot.
+			 *
+			 * <profile name="dev">
+			 * Measured: {1:.1m/s}, limit: {2:.1m/s}.
+			 *
+			 * This check can be configured via <param>COM_ARM_ARSP_EN</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(NavModes::None, health_component_t::differential_pressure,
+					events::ID("check_airspeed_too_high"),
+					events::Log::Error, "Airspeed too high", airspeed_validated.calibrated_airspeed_m_s, arming_max_airspeed_allowed);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed too high - check cal or pitot");
+			}
+		}
+
+	} else if (!optional) {
+
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * Most likely the airspeed selector module is not running.
+		 * This check can be configured via <param>CBRK_AIRSPD_CHK</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
+				       events::ID("check_airspeed_no_data"),
+				       events::Log::Error, "No airspeed data");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed selector module down");
+		}
+	}
+
+}

src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed_validated.h>
+
+class AirspeedChecks : public HealthAndArmingCheckBase
+{
+public:
+	AirspeedChecks();
+	~AirspeedChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+
+	const param_t _param_fw_arsp_mode_handle;
+	const param_t _param_fw_airspd_trim_handle;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_ARM_ARSP_EN>) _param_com_arm_arsp_en
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp

@@ -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.
+ *
+ ****************************************************************************/
+
+#include "baroCheck.hpp"
+
+using namespace time_literals;
+
+void BaroChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (!_param_sys_has_baro.get()) {
+		return;
+	}
+
+	for (int instance = 0; instance < _sensor_baro_sub.size(); instance++) {
+		const bool is_required = instance == 0 || isBaroRequired(instance);
+
+		if (!is_required) {
+			continue;
+		}
+
+		const bool exists = _sensor_baro_sub[instance].advertised();
+		bool is_valid = false;
+
+		if (exists) {
+			sensor_baro_s baro_data;
+			is_valid = _sensor_baro_sub[instance].copy(&baro_data) && (baro_data.device_id != 0) && (baro_data.timestamp != 0)
+				   && (hrt_elapsed_time(&baro_data.timestamp) < 1_s);
+			reporter.setIsPresent(health_component_t::absolute_pressure);
+		}
+
+		const bool is_sensor_ok = is_valid;
+
+		if (!is_sensor_ok) {
+			if (!exists) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::absolute_pressure,
+								events::ID("check_baro_missing"),
+								events::Log::Error, "Barometer {1} missing", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: barometer %u missing", instance);
+				}
+
+			} else if (!is_valid) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::absolute_pressure,
+								events::ID("check_baro_no_data"),
+								events::Log::Error, "No valid data from barometer {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Baro %u", instance);
+				}
+			}
+		}
+	}
+}
+
+bool BaroChecks::isBaroRequired(int instance)
+{
+	sensor_baro_s sensor_baro;
+
+	if (!_sensor_baro_sub[instance].copy(&sensor_baro)) {
+		return false;
+	}
+
+	const uint32_t device_id = static_cast<uint32_t>(sensor_baro.device_id);
+
+	if (device_id == 0) {
+		return false;
+	}
+
+	bool is_used_by_nav = false;
+
+	for (int i = 0; i < _estimator_status_sub.size(); i++) {
+		estimator_status_s estimator_status;
+
+		if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.baro_device_id == device_id) {
+			is_used_by_nav = true;
+			break;
+		}
+	}
+
+	return is_used_by_nav;
+}

src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/estimator_status.h>
+#include <lib/sensor_calibration/Barometer.hpp>
+
+class BaroChecks : public HealthAndArmingCheckBase
+{
+public:
+	BaroChecks() = default;
+	~BaroChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	bool isBaroRequired(int instance);
+
+	uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
+	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 "batteryCheck.hpp"
+
+using namespace time_literals;
+
+void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (!context.status().battery_healthy) {
+
+		/* EVENT
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
+				       events::Log::Error, "Battery unhealthy");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Battery unhealthy");
+		}
+	}
+
+	reporter.setIsPresent(health_component_t::battery); // assume it's present
+
+}

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+
+class BatteryChecks : public HealthAndArmingCheckBase
+{
+public:
+	BatteryChecks() = default;
+	~BatteryChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+};

src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "cpuResourceCheck.hpp"
+
+using namespace time_literals;
+
+void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_com_cpu_max.get() < FLT_EPSILON) {
+		return;
+	}
+
+	cpuload_s cpuload;
+
+	if (!_cpuload_sub.copy(&cpuload) || hrt_elapsed_time(&cpuload.timestamp) > 2_s) {
+
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * If the system does not provide any CPU load information, use the parameter <param>COM_CPU_MAX</param>
+		 * to disable the check.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"),
+				       events::Log::Error, "No CPU load information");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
+		}
+
+	} else {
+		const float cpuload_percent = cpuload.load * 100.f;
+
+		if (cpuload_percent > _param_com_cpu_max.get()) {
+
+			/* EVENT
+			 * @description
+			 * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
+			 * rate via <param>IMU_GYRO_RATEMAX</param>.
+			 *
+			 * <profile name="dev">
+			 * The threshold can be adjusted via <param>COM_CPU_MAX</param> parameter.
+			 * </profile>
+			 */
+			reporter.healthFailure<float>(NavModes::All, health_component_t::system, events::ID("check_cpuload_too_high"),
+						      events::Log::Error, "CPU load too high: {1:.1}%", cpuload_percent);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/cpuload.h>
+
+class CpuResourceChecks : public HealthAndArmingCheckBase
+{
+public:
+	CpuResourceChecks() = default;
+	~CpuResourceChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "distanceSensorChecks.hpp"
+
+using namespace time_literals;
+
+void DistanceSensorChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_sys_has_num_dist.get() <= 0) {
+		return;
+	}
+
+	for (int instance = 0; instance < _distance_sensor_sub.size(); instance++) {
+		const bool exists = _distance_sensor_sub[instance].advertised();
+		const bool is_mandatory = instance < _param_sys_has_num_dist.get();
+		bool valid = false;
+
+		if (exists) {
+			distance_sensor_s dist_sens;
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
+			reporter.setIsPresent(health_component_t::distance_sensor);
+		}
+
+		if (is_mandatory) {
+			if (!exists) {
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * This check can be configured via <param>SYS_HAS_NUM_DIST</param> parameter.
+				 * </profile>
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::distance_sensor,
+								events::ID("check_distance_sensor_missing"),
+								events::Log::Error, "Distance sensor {1} missing", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Distance Sensor %u missing", instance);
+				}
+
+			} else if (!valid) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::distance_sensor,
+								events::ID("check_distance_sensor_invalid"),
+								events::Log::Error, "No valid data from distance sensor {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: no valid data from distance sensor %u", instance);
+				}
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/distance_sensor.h>
+
+class DistanceSensorChecks : public HealthAndArmingCheckBase
+{
+public:
+	DistanceSensorChecks() = default;
+	~DistanceSensorChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_sub{ORB_ID::distance_sensor};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::SYS_HAS_NUM_DIST>) _param_sys_has_num_dist
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -0,0 +1,473 @@
+/****************************************************************************
+ *
+ *   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 "estimatorCheck.hpp"
+
+using namespace time_literals;
+
+void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_sys_mc_est_group.get() != 2) {
+		return;
+	}
+
+	const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
+	bool missing_data = false;
+
+	if (_param_sens_imu_mode.get() == 0) { // multi-ekf
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance)
+						&& _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+
+			if (!instance_changed) {
+				missing_data = true;
+			}
+
+		} else {
+			missing_data = true;
+		}
+	}
+
+	if (!missing_data) {
+		estimator_status_s estimator_status;
+
+		if (_estimator_status_sub.copy(&estimator_status)) {
+
+			checkEstimatorStatus(context, reporter, estimator_status, required_groups);
+
+		} else {
+			missing_data = true;
+		}
+	}
+
+	if (missing_data) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_missing_data"),
+					    events::Log::Info, "Waiting for estimator to initialize");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ekf2 missing data");
+		}
+
+	} else {
+		reporter.setIsPresent(health_component_t::local_position_estimate);
+		checkSensorBias(context, reporter, required_groups);
+	}
+}
+
+void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
+		const estimator_status_s &estimator_status, NavModes required_groups)
+{
+	if (estimator_status.pre_flt_fail_innov_heading) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_heading_not_stable"),
+					    events::Log::Error, "Heading estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
+		}
+
+	} else if (estimator_status.pre_flt_fail_innov_vel_horiz) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hor_vel_not_stable"),
+					    events::Log::Error, "Horizontal velocity estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity estimate not stable");
+		}
+
+	} else if (estimator_status.pre_flt_fail_innov_vel_vert) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_vert_vel_not_stable"),
+					    events::Log::Error, "Vertical velocity estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity estimate not stable");
+		}
+
+	} else if (estimator_status.pre_flt_fail_innov_height) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hgt_not_stable"),
+					    events::Log::Error, "Height estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
+		}
+	}
+
+
+	if ((_param_com_arm_mag_str.get() >= 1) && estimator_status.pre_flt_fail_mag_field_disturbed) {
+		NavModes required_groups_mag = required_groups;
+
+		if (_param_com_arm_mag_str.get() != 1) {
+			required_groups_mag = NavModes::None; // optional
+		}
+
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_interference"),
+					    events::Log::Warning, "Strong magnetic interference detected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference detected");
+		}
+	}
+
+	// check vertical position innovation test ratio
+	if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) {
+		/* EVENT
+		 * <profile name="dev">
+		 * Test ratio: {1:.3}, limit: {2:.3}.
+		 *
+		 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+				events::ID("check_estimator_hgt_est_err"),
+				events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
+		}
+	}
+
+	// check velocity innovation test ratio
+	if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) {
+		/* EVENT
+		 * <profile name="dev">
+		 * Test ratio: {1:.3}, limit: {2:.3}.
+		 *
+		 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+				events::ID("check_estimator_vel_est_err"),
+				events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
+		}
+	}
+
+	// check horizontal position innovation test ratio
+	if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) {
+		/* EVENT
+		 * <profile name="dev">
+		 * Test ratio: {1:.3}, limit: {2:.3}.
+		 *
+		 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+				events::ID("check_estimator_pos_est_err"),
+				events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
+		}
+	}
+
+	// check magnetometer innovation test ratio
+	if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) {
+		/* EVENT
+		 * <profile name="dev">
+		 * Test ratio: {1:.3}, limit: {2:.3}.
+		 *
+		 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+				events::ID("check_estimator_yaw_est_err"),
+				events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
+		}
+	}
+
+	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+	if (_param_sys_has_gps.get()) {
+		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
+
+		if (ekf_gps_fusion) {
+			reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
+		}
+
+		if (ekf_gps_check_fail) {
+			NavModes required_groups_gps = required_groups;
+			events::Log log_level = events::Log::Error;
+
+			if (_param_com_arm_wo_gps.get()) {
+				required_groups_gps = NavModes::None; // optional
+				log_level = events::Log::Warning;
+			}
+
+			// Only report the first failure to avoid spamming
+			const char *message = nullptr;
+
+			if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+				message = "Preflight%s: GPS fix too low";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_fix_too_low"),
+							    log_level, "GPS fix too low");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+				message = "Preflight%s: not enough GPS Satellites";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_num_sats_too_low"),
+							    log_level, "Not enough GPS Satellites");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
+				message = "Preflight%s: GPS PDOP too high";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_pdop_too_high"),
+							    log_level, "GPS PDOP too high");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+				message = "Preflight%s: GPS Horizontal Pos Error too high";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_hor_pos_err_too_high"),
+							    log_level, "GPS Horizontal Position Error too high");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+				message = "Preflight%s: GPS Vertical Pos Error too high";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_vert_pos_err_too_high"),
+							    log_level, "GPS Vertical Position Error too high");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+				message = "Preflight%s: GPS Speed Accuracy too low";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_speed_acc_too_low"),
+							    log_level, "GPS Speed Accuracy too low");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+				message = "Preflight%s: GPS Horizontal Pos Drift too high";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
+							    log_level, "GPS Horizontal Position Drift too high");
+
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+				message = "Preflight%s: GPS Vertical Pos Drift too high";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
+							    log_level, "GPS Vertical Position Drift too high");
+
+			} else if (estimator_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";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
+							    log_level, "GPS Horizontal Speed Drift too high");
+
+			} else if (estimator_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";
+				/* EVENT
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
+							    log_level, "GPS Vertical Speed Drift too high");
+
+			} else {
+				if (!ekf_gps_fusion) {
+					// Likely cause unknown
+					message = "Preflight%s: Estimator not using GPS";
+					/* EVENT
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_not_fusing"),
+								    log_level, "Estimator not using GPS");
+
+				} 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";
+					/* EVENT
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_generic"),
+								    log_level, "Poor GPS Quality");
+				}
+			}
+
+			if (message && reporter.mavlink_log_pub()) {
+				if (!_param_com_arm_wo_gps.get()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+
+				} else {
+					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+				}
+			}
+		}
+	}
+
+}
+
+void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups)
+{
+	// _estimator_sensor_bias_sub instance got changed above already
+	estimator_sensor_bias_s bias;
+
+	if (_estimator_sensor_bias_sub.copy(&bias) && hrt_elapsed_time(&bias.timestamp) < 30_s) {
+
+		// check accelerometer bias estimates
+		if (bias.accel_bias_valid) {
+			const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit;
+
+			for (int axis_index = 0; axis_index < 3; axis_index++) {
+				// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
+
+				if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
+					/* EVENT
+					 * An accelerometer recalibration might help.
+					 *
+					 * <profile name="dev">
+					 * Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
+					 *
+					 * This check can be configured via <param>EKF2_ABL_LIM</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure<uint8_t, float, float, float>(required_groups, health_component_t::local_position_estimate,
+							events::ID("check_estimator_high_accel_bias"),
+							events::Log::Error, "High Accelerometer Bias", axis_index,
+							bias.accel_bias[axis_index], ekf_ab_test_limit, test_uncertainty);
+
+					if (reporter.mavlink_log_pub()) {
+						mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Accelerometer Bias");
+					}
+
+					return; // avoid showing more than one error
+				}
+			}
+		}
+
+		// check gyro bias estimates
+		if (bias.gyro_bias_valid) {
+			const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
+
+			for (int axis_index = 0; axis_index < 3; axis_index++) {
+				// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
+
+				if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
+					/* EVENT
+					 * A Gyro recalibration might help.
+					 *
+					 * <profile name="dev">
+					 * Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
+					 *
+					 * This check can be configured via <param>EKF2_ABL_GYRLIM</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure<uint8_t, float, float, float>(required_groups, health_component_t::local_position_estimate,
+							events::ID("check_estimator_high_gyro_bias"),
+							events::Log::Error, "High Gyro Bias", axis_index,
+							bias.gyro_bias[axis_index], ekf_gb_test_limit, test_uncertainty);
+
+					if (reporter.mavlink_log_pub()) {
+						mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Gyro Bias");
+					}
+
+					return; // avoid showing more than one error
+				}
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/estimator_sensor_bias.h>
+
+
+class EstimatorChecks : public HealthAndArmingCheckBase
+{
+public:
+	EstimatorChecks() = default;
+	~EstimatorChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
+				  NavModes required_groups);
+	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
+
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
+					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
+					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
+					(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
+					(ParamFloat<px4::params::COM_ARM_EKF_VEL>) _param_com_arm_ekf_vel,
+					(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
+					(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
+					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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 "failureDetectorCheck.hpp"
+
+void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+			/* EVENT
+			 * @description
+			 * The vehicle exceeded the maximum configured roll angle.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>FD_FAIL_R</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
+						    events::Log::Critical, "Attitude failure detected");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
+			}
+
+		} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
+			/* EVENT
+			 * @description
+			 * The vehicle exceeded the maximum configured pitch angle.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>FD_FAIL_P</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
+						    events::Log::Critical, "Attitude failure detected");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
+			}
+		}
+
+		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
+						    events::Log::Critical, "Altitude failure detected");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
+			}
+		}
+
+		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
+						    events::Log::Critical, "Failure triggered by external system");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
+			}
+		}
+
+		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+			/* EVENT
+			 * @description
+			 * One or more ESCs failed to arm.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>FD_ESCS_EN</param> parameter.
+			 * </profile>
+			 */
+			reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
+					       events::Log::Critical, "ESC failure");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+
+class FailureDetectorChecks : public HealthAndArmingCheckBase
+{
+public:
+	FailureDetectorChecks() = default;
+	~FailureDetectorChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+};

src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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 "gyroCheck.hpp"
+
+#include <lib/sensor_calibration/Utilities.hpp>
+
+using namespace time_literals;
+
+void GyroChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	for (int instance = 0; instance < _sensor_gyro_sub.size(); instance++) {
+		const bool is_required = instance == 0 || isGyroRequired(instance);
+
+		if (!is_required) {
+			continue;
+		}
+
+		const bool exists = _sensor_gyro_sub[instance].advertised();
+		bool is_valid = false;
+		bool is_calibration_valid = false;
+
+		if (exists) {
+			sensor_gyro_s gyro_data;
+			is_valid = _sensor_gyro_sub[instance].copy(&gyro_data) && (gyro_data.device_id != 0) && (gyro_data.timestamp != 0)
+				   && (hrt_elapsed_time(&gyro_data.timestamp) < 1_s);
+
+			if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
+				is_calibration_valid = true;
+
+			} else {
+				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
+			}
+
+			reporter.setIsPresent(health_component_t::gyro);
+		}
+
+		const bool is_sensor_ok = is_valid && is_calibration_valid;
+
+		if (!is_sensor_ok) {
+			if (!exists) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_missing"),
+								events::Log::Error, "Gyro sensor {1} missing", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro Sensor %u missing", instance);
+				}
+
+			} else if (!is_valid) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_no_data"),
+								events::Log::Error, "No valid data from Gyro {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Gyro %u", instance);
+				}
+
+			} else if (!is_calibration_valid) {
+				/* EVENT
+				 */
+				reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_not_calibrated"),
+								     events::Log::Error, "Gyro {1} uncalibrated", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u uncalibrated", instance);
+				}
+			}
+		}
+	}
+}
+
+bool GyroChecks::isGyroRequired(int instance)
+{
+	sensor_gyro_s sensor_gyro;
+
+	if (!_sensor_gyro_sub[instance].copy(&sensor_gyro)) {
+		return false;
+	}
+
+	const uint32_t device_id = static_cast<uint32_t>(sensor_gyro.device_id);
+
+	if (device_id == 0) {
+		return false;
+	}
+
+	bool is_used_by_nav = false;
+
+	for (int i = 0; i < _estimator_status_sub.size(); i++) {
+		estimator_status_s estimator_status;
+
+		if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.gyro_device_id == device_id) {
+			is_used_by_nav = true;
+			break;
+		}
+	}
+
+	return is_used_by_nav;
+}

src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+
+class GyroChecks : public HealthAndArmingCheckBase
+{
+public:
+	GyroChecks() = default;
+	~GyroChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	bool isGyroRequired(int instance);
+
+	uORB::SubscriptionMultiArray<sensor_gyro_s, calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
+	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
+};

src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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 "imuConsistencyCheck.hpp"
+
+void ImuConsistencyChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	sensors_status_imu_s imu;
+
+	if (!_sensors_status_imu_sub.copy(&imu) || context.isArmed()) {
+		return;
+	}
+
+	// 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.
+	for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
+		if (imu.accel_device_ids[i] != 0) {
+
+			const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
+
+			NavModes required_groups = NavModes::None;
+
+			if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get()) {
+				required_groups = NavModes::All;
+			}
+
+			if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get() * 0.8f) {
+				/* EVENT
+				 * @description
+				 * Check the calibration.
+				 *
+				 * Inconsistency value: {2}.
+				 * Configured Threshold: {3}.
+				 *
+				 * <profile name="dev">
+				 * This check can be configured via <param>COM_ARM_IMU_ACC</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::accel,
+						events::ID("check_imu_accel_inconsistent"),
+						events::Log::Warning, "Accel {1} inconsistent", i, accel_inconsistency_m_s_s, _param_com_arm_imu_acc.get());
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u inconsistent - check cal", i);
+				}
+
+				break;
+			}
+		}
+	}
+
+	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
+	for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
+		if (imu.gyro_device_ids[i] != 0) {
+
+			const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
+
+			NavModes required_groups = NavModes::None;
+
+			if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get()) {
+				required_groups = NavModes::All;
+			}
+
+			if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get() * 0.5f) {
+				/* EVENT
+				 * @description
+				 * Check the calibration.
+				 *
+				 * Inconsistency value: {2}.
+				 * Configured Threshold: {3}.
+				 *
+				 * <profile name="dev">
+				 * This check can be configured via <param>COM_ARM_IMU_GYR</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::gyro,
+						events::ID("check_imu_gyro_inconsistent"),
+						events::Log::Warning, "Gyro {1} inconsistent", i, gyro_inconsistency_rad_s, _param_com_arm_imu_gyr.get());
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u inconsistent - check cal", i);
+				}
+
+				break;
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensors_status_imu.h>
+
+class ImuConsistencyChecks : public HealthAndArmingCheckBase
+{
+public:
+	ImuConsistencyChecks() = default;
+	~ImuConsistencyChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamFloat<px4::params::COM_ARM_IMU_ACC>) _param_com_arm_imu_acc,
+					(ParamFloat<px4::params::COM_ARM_IMU_GYR>) _param_com_arm_imu_gyr
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp

@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   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 "magnetometerCheck.hpp"
+
+#include <lib/sensor_calibration/Utilities.hpp>
+
+using namespace time_literals;
+
+void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (!_param_sys_has_mag.get()) {
+		return;
+	}
+
+	bool had_failure = false;
+
+	for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) {
+		bool is_mag_fault = false;
+		const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault);
+
+		if (!is_required) {
+			continue;
+		}
+
+		const bool exists = _sensor_mag_sub[instance].advertised();
+		bool is_valid = false;
+		bool is_calibration_valid = false;
+
+		if (exists) {
+			sensor_mag_s mag_data;
+			is_valid = _sensor_mag_sub[instance].copy(&mag_data) && (mag_data.device_id != 0) && (mag_data.timestamp != 0)
+				   && (hrt_elapsed_time(&mag_data.timestamp) < 1_s);
+
+			if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
+				is_calibration_valid = true;
+
+			} else {
+				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0);
+			}
+
+			reporter.setIsPresent(health_component_t::magnetometer);
+		}
+
+		const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault;
+
+		if (!is_sensor_ok) {
+			if (!exists) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_missing"),
+								events::Log::Error, "Compass sensor {1} missing", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass Sensor %u missing", instance);
+				}
+
+			} else if (!is_valid) {
+				/* EVENT
+				 */
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_no_data"),
+								events::Log::Error, "No valid data from Compass {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Compass %u", instance);
+				}
+
+			} else if (!is_calibration_valid) {
+				/* EVENT
+				 */
+				reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::magnetometer,
+								     events::ID("check_mag_not_calibrated"),
+								     events::Log::Error, "Compass {1} uncalibrated", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass %u uncalibrated", instance);
+				}
+
+			} else if (is_mag_fault) {
+				/* EVENT
+				 * @description
+				 * Recalibrate the compass and check the orientation.
+				 */
+				reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_fault"),
+								     events::Log::Error, "Estimator fault due to Compass {1}", instance);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass %u fault", instance);
+				}
+			}
+
+			had_failure = true;
+		}
+	}
+
+	if (!had_failure && !context.isArmed()) {
+		consistencyCheck(context, reporter);
+	}
+}
+
+bool MagnetometerChecks::isMagRequired(int instance, bool &mag_fault)
+{
+	sensor_mag_s sensor_mag;
+
+	if (!_sensor_mag_sub[instance].copy(&sensor_mag)) {
+		return false;
+	}
+
+	const uint32_t device_id = static_cast<uint32_t>(sensor_mag.device_id);
+
+	if (device_id == 0) {
+		return false;
+	}
+
+	bool is_used_by_nav = false;
+
+	for (int i = 0; i < _estimator_status_sub.size(); i++) {
+		estimator_status_s estimator_status;
+
+		if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.mag_device_id == device_id) {
+			mag_fault = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT);
+			is_used_by_nav = true;
+			break;
+		}
+	}
+
+	return is_used_by_nav;
+}
+
+
+void MagnetometerChecks::consistencyCheck(const Context &context, Report &reporter)
+{
+	if (_param_com_arm_mag_ang.get() < 0) { // Check disabled
+		return;
+	}
+
+	sensor_preflight_mag_s sensors;
+
+	if (!_sensor_preflight_mag_sub.copy(&sensors)) {
+		// can happen if not advertised (yet)
+		return;
+	}
+
+	// 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.
+	if (sensors.mag_inconsistency_angle > math::radians<float>(_param_com_arm_mag_ang.get())) {
+		int inconsistency_angle_deg = static_cast<int>(math::degrees<float>(sensors.mag_inconsistency_angle));
+		/* EVENT
+		 * @description
+		 * Check the compass orientations and recalibrate.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MAG_ANG</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<int16_t>(NavModes::All, health_component_t::magnetometer,
+						     events::ID("check_mag_consistency"),
+						     events::Log::Error, "Compass inconsistent by {1} degrees", inconsistency_angle_deg);
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compasses %d° inconsistent",
+					     inconsistency_angle_deg);
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_preflight_mag.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/estimator_status.h>
+#include <lib/sensor_calibration/Magnetometer.hpp>
+
+class MagnetometerChecks : public HealthAndArmingCheckBase
+{
+public:
+	MagnetometerChecks() = default;
+	~MagnetometerChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	bool isMagRequired(int instance, bool &mag_fault);
+	void consistencyCheck(const Context &context, Report &reporter);
+
+	uORB::SubscriptionMultiArray<sensor_mag_s, calibration::Magnetometer::MAX_SENSOR_COUNT> _sensor_mag_sub{ORB_ID::sensor_mag};
+	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
+
+	uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+					(ParamInt<px4::params::COM_ARM_MAG_ANG>) _param_com_arm_mag_ang
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "manualControlCheck.hpp"
+
+using namespace time_literals;
+
+void ManualControlChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (context.isArmed()) {
+		return;
+	}
+
+	manual_control_switches_s manual_control_switches;
+
+	if (_manual_control_switches_sub.copy(&manual_control_switches)) {
+
+		// check action switches
+		if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control,
+						    events::ID("check_man_control_rtl_engaged"),
+						    events::Log::Error, "RTL switch engaged");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RTL switch engaged");
+			}
+		}
+
+		if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control,
+						    events::ID("check_man_control_kill_engaged"),
+						    events::Log::Error, "Kill switch engaged");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Kill switch engaged");
+			}
+		}
+
+		if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control,
+						    events::ID("check_man_control_landing_gear_up"),
+						    events::Log::Error, "Landing gear switch set in UP position");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Landing gear switch set in UP position");
+			}
+		}
+
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.hpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_switches.h>
+
+class ManualControlChecks : public HealthAndArmingCheckBase
+{
+public:
+	ManualControlChecks() = default;
+	~ManualControlChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
+};

src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 "modeCheck.hpp"
+
+void ModeChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (!context.isArmed()) {
+		checkArmingRequirement(context, reporter);
+	}
+
+	// Failing mode requirements generally also clear the can_run bits which prevents mode switching and
+	// might trigger a failsafe if already in that mode.
+
+	if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
+		/* EVENT
+		 * @description
+		 * Make sure the gyroscope is providing valid data.
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_angular_velocity, health_component_t::system,
+					    events::ID("check_modes_angular_velocity"),
+					    events::Log::Critical, "Angular velocity not valid");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity);
+	}
+
+	if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) {
+		/* EVENT
+		 * @description
+		 * Wait until the estimator initialized
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::system,
+					    events::ID("check_modes_attitude"),
+					    events::Log::Critical, "No valid attitude estimate");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude);
+	}
+
+	if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_position, health_component_t::system,
+					    events::ID("check_modes_local_pos"),
+					    events::Log::Error, "No valid local position estimate");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_position);
+	}
+
+	if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system,
+					    events::ID("check_modes_global_pos"),
+					    events::Log::Error, "No valid global position estimate");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
+	}
+
+	if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system,
+					    events::ID("check_modes_local_alt"),
+					    events::Log::Critical, "No valid altitude estimate");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt);
+	}
+
+	if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) {
+		/* EVENT
+		 * @description
+		 * Upload a mission first.
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
+					    events::ID("check_modes_mission"),
+					    events::Log::Info, "No mission available");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
+	}
+
+	if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) {
+		/* EVENT
+		 * @description
+		 * The offboard component is not sending setpoints.
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system,
+					    events::ID("check_modes_offboard_signal"),
+					    events::Log::Error, "No offboard signal");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal);
+	}
+
+	if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system,
+					    events::ID("check_modes_home_position"),
+					    events::Log::Info, "Home position not set");
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_home_position);
+	}
+
+	if (reporter.failsafeFlags().mode_req_other != 0) {
+		// Here we expect there is already an event reported for the failing check (this is for external modes)
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);
+	}
+}
+
+void ModeChecks::checkArmingRequirement(const Context &context, Report &reporter)
+{
+	if (reporter.failsafeFlags().mode_req_prevent_arming & (1u << context.status().nav_state)) {
+		/* EVENT
+		 * @description
+		 * Switch to another mode first.
+		 */
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_prevent_arming, health_component_t::system,
+					    events::ID("check_modes_cannot_takeoff"),
+					    events::Log::Info, "Mode not suitable for arming");
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+
+class ModeChecks : public HealthAndArmingCheckBase
+{
+public:
+	ModeChecks() = default;
+	~ModeChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	void checkArmingRequirement(const Context &context, Report &reporter);
+
+};

src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "parachuteCheck.hpp"
+
+
+using namespace time_literals;
+
+void ParachuteChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (!_param_com_parachute.get()) {
+		return;
+	}
+
+	if (!context.status().parachute_system_present) {
+		/* EVENT
+		 * @description
+		 * Parachute system failed to report. Make sure it it setup and installed properly.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_PARACHUTE</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_missing"),
+				       events::Log::Error, "Parachute system missing");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing");
+		}
+
+	} else if (!context.status().parachute_system_healthy) {
+
+		/* EVENT
+		 * @description
+		 * Parachute system reported being unhealth.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_PARACHUTE</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_unhealthy"),
+				       events::Log::Error, "Parachute system not ready");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready");
+		}
+	}
+
+	if (context.status().parachute_system_present) {
+		reporter.setIsPresent(health_component_t::parachute);
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+class ParachuteChecks : public HealthAndArmingCheckBase
+{
+public:
+	ParachuteChecks() = default;
+	~ParachuteChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_PARACHUTE>) _param_com_parachute
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 "powerCheck.hpp"
+
+using namespace time_literals;
+
+void PowerChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (context.status().circuit_breaker_engaged_power_check) {
+		return;
+	}
+
+	if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
+		// Ignore power check in HITL.
+		return;
+	}
+
+	if (!context.status().power_input_valid) {
+
+		/* EVENT
+		 * @description
+		 * Note that USB must be disconnected as well.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>CBRK_SUPPLY_CHK</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_missing"),
+				       events::Log::Error, "Power module not connected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Power module not connected");
+		}
+
+		return;
+	}
+
+	system_power_s system_power;
+
+	if (_system_power_sub.copy(&system_power)) {
+		// Check avionics rail voltages (if USB isn't connected)
+		if (!system_power.usb_connected) {
+			float avionics_power_rail_voltage = system_power.voltage5v_v;
+
+			const float low_error_threshold = 4.5f;
+			const float low_warning_threshold = 4.8f;
+			const float high_warning_threshold = 5.4f;
+
+			if (avionics_power_rail_voltage < low_warning_threshold) {
+				NavModes affected_groups = NavModes::None;
+
+				if (avionics_power_rail_voltage < low_error_threshold) {
+					affected_groups = NavModes::All;
+				}
+
+				/* EVENT
+				 * @description
+				 * Check the voltage supply to the FMU, it must be above {2:.2} Volt.
+				 *
+				 * <profile name="dev">
+				 * This check can be configured via <param>CBRK_SUPPLY_CHK</param> parameter.
+				 * </profile>
+				 */
+				reporter.healthFailure<float, float>(affected_groups, health_component_t::system,
+								     events::ID("check_avionics_power_low"),
+								     events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_warning_threshold);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power low: %6.2f Volt",
+							     (double)avionics_power_rail_voltage);
+				}
+
+			} else if (avionics_power_rail_voltage > high_warning_threshold) {
+				/* EVENT
+				 * @description
+				 * Check the voltage supply to the FMU, it must be below {2:.2} Volt.
+				 *
+				 * <profile name="dev">
+				 * This check can be configured via <param>CBRK_SUPPLY_CHK</param> parameter.
+				 * </profile>
+				 */
+				reporter.healthFailure<float, float>(NavModes::All, health_component_t::system,
+								     events::ID("check_avionics_power_high"),
+								     events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_warning_threshold);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power high: %6.2f Volt",
+							     (double)avionics_power_rail_voltage);
+				}
+			}
+
+			const int power_module_count = math::countSetBits(system_power.brick_valid);
+
+			if (power_module_count < _param_com_power_count.get()) {
+				/* EVENT
+				 * @description
+				 * Available power modules: {1}.
+				 * Required power modules: {2}.
+				 *
+				 * <profile name="dev">
+				 * This check can be configured via <param>COM_POWER_COUNT</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure<uint8_t, uint8_t>(NavModes::All, health_component_t::system,
+						events::ID("check_avionics_power_redundancy"),
+						events::Log::Error, "Power redundancy not met", power_module_count, _param_com_power_count.get());
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Power redundancy not met: %d instead of %" PRId32 "",
+							     power_module_count, _param_com_power_count.get());
+				}
+			}
+		}
+
+	} else {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * Ensure the ADC is working and the system_power topic is published.
+		 *
+		 * This check can be configured via <param>CBRK_SUPPLY_CHK</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_system_power"),
+				       events::Log::Error, "System power unavailable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: system power unavailable");
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/system_power.h>
+
+class PowerChecks : public HealthAndArmingCheckBase
+{
+public:
+	PowerChecks() = default;
+	~PowerChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _system_power_sub{ORB_ID(system_power)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_POWER_COUNT>) _param_com_power_count
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   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 "rcCalibrationCheck.hpp"
+
+/**
+ * Maximum deadzone value
+ */
+#define RC_INPUT_MAX_DEADZONE_US	500
+
+/**
+ * Minimum value
+ */
+#define RC_INPUT_LOWEST_MIN_US	0
+
+/**
+ * Maximum value
+ */
+#define RC_INPUT_HIGHEST_MAX_US	2500
+
+RcCalibrationChecks::RcCalibrationChecks()
+{
+	char nbuf[20];
+
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_param_handles[i].min = param_find(nbuf);
+
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_param_handles[i].trim = param_find(nbuf);
+
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_param_handles[i].max = param_find(nbuf);
+
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_param_handles[i].rev = param_find(nbuf);
+
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_param_handles[i].dz = param_find(nbuf);
+	}
+
+}
+
+void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (context.isArmed()) {
+		return;
+	}
+
+	if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3) {
+		return;
+	}
+
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		/* initialize values to values failing the check */
+		float param_min = 0.0f;
+		float param_max = 0.0f;
+		float param_trim = 0.0f;
+		float param_rev = 0.0f;
+		float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
+
+		param_get(_param_handles[i].min, &param_min);
+		param_get(_param_handles[i].trim, &param_trim);
+		param_get(_param_handles[i].max, &param_max);
+		param_get(_param_handles[i].rev, &param_rev);
+		param_get(_param_handles[i].dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < RC_INPUT_LOWEST_MIN_US) {
+			/* EVENT
+			 * @description
+			 * Recalibrate the RC.
+			 */
+			reporter.armingCheckFailure<uint8_t, uint16_t>(NavModes::All, health_component_t::remote_control,
+					events::ID("check_rc_min_too_small"),
+					events::Log::Error, "RC calibration for channel {1} invalid: MIN less than {2}", i + 1, RC_INPUT_LOWEST_MIN_US);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_MIN < %u", i + 1,
+						     RC_INPUT_LOWEST_MIN_US);
+			}
+		}
+
+		if (param_max > RC_INPUT_HIGHEST_MAX_US) {
+			/* EVENT
+			 * @description
+			 * Recalibrate the RC.
+			 */
+			reporter.armingCheckFailure<uint8_t, uint16_t>(NavModes::All, health_component_t::remote_control,
+					events::ID("check_rc_max_too_high"),
+					events::Log::Error, "RC calibration for channel {1} invalid: MAX greater than {2}", i + 1, RC_INPUT_HIGHEST_MAX_US);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_MAX > %u", i + 1,
+						     RC_INPUT_HIGHEST_MAX_US);
+			}
+		}
+
+		if (param_trim < param_min) {
+			/* EVENT
+			 * @description
+			 * Recalibrate the RC.
+			 */
+			reporter.armingCheckFailure<uint8_t, int16_t, int16_t>(NavModes::All, health_component_t::remote_control,
+					events::ID("check_rc_trim_too_small"),
+					events::Log::Error, "RC calibration for channel {1} invalid: TRIM less than MIN ({2} less than {3})", i + 1,
+					(int16_t)param_trim, (int16_t)param_min);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_TRIM < MIN (%d/%d)", i + 1,
+						     (int)param_trim, (int)param_min);
+			}
+		}
+
+		if (param_trim > param_max) {
+			/* EVENT
+			 * @description
+			 * Recalibrate the RC.
+			 */
+			reporter.armingCheckFailure<uint8_t, int16_t, int16_t>(NavModes::All, health_component_t::remote_control,
+					events::ID("check_rc_trim_too_high"),
+					events::Log::Error, "RC calibration for channel {1} invalid: TRIM greater than MAX ({2} greater than {3})", i + 1,
+					(int16_t)param_trim, (int16_t)param_max);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_TRIM > MAX (%d/%d)", i + 1,
+						     (int)param_trim, (int)param_max);
+			}
+		}
+
+		/* assert deadzone is sane */
+		if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
+			/* EVENT
+			 * @description
+			 * Recalibrate the RC.
+			 */
+			reporter.armingCheckFailure<uint8_t, uint16_t>(NavModes::All, health_component_t::remote_control,
+					events::ID("check_rc_dz_too_high"),
+					events::Log::Error, "RC calibration for channel {1} invalid: DZ greater than {2}", i + 1, RC_INPUT_MAX_DEADZONE_US);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_DZ > %u", i + 1,
+						     RC_INPUT_MAX_DEADZONE_US);
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.hpp

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/input_rc.h>
+
+class RcCalibrationChecks : public HealthAndArmingCheckBase
+{
+public:
+	RcCalibrationChecks();
+	~RcCalibrationChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	struct ParamHandles {
+		param_t min;
+		param_t trim;
+		param_t max;
+		param_t rev;
+		param_t dz;
+	};
+	ParamHandles _param_handles[input_rc_s::RC_INPUT_MAX_CHANNELS];
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "sdcardCheck.hpp"
+
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+
+void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_com_arm_sdcard.get() <= 0) {
+		return;
+	}
+
+	struct statfs statfs_buf;
+
+	if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+		// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+		_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
+	}
+
+	if (!_sdcard_detected) {
+		NavModes affected_modes{NavModes::None};
+
+		if (_param_com_arm_sdcard.get() == 2) {
+			// disallow arming without sd card
+			affected_modes = NavModes::All;
+		}
+
+		/* EVENT
+		 * @description
+		 * Insert an SD Card to the autopilot and reboot the system.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"),
+					    events::Log::Error, "Missing FMU SD Card");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+class SdCardChecks : public HealthAndArmingCheckBase
+{
+public:
+	SdCardChecks() = default;
+	~SdCardChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	bool _sdcard_detected{false};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp

@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 "systemCheck.hpp"
+
+#include "../../Arming/ArmAuthorization/ArmAuthorization.h"
+#include <uORB/topics/vehicle_command_ack.h>
+
+void SystemChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	actuator_armed_s actuator_armed;
+
+	if (_actuator_armed_sub.copy(&actuator_armed)) {
+		if (actuator_armed.force_failsafe || actuator_armed.manual_lockdown) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_flight_term_active"),
+						    events::Log::Critical, "Flight termination active");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Flight termination active");
+			}
+		}
+	}
+
+	// USB not connected
+	if (!context.status().circuit_breaker_engaged_usb_check && context.status().usb_connected) {
+		/* EVENT
+		 * @description
+		 * Flying with USB is not safe. Disconnect it and reboot the FMU.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>CBRK_USB_CHK</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_usb_connected"),
+					    events::Log::Error, "USB connected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Flying with USB is not safe");
+		}
+	}
+
+	// Arm Requirements: mission
+	if (_param_com_arm_mis_req.get() && !context.isArmed()) {
+		if (!context.status().auto_mission_available) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"),
+						    events::Log::Error, "No valid mission");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission");
+			}
+		}
+	}
+
+	reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available;
+
+	// Global position required
+	if (!_param_com_arm_wo_gps.get() && !context.isArmed()) {
+		if (!reporter.failsafeFlags().global_position_valid) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_WO_GPS</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_global_pos"),
+						    events::Log::Error, "Global position required");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required");
+			}
+
+		} else if (!reporter.failsafeFlags().home_position_valid) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_WO_GPS</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_home_pos"),
+						    events::Log::Error, "Home position required");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Home position required");
+			}
+		}
+	}
+
+	// safety button
+	if (context.status().safety_button_available && !context.status().safety_off && !context.isArmed()) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>CBRK_IO_SAFETY</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_safety_button"),
+					    events::Log::Info, "Press safety button first");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Press safety button first");
+		}
+	}
+
+	// avoidance system
+	if (context.status().avoidance_system_required) {
+		if (context.status().avoidance_system_valid) {
+			reporter.setIsPresent(health_component_t::avoidance);
+
+		} else {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_OBS_AVOID</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"),
+						    events::Log::Error, "Avoidance system not ready");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready");
+			}
+		}
+	}
+
+	// ESC checks
+	if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_error) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_offline"),
+				       events::Log::Error, "One or more ESCs are offline");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs are offline");
+		}
+	}
+
+	if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_failure) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_failure"),
+				       events::Log::Error, "One or more ESCs have a failure");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs have a failure");
+		}
+	}
+
+	reporter.setIsPresent(health_component_t::motors_escs); // TODO: based on telemetry
+
+	// VTOL in transition
+	if (context.status().is_vtol && !context.isArmed()) {
+		if (context.status().in_transition_mode) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_vtol_in_transition"),
+						    events::Log::Warning, "Vehicle is in transition state");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle is in transition state");
+			}
+		}
+
+		if (!context.status().circuit_breaker_vtol_fw_arming_check
+		    && context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>CBRK_VTOLARMING</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_vtol_in_fw_mode"),
+						    events::Log::Warning, "Vehicle is not in multicopter mode");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle is not in multicopter mode");
+			}
+		}
+	}
+
+	if (_param_gf_action.get() != 0 && context.status().geofence_violated) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>GF_ACTION</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_gf_violation"),
+					    events::Log::Error, "Vehicle outside geofence");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence");
+		}
+	}
+
+	// Arm Requirements: authorization
+	if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) {
+		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_arm_auth_failed"),
+						    events::Log::Error, "Arm authorization denied");
+		}
+	}
+
+	if (reporter.failsafeFlags().rc_signal_found_once) {
+		reporter.setIsPresent(health_component_t::remote_control);
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp

@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_armed.h>
+
+class SystemChecks : public HealthAndArmingCheckBase
+{
+public:
+	SystemChecks() = default;
+	~SystemChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
+					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
+					(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
+					(ParamInt<px4::params::GF_ACTION>) _param_gf_action
+				       )
+};