dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Julian Oes 2021-03-08 15:28:54 +01:00 committed by Daniel Agar
parent 2f024a9a8a
commit a13459858d
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -236,18 +236,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
} }
if (estimator_type == 2) { 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 const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
if (time_since_boot > 10_s) { ekf2CheckSensorBias(mavlink_log_pub, report_failures);
ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && // For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
ekf2CheckSensorBias(mavlink_log_pub, report_failures); // 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 { } 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; failed |= !ekf_healthy;