From c08d0ac1b313ee74a82c65b074ad0cf8e58804e2 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 22 Apr 2014 20:39:44 +0100
Subject: [PATCH] BUGFIX - gyro calibration too short when movement detected
 during calibration.

When the model is moved during the gyro calibration period too much the
code that detects the movement (gyroMovementCalibrationThreshold aka
'moron threshold') did not restart the calibration correctly and one too
few calibration cycles were used when restarting.

In addition to this there was unnecessary calibration reset code - the
reset of calibration variables is supposed to happen at the start of
calibration; due to the bug the first calibration cycle could not be
detected when restarting so the old reset code was required.

This commit also cleans up the Gyro code so the it is in a similar style
to the recently cleaned acceleration code.
---
 src/sensors_gyro.c | 88 +++++++++++++++++++++++++++++-----------------
 1 file changed, 56 insertions(+), 32 deletions(-)

diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c
index f2c79b3226..720b6758ec 100644
--- a/src/sensors_gyro.c
+++ b/src/sensors_gyro.c
@@ -24,44 +24,63 @@ void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
     calibratingG = calibrationCyclesRequired;
 }
 
-static void gyroCommon(uint8_t gyroMovementCalibrationThreshold)
+bool isGyroCalibrationComplete(void)
 {
-    int axis;
+    return calibratingG == 0;
+}
+
+bool isOnFinalGyroCalibrationCycle(void)
+{
+    return calibratingG == 1;
+}
+
+bool isOnFirstGyroCalibrationCycle(void)
+{
+    return calibratingG == CALIBRATING_GYRO_CYCLES;
+}
+
+static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
+{
+    int8_t axis;
     static int32_t g[3];
     static stdev_t var[3];
 
-    if (calibratingG > 0) {
-        for (axis = 0; axis < 3; axis++) {
-            // Reset g[axis] at start of calibration
-            if (calibratingG == CALIBRATING_GYRO_CYCLES) {
-                g[axis] = 0;
-                devClear(&var[axis]);
-            }
-            // Sum up 1000 readings
-            g[axis] += gyroADC[axis];
-            devPush(&var[axis], gyroADC[axis]);
-            // Clear global variables for next reading
-            gyroADC[axis] = 0;
-            gyroZero[axis] = 0;
-            if (calibratingG == 1) {
-                float dev = devStandardDeviation(&var[axis]);
-                // check deviation and startover if idiot was moving the model
-                if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
-                    calibratingG = CALIBRATING_GYRO_CYCLES;
-                    devClear(&var[0]);
-                    devClear(&var[1]);
-                    devClear(&var[2]);
-                    g[0] = g[1] = g[2] = 0;
-                    continue;
-                }
-                gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
-                blinkLedAndSoundBeeper(10, 15, 1);
-            }
+    for (axis = 0; axis < 3; axis++) {
+
+        // Reset g[axis] at start of calibration
+        if (isOnFirstGyroCalibrationCycle()) {
+            g[axis] = 0;
+            devClear(&var[axis]);
+        }
+
+        // Sum up CALIBRATING_GYRO_CYCLES readings
+        g[axis] += gyroADC[axis];
+        devPush(&var[axis], gyroADC[axis]);
+
+        // Reset global variables to prevent other code from using un-calibrated data
+        gyroADC[axis] = 0;
+        gyroZero[axis] = 0;
+
+        if (isOnFinalGyroCalibrationCycle()) {
+            float dev = devStandardDeviation(&var[axis]);
+            // check deviation and startover if idiot was moving the model
+            if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
+                gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+                return;
+            }
+            gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
+            blinkLedAndSoundBeeper(10, 15, 1);
         }
-        calibratingG--;
     }
-    for (axis = 0; axis < 3; axis++)
+    calibratingG--;
+}
+
+static void applyGyroZero(void)
+{
+    int8_t axis;
+    for (axis = 0; axis < 3; axis++) {
         gyroADC[axis] -= gyroZero[axis];
+    }
 }
 
 void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
@@ -69,5 +88,10 @@ void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
     // range: +/- 8192; +/- 2000 deg/sec
     gyro.read(gyroADC);
     alignSensors(gyroADC, gyroADC, gyroAlign);
-    gyroCommon(gyroMovementCalibrationThreshold);
+
+    if (!isGyroCalibrationComplete()) {
+        performAcclerationCalibration(gyroMovementCalibrationThreshold);
+    }
+
+    applyGyroZero();
 }