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

AP_NavEKF3: Allow wind states to recover faster when airspeed sensor failed

Browse Source
This commit is contained in:
Paul Riseborough 2022-09-23 09:16:48 +10:00 committed by Randy Mackay
parent d27c3bca38
commit cd3b7389ea
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1109,6 +1109,10 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
if (!inhibitWindStates) { if (!inhibitWindStates) {
ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
if (!tasDataDelayed.allowFusion) {
// Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor
windVelVar *= 10.0f;
}
for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
} }