Added support for 32kHz gyro updates

src/main/drivers/accgyro.h

@@ -26,6 +26,10 @@
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
+#if defined(USE_GYRO_SPI_MPU6500) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+#define GYRO_SUPPORTS_32KHZ
+#endif
+
 #define GYRO_LPF_256HZ      0
 #define GYRO_LPF_188HZ      1
 #define GYRO_LPF_98HZ       2
@@ -35,6 +39,12 @@
 #define GYRO_LPF_5HZ        6
 #define GYRO_LPF_NONE       7
 
+typedef enum {
+    GYRO_RATE_1_kHz,
+    GYRO_RATE_8_kHz,
+    GYRO_RATE_32_kHz,
+} gyroRateKHz_e;
+
 typedef struct gyroDev_s {
     sensorGyroInitFuncPtr init;                             // initialize function
     sensorGyroReadFuncPtr read;                             // read 3 axis data function
@@ -44,7 +54,9 @@ typedef struct gyroDev_s {
     extiCallbackRec_t exti;
     float scale;                                            // scalefactor
     int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-    uint16_t lpf;
+    uint8_t lpf;
+    gyroRateKHz_e gyroRateKHz;
+    uint8_t mpuDividerDrops;
     volatile bool dataReady;
     sensor_align_e gyroAlign;
     mpuDetectionResult_t mpuDetectionResult;

src/main/drivers/accgyro_mpu6050.c

@@ -82,7 +82,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
     gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
     gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
-    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
     gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec

src/main/drivers/accgyro_mpu6500.c

@@ -63,13 +63,14 @@ void mpu6500GyroInit(gyroDev_t *gyro)
     delay(100);
     gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
     delay(15);
-    gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
+    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
+    gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
     delay(15);
     gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
     gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
-    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
+    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
     delay(100);
 
     // Data ready interrupt configuration

src/main/drivers/accgyro_spi_icm20689.c

@@ -138,13 +138,14 @@ void icm20689GyroInit(gyroDev_t *gyro)
 //    delay(100);
     gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
     delay(15);
-    gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
+    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
+    gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
     delay(15);
     gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
     gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
-    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
+    gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
     delay(100);
 
     // Data ready interrupt configuration

src/main/drivers/accgyro_spi_mpu6000.c

@@ -45,7 +45,7 @@
 
 #include "accgyro_spi_mpu6000.h"
 
-static void mpu6000AccAndGyroInit(void);
+static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
 
 static bool mpuSpi6000InitDone = false;
 
@@ -128,7 +128,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
-    mpu6000AccAndGyroInit();
+    mpu6000AccAndGyroInit(gyro);
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
@@ -201,7 +201,7 @@ bool mpu6000SpiDetect(void)
     return false;
 }
 
-static void mpu6000AccAndGyroInit(void)
+static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
 {
     if (mpuSpi6000InitDone) {
         return;
@@ -229,7 +229,7 @@ static void mpu6000AccAndGyroInit(void)
 
     // Accel Sample Rate 1kHz
     // Gyroscope Output Rate =  1kHz when the DLPF is enabled
-    mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
+    mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
     delayMicroseconds(15);
 
     // Gyro +/- 1000 DPS Full Scale

src/main/drivers/accgyro_spi_mpu9250.c

@@ -46,7 +46,7 @@
 #include "accgyro_mpu.h"
 #include "accgyro_spi_mpu9250.h"
 
-static void mpu9250AccAndGyroInit(uint8_t lpf);
+static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
 
 static bool mpuSpi9250InitDone = false;
 
@@ -100,7 +100,7 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
-    mpu9250AccAndGyroInit(gyro->lpf);
+    mpu9250AccAndGyroInit(gyro);
 
     spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 
@@ -140,7 +140,7 @@ bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
     return false;
 }
 
-static void mpu9250AccAndGyroInit(uint8_t lpf) {
+static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
 
     if (mpuSpi9250InitDone) {
         return;
@@ -153,17 +153,19 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) {
 
     verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 
-    verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
+    //Fchoice_b defaults to 00 which makes fchoice 11
+    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
+    verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
 
-    if (lpf == 4) {
+    if (gyro->lpf == 4) {
         verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
-    } else if (lpf < 4) {
+    } else if (gyro->lpf < 4) {
         verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
-    } else if (lpf > 4) {
+    } else if (gyro->lpf > 4) {
         verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
     }
 
-    verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
+    verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
 
     verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
     verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN

src/main/drivers/gyro_sync.c

@@ -14,7 +14,6 @@
 #include "accgyro.h"
 #include "gyro_sync.h"
 
-static uint8_t mpuDividerDrops;
 
 bool gyroSyncCheckUpdate(gyroDev_t *gyro)
 {
@@ -23,24 +22,35 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
     return gyro->intStatus(gyro);
 }
 
-uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
+uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz)
 {
+#ifndef GYRO_SUPPORTS_32KHZ
+    if (gyro_use_32khz) {
+        gyro_use_32khz = false;
+    }
+#endif
     int gyroSamplePeriod;
 
     if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
+        gyro->gyroRateKHz = GYRO_RATE_8_kHz;
         gyroSamplePeriod = 125;
     } else {
+        gyro->gyroRateKHz = GYRO_RATE_1_kHz;
         gyroSamplePeriod = 1000;
         gyroSyncDenominator = 1; // Always full Sampling 1khz
     }
 
     // calculate gyro divider and targetLooptime (expected cycleTime)
-    mpuDividerDrops  = gyroSyncDenominator - 1;
+    gyro->mpuDividerDrops  = gyroSyncDenominator - 1;
-    const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
+    uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
+    if (gyro_use_32khz) {
+        gyro->gyroRateKHz = GYRO_RATE_32_kHz;
+        targetLooptime /= 4;
+    }
     return targetLooptime;
 }
 
-uint8_t gyroMPU6xxxGetDividerDrops(void)
+uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro)
 {
-    return mpuDividerDrops;
+    return gyro->mpuDividerDrops;
 }

src/main/drivers/gyro_sync.h

@@ -5,8 +5,8 @@
  *      Author: borisb
  */
 
-struct gyroDev_s;
+#include "drivers/accgyro.h"
 
-bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
+bool gyroSyncCheckUpdate(gyroDev_t *gyro);
-uint8_t gyroMPU6xxxGetDividerDrops(void);
+uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro);
-uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
+uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz);

src/main/drivers/nvic.h

@@ -6,9 +6,9 @@
 #define NVIC_PRIO_MAX                      NVIC_BUILD_PRIORITY(0, 1)
 #define NVIC_PRIO_TIMER                    NVIC_BUILD_PRIORITY(1, 1)
 #define NVIC_PRIO_BARO_EXTI                NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_SONAR_EXTI               NVIC_BUILD_PRIORITY(2, 0)  // maybe increate slightly
+#define NVIC_PRIO_SONAR_EXTI               NVIC_BUILD_PRIORITY(2, 0)  // maybe increase slightly
 #define NVIC_PRIO_TRANSPONDER_DMA          NVIC_BUILD_PRIORITY(3, 0)
-#define NVIC_PRIO_MPU_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
+#define NVIC_PRIO_MPU_INT_EXTI             NVIC_BUILD_PRIORITY(2, 0)
 #define NVIC_PRIO_MAG_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 #define NVIC_PRIO_WS2811_DMA               NVIC_BUILD_PRIORITY(1, 2)  // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
 #define NVIC_PRIO_SERIALUART1_TXDMA        NVIC_BUILD_PRIORITY(1, 1)

src/main/fc/config.c

@@ -1043,6 +1043,14 @@ void validateAndFixGyroConfig(void)
 
     float samplingTime = 0.000125f;
 
+    if (gyroConfig()->gyro_use_32khz) {
+#ifdef GYRO_SUPPORTS_32KHZ
+        samplingTime = 0.00003125;
+#else
+        gyroConfig()->gyro_use_32khz = false;
+#endif
+    }
+
     if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
         pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
         gyroConfig()->gyro_sync_denom = 1;

src/main/fc/fc_msp.c

@@ -1140,6 +1140,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
         sbufWriteU16(dst, motorConfig()->motorPwmRate);
         sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100));
+        //!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator
+        //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
+        //sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
         break;
 
     case MSP_FILTER_CONFIG :
@@ -1483,6 +1486,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         if (dataSize > 7) {
             motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
         }
+        //!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator
+        /*if (sbufBytesRemaining(src)) {
+            gyroConfig()->gyro_isr_update = sbufReadU8(src);
+        }
+        if (sbufBytesRemaining(src)) {
+            gyroConfig()->gyro_use_32khz = sbufReadU8(src);
+        }*/
+        validateAndFixGyroConfig();
         break;
 
     case MSP_SET_FILTER_CONFIG:

src/main/fc/serial_cli.c

@@ -600,9 +600,12 @@ const clivalue_t valueTable[] = {
     { "align_board_yaw",            VAR_INT16  | MASTER_VALUE,  &boardAlignment()->yawDegrees, .config.minmax = { -180,  360 } },
 
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
-    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &gyroConfig()->gyro_sync_denom, .config.minmax = { 1,  8 } },
+    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &gyroConfig()->gyro_sync_denom, .config.minmax = { 1,  32 } },
 #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
     { "gyro_isr_update",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &gyroConfig()->gyro_isr_update, .config.lookup = { TABLE_OFF_ON } },
+#ifdef GYRO_SUPPORTS_32KHZ
+    { "gyro_use_32khz",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &gyroConfig()->gyro_use_32khz, .config.lookup = { TABLE_OFF_ON } },
+#endif
 #endif
     { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
     { "gyro_lowpass",               VAR_UINT8  | MASTER_VALUE,  &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0,  255 } },
@@ -3226,7 +3229,7 @@ static void cliTasks(char *cmdline)
     int averageLoadSum = 0;
 
 #ifndef CLI_MINIMAL_VERBOSITY
-    cliPrintf("Task list          rate/hz  max/us  avg/us maxload avgload     total/ms\r\n");
+    cliPrintf("Task list           rate/hz  max/us  avg/us maxload avgload     total/ms\r\n");
 #endif
     for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
         cfTaskInfo_t taskInfo;

src/main/sensors/gyro.c

@@ -240,7 +240,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
     if (!gyroDetect(&gyro.dev)) {
         return false;
     }
-    gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom);    // Set gyro sample rate before initialisation
+    // Must set gyro sample rate before initialisation
+    gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz);
     gyro.dev.lpf = gyroConfig->gyro_lpf;
     gyro.dev.init(&gyro.dev);
     gyroInitFilters();

src/main/sensors/gyro.h

@@ -52,6 +52,7 @@ typedef struct gyroConfig_s {
     uint8_t  gyro_soft_lpf_type;
     uint8_t  gyro_soft_lpf_hz;
     bool     gyro_isr_update;
+    bool     gyro_use_32khz;
     uint16_t gyro_soft_notch_hz_1;
     uint16_t gyro_soft_notch_cutoff_1;
     uint16_t gyro_soft_notch_hz_2;