From cee401666598b54e268d65a79a51fa7841ceba49 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 25 Sep 2020 13:10:10 +0200 Subject: [PATCH] Preflight checks: Airspeed checks update -check for valid airspeed_validated (declared valid plus updated less than 1s ago) -added param (COM_ARM_ARSP_EN) to enable/disable check for max airspeed for arming set max airspeed limit to half of stall airspeed Signed-off-by: Silvan Fuhrer --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 12 ++++++- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 2 +- .../PreFlightCheck/checks/airspeedCheck.cpp | 33 ++++++++++--------- src/modules/commander/commander_params.c | 12 +++++++ 4 files changed, 41 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 134ccd8dd158..64e529f4bc58 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -180,7 +180,17 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu param_get(param_find("FW_ARSP_MODE"), &airspeed_mode); const bool optional = (airspeed_mode == 1); - if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm) && !optional) { + int32_t max_airspeed_check_en = 0; + param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en); + + float airspeed_stall = 10.0f; + param_get(param_find("ASPD_STALL"), &airspeed_stall); + + const float arming_max_airspeed_allowed = airspeed_stall / 2.0f; // set to half of stall speed + + if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en, + arming_max_airspeed_allowed) + && !(bool)optional) { failed = true; } } diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 1add8ba0d86e..4a77a8730315 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -104,7 +104,7 @@ class PreFlightCheck 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 report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed); static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL); static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp index 358bac25d69c..ffaceea47105 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp @@ -38,23 +38,26 @@ #include #include #include -#include +#include 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 prearm) + const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed) { bool present = true; bool success = true; - uORB::SubscriptionData airspeed_sub{ORB_ID(airspeed)}; - airspeed_sub.update(); - const airspeed_s &airspeed = airspeed_sub.get(); + uORB::SubscriptionData airspeed_validated_sub{ORB_ID(airspeed_validated)}; + airspeed_validated_sub.update(); + const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get(); - if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) { + /* + * 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 Sensor missing"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down."); } present = false; @@ -63,14 +66,11 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status } /* - * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise - * on the bench and trigger false positives of the voter. Therefore only fail this - * for a pre-arm check, as then the cover is off and the natural airflow in the field - * will ensure there is not zero noise. + * Check if airspeed is declared valid or not by airspeed selector. */ - if (prearm && fabsf(airspeed.confidence) < 0.95f) { + if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid."); } present = true; @@ -79,13 +79,14 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status } /* - * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying + * 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 (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && prearm) { + if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed + && prearm) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot"); } present = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 29afa848bcc8..f0fe913968ec 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -956,3 +956,15 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); * @decimal 3 */ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); + +/** +* Enable preflight check for maximal allowed airspeed when arming. +* +* Deny arming if the current airspeed measurement is greater than half the stall speed (ASPD_STALL). +* Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration. +* +* @group Commander +* @value 0 Disabled +* @value 1 Enabled +*/ +PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);