commander preflight checks pass status and status_flags messages

src/modules/commander/PreflightCheck.cpp

@@ -109,6 +109,7 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
 				break;
 			}
 		}
+
 		instance++;
 	}
 
@@ -140,6 +141,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		}
+
 		success = false;
 		goto out;
 	}
@@ -150,6 +152,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
 		}
+
 		success = false;
 		goto out;
 	}
@@ -175,6 +178,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 	// Use the difference between IMU's to detect a bad calibration.
 	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
 	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
+
 	if (sensors.accel_inconsistency_m_s_s > test_limit) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
@@ -191,10 +195,12 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 
 	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
 	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
+
 	if (sensors.gyro_inconsistency_rad_s > test_limit) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
 		}
+
 		success = false;
 		goto out;
 
@@ -215,27 +221,32 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 	// get the sensor preflight data
 	int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
 	struct sensor_preflight_s sensors = {};
+
 	if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
 		// can happen if not advertised (yet)
 		return true;
 	}
+
 	orb_unsubscribe(sensors_sub);
 
 	// 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.
 	float test_limit;
 	param_get(param_find("COM_ARM_MAG"), &test_limit);
+
 	if (sensors.mag_inconsistency_ga > test_limit) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
 		}
+
 		return false;
 	}
 
 	return true;
 }
 
-static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
+static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic,
+			       int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -260,6 +271,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		}
+
 		success = false;
 		goto out;
 	}
@@ -270,11 +282,13 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
 		}
+
 		success = false;
 		goto out;
 	}
 
 #ifdef __PX4_NUTTX
+
 	if (dynamic) {
 		/* check measurement result range */
 		struct accel_report acc;
@@ -288,19 +302,23 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
 				if (report_fail) {
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
 				}
+
 				/* this is frickin' fatal */
 				success = false;
 				goto out;
 			}
+
 		} else {
 			if (report_fail) {
 				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
 			}
+
 			/* this is frickin' fatal */
 			success = false;
 			goto out;
 		}
 	}
+
 #endif
 
 out:
@@ -333,6 +351,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		}
+
 		success = false;
 		goto out;
 	}
@@ -343,6 +362,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
 		}
+
 		success = false;
 		goto out;
 	}
@@ -403,6 +423,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
 		}
+
 		success = false;
 		goto out;
 	}
@@ -412,6 +433,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
 		}
+
 		success = false;
 		goto out;
 	}
@@ -426,6 +448,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
 		}
+
 		success = false;
 		goto out;
 	}
@@ -439,6 +462,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
 		}
+
 		success = false;
 		goto out;
 	}
@@ -454,8 +478,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
 	bool success = true;
 
 	if (!prearm) {
-	    // Ignore power check after arming.
-	    return true;
+		// Ignore power check after arming.
+		return true;
 
 	} else {
 		int system_power_sub = orb_subscribe(ORB_ID(system_power));
@@ -515,66 +539,81 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check vertical position innovation test ratio
 	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
+
 	if (status.hgt_test_ratio > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HGT ERROR");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check velocity innovation test ratio
 	param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
+
 	if (status.vel_test_ratio > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF VEL ERROR");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check horizontal position innovation test ratio
 	param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
+
 	if (status.pos_test_ratio > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HORIZ POS ERROR");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check magnetometer innovation test ratio
 	param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
+
 	if (status.mag_test_ratio > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF YAW ERROR");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check accelerometer delta velocity bias estimates
 	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
-	if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) {
+
+	if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit
+	    || fabsf(status.states[15]) > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS");
 		}
+
 		success = false;
 		goto out;
 	}
 
 	// check gyro delta angle bias estimates
 	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
-	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) {
+
+	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
+	    || fabsf(status.states[12]) > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS");
 		}
+
 		success = false;
 		goto out;
 	}
@@ -583,30 +622,36 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	if (enforce_gps_required) {
 		bool ekf_gps_fusion = status.control_mode_flags & (1 << 2);
 		bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
+
 		if (!ekf_gps_fusion) {
 			// The EKF is not using GPS
 			if (report_fail) {
 				if (ekf_gps_check_fail) {
 					// Poor GPS qulaity is the likely cause
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
+
 				} else {
 					// Likely cause unknown
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
 				}
 			}
+
 			success = false;
 			goto out;
+
 		} else {
 			// The EKF is using GPS so check for bad quality on key performance indicators
 			bool gps_quality_fail = ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)
-										 + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)
-										 + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)
-										 + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)
-										 + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0);
+						  + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)
+						  + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)
+						  + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)
+						  + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0);
+
 			if (gps_quality_fail) {
 				if (report_fail) {
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
 				}
+
 				success = false;
 				goto out;
 			}
@@ -618,8 +663,9 @@ out:
 	return success;
 }
 
-bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
-		    bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot)
+bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+		    const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
+		    const hrt_abstime &time_since_boot)
 {
 
 	if (time_since_boot < 2000000) {
@@ -627,6 +673,23 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 		reportFailures = false;
 	}
 
+	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+	bool checkSensors = !hil_enabled;
+	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
+	const bool checkDynamic = !hil_enabled;
+	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
+
+	bool checkAirspeed = false;
+
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) {
+		checkAirspeed = true;
+	}
+
+	reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout);
+
 #ifdef __PX4_QURT
 	// WARNING: Preflight checks are important and should be added back when
 	// all the sensors are supported
@@ -673,6 +736,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
 			}
+
 			failed = true;
 		}
 
@@ -696,7 +760,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 			bool required = (i < max_mandatory_accel_count);
 			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures && !accel_fail_reported)) && required) {
+			if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures
+						&& !accel_fail_reported)) && required) {
 				failed = true;
 				accel_fail_reported = true;
 			}
@@ -711,6 +776,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
 			}
+
 			failed = true;
 		}
 	}
@@ -743,6 +809,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
 			}
+
 			failed = true;
 		}
 	}
@@ -776,6 +843,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 			if (reportFailures && !failed) {
 				mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
 			}
+
 			failed = true;
 		}
 	}
@@ -796,7 +864,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
-		if (rc_calibration_check(mavlink_log_pub, reportFailures, isVTOL) != OK) {
+		if (rc_calibration_check(mavlink_log_pub, reportFailures, status.is_vtol) != OK) {
 			if (reportFailures) {
 				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
 			}
@@ -816,6 +884,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
 	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
 	int32_t estimator_type;
 	param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
+
 	if (estimator_type == 2) {
 		// don't report ekf failures for the first 10 seconds to allow time for the filter to start
 		bool report_ekf_fail = (time_since_boot > 10 * 1000000);

src/modules/commander/PreflightCheck.h

@@ -40,6 +40,8 @@
  */
 
 #include <drivers/drv_hrt.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
 
 #pragma once
 
@@ -70,8 +72,9 @@ namespace Preflight
 * @param checkPower
 *   true if the system power should be checked
 **/
-bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
-		    bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot);
+bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+		    const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
+		    const hrt_abstime &time_since_boot);
 
 static constexpr unsigned max_mandatory_gyro_count = 1;
 static constexpr unsigned max_optional_gyro_count = 3;

src/modules/commander/commander.cpp

@@ -4032,26 +4032,9 @@ void Commander::mission_init()
 
 bool Commander::preflight_check(bool report)
 {
-	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
-
-	const bool checkSensors = !hil_enabled;
-	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
 	const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
-	const bool checkDynamic = !hil_enabled;
-	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
-
-	const bool reportFailures = (report && status_flags.condition_system_hotplug_timeout);
-
-	bool checkAirspeed = false;
-
-	/* Perform airspeed check only if circuit breaker is not
-	 * engaged and it's not a rotary wing */
-	if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) {
-		checkAirspeed = true;
-	}
 
-	bool success = Preflight::preflightCheck(&mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower,
-						status.is_vtol, reportFailures, false, hrt_elapsed_time(&commander_boot_timestamp));
+	bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, hrt_elapsed_time(&commander_boot_timestamp));
 
 	status_flags.condition_system_sensors_initialized = success;
 

src/modules/commander/state_machine_helper.cpp

@@ -75,7 +75,7 @@ static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MA
 };
 
 // You can index into the array with an arming_state_t in order to get its textual representation
-const char * const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
+const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"INIT",
 	"STANDBY",
 	"ARMED",
@@ -120,27 +120,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 		bool preflight_check_ret = true;
 		bool prearm_check_ret = true;
 
-		const bool checkSensors = !hil_enabled;
-		const bool checkRC = (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
 		const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
-		const bool checkDynamic = !hil_enabled;
-		const bool checkPower = (status_flags->condition_power_input_valid
-					 && !status_flags->circuit_breaker_engaged_power_check);
-
-		bool checkAirspeed = false;
-
-		/* Perform airspeed check only if circuit breaker is not
-		 * engaged and it's not a rotary wing */
-		if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
-			checkAirspeed = true;
-		}
 
 		/* only perform the pre-arm check if we have to */
 		if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
 		    && !hil_enabled) {
 
-			preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS,
-					      checkDynamic, checkPower, status->is_vtol, true, true, time_since_boot);
+			preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot);
 
 			if (preflight_check_ret) {
 				status_flags->condition_system_sensors_initialized = true;
@@ -156,8 +142,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 
 			if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
 
-				status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, checkSensors,
-						checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, status->is_vtol, false, false, time_since_boot);
+				status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot);
 
 				last_preflight_check = hrt_absolute_time();
 			}