Merge pull request #2014 from martinbudden/bf_gyro_isr

Allow gyro read and filter in ISR for SPI gyros

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,15 +39,24 @@
 #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
     sensorGyroReadDataFuncPtr temperature;                  // read temperature if available
     sensorGyroInterruptStatusFuncPtr intStatus;
+    sensorGyroUpdateFuncPtr update;
     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_mpu.c

@@ -22,6 +22,7 @@
 
 #include "platform.h"
 
+#include "build/atomic.h"
 #include "build/build_config.h"
 #include "build/debug.h"
 
@@ -46,7 +47,6 @@
 #include "accgyro_spi_mpu9250.h"
 #include "accgyro_mpu.h"
 
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 mpuResetFuncPtr mpuReset;
 
@@ -220,15 +220,20 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
 #if defined(MPU_INT_EXTI)
 static void mpuIntExtiHandler(extiCallbackRec_t *cb)
 {
+#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
+    static uint32_t lastCalledAtUs = 0;
+    const uint32_t nowUs = micros();
+    debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
+    lastCalledAtUs = nowUs;
+#endif
     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
     gyro->dataReady = true;
-
+    if (gyro->update) {
+        gyro->update(gyro);
+    }
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
-    static uint32_t lastCalledAt = 0;
+    const uint32_t now2Us = micros();
-    uint32_t now = micros();
+    debug[1] = (uint16_t)(now2Us - nowUs);
-    uint32_t callDelta = now - lastCalledAt;
-    debug[0] = callDelta;
-    lastCalledAt = now;
 #endif
 }
 #endif
@@ -296,6 +301,13 @@ bool mpuAccRead(accDev_t *acc)
     return true;
 }
 
+void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
+{
+    ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
+        gyro->update = updateFn;
+    }
+}
+
 bool mpuGyroRead(gyroDev_t *gyro)
 {
     uint8_t data[6];

src/main/drivers/accgyro_mpu.h

@@ -18,6 +18,13 @@
 #pragma once
 
 #include "exti.h"
+#include "sensor.h"
+
+//#define DEBUG_MPU_DATA_READY_INTERRUPT
+
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+#define GYRO_USES_SPI
+#endif
 
 // MPU6050
 #define MPU_RA_WHO_AM_I         0x75
@@ -190,3 +197,5 @@ bool mpuAccRead(struct accDev_s *acc);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
 bool mpuCheckDataReady(struct gyroDev_s *gyro);
+void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
+

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,37 @@ 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)
 {
-    int gyroSamplePeriod;
+#ifndef GYRO_SUPPORTS_32KHZ
+    if (gyro_use_32khz) {
+        gyro_use_32khz = false;
+    }
+#endif
+
+    float gyroSamplePeriod;
 
     if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
-        gyroSamplePeriod = 125;
+        if (gyro_use_32khz) {
+            gyro->gyroRateKHz = GYRO_RATE_32_kHz;
+            gyroSamplePeriod = 31.5f;
+        } else {
+            gyro->gyroRateKHz = GYRO_RATE_8_kHz;
+            gyroSamplePeriod = 125.0f;
+        }
     } else {
-        gyroSamplePeriod = 1000;
+        gyro->gyroRateKHz = GYRO_RATE_1_kHz;
+        gyroSamplePeriod = 1000.0f;
         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;
+    const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
     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/drivers/sensor.h

@@ -38,5 +38,6 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
 struct gyroDev_s;
 typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
 typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro);
 typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
 typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);

src/main/fc/config.c

@@ -1046,6 +1046,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));
+        sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
+        //!!TODO gyro_isr_update to be added pending decision
+        //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
         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;
         }
+        if (sbufBytesRemaining(src)) {
+            gyroConfig()->gyro_use_32khz = sbufReadU8(src);
+        }
+        //!!TODO gyro_isr_update to be added pending decision
+        /*if (sbufBytesRemaining(src)) {
+            gyroConfig()->gyro_isr_update = sbufReadU8(src);
+        }*/
+        validateAndFixGyroConfig();
         break;
 
     case MSP_SET_FILTER_CONFIG:

src/main/fc/serial_cli.c

@@ -600,7 +600,13 @@ 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 } },
     { "gyro_notch1_hz",             VAR_UINT16 | MASTER_VALUE,  &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0,  1000 } },
@@ -3223,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/scheduler/scheduler.c

@@ -185,6 +185,23 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
     }
 }
 
+void schedulerResetTaskStatistics(cfTaskId_e taskId)
+{
+#ifdef SKIP_TASK_STATISTICS
+    UNUSED(taskId);
+#else
+    if (taskId == TASK_SELF) {
+        currentTask->movingSumExecutionTime = 0;
+        currentTask->totalExecutionTime = 0;
+        currentTask->maxExecutionTime = 0;
+    } else if (taskId < TASK_COUNT) {
+        cfTasks[taskId].movingSumExecutionTime = 0;
+        cfTasks[taskId].totalExecutionTime = 0;
+        cfTasks[taskId].totalExecutionTime = 0;
+    }
+#endif
+}
+
 void schedulerInit(void)
 {
     queueClear();

src/main/scheduler/scheduler.h

@@ -144,6 +144,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
 void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
 uint32_t getTaskDeltaTime(cfTaskId_e taskId);
+void schedulerResetTaskStatistics(cfTaskId_e taskId);
 
 void schedulerInit(void);
 void scheduler(void);

src/main/sensors/gyro.c

@@ -54,6 +54,8 @@
 #include "io/beeper.h"
 #include "io/statusindicator.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
@@ -240,7 +242,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();
@@ -358,26 +361,69 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
     }
 
     if (isOnFinalGyroCalibrationCycle()) {
+        schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
         beeper(BEEPER_GYRO_CALIBRATED);
     }
     calibratingG--;
 
 }
 
+#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
+static bool gyroUpdateISR(gyroDev_t* gyroDev)
+{
+    if (!gyroDev->dataReady || !gyroDev->read(gyroDev)) {
+        return false;
+    }
+#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
+    debug[2] = (uint16_t)(micros() & 0xffff);
+#endif
+    gyroDev->dataReady = false;
+    // move gyro data into 32-bit variables to avoid overflows in calculations
+    gyroADC[X] = gyroDev->gyroADCRaw[X];
+    gyroADC[Y] = gyroDev->gyroADCRaw[Y];
+    gyroADC[Z] = gyroDev->gyroADCRaw[Z];
+
+    alignSensors(gyroADC, gyroDev->gyroAlign);
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        gyroADC[axis] -= gyroZero[axis];
+        // scale gyro output to degrees per second
+        float gyroADCf = (float)gyroADC[axis] * gyroDev->scale;
+        gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
+        gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
+        gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
+    }
+    return true;
+}
+#endif
+
 void gyroUpdate(void)
 {
     // range: +/- 8192; +/- 2000 deg/sec
-    if (!gyro.dev.read(&gyro.dev)) {
+    if (!gyro.dev.dataReady || !gyro.dev.read(&gyro.dev)) {
         return;
     }
+    const bool calibrationComplete = isGyroCalibrationComplete();
+    if (calibrationComplete) {
+#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
+        // SPI-based gyro so can read and update in ISR
+        if (gyroConfig->gyro_isr_update) {
+            mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
+            return;
+        }
+#endif
+#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
+        debug[3] = (uint16_t)(micros() & 0xffff);
+#endif
+    }
     gyro.dev.dataReady = false;
+    // move gyro data into 32-bit variables to avoid overflows in calculations
     gyroADC[X] = gyro.dev.gyroADCRaw[X];
     gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
     gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
 
     alignSensors(gyroADC, gyro.dev.gyroAlign);
 
-    const bool calibrationComplete = isGyroCalibrationComplete();
     if (!calibrationComplete) {
         performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }
@@ -385,17 +431,17 @@ void gyroUpdate(void)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         gyroADC[axis] -= gyroZero[axis];
         // scale gyro output to degrees per second
-        gyro.gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale;
+        float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
 
-        DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyro.gyroADCf[axis]));
+        DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
-        gyro.gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyro.gyroADCf[axis]);
+        gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
 
-        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyro.gyroADCf[axis]));
+        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
 
-        gyro.gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyro.gyroADCf[axis]);
+        gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
 
-        gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyro.gyroADCf[axis]);
+        gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
 
         if (!calibrationComplete) {
             gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);

src/main/sensors/gyro.h

@@ -51,6 +51,8 @@ typedef struct gyroConfig_s {
     uint8_t  gyro_lpf;                         // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
     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;