From 451060a8aba5cda496642b08e0aba0290a56a84f Mon Sep 17 00:00:00 2001
From: Zap Andersson <zap@master-zap.com>
Date: Sat, 19 Mar 2016 16:58:28 +0100
Subject: [PATCH] Revert "Removed the hard accelerometer cutoff, replaced my a
 smooth weighting."

This reverts commit 587ab7178a45cb4fee196dfdb45bd206139204ac.
---
 src/main/flight/imu.c | 37 ++++++++++++++++++-------------------
 1 file changed, 18 insertions(+), 19 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 77590eb96c..d72331ddd1 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -224,7 +224,7 @@ static float imuGetPGainScaleFactor(void)
 }
 
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
-                                float useAcc, float ax, float ay, float az,
+                                bool useAcc, float ax, float ay, float az,
                                 bool useMag, float mx, float my, float mz,
                                 bool useYaw, float yawError)
 {
@@ -272,21 +272,19 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
         ez += rMat[2][2] * ez_ef;
     }
 
-    if (useAcc > 0.0f) {
-		// Just scale by 1G length - That's our vector adjustment. Rather than 
-		// using one-over-exact length (which needs a costly square root), we already 
-		// know the vector is enough "roughly unit length" and since it is only weighted
-		// in by a certain amount anyway later, having that exact is meaningless.
- 	    recipNorm = 1.0f / acc_1G;; 		
-
+    // Use measured acceleration vector
+    recipNorm = sq(ax) + sq(ay) + sq(az);
+    if (useAcc && recipNorm > 0.01f) {
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(recipNorm);
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;
-        
 
-        ex += (ay * rMat[2][2] - az * rMat[2][1]) * useAcc;
-        ey += (az * rMat[2][0] - ax * rMat[2][2]) * useAcc;
-        ez += (ax * rMat[2][1] - ay * rMat[2][0]) * useAcc;
+        // Error is sum of cross product between estimated direction and measured direction of gravity
+        ex += (ay * rMat[2][2] - az * rMat[2][1]);
+        ey += (az * rMat[2][0] - ax * rMat[2][2]);
+        ez += (ax * rMat[2][1] - ay * rMat[2][0]);
     }
 
     // Compute and apply integral feedback if enabled
@@ -355,7 +353,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     }
 }
 
-static float imuAccelerometerWeight(void)
+static bool imuIsAccelerometerHealthy(void)
 {
     int32_t axis;
     int32_t accMagnitude = 0;
@@ -364,11 +362,10 @@ static float imuAccelerometerWeight(void)
         accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
     }
 
-    accMagnitude = accMagnitude * 100 / ((((int32_t)acc_1G)*((int32_t)acc_1G)));
+    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G));
 
-	int32_t nearness = ABS(100 - accMagnitude);
-	if (nearness > 25) return 0.0f;
-	return 1.0f - (nearness / 25.0f);
+    // Accept accel readings only in range 0.90g - 1.10g
+    return (81 < accMagnitude) && (accMagnitude < 121);
 }
 
 static bool isMagnetometerHealthy(void)
@@ -380,7 +377,7 @@ static void imuCalculateEstimatedAttitude(void)
 {
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
-    float useAcc = 0.0f;
+    bool useAcc = false;
     bool useMag = false;
     bool useYaw = false;
 
@@ -388,7 +385,9 @@ static void imuCalculateEstimatedAttitude(void)
     uint32_t deltaT = currentTime - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTime;
 
-    useAcc = imuAccelerometerWeight();
+    if (imuIsAccelerometerHealthy()) {
+        useAcc = true;
+    }
 
     if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
         useMag = true;