From de9f1095fb05bc7f4b0586776edbc7dc89f60d0c Mon Sep 17 00:00:00 2001
From: luggi <luke128@cnh.at>
Date: Wed, 16 Apr 2014 15:38:53 +0200
Subject: [PATCH] keep EstN from diverging to zero

---
 src/imu.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/imu.c b/src/imu.c
index 9d26de02e..9c7ce0316 100644
--- a/src/imu.c
+++ b/src/imu.c
@@ -267,10 +267,12 @@ static void getEstimatedAttitude(void)
     accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
 
     rotateV(&EstG.V, deltaGyroAngle);
-    if (sensors(SENSOR_MAG))
+    if (sensors(SENSOR_MAG)) {
         rotateV(&EstM.V, deltaGyroAngle);
-    else
+    } else {
         rotateV(&EstN.V, deltaGyroAngle);
+        normalizeV(&EstN.V, &EstN.V);
+    }
 
     // Apply complimentary filter (Gyro drift correction)
     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
@@ -309,7 +311,7 @@ static void getEstimatedAttitude(void)
 
         if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
             throttleAngleCorrection = 0;
-        } else {
+        } else {
             int angle = lrintf(acosf(cosZ) * throttleAngleScale);
             if (angle > 900)
                 angle = 900;