ensure cogYawGain is 1.0 except during gps rescue

betaflight · May 9, 2023 · e00970b · e00970b
1 parent f4aad6c
commit e00970b
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
@@ -220,7 +220,6 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
         }
 
         const float ez_ef = cogYawGain * (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
-
         ex = rMat[2][0] * ez_ef;
         ey = rMat[2][1] * ez_ef;
         ez = rMat[2][2] * ez_ef;
@@ -476,8 +475,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     static timeUs_t previousIMUUpdateTime;
     bool useAcc = false;
     bool useMag = false;
-    float cogYawGain = 0.0f; // Yaw correction Gain to be applied in imuMahonyAHRSupdate from our ground course
-    float courseOverGround = 0; // To be used when cogYawGain is non-zero.  Stored in Radians
+    float cogYawGain = 1.0f; // Yaw correction gain to be applied in imuMahonyAHRSupdate from ground course
+    float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
 
     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTimeUs;