gyro: read the ICM426xx through its FIFO on data-ready (off by default)

docs/Settings.md

@@ -803,6 +803,7 @@ Defines debug values exposed in debug variables (developer / debugging setting)
 | ESC |  |
 | FW_TURN |  |
 | MAG |  |
+| GYRO_FIFO |  |
 
 ---
 
@@ -2140,7 +2141,7 @@ Gyro processing anti-aliasing filter cutoff frequency. In normal operation this
 
 ### gyro_data_ready
 
-Read the gyro when it signals a new sample, on its data-ready interrupt, instead of polling it: every sample once, and the CPU no longer waits on the bus. Needs a supported IMU (ICM-42605, ICM-42688-P) and a target that declares the interrupt pin. AUTO and ON do it, OFF polls the gyro.
+Read the gyro when it signals a new sample, on its data-ready interrupt, instead of polling it: every sample once, and the CPU no longer waits on the bus. Needs a supported IMU (ICM-42605, ICM-42688-P) and a target that declares the interrupt pin; without interrupts the gyro is read as before. AUTO and ON do it, OFF polls the gyro.
 
 | Allowed Values |  |
 | --- | --- |
@@ -2180,6 +2181,16 @@ Minimum frequency of the gyro Dynamic LPF
 
 ---
 
+### gyro_fifo_samples
+
+With gyro_data_ready, the IMU queues this many samples in its FIFO and signals once for all of them, read in one transfer: fewer interrupts, and a sample read late is kept, but the older samples of a group wait up to that many sample periods. 0 reads each sample from the IMU's registers. ICM-42605 and ICM-42688-P.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 4 |
+
+---
+
 ### gyro_filter_mode
 
 Specifies the type of the software LPF of the gyro signals.

src/main/build/debug.h

@@ -87,6 +87,7 @@ typedef enum {
     DEBUG_ESC,
     DEBUG_FW_TURN,
     DEBUG_MAG,
+    DEBUG_GYRO_FIFO,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/drivers/accgyro/accgyro.h

@@ -36,13 +36,19 @@ typedef struct {
     uint8_t gyroConfigValues[2];
 } gyroFilterAndRateConfig_t;
 
+#ifdef USE_GYRO_FIFO
+// Two reads' worth of FIFO samples (gyro_fifo_samples up to 4) plus margin: the interrupt brings
+// them at the sensor's rate, the task only follows on average
+#define GYRO_SAMPLE_QUEUE_LENGTH 16
+#else
 // 2 ms at 4 kHz: the interrupt brings samples at the sensor's rate, the task follows on average
 #define GYRO_SAMPLE_QUEUE_LENGTH 8
+#endif
 
 // Whether a driver that can reads its gyro on the data-ready interrupt (gyro_data_ready)
 typedef enum {
     GYRO_DEV_DATA_READY_OFF = 0,
-    GYRO_DEV_DATA_READY_WHERE_TESTED,   // only where that driver has been tested
+    GYRO_DEV_DATA_READY_WHERE_TESTED,   // where the driver turns it on for AUTO
     GYRO_DEV_DATA_READY_ON,
 } gyroDevDataReady_e;
 
@@ -69,6 +75,7 @@ typedef struct gyroDev_s {
     // Set by gyro.c before initFn
     gyroDevDataReady_e readOnDataReady;
     bool readOnDataReadyWithAcc;
+    uint8_t dataReadyFifoSamples;                       // samples per interrupt through the IMU's FIFO, 0 for none
     // Set by a driver that can (accgyro_data_ready.h); started at the end of init
     const struct gyroDataReadyDriver_s *dataReadyDriver;
     // Set once reading on data-ready: readFn then returns false when no sample came, not a failure

src/main/drivers/accgyro/accgyro_data_ready.c

@@ -29,7 +29,9 @@
 #if defined(USE_SPI_DATA_READY)
 
 #include "build/atomic.h"
+#include "build/debug.h"
 
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "drivers/accgyro/accgyro.h"
@@ -56,8 +58,33 @@ struct gyroDataReady_s {
     volatile int16_t temp;
     volatile timeUs_t lastSampleUs;
     timeUs_t resumedUs;                 // when a read after a pause turned the interrupt back on
+#ifdef USE_GYRO_FIFO
+    uint8_t fifoSamples;                // samples per interrupt through the IMU's FIFO, 0 for register reads
+    uint8_t fifoPackets;                // packets each read brings: one more than that where it fits
+    uint8_t fifoPacketLen;
+    volatile bool fifoFlush;            // the FIFO fell behind or out of step: empty it from the task
+    uint8_t fifoFruitlessReads;         // reads in a row that found packets counted but no sample
+    uint8_t fifoFaults;                 // flushes for a fault since fifoFaultsSinceUs
+    timeUs_t fifoFaultsSinceUs;
+    bool fifoGaveUp;                    // read from the registers for the rest of the session
+    uint16_t fifoFlushes;               // for the GYRO_FIFO debug mode
+    uint16_t fifoDropped;
+#endif
 };
 
+#ifdef USE_GYRO_FIFO
+// Reads in a row with packets counted but none held: out of step. One alone is normal (a packet
+// counted just before it can be read); a read that found the queue full doesn't count
+#define GYRO_DATA_READY_FIFO_FRUITLESS 4
+
+// Further behind than this many reads' worth, the FIFO is emptied rather than caught up
+#define GYRO_DATA_READY_FIFO_BEHIND 2
+
+// This many fault flushes within the window is a fault the FIFO won't get over: the registers are read from then on
+#define GYRO_DATA_READY_FIFO_MAX_FAULTS 3
+#define GYRO_DATA_READY_FIFO_FAULT_WINDOW_US 1000000
+#endif
+
 // The primary gyro and the secondary
 static gyroDataReady_t gyroDataReady[2];
 
@@ -124,6 +151,79 @@ static void gyroDataReadySample(void *arg, const uint8_t *data)
     d->lastSampleUs = microsISR();
 }
 
+#ifdef USE_GYRO_FIFO
+// The count, then fifoPackets packets, oldest first. Every packet read has left the FIFO, even
+// one that came after the count, so all are decoded up to the first empty one
+static void gyroDataReadyFifoSamples(void *arg, const uint8_t *data)
+{
+    gyroDataReady_t *d = arg;
+    const gyroDataReadyFifo_t *fifo = d->driver->fifo;
+    const uint16_t count = (data[0] << 8) | data[1];
+    const uint8_t *packet = &data[2];
+    int16_t gyro[XYZ_AXIS_COUNT];
+    int16_t acc[XYZ_AXIS_COUNT];
+    int16_t temp = 0;
+
+    if (count > GYRO_DATA_READY_FIFO_BEHIND * d->fifoSamples) {
+        d->fifoFlush = true;
+    }
+
+    unsigned pushed = 0;
+    for (unsigned i = 0; i < d->fifoPackets; i++, packet += d->fifoPacketLen) {
+        const gyroFifoPacket_e what = fifo->parse(packet, d->withAccAndTemp, gyro, acc, &temp);
+        if (what == GYRO_FIFO_PACKET_EMPTY) {
+            break;
+        }
+        if (what == GYRO_FIFO_PACKET_UNEXPECTED) {
+            d->fifoFlush = true;
+            break;
+        }
+        if (what == GYRO_FIFO_PACKET_INVALID) {
+            continue;
+        }
+        if (d->withAccAndTemp) {
+            d->acc[X] = acc[X];
+            d->acc[Y] = acc[Y];
+            d->acc[Z] = acc[Z];
+            d->temp = temp;
+        }
+        if (!gyroSampleQueuePush(d->gyro, gyro[X], gyro[Y], gyro[Z])) {
+            EXTIEnable(d->gyro->busDev->irqPin, false);
+            d->paused = true;
+            break;
+        }
+        pushed++;
+    }
+
+    if (pushed) {
+        d->fifoFruitlessReads = 0;
+        d->lastSampleUs = microsISR();
+    } else if (count && !d->paused && ++d->fifoFruitlessReads >= GYRO_DATA_READY_FIFO_FRUITLESS) {
+        // Until the flush, and whenever no sample comes for a while, the task reads the registers
+        d->fifoFlush = true;
+        d->fifoFruitlessReads = 0;
+    }
+    if (d == &gyroDataReady[0]) {
+        DEBUG_SET(DEBUG_GYRO_FIFO, 2, pushed);
+    }
+}
+
+static void gyroDataReadyFifoFault(gyroDataReady_t *d)
+{
+    const timeUs_t now = micros();
+    if (cmpTimeUs(now, d->fifoFaultsSinceUs) > GYRO_DATA_READY_FIFO_FAULT_WINDOW_US) {
+        d->fifoFaultsSinceUs = now;
+        d->fifoFaults = 0;
+    }
+    if (++d->fifoFaults >= GYRO_DATA_READY_FIFO_MAX_FAULTS) {
+        ATOMIC_BLOCK(NVIC_PRIO_GYRO_DATA_READY) {
+            EXTIEnable(d->gyro->busDev->irqPin, false);
+            d->fifoGaveUp = true;
+        }
+    }
+}
+#endif
+
 static void gyroDataReadyExti(extiCallbackRec_t *cb)
 {
     gyroDataReady_t *d = (gyroDataReady_t *)cb;
@@ -135,6 +235,44 @@ static bool gyroDataReadyRead(gyroDev_t *gyro)
 {
     gyroDataReady_t *d = gyro->dataReadyState;
 
+#ifdef USE_GYRO_FIFO
+    if (d->fifoGaveUp) {
+        return d->driver->registerRead(gyro);
+    }
+
+    if (d->fifoSamples && (d->paused || d->fifoFlush)) {
+        // A fault, not the restart after a pause
+        const bool fault = d->fifoFlush;
+        // No read starts between the flush and the clear; the one in flight can only re-raise what the flush empties
+        const SPIDevice bus = gyro->busDev->busdev.spi.spiBus;
+        spiBusAcquire(bus);
+        // What the FIFO holds is old, or out of step with the reads, and so is what came from it
+        d->driver->fifo->flush(gyro);
+        d->fifoFlush = false;
+        ATOMIC_BLOCK(NVIC_PRIO_GYRO_DATA_READY) {
+            d->fifoDropped += (gyro->sampleQueueHead + GYRO_SAMPLE_QUEUE_LENGTH - gyro->sampleQueueTail) % GYRO_SAMPLE_QUEUE_LENGTH;
+            gyro->sampleQueueTail = gyro->sampleQueueHead;
+            if (d->paused) {
+                d->paused = false;
+                EXTIEnable(gyro->busDev->irqPin, true);
+            }
+        }
+        spiBusRelease(bus);
+        d->resumedUs = micros();
+        d->fifoFlushes++;
+        if (fault) {
+            gyroDataReadyFifoFault(d);
+        }
+        if (d == &gyroDataReady[0]) {
+            DEBUG_SET(DEBUG_GYRO_FIFO, 0, d->fifoFlushes);
+            DEBUG_SET(DEBUG_GYRO_FIFO, 1, d->fifoDropped);
+            DEBUG_SET(DEBUG_GYRO_FIFO, 3, d->fifoGaveUp);
+        }
+        // No register read: a sample taken during the flush would come again from the FIFO
+        return false;
+    }
+#endif
+
     if (d->paused) {
         // After a pause the queue is old: turn the interrupt back on and read the registers
         ATOMIC_BLOCK(NVIC_PRIO_GYRO_DATA_READY) {
@@ -205,18 +343,43 @@ void gyroDataReadyStart(gyroDev_t *gyro)
     d->driver = driver;
     d->lastSampleUs = micros();
 
-    d->withAccAndTemp = gyro->readOnDataReadyWithAcc && driver->withAccAndTemp.len;
-    if (!d->withAccAndTemp || !spiDataReadyInit(dev, driver->withAccAndTemp.reg, driver->withAccAndTemp.len, gyroDataReadySample, d)) {
-        d->withAccAndTemp = false;
-        if (!driver->gyroOnly.len || !spiDataReadyInit(dev, driver->gyroOnly.reg, driver->gyroOnly.len, gyroDataReadySample, d)) {
-            return;
+    bool viaFifo = false;
+#ifdef USE_GYRO_FIFO
+    const gyroDataReadyFifo_t *fifo = driver->fifo;
+    d->fifoSamples = 0;
+    if (gyro->dataReadyFifoSamples && fifo) {
+        d->withAccAndTemp = gyro->readOnDataReadyWithAcc && fifo->packetLen;
+        d->fifoPacketLen = d->withAccAndTemp ? fifo->packetLen : fifo->gyroOnlyPacketLen;
+        // One packet more than the threshold where it fits, or a read behind by one stays behind
+        for (d->fifoPackets = gyro->dataReadyFifoSamples + 1; d->fifoPacketLen && d->fifoPackets >= gyro->dataReadyFifoSamples; d->fifoPackets--) {
+            if (spiDataReadyInit(dev, fifo->countReg, 2 + d->fifoPackets * d->fifoPacketLen, gyroDataReadyFifoSamples, d)) {
+                d->fifoSamples = gyro->dataReadyFifoSamples;
+                fifo->start(gyro, d->withAccAndTemp, d->fifoSamples);
+                viaFifo = true;
+                break;
+            }
+        }
+    }
+#endif
+
+    if (!viaFifo) {
+        d->withAccAndTemp = gyro->readOnDataReadyWithAcc && driver->withAccAndTemp.len;
+        if (!d->withAccAndTemp || !spiDataReadyInit(dev, driver->withAccAndTemp.reg, driver->withAccAndTemp.len, gyroDataReadySample, d)) {
+            d->withAccAndTemp = false;
+            if (!driver->gyroOnly.len || !spiDataReadyInit(dev, driver->gyroOnly.reg, driver->gyroOnly.len, gyroDataReadySample, d)) {
+                return;
+            }
         }
     }
     d->gyro = gyro;
 
     IOInit(dev->irqPin, OWNER_MPU, RESOURCE_EXTI, RESOURCE_INDEX(gyro->imuSensorToUse));
     EXTIHandlerInit(&d->exti, gyroDataReadyExti);
-    EXTIConfig(dev->irqPin, &d->exti, NVIC_PRIO_GYRO_DATA_READY, IOCFG_IN_FLOATING);
+#if defined(STM32F7) || defined(STM32H7)
+    EXTIConfig(dev->irqPin, &d->exti, NVIC_PRIO_GYRO_DATA_READY, IOCFG_IN_FLOATING);   // always on the rising edge
+#else
+    EXTIConfig(dev->irqPin, &d->exti, NVIC_PRIO_GYRO_DATA_READY, EXTI_Trigger_Rising);
+#endif
     EXTIEnable(dev->irqPin, true);
 
     gyro->dataReadyState = d;

src/main/drivers/accgyro/accgyro_data_ready.h

@@ -33,13 +33,37 @@ typedef struct gyroDataReadyRead_s {
     uint8_t len;                        // bytes after the address, 0 for no such read
 } gyroDataReadyRead_t;
 
+// What one FIFO packet held
+typedef enum {
+    GYRO_FIFO_PACKET_SAMPLE,            // a sample
+    GYRO_FIFO_PACKET_INVALID,           // a packet without a valid sample, such as one before the first ODR
+    GYRO_FIFO_PACKET_EMPTY,             // counted a moment before but not there yet: it comes with the next read
+    GYRO_FIFO_PACKET_UNEXPECTED,        // not a packet the FIFO was set up for: the reads are out of step
+} gyroFifoPacket_e;
+
+// An IMU FIFO signalling once for several samples: one read from the count register brings the
+// count and the packets after it
+typedef struct gyroDataReadyFifo_s {
+    uint8_t countReg;                   // as sent (read bit included), the packets follow it
+    uint8_t packetLen;                  // with the accelerometer and temperature
+    uint8_t gyroOnlyPacketLen;
+    // Puts the samples in the FIFO and the interrupt on `samples` of them
+    void (*start)(gyroDev_t *gyro, bool withAccAndTemp, uint8_t samples);
+    // Empties the FIFO, from the gyro task
+    void (*flush)(gyroDev_t *gyro);
+    gyroFifoPacket_e (*parse)(const uint8_t *packet, bool withAccAndTemp, int16_t *gyro, int16_t *acc, int16_t *temp);
+} gyroDataReadyFifo_t;
+
 typedef struct gyroDataReadyDriver_s {
     gyroDataReadyRead_t withAccAndTemp; // the gyro with the accelerometer (and temperature)
     gyroDataReadyRead_t gyroOnly;
     bool hasTemp;                       // the first read brings the temperature too
     gyroDataReadyParseFn parse;
     sensorGyroReadFuncPtr registerRead; // the driver's own read, for when samples stop coming
     bool tested;                        // gyro_data_ready AUTO turns it on
+#ifdef USE_GYRO_FIFO
+    const gyroDataReadyFifo_t *fifo;    // NULL if the IMU has no FIFO this can use
+#endif
 } gyroDataReadyDriver_t;
 
 typedef struct gyroDataReady_s gyroDataReady_t;