From 353aedb5476e7d3e2ea99c7a3e74b3b1b1181c63 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 26 Aug 2022 11:43:59 +0100 Subject: [PATCH] Copter: Arming: check EKF height varance --- ArduCopter/AP_Arming.cpp | 6 +++++- ArduCopter/Parameters.cpp | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index deb66f4737..ae20c099ce 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -498,7 +498,7 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) } } - // check EKF's compass, position and velocity variances are below failsafe threshold + // check EKF's compass, position, height 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; @@ -515,6 +515,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) check_failed(display_failure, "EKF velocity variance"); return false; } + if (hgt_variance >= copter.g.fs_ekf_thresh) { + check_failed(display_failure, "EKF height variance"); + return false; + } } // check if home is too far from EKF origin diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index fb5b9a1f5b..79b185f2cc 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -404,7 +404,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_EKF_THRESH // @DisplayName: EKF failsafe variance threshold - // @Description: Allows setting the maximum acceptable compass and velocity variance + // @Description: Allows setting the maximum acceptable compass, velocity, position and height variances. Used in arming check and EKF failsafe. // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed // @User: Advanced GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT),