From aad11ecc65e478b1e52c29576470fec26e60d1ab Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 8 Sep 2021 11:18:35 +0200 Subject: [PATCH] PreFltCheck: do not force to report ekf2 failures on GCS connection EKF2 has a grace period of 10 seconds after boot where it doesn't need to warn the user while the sensors (especially GNSS) are still converging. A connection to a GCS shouldn't skip this grace period but an arming request should. --- .../commander/Arming/PreFlightCheck/PreFlightCheck.cpp | 10 ++++++---- src/modules/commander/Commander.cpp | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 80abab1304..1c5060324c 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -237,14 +237,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (estimator_type == 2) { - const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && - ekf2CheckSensorBias(mavlink_log_pub, report_failures); + const bool in_grace_period = time_since_boot < 10_s; + const bool do_report_ekf2_failures = report_failures && (!in_grace_period || prearm); + const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, do_report_ekf2_failures) && + ekf2CheckSensorBias(mavlink_log_pub, do_report_ekf2_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. + // After that or if we're forced to report, we'll set the flags as is. - if (!ekf_healthy && time_since_boot < 10_s && !report_failures) { + if (!ekf_healthy && !do_report_ekf2_failures) { set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status); } else { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index dedf87a543..233df5c17b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3528,7 +3528,7 @@ void Commander::data_link_check() if (!_armed.armed && !_status_flags.condition_calibration_enabled) { // make sure to report preflight check failures to a connecting GCS - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, true, + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, false, hrt_elapsed_time(&_boot_timestamp)); } }