From 5f43be9d60d6dc1c2ad53a3e6692aab49c0c17b7 Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Wed, 9 Mar 2022 08:26:57 +0100 Subject: [PATCH] preflight checks: add check for distance sensor (via parameter) --- src/lib/systemlib/system_params.c | 15 +++ .../Arming/PreFlightCheck/CMakeLists.txt | 1 + .../Arming/PreFlightCheck/PreFlightCheck.cpp | 107 ++++++++---------- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 9 ++ .../checks/distanceSensorChecks.cpp | 71 ++++++++++++ 5 files changed, 141 insertions(+), 62 deletions(-) create mode 100644 src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 43245b2d80..1ae3cf9ef0 100644 --- a/src/lib/systemlib/system_params.c +++ b/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 * diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt index a4e3f4d3cc..edec715b23 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/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 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index b070dc882c..a04b8e0b16 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/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) { - - /* check all sensors individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_mag_count; i++) { - const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); - 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 - } - } + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count, + mavlink_log_pub, status, magnetometerCheck); // 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 */ - for (unsigned i = 0; i < max_optional_accel_count; i++) { - 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 - } - } + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count, + mavlink_log_pub, status, accelerometerCheck); // TODO: highest priority (from params) } /* ---- GYRO ---- */ { - /* check all sensors individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_gyro_count; i++) { - const bool required = (i < max_mandatory_gyro_count); - 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 - } - } + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count, + mavlink_log_pub, status, gyroCheck); // 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; - - /* check all sensors, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_baro_count; i++) { - const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1); - 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 - } + if (sys_has_baro == 1) { + static_cast(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count, + mavlink_log_pub, status, baroCheck)); } } @@ -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(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; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 492caeaddb..cb7589a4c8 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -40,12 +40,16 @@ #pragma once +#include #include #include #include #include #include +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); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp new file mode 100644 index 0000000000..387277b856 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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 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; +}