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

AP_NavEKF2: reduce effect of acceleration on non-GPS mode attitude

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-21 11:10:18 +10:00 committed by Andrew Tridgell
parent d33bbf8cc7
commit addd213af0
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -511,6 +511,14 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
accNavMag = velDotNEDfilt.length();
accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y);
// if we are not aiding, then limit the horizontal magnitude of acceleration
// to prevent large manoeuvre transients disturbing the attitude
if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {
float gain = 5.0f/accNavMagHoriz;
delVelNav.x *= gain;
delVelNav.y *= gain;
}
// save velocity for use in trapezoidal integration for position calcuation
Vector3f lastVelocity = stateStruct.velocity;