From e0a8baccfd3cec433e7a27066e542e3675ccd402 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Mar 2021 14:26:16 +0900 Subject: [PATCH] Copter: pre-arm check of EKF pos and vel variances --- ArduCopter/AP_Arming.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 24b1350885..f48eb78ff7 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -554,13 +554,23 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) } } - // check EKF compass variance is below failsafe threshold - float vel_variance, pos_variance, hgt_variance, tas_variance; - Vector3f mag_variance; - ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance); - if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) { - check_failed(display_failure, "EKF compass variance"); - return false; + // check EKF's compass, position and velocity variances are below failsafe threshold + if (copter.g.fs_ekf_thresh > 0.0f) { + float vel_variance, pos_variance, hgt_variance, tas_variance; + Vector3f mag_variance; + ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance); + if (mag_variance.length() >= copter.g.fs_ekf_thresh) { + check_failed(display_failure, "EKF compass variance"); + return false; + } + if (pos_variance >= copter.g.fs_ekf_thresh) { + check_failed(display_failure, "EKF position variance"); + return false; + } + if (vel_variance >= copter.g.fs_ekf_thresh) { + check_failed(display_failure, "EKF velocity variance"); + return false; + } } // check home and EKF origin are not too far