Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: constrain vertical error

Browse Source 
this prevents a floating point exception with external AHRS
This commit is contained in:
Andrew Tridgell 2021-01-01 19:24:01 +11:00 committed by Peter Barker
parent 1e75bb971e
commit 59fa794818
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -824,7 +824,7 @@ void NavEKF3_core::calcOutputStates()
// Coefficients selected to place all three filter poles at omega
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 pos_err = constrain_float(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5);
float integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;
vertCompFiltState.acc += integ1_input;
float integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT;