Better way of fusing gyros together using variance.

src/main/cli/cli.c

@@ -3632,11 +3632,11 @@ static void cliDumpGyroRegisters(const char *cmdName, char *cmdline)
     UNUSED(cmdline);
 
 #ifdef USE_MULTI_GYRO
-    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
+    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE)) {
         cliPrintLinef("\r\n# Gyro 1");
         cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
     }
-    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
+    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE)) {
         cliPrintLinef("\r\n# Gyro 2");
         cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2);
     }

src/main/cli/settings.c

@@ -191,7 +191,7 @@ static const char * const lookupTableAlignment[] = {
 
 #ifdef USE_MULTI_GYRO
 static const char * const lookupTableGyro[] = {
-    "FIRST", "SECOND", "BOTH"
+     "FIRST", "SECOND", "BOTH_SIMPLE", "BOTH_VARIANCE"
 };
 #endif
 

src/main/cms/cms_menu_imu.c

@@ -81,7 +81,7 @@ static const char * const osdTableThrottleLimitType[] = {
 
 #ifdef USE_MULTI_GYRO
 static const char * const osdTableGyroToUse[] = {
-    "FIRST", "SECOND", "BOTH"
+    "FIRST", "SECOND", "BOTH_SIMP", "BOTH_VAR"
 };
 #endif
 
@@ -742,7 +742,7 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 } },
     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
 #ifdef USE_MULTI_GYRO
-    { "GYRO TO USE",  OME_TAB | REBOOT_REQUIRED,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse} },
+    { "GYRO TO USE",  OME_TAB | REBOOT_REQUIRED,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  3, osdTableGyroToUse} },
 #endif
 
     { "BACK", OME_Back, NULL, NULL },

src/main/drivers/accgyro/accgyro.h

@@ -38,6 +38,7 @@
 
 #define GYRO_SCALE_2000DPS (2000.0f / (1 << 15))   // 16.384 dps/lsb scalefactor for 2000dps sensors
 #define GYRO_SCALE_4000DPS (4000.0f / (1 << 15))   //  8.192 dps/lsb scalefactor for 4000dps sensors
+#define GYRO_VARIANCE_WINDOW 8
 
 typedef enum {
     GYRO_NONE = 0,
@@ -90,6 +91,18 @@ typedef enum {
     GYRO_EXTI_NO_INT
 } gyroModeSPI_e;
 
+typedef struct gyroVariance_s {
+    uint16_t windex;
+    uint16_t w;
+    float axisWindow[GYRO_VARIANCE_WINDOW];
+    float varianceWindow[GYRO_VARIANCE_WINDOW];
+    float axisSumVar;
+    float axisVar;
+    float axisSumMean;
+    float axisMean;
+    float inverseN;
+} gyroVariance_t;
+
 typedef struct gyroDev_s {
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
     pthread_mutex_t lock;
@@ -128,6 +141,7 @@ typedef struct gyroDev_s {
     fp_rotationMatrix_t rotationMatrix;
     uint16_t gyroSampleRateHz;
     uint16_t accSampleRateHz;
+    gyroVariance_t variance[XYZ_AXIS_COUNT];
     uint8_t accDataReg;
     uint8_t gyroDataReg;
 } gyroDev_t;

src/main/msp/msp.c

@@ -1741,7 +1741,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         case GYRO_CONFIG_USE_GYRO_2:
             gyroAlignment = gyroDeviceConfig(1)->alignment;
             break;
-        case GYRO_CONFIG_USE_GYRO_BOTH:
+        case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
+        case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
             // for dual-gyro in "BOTH" mode we only read/write gyro 0
         default:
             gyroAlignment = gyroDeviceConfig(0)->alignment;
@@ -2813,7 +2814,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             case GYRO_CONFIG_USE_GYRO_2:
                 gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
                 break;
-            case GYRO_CONFIG_USE_GYRO_BOTH:
+            case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
+            case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
                 // For dual-gyro in "BOTH" mode we'll only update gyro 0
             default:
                 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;

src/main/sensors/gyro.c

@@ -26,6 +26,8 @@
 
 #include "platform.h"
 
+#include "arm_math.h"
+
 #include "build/debug.h"
 
 #include "common/axis.h"
@@ -151,7 +153,8 @@ FAST_CODE bool gyroIsCalibrationComplete(void)
         case GYRO_CONFIG_USE_GYRO_2: {
             return isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
         }
-        case GYRO_CONFIG_USE_GYRO_BOTH: {
+        case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
+        case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE: {
             return isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
         }
 #endif
@@ -239,6 +242,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
             if (axis == Z) {
               gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
             }
+
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                gyroSensor->gyroDev.variance[axis].inverseN = 1.0f / 8.0f;
+                gyroSensor->gyroDev.variance[axis].w = 8;
+            }
         }
     }
 
@@ -402,6 +410,32 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
     }
 }
 
+FAST_CODE float gyroVariance(gyroVariance_t *variance, float gyroRate) {
+    variance->axisWindow[variance->windex] = gyroRate;
+    variance->axisSumMean += variance->axisWindow[variance->windex];
+
+    float varianceElement = variance->axisWindow[variance->windex] - variance->axisMean;
+    varianceElement = varianceElement * varianceElement;
+    variance->axisSumVar += varianceElement;
+    variance->varianceWindow[variance->windex] = varianceElement;
+    variance->windex++;
+
+    if (variance->windex > variance->w) {
+        variance->windex = 0;
+    }
+
+    variance->axisSumMean -= variance->axisWindow[variance->windex];
+    variance->axisSumVar -= variance->varianceWindow[variance->windex];
+
+    //New mean
+    variance->axisMean = variance->axisSumMean * variance->inverseN;
+    variance->axisVar = variance->axisSumVar * variance->inverseN;
+
+    float squirt = 0;
+    arm_sqrt_f32(variance->axisVar, &squirt);
+    return squirt;
+}
+
 FAST_CODE void gyroUpdate(void)
 {
     switch (gyro.gyroToUse) {
@@ -422,7 +456,7 @@ FAST_CODE void gyroUpdate(void)
             gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale;
         }
         break;
-    case GYRO_CONFIG_USE_GYRO_BOTH:
+     case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
         gyroUpdateSensor(&gyro.gyroSensor1);
         gyroUpdateSensor(&gyro.gyroSensor2);
         if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
@@ -431,6 +465,30 @@ FAST_CODE void gyroUpdate(void)
             gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
         }
         break;
+    case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
+        gyroUpdateSensor(&gyro.gyroSensor1);
+        gyroUpdateSensor(&gyro.gyroSensor2);
+        if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
+            float gyroScaled1, gyroScaled2, varianceGyro1, varianceGyro2;
+            gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale;
+            gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale;
+            varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[X], gyroScaled1);
+            varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[X], gyroScaled2);
+            gyro.gyroADC[X] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);
+
+            gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale;
+            gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale;
+            varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[Y], gyroScaled1);
+            varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[Y], gyroScaled2);
+            gyro.gyroADC[Y] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);
+
+            gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale;
+            gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale;
+            varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[Z], gyroScaled1);
+            varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[Z], gyroScaled2);
+            gyro.gyroADC[Z] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);
+        }
+        break;
 #endif
     }
 
@@ -495,7 +553,8 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale));
             break;
 
-    case GYRO_CONFIG_USE_GYRO_BOTH:
+        case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
+        case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
@@ -577,7 +636,8 @@ void gyroReadTemperature(void)
         gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor2);
         break;
 
-    case GYRO_CONFIG_USE_GYRO_BOTH:
+    case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
+    case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
         gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyro.gyroSensor1), gyroReadSensorTemperature(gyro.gyroSensor2));
         break;
 #endif // USE_MULTI_GYRO

src/main/sensors/gyro.h

@@ -155,7 +155,8 @@ typedef enum {
 
 #define GYRO_CONFIG_USE_GYRO_1      0
 #define GYRO_CONFIG_USE_GYRO_2      1
-#define GYRO_CONFIG_USE_GYRO_BOTH   2
+#define GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE   2
+#define GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE  3
 
 enum {
     FILTER_LPF1 = 0,

src/main/sensors/gyro_init.c

@@ -616,7 +616,7 @@ bool gyroInit(void)
     }
 
 #if defined(USE_MULTI_GYRO)
-    if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
+    if ((gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
         if (gyroDetectionFlags & GYRO_1_MASK) {
@@ -632,14 +632,14 @@ bool gyroInit(void)
     // Only allow using both gyros simultaneously if they are the same hardware type.
     if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
         gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
-    } else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+    } else if (gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
         // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
         gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
         eepromWriteRequired = true;
     }
 
-    if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+    if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
         static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
         // SPI DMA buffer required per device
         gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
@@ -655,7 +655,7 @@ bool gyroInit(void)
         writeEEPROM();
     }
 
-    if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+    if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
         static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
         // SPI DMA buffer required per device
         gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;

src/main/target/SPRACINGH7EXTREME/target.h

@@ -161,7 +161,7 @@
 #define GYRO_2_ALIGN        ALIGN_CUSTOM
 #define GYRO_2_CUSTOM_ALIGN SENSOR_ALIGNMENT(  0, 0, 225)
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE
 
 #define USE_FLASHFS
 #define USE_FLASH_TOOLS