aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: reduce EK3_HRT_FILT max to 30

Browse Source
This commit is contained in:
Randy Mackay 2019-10-17 11:03:47 +09:00
parent 5b1d9ed868
commit 5baed38266
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -591,7 +591,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: HRT_FILT // @Param: HRT_FILT
// @DisplayName: Height rate filter crossover frequency // @DisplayName: Height rate filter crossover frequency
// @Description: Specifies the crossover frequency of the complementary filter used to calculate the output predictor height rate derivative. // @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 // @Units: Hz
// @RebootRequired: False // @RebootRequired: False
AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f), AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f),

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -767,7 +767,7 @@ void NavEKF3_core::calcOutputStates()
// Perform filter calculation using backwards Euler integration // Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega // 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 omega2 = CompFiltOmega * CompFiltOmega;
float pos_err = outputDataNew.position.z - vertCompFiltState.pos; float pos_err = outputDataNew.position.z - vertCompFiltState.pos;
float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT; float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;