From 8425bf46dd5febdbb90b5a35b11fa6bd5dee3b32 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 Jan 2025 13:08:51 +1100 Subject: [PATCH] Copter: remove unused hgt_variance_filt this filter is populated but never checked when checking variances --- ArduCopter/Copter.cpp | 1 - ArduCopter/Copter.h | 1 - ArduCopter/ekf_check.cpp | 1 - ArduCopter/system.cpp | 1 - 4 files changed, 4 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 420b35ab74ff17..0b55d48e688e13 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -962,7 +962,6 @@ Copter::Copter(void) flight_modes(&g.flight_mode1), pos_variance_filt(FS_EKF_FILT_DEFAULT), vel_variance_filt(FS_EKF_FILT_DEFAULT), - hgt_variance_filt(FS_EKF_FILT_DEFAULT), flightmode(&mode_stabilize), simple_cos_yaw(1.0f), super_simple_cos_yaw(1.0), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 64c206b6f4d949..e3a9ac97862d11 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -335,7 +335,6 @@ class Copter : public AP_Vehicle { // thus failsafes should be triggered on filtered values in order to avoid transient errors LowPassFilterFloat pos_variance_filt; LowPassFilterFloat vel_variance_filt; - LowPassFilterFloat hgt_variance_filt; bool variances_valid; uint32_t last_ekf_check_us; diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index c05e7a279cc49f..5bd2a248cf0fea 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -128,7 +128,6 @@ bool Copter::ekf_over_threshold() // always update filtered values as this serves the vibration check as well position_var = pos_variance_filt.apply(position_var, dt); vel_var = vel_variance_filt.apply(vel_var, dt); - height_var = hgt_variance_filt.apply(height_var, dt); last_ekf_check_us = now_us; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 1709fb076923be..9f167d2bdbaeaf 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -200,7 +200,6 @@ void Copter::init_ardupilot() pos_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); - hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); // flag that initialisation has completed ap.initialised = true;