diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 5298958ae7..f7193debee 100644 --- a/src/main/cli/cli.c +++ b/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); } diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 819b5ae15c..d2d43aecbe 100644 --- a/src/main/cli/settings.c +++ b/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 diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index edb0be75b6..fffd38ff43 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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 }, diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 29ba307ac5..bb3a832e9f 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/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; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index d2b60a254d..b7b4e26bfd 100644 --- a/src/main/msp/msp.c +++ b/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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index abccba6d9b..c4dbf3de41 100644 --- a/src/main/sensors/gyro.c +++ b/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 diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5ff3eb8889..6fb98d78ac 100644 --- a/src/main/sensors/gyro.h +++ b/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, diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 0d434a9a89..aba0ab1f23 100644 --- a/src/main/sensors/gyro_init.c +++ b/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; diff --git a/src/main/target/SPRACINGH7EXTREME/target.h b/src/main/target/SPRACINGH7EXTREME/target.h index 3af9f095fe..00ff199d02 100644 --- a/src/main/target/SPRACINGH7EXTREME/target.h +++ b/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