From 8f5f564c05b5a0e12989d6e56642cc0b453ec45d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Nov 2020 11:41:37 -0500 Subject: [PATCH] commander: preflightcheck only report failure once per sensor type - either way the user action is to recalibrate, so we might as well try to minimize the noise/annoyance --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 51 +++++++++++-------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 5991effbde..e11202fdaf 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -55,11 +55,11 @@ static constexpr unsigned max_mandatory_baro_count = 1; static constexpr unsigned max_optional_baro_count = 4; bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, + vehicle_status_flags_s &status_flags, const bool checkGNSS, bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) { - reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout - && !status_flags.condition_calibration_enabled); + report_failures = (report_failures && status_flags.condition_system_hotplug_timeout + && !status_flags.condition_calibration_enabled); bool failed = false; @@ -75,7 +75,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* 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); - const bool report_fail = (reportFailures); + bool report_fail = report_failures; int32_t device_id = -1; @@ -83,13 +83,15 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (required) { failed = true; } + + report_fail = false; // only report the first failure } } // TODO: highest priority mag /* mag consistency checks (need to be performed after the individual checks) */ - if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) { + if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) { failed = true; } } @@ -100,7 +102,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* 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); - const bool report_fail = (reportFailures); + bool report_fail = report_failures; int32_t device_id = -1; @@ -108,6 +110,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (required) { failed = true; } + + report_fail = false; // only report the first failure } } @@ -119,12 +123,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* 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, reportFailures)) { + if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { if (required) { failed = true; } + + report_fail = false; // only report the first failure } } @@ -141,7 +149,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* 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); - const bool report_fail = (reportFailures && !baro_fail_reported); + bool report_fail = (report_failures && !baro_fail_reported); int32_t device_id = -1; @@ -149,6 +157,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (required) { baro_fail_reported = true; } + + report_fail = false; // only report the first failure } } } @@ -156,7 +166,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- IMU CONSISTENCY ---- */ // To be performed after the individual sensor checks have completed { - if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) { + if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) { failed = true; } } @@ -166,18 +176,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (!status_flags.circuit_breaker_engaged_airspd_check && (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) { - int32_t optional = 0; - param_get(param_find("FW_ARSP_MODE"), &optional); + int32_t airspeed_mode = 0; + param_get(param_find("FW_ARSP_MODE"), &airspeed_mode); + const bool optional = (airspeed_mode == 1); - if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures, prearm) && !(bool)optional) { + if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm) && !optional) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) { - if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) { - if (reportFailures) { + if (rcCalibrationCheck(mavlink_log_pub, report_failures, status.is_vtol) != OK) { + if (report_failures) { mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); } @@ -196,7 +207,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- SYSTEM POWER ---- */ if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) { - if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) { + if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) { failed = true; } } @@ -217,22 +228,22 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu // 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_s); - if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) { + if (!ekf2Check(mavlink_log_pub, status, false, report_failures && report_ekf_fail, checkGNSS)) { failed = true; } - if (!ekf2CheckStates(mavlink_log_pub, reportFailures && report_ekf_fail)) { + if (!ekf2CheckStates(mavlink_log_pub, report_failures && report_ekf_fail)) { failed = true; } } /* ---- Failure Detector ---- */ - if (!failureDetectorCheck(mavlink_log_pub, status, reportFailures, prearm)) { + if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) { failed = true; } - failed = failed || !manualControlCheck(mavlink_log_pub, reportFailures); - failed = failed || !cpuResourceCheck(mavlink_log_pub, reportFailures); + failed = failed || !manualControlCheck(mavlink_log_pub, report_failures); + failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures); /* Report status */ return !failed;