[ACC] Make accelerometer calibration more robust

src/main/common/calibration.h

@@ -33,7 +33,7 @@
 #include "common/vector.h"
 
 typedef enum {
-    ZERO_CALIBRATION_NONE,
+    ZERO_CALIBRATION_NONE = 0,
     ZERO_CALIBRATION_IN_PROGRESS,
     ZERO_CALIBRATION_DONE,
     ZERO_CALIBRATION_FAIL,

src/main/common/maths.c

@@ -475,7 +475,19 @@ static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
     sensorCalibration_BackwardSubstitution(A, x, y);
 }
 
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationValidateResult(const float result[3])
+{
+    // Validate that result is not INF and not NAN
+    for (int i = 0; i < 3; i++) {
+        if (isnan(result[i]) && isinf(result[i])) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -483,9 +495,11 @@ void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float res
     for (int i = 0; i < 3; i++) {
         result[i] = beta[i] / 2;
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -493,6 +507,8 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     for (int i = 0; i < 3; i++) {
         result[i] = sqrtf(beta[i]);
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
 float bellCurve(const float x, const float curveWidth)

src/main/common/maths.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #ifndef sq
 #define sq(x) ((x)*(x))
@@ -122,8 +123,8 @@ typedef struct {
 void sensorCalibrationResetState(sensorCalibrationState_t * state);
 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);

src/main/sensors/acceleration.c

@@ -350,11 +350,10 @@ bool accInit(uint32_t targetLooptime)
 
 static bool calibratedPosition[6];
 static int32_t accSamples[6][3];
-static int  calibratedAxisCount = 0;
 
 uint8_t accGetCalibrationAxisFlags(void)
 {
-    if (accIsCalibrationComplete() && STATE(ACCELEROMETER_CALIBRATED)) {
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
         return 0x3F;    // All 6 bits are set
     }
 
@@ -393,19 +392,20 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
         return -1;
 }
 
-bool accIsCalibrationComplete(void)
-{
-    return zeroCalibrationIsCompleteV(&zeroCalibration);
-}
-
 void accStartCalibration(void)
 {
     int positionIndex = getPrimaryAxisIndex(accADC);
 
+    // Fail if we can't detect the side
     if (positionIndex < 0) {
         return;
     }
 
+    // Fail if we have accelerometer fully calibrated and are NOT starting with TOP-UP position
+    if (STATE(ACCELEROMETER_CALIBRATED) && positionIndex != 0) {
+        return;
+    }
+
     // Top+up and first calibration cycle, reset everything
     if (positionIndex == 0) {
         for (int axis = 0; axis < 6; axis++) {
@@ -415,7 +415,6 @@ void accStartCalibration(void)
             accSamples[axis][Z] = 0;
         }
 
-        calibratedAxisCount = 0;
         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
     }
 
@@ -423,8 +422,30 @@ void accStartCalibration(void)
     zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.05f, true);
 }
 
+static bool allOrientationsHaveCalibrationDataCollected(void)
+{
+    // Return true only if we have calibration data for all 6 positions
+    return calibratedPosition[0] && calibratedPosition[1] && calibratedPosition[2] &&
+           calibratedPosition[3] && calibratedPosition[4] && calibratedPosition[5];
+}
+
+bool accIsCalibrationComplete(void)
+{
+    return zeroCalibrationIsCompleteV(&zeroCalibration);
+}
+
 static void performAcclerationCalibration(void)
 {
+    // Shortcut - no need to do any math if acceleromter is marked as calibrated
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
+        return;
+    }
+
+    // If zero calibration logic is finished - no need to do anything
+    if (accIsCalibrationComplete()) {
+        return;
+    }
+
     fpVector3_t v;
     int positionIndex = getPrimaryAxisIndex(accADC);
 
@@ -449,7 +470,6 @@ static void performAcclerationCalibration(void)
                 accSamples[positionIndex][Z] = v.v[Z];
 
                 calibratedPosition[positionIndex] = true;
-                calibratedAxisCount++;
             }
             else {
                 calibratedPosition[positionIndex] = false;
@@ -459,9 +479,10 @@ static void performAcclerationCalibration(void)
         }
     }
 
-    if (calibratedAxisCount == 6) {
+    if (allOrientationsHaveCalibrationDataCollected()) {
         sensorCalibrationState_t calState;
         float accTmp[3];
+        bool calFailed = false;
 
         /* Calculate offset */
         sensorCalibrationResetState(&calState);
@@ -470,7 +491,12 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForOffsetCalculation(&calState, accSamples[axis]);
         }
 
-        sensorCalibrationSolveForOffset(&calState, accTmp);
+        if (!sensorCalibrationSolveForOffset(&calState, accTmp)) {
+            accTmp[0] = 0.0f;
+            accTmp[1] = 0.0f;
+            accTmp[1] = 0.0f;
+            calFailed = true;
+        }
 
         accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
         accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
@@ -489,13 +515,28 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
         }
 
-        sensorCalibrationSolveForScale(&calState, accTmp);
+        if (!sensorCalibrationSolveForScale(&calState, accTmp)) {
+            accTmp[0] = 1.0f;
+            accTmp[1] = 1.0f;
+            accTmp[1] = 1.0f;
+            calFailed = true;
+        }
 
         for (int axis = 0; axis < 3; axis++) {
             accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
         }
 
-        saveConfigAndNotify();
+        if (calFailed) {
+            // If failed - don't save and also invalidate the calibration data for all positions
+            for (int axis = 0; axis < 6; axis++) {
+                calibratedPosition[axis] = false;
+            }
+        }
+        else {
+            // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
+            // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
+            saveConfigAndNotify();
+        }
     }
 }
 
@@ -540,10 +581,7 @@ void accUpdate(void)
         DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
     }
 
-    if (!accIsCalibrationComplete()) {
-        performAcclerationCalibration();
-        return;
-    }
+    performAcclerationCalibration();
 
     applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);