From 5baed382663644737841719cd2e3bcf1a2584f7d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 17 Oct 2019 11:03:47 +0900 Subject: [PATCH] AP_NavEKF3: reduce EK3_HRT_FILT max to 30 --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 9c7c281ed0..71cf2f515e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -591,7 +591,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: HRT_FILT // @DisplayName: Height rate filter crossover frequency // @Description: Specifies the crossover frequency of the complementary filter used to calculate the output predictor height rate derivative. - // @Range: 0.1 100.0 + // @Range: 0.1 30.0 // @Units: Hz // @RebootRequired: False AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index fe80fe062d..30acc09497 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -767,7 +767,7 @@ void NavEKF3_core::calcOutputStates() // Perform filter calculation using backwards Euler integration // Coefficients selected to place all three filter poles at omega - const float CompFiltOmega = M_2PI * constrain_float(frontend->_hrt_filt_freq, 0.1f, 100.0f); + const float CompFiltOmega = M_2PI * constrain_float(frontend->_hrt_filt_freq, 0.1f, 30.0f); float omega2 = CompFiltOmega * CompFiltOmega; float pos_err = outputDataNew.position.z - vertCompFiltState.pos; float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;