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

AP_NavEKF3: Ensure yaw fusion commences when starting from rest

Browse Source
This commit is contained in:
Paul Riseborough 2020-04-09 11:10:51 +10:00 committed by Andrew Tridgell
parent a3725e2581
commit 33ab1a7b15
2 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -278,20 +278,24 @@ void NavEKF3_core::SelectMagFusion()
last_gps_yaw_fusion_ms = now;
} else if (tiltAlignComplete && !yawAlignComplete && (imuSampleTime_ms > lastSynthYawTime_ms + 140)) {
yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
if (!onGroundNotMoving) {
yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
}
yawAngDataDelayed.type = 2;
} else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
if (!onGroundNotMoving) {
yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
}
yawAngDataDelayed.type = 1;
}
if (onGroundNotMoving) {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw(false, true);
} else {
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
yawAngDataDelayed.type = 2;
} else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
yawAngDataDelayed.type = 1;
}
// prevent uncontrolled yaw variance growth by fusing a zero innovation
fuseEulerYaw(true, true);
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -177,7 +177,7 @@ void NavEKF3_core::InitialiseVariables()
lastPosPassTime_ms = 0;
lastHgtPassTime_ms = 0;
lastTasPassTime_ms = 0;
lastSynthYawTime_ms = imuSampleTime_ms;
lastSynthYawTime_ms = 0;
lastTimeGpsReceived_ms = 0;
secondLastGpsTime_ms = 0;
lastDecayTime_ms = imuSampleTime_ms;