From 1837a9a49bb6f23a86cf535c1590a0d2ef2d135d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 9 Feb 2024 10:33:47 +0000 Subject: [PATCH] Copter: cleanup EKF failsafe units and division --- ArduCopter/Parameters.cpp | 1 + ArduCopter/ekf_check.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 35b1780ff9cc1a..29304007b63fb1 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1238,6 +1238,7 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @DisplayName: EKF Failsafe filter cutoff // @Description: EKF Failsafe filter cutoff frequency. EKF variances are filtered using this value to avoid spurious failsafes from transient high variances. A higher value means the failsafe is more likely to trigger. // @Range: 0 10 + // @Units: Hz // @User: Advanced AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT), diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 47d9121be9b742..c05e7a279cc49f 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -123,7 +123,7 @@ bool Copter::ekf_over_threshold() } uint32_t now_us = AP_HAL::micros(); - float dt = (now_us - last_ekf_check_us) / 1000000.0f; + float dt = (now_us - last_ekf_check_us) * 1e-6f; // always update filtered values as this serves the vibration check as well position_var = pos_variance_filt.apply(position_var, dt);