commander: rework arming checks to use the events interface

msg/event.msg

@@ -7,4 +7,4 @@ uint8[25] arguments         # (optional) arguments, depend on event id
 
 uint8 log_levels            # Log levels: 4 bits MSB: internal, 4 bits LSB: external
 
-uint8 ORB_QUEUE_LENGTH = 8
+uint8 ORB_QUEUE_LENGTH = 16

msg/vehicle_status.msg

@@ -113,3 +113,22 @@ uint64 onboard_control_sensors_health
 
 bool safety_button_available # Set to true if a safety button is connected
 bool safety_off # Set to true if safety is off
+bool auto_mission_available
+
+bool power_input_valid                            # set if input power is valid
+bool usb_connected                                # set to true (never cleared) once telemetry received from usb link
+
+bool parachute_system_present
+bool parachute_system_healthy
+
+bool avoidance_system_required                    # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
+bool avoidance_system_valid                       # Status of the obstacle avoidance system
+
+bool battery_healthy                	# set if battery is available and not low
+
+bool circuit_breaker_engaged_power_check
+bool circuit_breaker_engaged_airspd_check
+bool circuit_breaker_flight_termination_disabled
+bool circuit_breaker_engaged_usb_check
+bool circuit_breaker_vtol_fw_arming_check         # set to true if for VTOLs arming in fixed-wing mode should be allowed
+

msg/vehicle_status_flags.msg

@@ -1,7 +1,21 @@
-# This is a struct used by the commander internally.
+# TODO: rename to failsafe_flags (will be input to failsafe state machine)
+#
 
 uint64 timestamp				# time since system start (microseconds)
 
+# Per-mode requirements
+uint32 mode_req_angular_velocity
+uint32 mode_req_attitude
+uint32 mode_req_local_position
+uint32 mode_req_global_position
+uint32 mode_req_local_alt
+uint32 mode_req_mission
+uint32 mode_req_offboard_signal
+uint32 mode_req_home_position
+uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
+uint32 mode_req_other             # other requirements, not covered above (for external modes)
+
+
 bool calibration_enabled
 bool pre_flight_checks_pass		# true if all checks necessary to arm pass
 bool auto_mission_available
@@ -13,8 +27,6 @@ bool local_velocity_valid		# set to true by the commander app if the quality of
 bool global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
 bool gps_position_valid
 bool home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
-bool power_input_valid                	# set if input power is valid
-bool battery_healthy                	# set if battery is available and not low
 bool escs_error		      		# set to true if one or more ESCs reporting esc_status are offline
 bool escs_failure		      	# set to true if one or more ESCs reporting esc_status has a failure
 
@@ -23,25 +35,10 @@ bool position_reliant_on_optical_flow
 bool position_reliant_on_vision_position
 
 bool dead_reckoning
-bool flight_terminated
-
-bool circuit_breaker_engaged_power_check
-bool circuit_breaker_engaged_airspd_check
-bool circuit_breaker_flight_termination_disabled
-bool circuit_breaker_engaged_usb_check
-bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in fixed-wing mode should be allowed
 
 bool offboard_control_signal_lost
 
 bool rc_signal_found_once
 bool rc_calibration_in_progress
-bool rc_calibration_valid                            # set if RC calibration is valid
 bool vtol_transition_failure                        # Set to true if vtol transition failed
-bool usb_connected                                # status of the USB power supply
-bool sd_card_detected_once                        # set to true if the SD card was detected
 
-bool avoidance_system_required					  # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
-bool avoidance_system_valid                       # Status of the obstacle avoidance system
-
-bool parachute_system_present
-bool parachute_system_healthy

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -39,10 +39,8 @@ constexpr bool
 ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
 
 transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
-		const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
+		const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks,
-		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
+		const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason)
-		orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
-		arm_disarm_reason_t calling_reason)
 {
 	// Double check that our static arrays are still valid
 	static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
@@ -67,10 +65,9 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		    && !(status.hil_state == vehicle_status_s::HIL_STATE_ON)
 		    && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) {
 
-			if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
+			checks.update();
-							    true, // report_failures
+
-							    safety_button_available, safety_off,
+			if (!checks.canArm(status.nav_state)) {
-							    true)) { // is_arm_attempt
 				feedback_provided = true; // Preflight checks report error messages
 				valid_transition = false;
 			}

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "../PreFlightCheck/PreFlightCheck.hpp"
+#include "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/events.h>
 #include <uORB/topics/actuator_armed.h>
@@ -54,11 +54,9 @@ public:
 	void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
 
 	transition_result_t
-	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode,
+	arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state,
-				const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
+				actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks,
-				actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
+				orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason);
-				vehicle_status_flags_s &status_flags,
-				arm_disarm_reason_t calling_reason);
 
 	// Getters
 	uint8_t getArmState() const { return _arm_state; }

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include <gtest/gtest.h>
-#include <ArmStateMachine.hpp>
+#include "ArmStateMachine.hpp"
 
 TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 {
@@ -61,12 +61,12 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 	} ArmingTransitionTest_t;
 
 	// We use these defines so that our test cases are more readable
-#define ATT_ARMED true
+	static constexpr bool ATT_ARMED = true;
-#define ATT_DISARMED false
+	static constexpr bool ATT_DISARMED = false;
-#define ATT_SAFETY_AVAILABLE true
+	static constexpr bool ATT_SAFETY_AVAILABLE = true;
-#define ATT_SAFETY_NOT_AVAILABLE true
+	static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true;
-#define ATT_SAFETY_OFF true
+	static constexpr bool ATT_SAFETY_OFF = true;
-#define ATT_SAFETY_ON false
+	static constexpr bool ATT_SAFETY_ON = false;
 
 	// These are test cases for arming_state_transition
 	static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
@@ -253,22 +253,17 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		// Setup initial machine state
 		arm_state_machine.forceArmState(test->current_state.arming_state);
 		status.hil_state = test->hil_state;
-		// The power status of the test unit is not relevant for the unit test
-		status_flags.circuit_breaker_engaged_power_check = true;
 
-		vehicle_control_mode_s control_mode{};
+		HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status);
 
 		// Attempt transition
 		transition_result_t result = arm_state_machine.arming_state_transition(
 						     status,
-						     control_mode,
-						     test->safety_button_available,
-						     test->safety_off,
 						     test->requested_state,
 						     armed,
+						     health_and_arming_checks,
 						     true /* enable pre-arm checks */,
 						     nullptr /* no mavlink_log_pub */,
-						     status_flags,
 						     arm_disarm_reason_t::unit_test);
 
 		// Validate result of transition

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

@@ -36,6 +36,9 @@ px4_add_library(ArmStateMachine
 	ArmStateMachine.cpp
 )
 target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck)
+target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks)
+
+px4_add_functional_gtest(SRC ArmStateMachineTest.cpp
+	LINKLIBS ArmStateMachine health_and_arming_checks sensor_calibration ArmAuthorization mode_util
+	)
 
-px4_add_functional_gtest(SRC ArmStateMachineTest.cpp LINKLIBS ArmStateMachine)

src/modules/commander/Arming/CMakeLists.txt

@@ -34,4 +34,3 @@
 add_subdirectory(ArmAuthorization)
 add_subdirectory(ArmStateMachine)
 add_subdirectory(HealthFlags)
-add_subdirectory(PreFlightCheck)

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

@@ -1,242 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/**
- * @file PreFlightCheck.cpp
- */
-
-#include "PreFlightCheck.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <HealthFlags.h>
-#include <lib/parameters/param.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-
-using namespace time_literals;
-
-static constexpr unsigned max_mandatory_mag_count = 1;
-static constexpr unsigned max_mandatory_gyro_count = 1;
-static constexpr unsigned max_mandatory_accel_count = 1;
-static constexpr unsigned max_mandatory_baro_count = 1;
-
-bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
-				    vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				    bool report_failures,
-				    const bool safety_button_available, const bool safety_off,
-				    const bool is_arm_attempt)
-{
-	report_failures = (report_failures && !status_flags.calibration_enabled);
-
-	bool failed = false;
-
-	failed = failed || !airframeCheck(mavlink_log_pub, status);
-	failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
-
-	/* ---- MAG ---- */
-	{
-		int32_t sys_has_mag = 1;
-		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
-
-		if (sys_has_mag == 1) {
-			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count,
-							   mavlink_log_pub, status, magnetometerCheck);
-
-			/* mag consistency checks (need to be performed after the individual checks) */
-			if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
-				failed = true;
-			}
-		}
-	}
-
-	/* ---- ACCEL ---- */
-	{
-		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count,
-						   mavlink_log_pub, status, accelerometerCheck);
-
-		// TODO: highest priority (from params)
-	}
-
-	/* ---- GYRO ---- */
-	{
-		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count,
-						   mavlink_log_pub, status, gyroCheck);
-
-		// TODO: highest priority (from params)
-	}
-
-	/* ---- BARO ---- */
-	{
-		int32_t sys_has_baro = 1;
-		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
-
-		if (sys_has_baro == 1) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
-					  mavlink_log_pub, status, baroCheck));
-		}
-	}
-
-	/* ---- IMU CONSISTENCY ---- */
-	// To be performed after the individual sensor checks have completed
-	{
-		if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
-			failed = true;
-		}
-	}
-
-	/* ---- Distance Sensor ---- */
-	{
-		int32_t sys_has_num_dist_sens = 0;
-		param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens);
-
-		if (sys_has_num_dist_sens > 0) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
-					  mavlink_log_pub, status, distSensCheck));
-		}
-
-	}
-
-	/* ---- AIRSPEED ---- */
-	/* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */
-	if (!status_flags.circuit_breaker_engaged_airspd_check &&
-	    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
-
-		int32_t airspeed_mode = 0;
-		param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
-		const bool optional = (airspeed_mode == 1);
-
-		int32_t max_airspeed_check_en = 0;
-		param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
-
-		float airspeed_trim = 10.0f;
-		param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);
-
-		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
-
-		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, is_arm_attempt, (bool)max_airspeed_check_en,
-				   arming_max_airspeed_allowed)
-		    && !(bool)optional) {
-			failed = true;
-		}
-	}
-
-	/* ---- RC CALIBRATION ---- */
-	int32_t com_rc_in_mode{0};
-	param_get(param_find("COM_RC_IN_MODE"), &com_rc_in_mode);
-
-	if (com_rc_in_mode == 0) {
-		if (rcCalibrationCheck(mavlink_log_pub, report_failures) != OK) {
-			if (report_failures) {
-				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
-			}
-
-			failed = true;
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
-			status_flags.rc_calibration_valid = false;
-
-		} else {
-			// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
-			status_flags.rc_calibration_valid = true;
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
-					 !status.rc_signal_lost, status);
-		}
-	}
-
-	/* ---- SYSTEM POWER ---- */
-	if (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
-		if (!powerCheck(mavlink_log_pub, status, report_failures)) {
-			failed = true;
-		}
-	}
-
-	/* ---- Navigation EKF ---- */
-	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
-	int32_t estimator_type = -1;
-
-	if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
-		param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
-
-	} else {
-		// EKF2 is currently the only supported option for FW & VTOL
-		estimator_type = 2;
-	}
-
-	if (estimator_type == 2) {
-		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) &&
-					 ekf2CheckSensorBias(mavlink_log_pub, report_failures);
-
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
-
-		if (control_mode.flag_control_attitude_enabled
-		    || control_mode.flag_control_velocity_enabled
-		    || control_mode.flag_control_position_enabled) {
-			// healthy estimator only required for dependent control modes
-			failed |= !ekf_healthy;
-		}
-	}
-
-	/* ---- Failure Detector ---- */
-	if (!failureDetectorCheck(mavlink_log_pub, status, report_failures)) {
-		failed = true;
-	}
-
-	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
-	failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
-	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
-	failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);
-	failed = failed || !preArmCheck(mavlink_log_pub, status_flags, control_mode,
-					safety_button_available, safety_off, status, report_failures, is_arm_attempt);
-
-	/* Report status */
-	return !failed;
-}
-
-bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure,
-		const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
-		vehicle_status_s &status, sens_check_func_t sens_check)
-{
-	bool pass_check = true;
-	bool report_fail = report_failure;
-
-	/* check all sensors, but fail only for mandatory ones */
-	for (uint8_t i = 0u; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		const bool is_mandatory = i < nb_mandatory_instances;
-
-		if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) {
-			pass_check = false;
-		}
-	}
-
-	return pass_check;
-}

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

@@ -1,109 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 - 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.
- *
- ****************************************************************************/
-
-/**
- * @file PreFlightCheck.hpp
- *
- * Check if flight is safely possible
- * if not prevent it and inform the user.
- */
-
-#pragma once
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status_flags.h>
-#include <uORB/topics/vehicle_status.h>
-#include <drivers/drv_hrt.h>
-#include "../../Safety.hpp"
-
-typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				  const bool is_mandatory, bool &report_fail);
-
-class PreFlightCheck
-{
-public:
-	PreFlightCheck() = default;
-	~PreFlightCheck() = default;
-
-	/**
-	* Runs a preflight check to determine if the system is ready to be armed
-	*
-	* @param mavlink_log_pub
-	*   Mavlink output orb handle reference for feedback when a sensor fails
-	**/
-	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
-				   vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				   bool reportFailures,
-				   const bool safety_button_available, const bool safety_off,
-				   const bool is_arm_attempt = false);
-
-private:
-	static bool sensorAvailabilityCheck(const bool report_failure,
-					    const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
-					    vehicle_status_s &status, sens_check_func_t sens_check);
-	static bool isMagRequired(uint8_t instance);
-	static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				      const bool is_mandatory, bool &report_fail);
-	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
-	static bool isAccelRequired(uint8_t instance);
-	static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				       const bool is_mandatory, bool &report_fail);
-	static bool isGyroRequired(uint8_t instance);
-	static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			      const bool is_mandatory, bool &report_fail);
-	static bool isBaroRequired(uint8_t instance);
-	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			      const bool is_mandatory, bool &report_fail);
-	static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				  const bool is_mandatory, bool &report_fail);
-	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
-	static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				  const bool report_fail, const bool is_arm_attempt, const bool max_airspeed_check_en,
-				  const float arming_max_airspeed_allowed);
-	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail);
-	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
-	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
-	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
-	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
-	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
-	static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
-	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
-	static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
-	static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
-	static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
-				   const vehicle_status_flags_s &status_flags);
-	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
-				vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt);
-};

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

@@ -1,121 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <HealthFlags.h>
-#include <math.h>
-#include <px4_defines.h>
-#include <lib/sensor_calibration/Utilities.hpp>
-#include <lib/systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/sensor_accel.h>
-
-using namespace time_literals;
-
-bool PreFlightCheck::isAccelRequired(const uint8_t instance)
-{
-	uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
-	const uint32_t device_id = static_cast<uint32_t>(accel.get().device_id);
-
-	bool is_used_by_nav = false;
-
-	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
-
-		if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) {
-			is_used_by_nav = true;
-			break;
-		}
-	}
-
-	return is_used_by_nav;
-}
-
-bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-					const bool is_mandatory, bool &report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
-	const bool is_required = is_mandatory || isAccelRequired(instance);
-
-	bool is_valid = false;
-	bool is_calibration_valid = false;
-	bool is_value_valid = false;
-
-	if (exists) {
-		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
-
-		is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0)
-			   && (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
-
-		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			is_calibration_valid = true;
-
-		} else {
-			is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
-		}
-
-		const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
-						    + accel.get().y * accel.get().y
-						    + accel.get().z * accel.get().z);
-
-		if (accel_magnitude > 4.0f && accel_magnitude < 15.0f /* m/s^2 */) {
-			is_value_valid = true;
-		}
-	}
-
-	if (report_fail && is_required) {
-		if (!exists) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
-			report_fail = false;
-
-		} else if (!is_valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
-			report_fail = false;
-
-		} else if (!is_calibration_valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
-			report_fail = false;
-
-		} else if (!is_value_valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
-			report_fail = false;
-		}
-	}
-
-	const bool is_sensor_ok = is_valid && is_calibration_valid && is_value_valid;
-
-	return is_sensor_ok || !is_required;
-}

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

@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <HealthFlags.h>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/airspeed_validated.h>
-
-using namespace time_literals;
-
-bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				   const bool report_fail, const bool is_arm_attempt, const bool max_airspeed_check_en,
-				   const float arming_max_airspeed_allowed)
-{
-	bool present = true;
-	bool success = true;
-
-	uORB::SubscriptionData<airspeed_validated_s> airspeed_validated_sub{ORB_ID(airspeed_validated)};
-	airspeed_validated_sub.update();
-	const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get();
-
-	/*
-	 * Check if Airspeed Selector is up and running.
-	 */
-	if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
-		if (report_fail && !optional) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down.");
-		}
-
-		present = false;
-		success = false;
-		goto out;
-	}
-
-	/*
-	 * Check if airspeed is declared valid or not by airspeed selector.
-	 */
-	if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid.");
-		}
-
-		present = true;
-		success = false;
-		goto out;
-	}
-
-	/*
-	 * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
-	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
-	 * might have been removed.
-	 */
-	if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
-	    && !is_arm_attempt) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot");
-		}
-
-		present = true;
-		success = false;
-		goto out;
-	}
-
-out:
-	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
-
-	return success;
-}

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

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

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

@@ -1,297 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <HealthFlags.h>
-#include <math.h>
-#include <lib/parameters/param.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/estimator_selector_status.h>
-#include <uORB/topics/estimator_sensor_bias.h>
-#include <uORB/topics/estimator_status.h>
-
-using namespace time_literals;
-
-bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail)
-{
-	bool success = true; // start with a pass and change to a fail if any test fails
-
-	int32_t mag_strength_check = 1;
-	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);
-
-	float hgt_test_ratio_limit = 1.f;
-	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
-
-	float vel_test_ratio_limit = 1.f;
-	param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
-
-	float pos_test_ratio_limit = 1.f;
-	param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
-
-	float mag_test_ratio_limit = 1.f;
-	param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
-
-	int32_t arm_without_gps = 0;
-	param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
-
-	int32_t sys_has_gps = 1;
-	param_get(param_find("SYS_HAS_GPS"), &sys_has_gps);
-
-	bool gps_success = false;
-	bool gps_present = false;
-
-	// Get estimator status data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
-	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
-	const estimator_status_s &status = status_sub.get();
-
-	if (status.timestamp == 0) {
-		success = false;
-		goto out;
-	}
-
-	// Check if preflight check performed by estimator has failed
-	if (status.pre_flt_fail_innov_heading ||
-	    status.pre_flt_fail_innov_vel_horiz ||
-	    status.pre_flt_fail_innov_vel_vert ||
-	    status.pre_flt_fail_innov_height) {
-		if (report_fail) {
-			if (status.pre_flt_fail_innov_heading) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_vel_horiz) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_vel_vert) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
-
-			} else if (status.pre_flt_fail_innov_height) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
-			}
-		}
-
-		success = false;
-		goto out;
-	}
-
-	if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
-		const char *message = "Preflight%s: Strong magnetic interference detected";
-
-		if (mag_strength_check == 1) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, message, " Fail");
-			}
-
-			success = false;
-			goto out;
-
-		} else if (report_fail) {
-			mavlink_log_warning(mavlink_log_pub, message, "");
-		}
-	}
-
-	// check vertical position innovation test ratio
-	if (status.hgt_test_ratio > hgt_test_ratio_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check velocity innovation test ratio
-	if (status.vel_test_ratio > vel_test_ratio_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check horizontal position innovation test ratio
-	if (status.pos_test_ratio > pos_test_ratio_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// check magnetometer innovation test ratio
-	if (status.mag_test_ratio > mag_test_ratio_limit) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
-		}
-
-		success = false;
-		goto out;
-	}
-
-	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (sys_has_gps == 1) {
-		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
-		const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
-
-		gps_present = true;
-		gps_success = ekf_gps_fusion; // default to success if gps data is fused
-
-		if (ekf_gps_check_fail) {
-			if (report_fail) {
-				// Only report the first failure to avoid spamming
-				const char *message = nullptr;
-
-				if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
-					message = "Preflight%s: GPS fix too low";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
-					message = "Preflight%s: not enough GPS Satellites";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
-					message = "Preflight%s: GPS PDOP too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
-					message = "Preflight%s: GPS Horizontal Pos Error too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
-					message = "Preflight%s: GPS Vertical Pos Error too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
-					message = "Preflight%s: GPS Speed Accuracy too low";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
-					message = "Preflight%s: GPS Horizontal Pos Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
-					message = "Preflight%s: GPS Vertical Pos Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
-					message = "Preflight%s: GPS Hor Speed Drift too high";
-
-				} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
-					message = "Preflight%s: GPS Vert Speed Drift too high";
-
-				} else {
-					if (!ekf_gps_fusion) {
-						// Likely cause unknown
-						message = "Preflight%s: Estimator not using GPS";
-						gps_present = false;
-
-					} else {
-						// if we land here there was a new flag added and the code not updated. Show a generic message.
-						message = "Preflight%s: Poor GPS Quality";
-					}
-				}
-
-				if (message) {
-					if (!arm_without_gps) {
-						mavlink_log_critical(mavlink_log_pub, message, " Fail");
-
-					} else {
-						mavlink_log_warning(mavlink_log_pub, message, "");
-					}
-				}
-			}
-
-			gps_success = false;
-
-			if (!arm_without_gps) {
-				success = false;
-				goto out;
-			}
-		}
-	}
-
-out:
-	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status);
-	return success;
-}
-
-bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail)
-{
-	// Get estimator states data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
-	uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
-	const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
-
-	if (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 (uint8_t 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) {
-					if (report_fail) {
-						PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
-							(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
-						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
-					}
-
-					return false;
-				}
-			}
-		}
-
-		// check gyro bias estimates
-		if (bias.gyro_bias_valid) {
-			const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
-
-			for (uint8_t 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) {
-					if (report_fail) {
-						PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
-							(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
-						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
-					}
-
-					return false;
-				}
-			}
-		}
-	}
-
-	return true;
-}

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

@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <systemlib/mavlink_log.h>
-
-bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
-		const bool report_fail)
-{
-	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
-		if (report_fail) {
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
-			}
-		}
-
-		return false;
-	}
-
-	return true;
-}

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

@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <HealthFlags.h>
-#include <px4_defines.h>
-#include <lib/sensor_calibration/Utilities.hpp>
-#include <lib/systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/sensor_gyro.h>
-
-using namespace time_literals;
-
-bool PreFlightCheck::isGyroRequired(const uint8_t instance)
-{
-	uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
-	const uint32_t device_id = static_cast<uint32_t>(gyro.get().device_id);
-
-	bool is_used_by_nav = false;
-
-	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
-
-		if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) {
-			is_used_by_nav = true;
-			break;
-		}
-	}
-
-	return is_used_by_nav;
-}
-
-bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			       const bool is_mandatory, bool &report_fail)
-{
-	const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
-	const bool is_required = is_mandatory || isGyroRequired(instance);
-
-	bool is_valid = false;
-	bool is_calibration_valid = false;
-
-	if (exists) {
-		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
-
-		is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0)
-			   && (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
-
-		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			is_calibration_valid = true;
-
-		} else {
-			is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
-		}
-	}
-
-	if (report_fail && is_required) {
-		if (!exists) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance);
-			report_fail = false;
-
-		} else if (!is_valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
-			report_fail = false;
-
-		} else if (!is_calibration_valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
-			report_fail = false;
-		}
-	}
-
-	const bool is_sensor_ok = is_calibration_valid && is_valid;
-
-	return is_sensor_ok || !is_required;
-}

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

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

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

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

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

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

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

@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   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 "../PreFlightCheck.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <systemlib/mavlink_log.h>
-#include <lib/parameters/param.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/system_power.h>
-#include <math.h>
-
-using namespace time_literals;
-
-bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail)
-{
-	bool success = true;
-
-	if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-		// Ignore power check in HITL.
-		return true;
-	}
-
-	uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
-	system_power_sub.update();
-	const system_power_s &system_power = system_power_sub.get();
-
-	if (system_power.timestamp != 0) {
-		int32_t required_power_module_count = 0;
-		param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
-
-		// Check avionics rail voltages (if USB isn't connected)
-		if (!system_power.usb_connected) {
-			float avionics_power_rail_voltage = system_power.voltage5v_v;
-
-			if (avionics_power_rail_voltage < 4.5f) {
-				success = false;
-
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
-							     (double)avionics_power_rail_voltage);
-				}
-
-			} else if (avionics_power_rail_voltage < 4.8f) {
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
-				}
-
-			} else if (avionics_power_rail_voltage > 5.4f) {
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
-				}
-			}
-
-
-			const int power_module_count = math::countSetBits(system_power.brick_valid);
-
-			if (power_module_count < required_power_module_count) {
-				success = false;
-
-				if (report_fail) {
-					mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %" PRId32,
-							     power_module_count, required_power_module_count);
-				}
-			}
-		}
-
-	} else {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "system power unavailable");
-		}
-
-		success = false;
-	}
-
-	return success;
-}

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

@@ -1,250 +0,0 @@
-/****************************************************************************
- *
- *   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 "../PreFlightCheck.hpp"
-
-#include <ArmAuthorization.h>
-#include <HealthFlags.h>
-#include <lib/parameters/param.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/topics/vehicle_command_ack.h>
-
-bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				 const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
-				 vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt)
-{
-	bool prearm_ok = true;
-
-	// rate control mode require valid angular velocity
-	if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Angular velocity invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// attitude control mode require valid attitude
-	if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Attitude invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// velocity control mode require valid velocity
-	if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Velocity invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// position control mode require valid position
-	if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Position invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// manual control mode require valid manual control (rc)
-	if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Manual control unavailable"); }
-
-		prearm_ok = false;
-	}
-
-	if (status_flags.flight_terminated) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Flight termination active"); }
-
-		prearm_ok = false;
-	}
-
-	// USB not connected
-	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Flying with USB is not safe"); }
-
-		prearm_ok = false;
-	}
-
-	// battery and system power status
-	if (!status_flags.circuit_breaker_engaged_power_check) {
-
-		// Fail transition if power is not good
-		if (!status_flags.power_input_valid) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Connect power module"); }
-
-			prearm_ok = false;
-		}
-
-		// main battery level
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
-				 status_flags.battery_healthy, status);
-
-		// Only arm if healthy
-		if (!status_flags.battery_healthy) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Check battery"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// Arm Requirements: mission
-	int32_t _param_com_arm_mis_req = 0;
-	param_get(param_find("COM_ARM_MIS_REQ"), &_param_com_arm_mis_req);
-	const bool mission_required = (_param_com_arm_mis_req == 1);
-
-	if (mission_required) {
-		if (!status_flags.auto_mission_available) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "No valid mission"); }
-			}
-
-			prearm_ok = false;
-		}
-
-		if (!status_flags.global_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Missions require a global position"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	int32_t _param_com_arm_wo_gps = 1;
-	param_get(param_find("COM_ARM_WO_GPS"), &_param_com_arm_wo_gps);
-	const bool global_position_required = (_param_com_arm_wo_gps == 0);
-
-	if (global_position_required) {
-		if (!status_flags.global_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Global position required"); }
-			}
-
-			prearm_ok = false;
-		}
-
-		if (!status_flags.home_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Home position invalid"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// safety button
-	if (safety_button_available && !safety_off) {
-		// Fail transition if we need safety button press
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Press safety button first"); }
-		}
-
-		prearm_ok = false;
-	}
-
-	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Avoidance system not ready"); }
-		}
-
-		prearm_ok = false;
-
-	}
-
-	int32_t _param_com_arm_chk_escs = 1;
-	param_get(param_find("COM_ARM_CHK_ESCS"), &_param_com_arm_chk_escs);
-	const bool esc_checks_required = (_param_com_arm_chk_escs == 0);
-
-	if (esc_checks_required && status_flags.escs_error) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs are offline"); }
-
-			prearm_ok = false;
-		}
-	}
-
-	if (esc_checks_required && status_flags.escs_failure) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs have a failure"); }
-
-			prearm_ok = false;
-		}
-	}
-
-	if (status.is_vtol) {
-		if (status.in_transition_mode) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is in transition state"); }
-
-				prearm_ok = false;
-			}
-		}
-
-		if (!status_flags.circuit_breaker_vtol_fw_arming_check
-		    && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is not in multicopter mode"); }
-
-				prearm_ok = false;
-			}
-		}
-	}
-
-	int32_t _param_gf_action = 0;
-	param_get(param_find("GF_ACTION"), &_param_gf_action);
-	const bool gefence_action_configured = (_param_gf_action != 0);
-
-	if (gefence_action_configured && status.geofence_violated) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Vehicle outside geofence");
-		}
-
-		prearm_ok = false;
-	}
-
-	int32_t _param_com_arm_auth_req = 0;
-	param_get(param_find("COM_ARM_AUTH_REQ"), &_param_com_arm_auth_req);
-	const bool arm_authorization_configured = (_param_com_arm_auth_req != 0);
-
-	// Arm Requirements: authorization
-	// check last, and only if everything else has passed
-	// skip arm authorization check until actual arming attempt
-	if (arm_authorization_configured && prearm_ok && is_arm_attempt) {
-		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
-			// feedback provided in arm_auth_check
-			prearm_ok = false;
-		}
-	}
-
-	return prearm_ok;
-}

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

@@ -1,212 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "../PreFlightCheck.hpp"
-
-#include <parameters/param.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/topics/input_rc.h>
-
-/**
- * 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
-
-int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
-{
-	char nbuf[20];
-	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
-		_parameter_handles_rev, _parameter_handles_dz;
-
-	unsigned map_fail_count = 0;
-
-	const char *rc_map_mandatory[] = {	/*"RC_MAP_MODE_SW",*/
-		/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
-		nullptr /* end marker */
-	};
-
-	unsigned j = 0;
-
-	/* first check channel mappings */
-	while (rc_map_mandatory[j] != nullptr) {
-
-		param_t map_parm = param_find(rc_map_mandatory[j]);
-
-		if (map_parm == PARAM_INVALID) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-			map_fail_count++;
-			j++;
-			continue;
-		}
-
-		int32_t mapping;
-		param_get(map_parm, &mapping);
-
-		if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-			map_fail_count++;
-		}
-
-		if (mapping == 0) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-			map_fail_count++;
-		}
-
-		j++;
-	}
-
-	unsigned total_fail_count = 0;
-	unsigned channels_failed = 0;
-
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		/* should the channel be enabled? */
-		uint8_t count = 0;
-
-		/* 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;
-
-		/* min values */
-		sprintf(nbuf, "RC%d_MIN", i + 1);
-		_parameter_handles_min = param_find(nbuf);
-		param_get(_parameter_handles_min, &param_min);
-
-		/* trim values */
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
-		_parameter_handles_trim = param_find(nbuf);
-		param_get(_parameter_handles_trim, &param_trim);
-
-		/* max values */
-		sprintf(nbuf, "RC%d_MAX", i + 1);
-		_parameter_handles_max = param_find(nbuf);
-		param_get(_parameter_handles_max, &param_max);
-
-		/* channel reverse */
-		sprintf(nbuf, "RC%d_REV", i + 1);
-		_parameter_handles_rev = param_find(nbuf);
-		param_get(_parameter_handles_rev, &param_rev);
-
-		/* channel deadzone */
-		sprintf(nbuf, "RC%d_DZ", i + 1);
-		_parameter_handles_dz = param_find(nbuf);
-		param_get(_parameter_handles_dz, &param_dz);
-
-		/* assert min..center..max ordering */
-		if (param_min < RC_INPUT_LOWEST_MIN_US) {
-			count++;
-
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-		}
-
-		if (param_max > RC_INPUT_HIGHEST_MAX_US) {
-			count++;
-
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-		}
-
-		if (param_trim < param_min) {
-			count++;
-
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-		}
-
-		if (param_trim > param_max) {
-			count++;
-
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-		}
-
-		/* assert deadzone is sane */
-		if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); }
-
-			/* give system time to flush error message in case there are more */
-			px4_usleep(100000);
-			count++;
-		}
-
-		total_fail_count += count;
-
-		if (count) {
-			channels_failed++;
-		}
-	}
-
-	if (channels_failed) {
-		px4_sleep(2);
-
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
-					     total_fail_count,
-					     (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
-		}
-
-		px4_usleep(100000);
-	}
-
-	return total_fail_count + map_fail_count;
-}

src/modules/commander/CMakeLists.txt

@@ -34,6 +34,7 @@
 add_subdirectory(failure_detector)
 add_subdirectory(HealthAndArmingChecks)
 add_subdirectory(Arming)
+add_subdirectory(ModeUtil)
 
 px4_add_module(
 	MODULE modules__commander
@@ -62,12 +63,12 @@ px4_add_module(
 		geo
 		health_and_arming_checks
 		hysteresis
-		PreFlightCheck
 		ArmAuthorization
 		ArmStateMachine
 		HealthFlags
 		sensor_calibration
 		world_magnetic_model
+		mode_util
 	)
 
 if(PX4_TESTING)

src/modules/commander/Commander.cpp

@@ -45,7 +45,6 @@
 #include "Commander.hpp"
 
 /* commander module headers */
-#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "Arming/ArmAuthorization/ArmAuthorization.h"
 #include "Arming/HealthFlags/HealthFlags.h"
 #include "commander_helper.h"
@@ -300,26 +299,13 @@ int Commander::custom_command(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[0], "check")) {
-		uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
+		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
-		vehicle_status_s vehicle_status{};
+		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS);
-		vehicle_status_sub.copy(&vehicle_status);
 
-		uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
+		uORB::SubscriptionData<vehicle_status_flags_s> vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
-		vehicle_status_flags_s vehicle_status_flags{};
+		PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
-		vehicle_status_flags_sub.copy(&vehicle_status_flags);
 
-		uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+		print_health_flags(vehicle_status_sub.get());
-		vehicle_control_mode_s vehicle_control_mode{};
-		vehicle_control_mode_sub.copy(&vehicle_control_mode);
-
-		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
-					   vehicle_control_mode,
-					   true, // report_failures
-					   false, // safety_buttton_available not known
-					   false); // safety_off not known
-		PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
-
-		print_health_flags(vehicle_status);
 
 		return 0;
 	}
@@ -511,11 +497,9 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 
 bool Commander::shutdown_if_allowed()
 {
-	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
-			_safety.isButtonAvailable(), _safety.isSafetyOff(),
+			vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
+			false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
-			_actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
-			arm_disarm_reason_t::shutdown);
 }
 
 static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
@@ -764,11 +748,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		}
 	}
 
-	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
-					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
+					 vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
-					 vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks,
+					 &_mavlink_log_pub, calling_reason);
-					 &_mavlink_log_pub, _vehicle_status_flags,
-					 calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
@@ -808,11 +790,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		}
 	}
 
-	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
-					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
+					 vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
-					 vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false,
+					 &_mavlink_log_pub, calling_reason);
-					 &_mavlink_log_pub, _vehicle_status_flags,
-					 calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
@@ -834,7 +814,8 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 
 Commander::Commander() :
 	ModuleParams(nullptr),
-	_failure_detector(this)
+	_failure_detector(this),
+	_health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status)
 {
 	_vehicle_land_detected.landed = true;
 
@@ -854,7 +835,7 @@ Commander::Commander() :
 
 	_vehicle_status_flags.offboard_control_signal_lost = true;
 
-	_vehicle_status_flags.power_input_valid = true;
+	_vehicle_status.power_input_valid = true;
 
 	// default for vtol is rotary wing
 	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
@@ -868,12 +849,6 @@ Commander::Commander() :
 	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
 
 	updateParameters();
-
-	// run preflight immediately to find all relevant parameters, but don't report
-	PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
-				       false, // report_failures
-				       false, // safety_buttton_available not known
-				       false); // safety_off not known
 }
 
 Commander::~Commander()
@@ -1288,7 +1263,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 
 			// check if current mission and first item are valid
-			if (_vehicle_status_flags.auto_mission_available) {
+			if (_vehicle_status.auto_mission_available) {
 
 				// requested first mission item valid
 				if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
@@ -1422,10 +1397,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			} else {
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+				if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status,
-						_safety.isButtonAvailable(), _safety.isSafetyOff(),
+						vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks,
-						vehicle_status_s::ARMING_STATE_INIT, _actuator_armed,
+						false /* fRunPreArmChecks */, &_mavlink_log_pub,
-						false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
 						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
 				   ) {
 
@@ -2124,7 +2098,7 @@ void Commander::updateParameters()
 	}
 
 
-	_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
+	_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
 
 	_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
 	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
@@ -2149,19 +2123,6 @@ void Commander::updateParameters()
 	_vehicle_status.is_vtol = is_vtol(_vehicle_status);
 	_vehicle_status.is_vtol_tailsitter = is_vtol_tailsitter(_vehicle_status);
 
-	// CP_DIST: collision preventation enabled if CP_DIST > 0
-	if (is_rotary_wing(_vehicle_status) || is_vtol(_vehicle_status)) {
-		if (_param_cp_dist == PARAM_INVALID) {
-			_param_cp_dist = param_find("CP_DIST");
-		}
-
-		float cp_dist = 0;
-
-		if (_param_cp_dist != PARAM_INVALID && (param_get(_param_cp_dist, &cp_dist) == PX4_OK)) {
-			_collision_prevention_enabled = cp_dist > 0.f;
-		}
-	}
-
 	// _mode_switch_mapped = (RC_MAP_FLTMODE > 0)
 	if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) {
 		_mode_switch_mapped = (value_int32 > 0);
@@ -2230,7 +2191,7 @@ Commander::run()
 		}
 
 		/* Update OA parameter */
-		_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
+		_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
 
 #if defined(BOARD_HAS_POWER_CONTROL)
 
@@ -2260,10 +2221,10 @@ Commander::run()
 				    !system_power.brick_valid &&
 				    !system_power.usb_connected) {
 					/* flying only on servo rail, this is unsafe */
-					_vehicle_status_flags.power_input_valid = false;
+					_vehicle_status.power_input_valid = false;
 
 				} else {
-					_vehicle_status_flags.power_input_valid = true;
+					_vehicle_status.power_input_valid = true;
 				}
 
 				_system_power_usb_connected = system_power.usb_connected;
@@ -2476,11 +2437,9 @@ Commander::run()
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
 
-			_arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+			_arm_state_machine.arming_state_transition(_vehicle_status,
-					_safety.isButtonAvailable(), _safety.isSafetyOff(),
+					vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
-					vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed,
+					true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby);
-					true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
-					arm_disarm_reason_t::transition_to_standby);
 		}
 
 		/* start mission result check */
@@ -2494,7 +2453,7 @@ Commander::run()
 			const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
 						       && (mission_result.instance_count > 0);
 
-			_vehicle_status_flags.auto_mission_available = mission_result_ok && mission_result.valid;
+			_vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid;
 
 			if (mission_result_ok) {
 				if (_vehicle_status.mission_failure != mission_result.failure) {
@@ -2513,7 +2472,7 @@ Commander::run()
 				if (_vehicle_status_flags.home_position_valid &&
 				    (prev_mission_instance_count != mission_result.instance_count)) {
 
-					if (!_vehicle_status_flags.auto_mission_available) {
+					if (!_vehicle_status.auto_mission_available) {
 						/* the mission is invalid */
 						tune_mission_fail(true);
 
@@ -2535,7 +2494,7 @@ Commander::run()
 			    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
 			    && mission_result.finished) {
 
-				if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status_flags.auto_mission_available) {
+				if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) {
 					main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags,
 							      _commander_state);
 
@@ -2658,7 +2617,7 @@ Commander::run()
 
 		/* Check for mission flight termination */
 		if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination &&
-		    !_vehicle_status_flags.circuit_breaker_flight_termination_disabled) {
+		    !_vehicle_status.circuit_breaker_flight_termination_disabled) {
 
 
 			if (!_flight_termination_triggered && !_lockdown_triggered) {
@@ -2770,7 +2729,7 @@ Commander::run()
 						events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning},
 							     "Critical failure detected: lockdown");
 
-					} else if (!_vehicle_status_flags.circuit_breaker_flight_termination_disabled &&
+					} else if (!_vehicle_status.circuit_breaker_flight_termination_disabled &&
 						   !_flight_termination_triggered && !_lockdown_triggered) {
 
 						_actuator_armed.force_failsafe = true;
@@ -2868,8 +2827,6 @@ Commander::run()
 			checkWindSpeedThresholds();
 		}
 
-		_vehicle_status_flags.flight_terminated = _actuator_armed.force_failsafe || _actuator_armed.manual_lockdown;
-
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
@@ -3013,14 +2970,12 @@ Commander::run()
 		if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_changed
 		    || !(_actuator_armed == actuator_armed_prev)) {
 
-			// Evaluate current prearm status (skip during arm -> disarm transition)
+			// Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already)
-			if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
+			if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) {
 				perf_begin(_preflight_check_perf);
-				_vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status,
+				_health_and_arming_checks.update();
-						_vehicle_status_flags,
+				_vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm(
-						_vehicle_control_mode,
+							_vehicle_status.nav_state);
-						false, // report_failures
-						_safety.isButtonAvailable(), _safety.isSafetyOff());
 				perf_end(_preflight_check_perf);
 
 				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true,
@@ -3072,7 +3027,7 @@ Commander::run()
 			set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
 			_arm_tune_played = true;
 
-		} else if (!_vehicle_status_flags.usb_connected &&
+		} else if (!_vehicle_status.usb_connected &&
 			   (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
 			   (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
 			/* play tune on battery critical */
@@ -3151,17 +3106,17 @@ Commander::run()
 void
 Commander::get_circuit_breaker_params()
 {
-	_vehicle_status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
+	_vehicle_status.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
 			CBRK_SUPPLY_CHK_KEY);
-	_vehicle_status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
+	_vehicle_status.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
 			CBRK_USB_CHK_KEY);
-	_vehicle_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(
+	_vehicle_status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(
 				_param_cbrk_airspd_chk.get(),
 				CBRK_AIRSPD_CHK_KEY);
-	_vehicle_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(
+	_vehicle_status.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(
 				_param_cbrk_flightterm.get(),
 				CBRK_FLIGHTTERM_KEY);
-	_vehicle_status_flags.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(
+	_vehicle_status.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(
 				_param_cbrk_vtolarming.get(),
 				CBRK_VTOLARMING_KEY);
 }
@@ -3604,7 +3559,7 @@ void Commander::data_link_check()
 			switch (telemetry.type) {
 			case telemetry_status_s::LINK_TYPE_USB:
 				// set (but don't unset) telemetry via USB as active once a MAVLink connection is up
-				_vehicle_status_flags.usb_connected = true;
+				_vehicle_status.usb_connected = true;
 				break;
 
 			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
@@ -3641,13 +3596,6 @@ void Commander::data_link_check()
 						mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
 						events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
 					}
-
-					if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
-						// make sure to report preflight check failures to a connecting GCS
-						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
-									       true, // report_failures
-									       _safety.isButtonAvailable(), _safety.isSafetyOff());
-					}
 				}
 
 				_datalink_last_heartbeat_gcs = telemetry.timestamp;
@@ -3680,8 +3628,8 @@ void Commander::data_link_check()
 				bool healthy = telemetry.parachute_system_healthy;
 
 				_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
-				_vehicle_status_flags.parachute_system_present = true;
+				_vehicle_status.parachute_system_present = true;
-				_vehicle_status_flags.parachute_system_healthy = healthy;
+				_vehicle_status.parachute_system_healthy = healthy;
 				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _vehicle_status);
 			}
 
@@ -3692,7 +3640,7 @@ void Commander::data_link_check()
 				}
 
 				_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
-				_vehicle_status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
+				_vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy;
 			}
 		}
 	}
@@ -3729,21 +3677,21 @@ void Commander::data_link_check()
 	if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
 	    && !_parachute_system_lost) {
 		mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost");
-		_vehicle_status_flags.parachute_system_present = false;
+		_vehicle_status.parachute_system_present = false;
-		_vehicle_status_flags.parachute_system_healthy = false;
+		_vehicle_status.parachute_system_healthy = false;
 		_parachute_system_lost = true;
 		_status_changed = true;
 		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _vehicle_status);
 	}
 
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
-	if (_vehicle_status_flags.avoidance_system_required && !_onboard_controller_lost) {
+	if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) {
 		// if heartbeats stop
 		if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
 		    && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
 
 			_avoidance_system_lost = true;
-			_vehicle_status_flags.avoidance_system_valid = false;
+			_vehicle_status.avoidance_system_valid = false;
 		}
 	}
 
@@ -3776,18 +3724,18 @@ void Commander::avoidance_check()
 	}
 
 	const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
-	const bool cp_healthy = _vehicle_status_flags.avoidance_system_valid || distance_sensor_valid;
+	const bool cp_healthy = _vehicle_status.avoidance_system_valid || distance_sensor_valid;
 
-	const bool sensor_oa_present = cp_healthy || _vehicle_status_flags.avoidance_system_required
+	const bool sensor_oa_present = cp_healthy || _vehicle_status.avoidance_system_required
 				       || _collision_prevention_enabled;
 
 	const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
 	const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
 				   && _vehicle_control_mode.flag_control_position_enabled);
 
-	const bool sensor_oa_enabled = ((auto_mode && _vehicle_status_flags.avoidance_system_required) || (pos_ctl_mode
+	const bool sensor_oa_enabled = ((auto_mode && _vehicle_status.avoidance_system_required) || (pos_ctl_mode
 					&& _collision_prevention_enabled));
-	const bool sensor_oa_healthy = ((auto_mode && _vehicle_status_flags.avoidance_system_valid) || (pos_ctl_mode
+	const bool sensor_oa_healthy = ((auto_mode && _vehicle_status.avoidance_system_valid) || (pos_ctl_mode
 					&& cp_healthy));
 
 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
@@ -3939,7 +3887,7 @@ void Commander::battery_status_check()
 		_battery_warning = worst_warning;
 	}
 
-	_vehicle_status_flags.battery_healthy =
+	_vehicle_status.battery_healthy =
 		// All connected batteries are regularly being published
 		(hrt_elapsed_time(&oldest_update) < 5_s)
 		// There is at least one connected battery (in any slot)
@@ -4323,7 +4271,7 @@ void Commander::manual_control_check()
 
 		} else {
 			// if not mavlink also report valid RC calibration for health
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _vehicle_status_flags.rc_calibration_valid,
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true,
 					 _vehicle_status);
 		}
 

src/modules/commander/Commander.hpp

@@ -35,11 +35,11 @@
 
 /*   Helper classes  */
 #include "Arming/ArmStateMachine/ArmStateMachine.hpp"
-#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
 #include "Safety.hpp"
 #include "state_machine_helper.h"
 #include "worker_thread.hpp"
+#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 
 #include <containers/Bitset.hpp>
 #include <lib/controllib/blocks.hpp>
@@ -264,7 +264,6 @@ private:
 	)
 
 	// optional parameters
-	param_t _param_cp_dist{PARAM_INVALID};
 	param_t _param_mav_comp_id{PARAM_INVALID};
 	param_t _param_mav_sys_id{PARAM_INVALID};
 	param_t _param_mav_type{PARAM_INVALID};
@@ -317,7 +316,6 @@ private:
 	bool		_geofence_warning_action_on{false};
 	bool		_geofence_violated_prev{false};
 
-	bool            _collision_prevention_enabled{false};
 
 	bool		_rtl_time_actions_done{false};
 
@@ -425,7 +423,6 @@ private:
 	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
-	uORB::SubscriptionMultiArray<distance_sensor_s>         _distance_sensor_subs{ORB_ID::distance_sensor};
 	uORB::SubscriptionMultiArray<telemetry_status_s>        _telemetry_status_subs{ORB_ID::telemetry_status};
 
 #if defined(BOARD_HAS_POWER_CONTROL)
@@ -456,4 +453,5 @@ private:
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
+	HealthAndArmingChecks _health_and_arming_checks;
 };

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,38 +31,28 @@
  *
  ****************************************************************************/
 
-#include "../PreFlightCheck.hpp"
+#pragma once
 
-#include <lib/parameters/param.h>
+#include "../Common.hpp"
-#include <lib/systemlib/mavlink_log.h>
 
-using namespace time_literals;
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed_validated.h>
 
-bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+class AirspeedChecks : public HealthAndArmingCheckBase
-				    const vehicle_status_flags_s &status_flags)
 {
-	bool success = true;
+public:
+	AirspeedChecks();
+	~AirspeedChecks() = default;
 
-	int32_t param_com_parachute = false;
+	void checkAndReport(const Context &context, Report &reporter) override;
-	param_get(param_find("COM_PARACHUTE"), &param_com_parachute);
-	const bool parachute_required = param_com_parachute != 0;
 
-	if (parachute_required) {
+private:
-		if (!status_flags.parachute_system_present) {
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
-			success = false;
 
-			if (report_fail) {
+	const param_t _param_fw_arsp_mode_handle;
-				mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing");
+	const param_t _param_fw_airspd_trim_handle;
-			}
 
-		} else if (!status_flags.parachute_system_healthy) {
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-			success = false;
+					(ParamBool<px4::params::COM_ARM_ARSP_EN>) _param_com_arm_arsp_en
-
+				       )
-			if (report_fail) {
+};
-				mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system not ready");
-			}
-		}
-	}
-
-	return success;
-}

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

@@ -31,39 +31,31 @@
  *
  ****************************************************************************/
 
-#include "../PreFlightCheck.hpp"
+#pragma once
 
-#include <systemlib/mavlink_log.h>
+#include "../Common.hpp"
 
-using namespace time_literals;
+#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>
 
-bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+class BaroChecks : public HealthAndArmingCheckBase
-			       const vehicle_status_s &vehicle_status)
 {
-	bool success = true;
+public:
+	BaroChecks() = default;
+	~BaroChecks() = default;
 
-	switch (vehicle_status.nav_state) {
+	void checkAndReport(const Context &context, Report &reporter) override;
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
-	case vehicle_status_s::NAVIGATION_STATE_ACRO:
-	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-	case vehicle_status_s::NAVIGATION_STATE_STAB:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
-		break;
 
-	default:
+private:
-		success = false;
+	bool isBaroRequired(int instance);
 
-		if (report_fail) {
+	uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
-			mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff");
+	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
-		}
 
-		break;
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-	}
+					(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
-
+				       )
-	return success;
+};
-}

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,28 +31,24 @@
  *
  ****************************************************************************/
 
-#include "../PreFlightCheck.hpp"
+#include "batteryCheck.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
 
 using namespace time_literals;
 
-bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status)
+void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	bool success = true;
+	if (!context.status().battery_healthy) {
 
-#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2
+		/* EVENT
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
+				       events::Log::Error, "Battery unhealthy");
 
-	// We no longer support VTOL on fmu-v2, so we need to warn existing users.
+		if (reporter.mavlink_log_pub()) {
-	if (status.is_vtol) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Battery unhealthy");
-		mavlink_log_critical(mavlink_log_pub,
+		}
-				     "VTOL is not supported with fmu-v2, see docs.px4.io/main/en/config/firmware.html#bootloader");
-		success = false;
 	}
 
-#endif
+	reporter.setIsPresent(health_component_t::battery); // assume it's present
 
-	return success;
 }

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,46 +31,25 @@
  *
  ****************************************************************************/
 
-#include "../PreFlightCheck.hpp"
+#pragma once
+
+#include "../Common.hpp"
 
-#include <drivers/drv_hrt.h>
-#include <systemlib/mavlink_log.h>
-#include <lib/parameters/param.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/cpuload.h>
 
-using namespace time_literals;
+class CpuResourceChecks : public HealthAndArmingCheckBase
-
-bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
 {
-	bool success = true;
+public:
+	CpuResourceChecks() = default;
+	~CpuResourceChecks() = default;
 
-	uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
+	void checkAndReport(const Context &context, Report &reporter) override;
-	cpuload_sub.update();
 
-	float cpuload_percent_max;
+private:
-	param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max);
+	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
 
-	if (cpuload_percent_max > 0.f) {
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
-		if (hrt_elapsed_time(&cpuload_sub.get().timestamp) > 2_s) {
+				       )
-			success = false;
+};
-
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Fail: No CPU load information");
-			}
-		}
-
-		const float cpuload_percent = cpuload_sub.get().load * 100.f;
-
-		if (cpuload_percent > cpuload_percent_max) {
-			success = false;
-
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
-			}
-		}
-	}
-
-	return success;
-}

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

@@ -31,41 +31,26 @@
  *
  ****************************************************************************/
 
-#include "../PreFlightCheck.hpp"
+#pragma once
+
+#include "../Common.hpp"
 
-#include <drivers/drv_hrt.h>
-#include <px4_defines.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/uORB.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/distance_sensor.h>
 
-using namespace time_literals;
+class DistanceSensorChecks : public HealthAndArmingCheckBase
-
-bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				   const bool is_mandatory, bool &report_fail)
 {
-	const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
+public:
-	bool valid = false;
+	DistanceSensorChecks() = default;
+	~DistanceSensorChecks() = default;
 
-	if (exists) {
+	void checkAndReport(const Context &context, Report &reporter) override;
-		uORB::SubscriptionData<distance_sensor_s> dist_sens_sub{ORB_ID(distance_sensor), instance};
-		dist_sens_sub.update();
-		const distance_sensor_s &dist_sens_data = dist_sens_sub.get();
 
-		valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
+private:
-	}
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_sub{ORB_ID::distance_sensor};
 
-	if (report_fail && is_mandatory) {
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-		if (!exists) {
+					(ParamInt<px4::params::SYS_HAS_NUM_DIST>) _param_sys_has_num_dist
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance);
+				       )
-			report_fail = false;
+};
-
-		} else if (!valid) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
-			report_fail = false;
-		}
-	}
-
-	return valid || !is_mandatory;
-}

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
+				       )
+};