From 33ab1a7b1564925b062224a7eb6eca9d37517495 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Apr 2020 11:10:51 +1000 Subject: [PATCH] AP_NavEKF3: Ensure yaw fusion commences when starting from rest --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 24 +++++++++++-------- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 2cce3e4caf..6233ac8d24 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -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); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index b3090fdeae..0822584037 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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;