Merge pull request #4202 from iNavFlight/de_runtime_cal_refactor

Refactor run-time calibration

make/source.mk

@@ -11,6 +11,7 @@ COMMON_SRC = \
             common/encoding.c \
             common/filter.c \
             common/maths.c \
+            common/calibration.c \
             common/memory.c \
             common/olc.c \
             common/printf.c \

src/main/common/calibration.c

@@ -0,0 +1,189 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "build/debug.h"
+#include "drivers/time.h"
+#include "common/calibration.h"
+
+void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
+{
+    // Reset parameters and state
+    s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
+    s->params.startTimeMs = millis();
+    s->params.windowSizeMs = window;
+    s->params.stdDevThreshold = threshold;
+    s->params.allowFailure = allowFailure;
+
+    s->params.sampleCount = 0;
+    s->val.accumulatedValue = 0;
+    devClear(&s->val.stdDev);
+}
+
+bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s)
+{
+    return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
+}
+
+bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s)
+{
+    return (s->params.state == ZERO_CALIBRATION_DONE);
+}
+
+void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
+{
+    if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
+        return;
+    }
+
+    // Add value
+    s->val.accumulatedValue += v;
+    s->params.sampleCount++;
+    devPush(&s->val.stdDev, v);
+
+    // Check if calibration is complete
+    if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
+        const float stddev = devStandardDeviation(&s->val.stdDev);
+        if (stddev > s->params.stdDevThreshold) {
+            if (!s->params.allowFailure) {
+                // If deviation is too big - restart calibration
+                s->params.startTimeMs = millis();
+                s->params.sampleCount = 0;
+                s->val.accumulatedValue = 0;
+                devClear(&s->val.stdDev);
+            }
+            else {
+                // We are allowed to fail
+                s->params.state = ZERO_CALIBRATION_FAIL;
+            }
+        }
+        else {
+            // All seems ok - calculate average value
+            s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount;
+            s->params.state = ZERO_CALIBRATION_DONE;
+        }
+    }
+}
+
+void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v)
+{
+    if (s->params.state != ZERO_CALIBRATION_DONE) {
+        *v = 0.0f;
+    }
+    else {
+        *v = s->val.accumulatedValue;
+    }
+}
+
+void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure)
+{
+    // Reset parameters and state
+    s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
+    s->params.startTimeMs = millis();
+    s->params.windowSizeMs = window;
+    s->params.stdDevThreshold = threshold;
+    s->params.allowFailure = allowFailure;
+
+    s->params.sampleCount = 0;
+    for (int i = 0; i < 3; i++) {
+        s->val[i].accumulatedValue = 0;
+        devClear(&s->val[i].stdDev);
+    }
+}
+
+bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s)
+{
+    return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
+}
+
+bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s)
+{
+    return (s->params.state == ZERO_CALIBRATION_DONE);
+}
+
+void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v)
+{
+    if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
+        return;
+    }
+
+    // Add value
+    for (int i = 0; i < 3; i++) {
+        s->val[i].accumulatedValue += v->v[i];
+        devPush(&s->val[i].stdDev, v->v[i]);
+    }
+
+    s->params.sampleCount++;
+
+    // Check if calibration is complete
+    if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
+        bool needRecalibration = false;
+        
+        for (int i = 0; i < 3 && !needRecalibration; i++) {
+            const float stddev = devStandardDeviation(&s->val[i].stdDev);
+            //debug[i] = stddev;
+            if (stddev > s->params.stdDevThreshold) {
+                needRecalibration = true;
+            }
+        }
+
+        if (needRecalibration) {
+            if (!s->params.allowFailure) {
+                // If deviation is too big - restart calibration
+                s->params.startTimeMs = millis();
+                s->params.sampleCount = 0;
+                for (int i = 0; i < 3; i++) {
+                    s->val[i].accumulatedValue = 0;
+                    devClear(&s->val[i].stdDev);
+                }
+            }
+            else {
+                // We are allowed to fail
+                s->params.state = ZERO_CALIBRATION_FAIL;
+            }
+        }
+        else {
+            // All seems ok - calculate average value
+            s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount;
+            s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount;
+            s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount;
+            s->params.state = ZERO_CALIBRATION_DONE;
+        }
+    }
+}
+
+void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v)
+{
+    if (s->params.state != ZERO_CALIBRATION_DONE) {
+        vectorZero(v);
+    }
+    else {
+        v->v[0] = s->val[0].accumulatedValue;
+        v->v[1] = s->val[1].accumulatedValue;
+        v->v[2] = s->val[2].accumulatedValue;
+    }    
+}

src/main/common/calibration.h

@@ -0,0 +1,76 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once 
+
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "common/maths.h"
+#include "common/time.h"
+#include "common/vector.h"
+
+typedef enum {
+    ZERO_CALIBRATION_NONE,
+    ZERO_CALIBRATION_IN_PROGRESS,
+    ZERO_CALIBRATION_DONE,
+    ZERO_CALIBRATION_FAIL,
+} zeroCalibrationState_e;
+
+typedef struct {
+    zeroCalibrationState_e  state;
+    timeMs_t                startTimeMs;
+    timeMs_t                windowSizeMs;
+    bool                    allowFailure;
+    unsigned                sampleCount;
+    float                   stdDevThreshold;
+} zeroCalibrationParams_t;
+
+typedef struct {
+    float                   accumulatedValue;
+    stdev_t                 stdDev;
+} zeroCalibrationValue_t;
+
+typedef struct {
+    zeroCalibrationParams_t params;
+    zeroCalibrationValue_t  val;
+} zeroCalibrationScalar_t;
+
+typedef struct {
+    zeroCalibrationParams_t params;
+    zeroCalibrationValue_t  val[3];
+} zeroCalibrationVector_t;
+
+void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure);
+bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s);
+bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s);
+void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v);
+void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v);
+
+void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure);
+bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s);
+bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s);
+void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v);
+void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v);

src/main/fc/fc_init.c

@@ -629,7 +629,8 @@ void init(void)
     blackboxInit();
 #endif
 
-    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+    gyroStartCalibration();
+
 #ifdef USE_BARO
     baroStartCalibration();
 #endif

src/main/fc/fc_msp.c

@@ -2068,7 +2068,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_ACC_CALIBRATION:
         if (!ARMING_FLAG(ARMED))
-            accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
+            accStartCalibration();
         else
             return MSP_RESULT_ERROR;
         break;

src/main/fc/rc_controls.c

@@ -235,7 +235,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
     // GYRO calibration
     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
-        gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+        gyroStartCalibration();
         return;
     }
 
@@ -317,7 +317,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
     // Calibrating Acc
     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
-        accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
+        accStartCalibration();
         return;
     }
 

src/main/navigation/navigation_pos_estimator.c

@@ -342,18 +342,24 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs)
  * Update IMU topic
  *  Function is called at main loop rate
  */
+static void restartGravityCalibration(void)
+{
+    zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false);
+}
+
+static bool gravityCalibrationComplete(void)
+{
+    return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
+}
+
 static void updateIMUTopic(void)
 {
-    static float calibratedGravityCMSS = GRAVITY_CMSS;
-    static timeMs_t gravityCalibrationTimeout = 0;
-
     if (!isImuReady()) {
         posEstimator.imu.accelNEU.x = 0;
         posEstimator.imu.accelNEU.y = 0;
         posEstimator.imu.accelNEU.z = 0;
 
-        gravityCalibrationTimeout = millis();
+        restartGravityCalibration();
-        posEstimator.imu.gravityCalibrationComplete = false;
     }
     else {
         fpVector3_t accelBF;
@@ -377,24 +383,18 @@ static void updateIMUTopic(void)
         posEstimator.imu.accelNEU.z = accelBF.z;
 
         /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
-        if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) {
+        if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
-            // Slowly converge on calibrated gravity while level
+            zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
-            const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS;
-            calibratedGravityCMSS += gravityOffsetError * 0.0025f;
 
-            if (fabsf(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) {  // Error should be within 0.5% of calibrated gravity
+            if (gravityCalibrationComplete()) {
-                if ((millis() - gravityCalibrationTimeout) > 250) {
+                zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
-                    posEstimator.imu.gravityCalibrationComplete = true;
+                DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS));
-                }
-            }
-            else {
-                gravityCalibrationTimeout = millis();
             }
         }
 
         /* If calibration is incomplete - report zero acceleration */
-        if (posEstimator.imu.gravityCalibrationComplete) {
+        if (gravityCalibrationComplete()) {
-            posEstimator.imu.accelNEU.z -= calibratedGravityCMSS;
+            posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
         }
         else {
             posEstimator.imu.accelNEU.x = 0;
@@ -768,7 +768,7 @@ void initializePositionEstimator(void)
     posEstimator.est.flowCoordinates[X] = 0;
     posEstimator.est.flowCoordinates[Y] = 0;
 
-    posEstimator.imu.gravityCalibrationComplete = false;
+    restartGravityCalibration();
 
     for (axis = 0; axis < 3; axis++) {
         posEstimator.imu.accelBias.v[axis] = 0;
@@ -807,7 +807,7 @@ void updatePositionEstimator(void)
 
 bool navIsCalibrationComplete(void)
 {
-    return posEstimator.imu.gravityCalibrationComplete;
+    return gravityCalibrationComplete();
 }
 
 #endif

src/main/navigation/navigation_pos_estimator_private.h

@@ -23,6 +23,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/filter.h"
+#include "common/calibration.h"
 
 #include "sensors/sensors.h"
 
@@ -44,6 +45,8 @@
 #define INAV_SURFACE_TIMEOUT_MS             400     // Surface timeout    (missed 3 readings in a row)
 #define INAV_FLOW_TIMEOUT_MS                200
 
+#define CALIBRATING_GRAVITY_TIME_MS         2000
+
 // Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay
 #define INAV_BARO_AVERAGE_HZ                1.0f
 #define INAV_SURFACE_AVERAGE_HZ             1.0f
@@ -123,9 +126,10 @@ typedef struct {
 } navPositionEstimatorESTIMATE_t;
 
 typedef struct {
-    fpVector3_t     accelNEU;
+    fpVector3_t             accelNEU;
-    fpVector3_t     accelBias;
+    fpVector3_t             accelBias;
-    bool            gravityCalibrationComplete;
+    float                   calibratedGravityCMSS;
+    zeroCalibrationScalar_t gravityCalibration;
 } navPosisitonEstimatorIMU_t;
 
 typedef enum {

src/main/sensors/acceleration.c

@@ -27,6 +27,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/calibration.h"
 
 #include "config/config_reset.h"
 #include "config/parameter_group.h"
@@ -70,7 +71,7 @@
 
 FASTRAM acc_t acc;                       // acc access functions
 
-static uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
+STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration;
 
 STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
 
@@ -341,26 +342,6 @@ bool accInit(uint32_t targetLooptime)
     return true;
 }
 
-void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
-{
-    calibratingA = calibrationCyclesRequired;
-}
-
-bool accIsCalibrationComplete(void)
-{
-    return calibratingA == 0;
-}
-
-static bool isOnFinalAccelerationCalibrationCycle(void)
-{
-    return calibratingA == 1;
-}
-
-static bool isOnFirstAccelerationCalibrationCycle(void)
-{
-    return calibratingA == CALIBRATING_ACC_CYCLES;
-}
-
 static bool calibratedPosition[6];
 static int32_t accSamples[6][3];
 static int  calibratedAxisCount = 0;
@@ -406,17 +387,21 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
         return -1;
 }
 
-static void performAcclerationCalibration(void)
+bool accIsCalibrationComplete(void)
+{
+    return zeroCalibrationIsCompleteV(&zeroCalibration);
+}
+
+void accStartCalibration(void)
 {
     int positionIndex = getPrimaryAxisIndex(accADC);
 
-    // Check if sample is usable
     if (positionIndex < 0) {
         return;
     }
 
-    // Top-up and first calibration cycle, reset everything
+    // Top+up and first calibration cycle, reset everything
-    if (positionIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
+    if (positionIndex == 0) {
         for (int axis = 0; axis < 6; axis++) {
             calibratedPosition[axis] = false;
             accSamples[axis][X] = 0;
@@ -428,19 +413,40 @@ static void performAcclerationCalibration(void)
         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
     }
 
+    zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.01f, true);
+}
+
+static void performAcclerationCalibration(void)
+{
+    fpVector3_t v;
+    int positionIndex = getPrimaryAxisIndex(accADC);
+
+    // Check if sample is usable
+    if (positionIndex < 0) {
+        return;
+    }
+
     if (!calibratedPosition[positionIndex]) {
-        accSamples[positionIndex][X] += accADC[X];
+        v.v[0] = accADC[0];
-        accSamples[positionIndex][Y] += accADC[Y];
+        v.v[1] = accADC[1];
-        accSamples[positionIndex][Z] += accADC[Z];
+        v.v[2] = accADC[2];
 
-        if (isOnFinalAccelerationCalibrationCycle()) {
+        zeroCalibrationAddValueV(&zeroCalibration, &v);
-            calibratedPosition[positionIndex] = true;
 
-            accSamples[positionIndex][X] = accSamples[positionIndex][X] / CALIBRATING_ACC_CYCLES;
+        if (zeroCalibrationIsCompleteV(&zeroCalibration)) {
-            accSamples[positionIndex][Y] = accSamples[positionIndex][Y] / CALIBRATING_ACC_CYCLES;
+            if (zeroCalibrationIsSuccessfulV(&zeroCalibration)) {
-            accSamples[positionIndex][Z] = accSamples[positionIndex][Z] / CALIBRATING_ACC_CYCLES;
+                zeroCalibrationGetZeroV(&zeroCalibration, &v);
 
-            calibratedAxisCount++;
+                accSamples[positionIndex][X] = v.v[X];
+                accSamples[positionIndex][Y] = v.v[Y];
+                accSamples[positionIndex][Z] = v.v[Z];
+
+                calibratedPosition[positionIndex] = true;
+                calibratedAxisCount++;
+            }
+            else {
+                calibratedPosition[positionIndex] = false;
+            }
 
             beeperConfirmationBeeps(2);
         }
@@ -459,9 +465,9 @@ static void performAcclerationCalibration(void)
 
         sensorCalibrationSolveForOffset(&calState, accTmp);
 
-        for (int axis = 0; axis < 3; axis++) {
+        accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
-            accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]);
+        accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
-        }
+        accelerometerConfigMutable()->accZero.raw[Z] = lrintf(accTmp[Z]);
 
         /* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
         sensorCalibrationResetState(&calState);
@@ -484,8 +490,6 @@ static void performAcclerationCalibration(void)
 
         saveConfigAndNotify();
     }
-
-    calibratingA--;
 }
 
 static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)

src/main/sensors/acceleration.h

@@ -73,7 +73,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
 
 bool accInit(uint32_t accTargetLooptime);
 bool accIsCalibrationComplete(void);
-void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+void accStartCalibration(void);
 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
 void accGetVibrationLevels(fpVector3_t *accVibeLevels);
 float accGetVibrationLevel(void);

src/main/sensors/barometer.c

@@ -26,6 +26,7 @@
 #include "common/maths.h"
 #include "common/time.h"
 #include "common/utils.h"
+#include "common/calibration.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -66,8 +67,7 @@ PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
 
 #ifdef USE_BARO
 
-static timeMs_t baroCalibrationTimeout = 0;
+static zeroCalibrationScalar_t zeroCalibration;
-static bool baroCalibrationFinished = false;
 static float baroGroundAltitude = 0;
 static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere
 
@@ -183,17 +183,6 @@ bool baroInit(void)
     return true;
 }
 
-bool baroIsCalibrationComplete(void)
-{
-    return baroCalibrationFinished;
-}
-
-void baroStartCalibration(void)
-{
-    baroCalibrationTimeout = millis();
-    baroCalibrationFinished = false;
-}
-
 #define PRESSURE_SAMPLES_MEDIAN 3
 
 /*
@@ -275,27 +264,33 @@ static float pressureToAltitude(const float pressure)
     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 }
 
-static void performBaroCalibrationCycle(void)
+static float altitudeToPressure(const float altCm)
 {
-    const float baroGroundPressureError = baro.baroPressure - baroGroundPressure;
+    return powf(1.0f - (altCm / 4433000.0f), 5.254999) * 101325.0f;
-    baroGroundPressure += baroGroundPressureError * 0.15f;
+}
 
-    if (fabsf(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) {    // 0.005% calibration error (should give c. 10cm calibration error)
+bool baroIsCalibrationComplete(void)
-        if ((millis() - baroCalibrationTimeout) > 250) {
+{
-            baroGroundAltitude = pressureToAltitude(baroGroundPressure);
+    return zeroCalibrationIsCompleteS(&zeroCalibration) && zeroCalibrationIsSuccessfulS(&zeroCalibration);
-            baroCalibrationFinished = true;
+}
-            DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
+
-        }
+void baroStartCalibration(void)
-    }
+{
-    else {
+    const float acceptedPressureVariance = (101325.0f - altitudeToPressure(30.0f)); // max 30cm deviation during calibration (at sea level)
-        baroCalibrationTimeout = millis();
+    zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
-    }
 }
 
 int32_t baroCalculateAltitude(void)
 {
     if (!baroIsCalibrationComplete()) {
-        performBaroCalibrationCycle();
+        zeroCalibrationAddValueS(&zeroCalibration, baro.baroPressure);
+
+        if (zeroCalibrationIsCompleteS(&zeroCalibration)) {
+            zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure);
+            baroGroundAltitude = pressureToAltitude(baroGroundPressure);
+            DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
+        }
+
         baro.BaroAlt = 0;
     }
     else {

src/main/sensors/gyro.c

@@ -27,6 +27,7 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/calibration.h"
 #include "common/filter.h"
 #include "common/utils.h"
 
@@ -74,13 +75,7 @@ FASTRAM gyro_t gyro; // gyro sensor object
 STATIC_UNIT_TESTED gyroDev_t gyroDev0;  // Not in FASTRAM since it may hold DMA buffers
 STATIC_FASTRAM int16_t gyroTemperature0;
 
-typedef struct gyroCalibration_s {
+STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration;
-    int32_t g[XYZ_AXIS_COUNT];
-    stdev_t var[XYZ_AXIS_COUNT];
-    uint16_t calibratingG;
-} gyroCalibration_t;
-
-STATIC_FASTRAM_UNIT_TESTED gyroCalibration_t gyroCalibration;
 STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
 
 STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
@@ -344,64 +339,42 @@ void gyroInitFilters(void)
 #endif
 }
 
-void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
+void gyroStartCalibration(void)
 {
-    gyroCalibration.calibratingG = calibrationCyclesRequired;
+    zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
-}
-
-STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration)
-{
-    return gyroCalibration->calibratingG == 0;
 }
 
 bool gyroIsCalibrationComplete(void)
 {
-    return gyroCalibration.calibratingG == 0;
+    return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
 }
 
-static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
 {
-    return gyroCalibration->calibratingG == 1;
+    fpVector3_t v;
-}
 
-static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
+    // Consume gyro reading
-{
+    v.v[0] = dev->gyroADCRaw[0];
-    return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES;
+    v.v[1] = dev->gyroADCRaw[1];
-}
+    v.v[2] = dev->gyroADCRaw[2];
 
-STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
+    zeroCalibrationAddValueV(gyroCalibration, &v);
-{
-    for (int axis = 0; axis < 3; axis++) {
-        // Reset g[axis] at start of calibration
-        if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
-            gyroCalibration->g[axis] = 0;
-            devClear(&gyroCalibration->var[axis]);
-        }
 
-        // Sum up CALIBRATING_GYRO_CYCLES readings
+    // Check if calibration is complete after this cycle
-        gyroCalibration->g[axis] += dev->gyroADCRaw[axis];
+    if (zeroCalibrationIsCompleteV(gyroCalibration)) {
-        devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]);
+        zeroCalibrationGetZeroV(gyroCalibration, &v);
+        dev->gyroZero[0] = v.v[0];
+        dev->gyroZero[1] = v.v[1];
+        dev->gyroZero[2] = v.v[2];
 
-        // gyroZero is set to zero until calibration complete
-        dev->gyroZero[axis] = 0;
-
-        if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
-            const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
-            // check deviation and start over if the model was moved
-            if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
-                gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
-                return;
-            }
-            // calibration complete, so set the gyro zero values
-            dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
-        }
-    }
-
-    if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
         DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]);
         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
     }
-    gyroCalibration->calibratingG--;
+    else {
+        dev->gyroZero[0] = 0;
+        dev->gyroZero[1] = 0;
+        dev->gyroZero[2] = 0;
+    }
 }
 
 /*
@@ -418,7 +391,7 @@ void gyroUpdate()
 {
     // range: +/- 8192; +/- 2000 deg/sec
     if (gyroDev0.readFn(&gyroDev0)) {
-        if (isCalibrationComplete(&gyroCalibration)) {
+        if (zeroCalibrationIsCompleteV(&gyroCalibration)) {
             // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
             gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
             gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
@@ -426,7 +399,7 @@ void gyroUpdate()
             applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign);
             applyBoardAlignment(gyroADC);
         } else {
-            performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
+            performGyroCalibration(&gyroDev0, &gyroCalibration);
             // Reset gyro values to zero to prevent other code from using uncalibrated data
             gyro.gyroADCf[X] = 0.0f;
             gyro.gyroADCf[Y] = 0.0f;

src/main/sensors/gyro.h

@@ -68,7 +68,7 @@ bool gyroInit(void);
 void gyroInitFilters(void);
 void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
 void gyroUpdate();
-void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+void gyroStartCalibration(void);
 bool gyroIsCalibrationComplete(void);
 bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);

src/main/sensors/pitotmeter.c

@@ -153,27 +153,21 @@ bool pitotInit(void)
 
 bool pitotIsCalibrationComplete(void)
 {
-    return pitot.calibrationFinished;
+    return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
 }
 
 void pitotStartCalibration(void)
 {
-    pitot.calibrationTimeoutMs = millis();
+    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false);
-    pitot.calibrationFinished = false;
 }
 
 static void performPitotCalibrationCycle(void)
 {
-    const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero;
+    zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
-    pitot.pressureZero += pitotPressureZeroError * 0.25f;
 
-    if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) {
+    if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
-        if ((millis() - pitot.calibrationTimeoutMs) > 500) {
+        zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
-            pitot.calibrationFinished = true;
+        DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero));
-        }
-    }
-    else {
-        pitot.calibrationTimeoutMs = millis();
     }
 }
 

src/main/sensors/pitotmeter.h

@@ -19,6 +19,7 @@
 
 #include "config/parameter_group.h"
 #include "common/filter.h"
+#include "common/calibration.h"
 
 #include "drivers/pitotmeter.h"
 
@@ -46,11 +47,10 @@ typedef struct pito_s {
     pitotDev_t dev;
     float airSpeed;
 
+    zeroCalibrationScalar_t zeroCalibration;
     pt1Filter_t lpfState;
     timeUs_t lastMeasurementUs;
     timeMs_t lastSeenHealthyMs;
-    timeMs_t calibrationTimeoutMs;
-    bool calibrationFinished;
 
     float pressureZero;
     float pressure;

src/main/sensors/sensors.h

@@ -39,8 +39,10 @@ typedef union flightDynamicsTrims_u {
     flightDynamicsTrims_def_t values;
 } flightDynamicsTrims_t;
 
-#define CALIBRATING_GYRO_CYCLES             1000
+#define CALIBRATING_BARO_TIME_MS            2000
-#define CALIBRATING_ACC_CYCLES              400
+#define CALIBRATING_PITOT_TIME_MS           2000
+#define CALIBRATING_GYRO_TIME_MS            2000
+#define CALIBRATING_ACC_TIME_MS             500
 
 // These bits have to be aligned with sensorIndex_e
 typedef enum {

src/test/Makefile

@@ -105,6 +105,15 @@ $(OBJECT_DIR)/common/maths.o : \
 	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
 
 
+$(OBJECT_DIR)/common/calibration.o : \
+	$(USER_DIR)/common/calibration.c \
+	$(USER_DIR)/common/calibration.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/calibration.c -o $@
+
+
 $(OBJECT_DIR)/common/bitarray.o : \
 	$(USER_DIR)/common/bitarray.c \
 	$(USER_DIR)/common/bitarray.h \
@@ -639,6 +648,7 @@ $(OBJECT_DIR)/sensor_gyro_unittest.o : \
 $(OBJECT_DIR)/sensor_gyro_unittest : \
 	$(OBJECT_DIR)/build/debug.o \
 	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/common/calibration.o \
 	$(OBJECT_DIR)/common/filter.o \
 	$(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o \
 	$(OBJECT_DIR)/sensors/gyro.o \

src/test/unit/maths_unittest.cc

@@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b)
 
 TEST(MathsUnittest, TestRotateVectorWithNoAngle)
 {
-    fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
+    fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
     fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
 
     fpMat3_t rmat;
     rotationMatrixFromAngles(&rmat, &euler_angles);
     rotationMatrixRotateVector(&vector, &vector, &rmat);
 
-    fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
+    fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
     expectVectorsAreEqual(&vector, &expected_result);
 }
 
 TEST(MathsUnittest, TestRotateVectorAroundAxis)
 {
     // Rotate a vector <1, 0, 0> around an each axis x y and z.
-    fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
+    fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
     fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
 
     fpMat3_t rmat;
     rotationMatrixFromAngles(&rmat, &euler_angles);
     rotationMatrixRotateVector(&vector, &vector, &rmat);
 
-    fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
+    fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
     expectVectorsAreEqual(&vector, &expected_result);
 }
 

src/test/unit/sensor_gyro_unittest.cc

@@ -28,6 +28,7 @@ extern "C" {
     #include "build/debug.h"
     #include "common/axis.h"
     #include "common/maths.h"
+    #include "common/calibration.h"
     #include "common/utils.h"
     #include "drivers/accgyro/accgyro_fake.h"
     #include "drivers/logging_codes.h"
@@ -37,17 +38,11 @@ extern "C" {
     #include "sensors/acceleration.h"
     #include "sensors/sensors.h"
 
-    typedef struct gyroCalibration_s {
+    extern zeroCalibrationVector_t gyroCalibration;
-        int32_t g[XYZ_AXIS_COUNT];
-        stdev_t var[XYZ_AXIS_COUNT];
-        uint16_t calibratingG;
-    } gyroCalibration_t;
-    extern gyroCalibration_t gyroCalibration;
     extern gyroDev_t gyroDev0;
 
     STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
-    STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold);
+    STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
-    STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration);
 }
 
 #include "unittest_macros.h"
@@ -81,7 +76,7 @@ TEST(SensorGyro, Read)
 
 TEST(SensorGyro, Calibrate)
 {
-    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+    gyroStartCalibration();
     gyroInit();
     fakeGyroSet(5, 6, 7);
     const bool read = gyroDev0.readFn(&gyroDev0);
@@ -89,17 +84,16 @@ TEST(SensorGyro, Calibrate)
     EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
     EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
     EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
-    static const int gyroMovementCalibrationThreshold = 32;
     gyroDev0.gyroZero[X] = 8;
     gyroDev0.gyroZero[Y] = 9;
     gyroDev0.gyroZero[Z] = 10;
-    performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
+    performGyroCalibration(&gyroDev0, &gyroCalibration);
     EXPECT_EQ(0, gyroDev0.gyroZero[X]);
     EXPECT_EQ(0, gyroDev0.gyroZero[Y]);
     EXPECT_EQ(0, gyroDev0.gyroZero[Z]);
-    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    EXPECT_EQ(false, gyroIsCalibrationComplete());
-    while (!isCalibrationComplete(&gyroCalibration)) {
+    while (!gyroIsCalibrationComplete()) {
-        performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
+        performGyroCalibration(&gyroDev0, &gyroCalibration);
     }
     EXPECT_EQ(5, gyroDev0.gyroZero[X]);
     EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
@@ -108,16 +102,16 @@ TEST(SensorGyro, Calibrate)
 
 TEST(SensorGyro, Update)
 {
-    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+    gyroStartCalibration();
-    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    EXPECT_EQ(false, gyroIsCalibrationComplete());
     gyroInit();
     fakeGyroSet(5, 6, 7);
     gyroUpdate();
-    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    EXPECT_EQ(false, gyroIsCalibrationComplete());
-    while (!isCalibrationComplete(&gyroCalibration)) {
+    while (!gyroIsCalibrationComplete()) {
         gyroUpdate();
     }
-    EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration));
+    EXPECT_EQ(true, gyroIsCalibrationComplete());
     EXPECT_EQ(5, gyroDev0.gyroZero[X]);
     EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
     EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
@@ -140,7 +134,9 @@ TEST(SensorGyro, Update)
 // STUBS
 
 extern "C" {
+static timeMs_t milliTime = 0;
 
+timeMs_t millis(void) {return milliTime++;}
 uint32_t micros(void) {return 0;}
 void beeper(beeperMode_e) {}
 uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };