rebase off of hydra branch

emuflight · Nov 22, 2022 · f8dc7e0 · f8dc7e0
1 parent 22c7f07
commit f8dc7e0
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 3 deletions.
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
@@ -99,5 +99,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
-    "RX_STATE_TIME"
+    "RX_STATE_TIME",
+    "FUSION",
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
@@ -98,6 +98,7 @@ typedef enum {
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
     DEBUG_RX_STATE_TIME,
+    DEBUG_FUSION,
     DEBUG_COUNT
 } debugType_e;
 

diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
@@ -38,7 +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
+#define GYRO_VARIANCE_WINDOW 9
 
 typedef enum {
     GYRO_NONE = 0,

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -244,6 +244,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
             }
 
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                memset(&gyroSensor->gyroDev.variance[axis], 0, sizeof(gyroVariance_t));
                 gyroSensor->gyroDev.variance[axis].inverseN = 1.0f / 8.0f;
                 gyroSensor->gyroDev.variance[axis].w = 8;
             }
@@ -433,7 +434,7 @@ FAST_CODE float gyroVariance(gyroVariance_t *variance, float gyroRate) {
 
     float squirt = 0;
     arm_sqrt_f32(variance->axisVar, &squirt);
-    return squirt;
+    return squirt * 100.0f + 1.0f;
 }
 
 FAST_CODE void gyroUpdate(void)
@@ -475,6 +476,10 @@ FAST_CODE void gyroUpdate(void)
             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);
+            DEBUG_SET(DEBUG_FUSION, 0, lrintf(gyroScaled1));
+            DEBUG_SET(DEBUG_FUSION, 1, lrintf(gyroScaled2));
+            DEBUG_SET(DEBUG_FUSION, 2, lrintf(varianceGyro1));
+            DEBUG_SET(DEBUG_FUSION, 3, lrintf(varianceGyro2));
 
             gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale;
             gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale;