Browse Source

AP_NavEKF3: reduce EK3_HRT_FILT max to 30

master
Randy Mackay 5 years ago
parent
commit
5baed38266
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 2
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -591,7 +591,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { @@ -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),

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@ -767,7 +767,7 @@ void NavEKF3_core::calcOutputStates() @@ -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;

Loading…
Cancel
Save