From a13459858daec83eefea11e3d9eed94d455de534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Mar 2021 15:28:54 +0100 Subject: [PATCH] commander: improve logic for ekf2 preflight check With this change we prevent the case where arming silently fails within the first 10 seconds after boot. Also, we now set the sensors as healthy as soon as the ekf is healthy, and don't wait 10 seconds without actually checking. --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index b0555af020..027782b269 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -236,18 +236,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } if (estimator_type == 2) { - bool ekf_healthy = false; - // don't report ekf failures for the first 10 seconds to allow time for the filter to start - if (time_since_boot > 10_s) { + const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && + ekf2CheckSensorBias(mavlink_log_pub, report_failures); - ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && - ekf2CheckSensorBias(mavlink_log_pub, report_failures); + // For the first 10 seconds the ekf2 can be unhealthy, and we just mark it + // as not present. + // After that or if report_failures is true, we'll set the flags as is. - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); + if (!ekf_healthy && time_since_boot < 10_s && !report_failures) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status); } else { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); } failed |= !ekf_healthy;