Allow gyro read and filter in ISR for SPI gyros

src/main/drivers/accgyro.h

@@ -40,6 +40,7 @@ typedef struct gyroDev_s {
     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];

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/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/serial_cli.c

@@ -601,6 +601,9 @@ const clivalue_t valueTable[] = {
 
     { "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 } },
+#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 } },
+#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 } },

src/main/sensors/gyro.c

@@ -364,20 +364,62 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
 
 }
 
+#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 +427,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,7 @@ 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;
     uint16_t gyro_soft_notch_hz_1;
     uint16_t gyro_soft_notch_cutoff_1;
     uint16_t gyro_soft_notch_hz_2;