preflight checks: add check for distance sensor (via parameter)

src/lib/systemlib/system_params.c

@@ -218,6 +218,21 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
  */
 PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
 
+/**
+ * Control the number of distance sensors on the vehicle
+ *
+ * If set to the number of distance sensors, the preflight check will check
+ * for their presence and valid data publication. Disable with 0 if no distance
+ * sensor present or to disbale the preflight check.
+ *
+ * @reboot_required true
+ *
+ * @group System
+ * @min 0
+ * @max 4
+ */
+PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0);
+
 /**
  * Enable factory calibration mode
  *

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

@@ -41,6 +41,7 @@ px4_add_library(PreFlightCheck
 	checks/baroCheck.cpp
 	checks/imuConsistencyCheck.cpp
 	checks/airspeedCheck.cpp
+	checks/distanceSensorChecks.cpp
 	checks/rcCalibrationCheck.cpp
 	checks/powerCheck.cpp
 	checks/airframeCheck.cpp

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

@@ -72,22 +72,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
 
 		if (sys_has_mag == 1) {
-
+			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count,
-			/* check all sensors individually, but fail only for mandatory ones */
+							   mavlink_log_pub, status, magnetometerCheck);
-			for (unsigned i = 0; i < max_optional_mag_count; i++) {
-				const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
-				bool report_fail = report_failures;
-
-				int32_t device_id = -1;
-
-				if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-					if (required) {
-						failed = true;
-					}
-
-					report_fail = false; // only report the first failure
-				}
-			}
 
 			// TODO: highest priority mag
 
@@ -100,42 +86,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- ACCEL ---- */
 	{
-		/* check all sensors individually, but fail only for mandatory ones */
+		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count,
-		for (unsigned i = 0; i < max_optional_accel_count; i++) {
+						   mavlink_log_pub, status, accelerometerCheck);
-			const bool required = (i < max_mandatory_accel_count);
-			bool report_fail = report_failures;
-
-			int32_t device_id = -1;
-
-			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if (required) {
-					failed = true;
-				}
-
-				report_fail = false; // only report the first failure
-			}
-		}
 
 		// TODO: highest priority (from params)
 	}
 
 	/* ---- GYRO ---- */
 	{
-		/* check all sensors individually, but fail only for mandatory ones */
+		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count,
-		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
+						   mavlink_log_pub, status, gyroCheck);
-			const bool required = (i < max_mandatory_gyro_count);
-			bool report_fail = report_failures;
-
-			int32_t device_id = -1;
-
-			if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if (required) {
-					failed = true;
-				}
-
-				report_fail = false; // only report the first failure
-			}
-		}
 
 		// TODO: highest priority (from params)
 	}
@@ -145,22 +105,9 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		int32_t sys_has_baro = 1;
 		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
 
-		bool baro_fail_reported = false;
+		if (sys_has_baro == 1) {
-
+			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count,
-		/* check all sensors, but fail only for mandatory ones */
+					  mavlink_log_pub, status, baroCheck));
-		for (unsigned i = 0; i < max_optional_baro_count; i++) {
-			const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
-			bool report_fail = (required && report_failures && !baro_fail_reported);
-
-			int32_t device_id = -1;
-
-			if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if (required) {
-					baro_fail_reported = true;
-				}
-
-				report_fail = false; // only report the first failure
-			}
 		}
 	}
 
@@ -172,6 +119,18 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		}
 	}
 
+	/* ---- 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, 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 &&
@@ -278,3 +237,27 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	/* Report status */
 	return !failed;
 }
+
+bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
+		const uint8_t max_optional_count, 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 < max_optional_count; i++) {
+		const bool required = (i < max_mandatory_count);
+		int32_t device_id = -1;
+
+		if (!sens_check(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+			if (required) {
+				pass_check = false;
+			}
+
+			report_fail = false; // only report the first failure
+		}
+	}
+
+	return pass_check;
+}

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

@@ -40,12 +40,16 @@
 
 #pragma once
 
+#include <uORB/uORB.h>
 #include <uORB/topics/safety.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>
 
+typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				  const bool optional, int32_t &device_id, const bool report_fail);
+
 class PreFlightCheck
 {
 public:
@@ -95,6 +99,9 @@ public:
 				bool report_fail = true);
 
 private:
+	static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
+					    const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
+					    vehicle_status_s &status, sens_check_func_t sens_check);
 	static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
@@ -104,6 +111,8 @@ private:
 			      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				  const bool optional, int32_t &device_id, const bool report_fail);
 	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
 	static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
 				  const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);

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

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 "../PreFlightCheck.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/topics/distance_sensor.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
+				   const bool optional, int32_t &device_id, const bool report_fail)
+{
+	const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
+	bool check_valid = false;
+
+	if (exists) {
+		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();
+
+		check_valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
+
+		if (!check_valid) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
+			}
+		}
+
+	} else {
+		if (!optional && report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance);
+		}
+	}
+
+	return check_valid;
+}