gyro: BMI270, ICM-45686, LSM6DXX and the MPU IMUs on data-ready

docs/Settings.md

@@ -2140,7 +2140,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 and a target that declares the interrupt pin; without interrupts the gyro is read as before. AUTO does it with the ICM-42605, ICM-42688-P, BMI270 and LSM6DXX; ON also with the ICM-45686, MPU6000, MPU6500 (and the ICM-20602 and ICM-20608 it drives), MPU9250 and ICM-20689, not yet run on hardware. OFF polls the gyro.
 
 | Allowed Values |  |
 | --- | --- |

src/main/drivers/accgyro/accgyro.h

@@ -42,7 +42,7 @@ typedef struct {
 // 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;
 

src/main/drivers/accgyro/accgyro_bmi270.c

@@ -38,6 +38,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_data_ready.h"
 #include "drivers/accgyro/accgyro_bmi270.h"
 
 #define BMI270_CONFIG_SIZE 328
@@ -257,6 +258,17 @@ static bool bmi270yroReadScratchpad(gyroDev_t *gyro)
 
 static bool bmi270AccReadScratchpad(accDev_t *acc)
 {
+#if defined(USE_SPI_DATA_READY)
+    // The read on data-ready brought the accelerometer too
+    int16_t v[XYZ_AXIS_COUNT];
+    if (gyroDataReadyAcc(acc->busDev, v)) {
+        acc->ADCRaw[X] = v[X];
+        acc->ADCRaw[Y] = v[Y];
+        acc->ADCRaw[Z] = v[Z];
+        return true;
+    }
+#endif
+
     bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 
     if (ctx->lastReadStatus) {
@@ -284,9 +296,38 @@ static bool bmi270TemperatureRead(gyroDev_t *gyro, int16_t * data)
     return false;
 }
 
+#if defined(USE_SPI_DATA_READY)
+// After a dummy byte, from ACC_DATA_X_LSB: accelerometer X, Y, Z and gyro X, Y, Z, little-endian.
+// INT1 flags new data of either sensor, so the gyro-only read takes the accelerometer too
+static void bmi270DataReadyParse(const uint8_t *data, bool withAccAndTemp, int16_t *gyro, int16_t *acc, int16_t *temp)
+{
+    UNUSED(temp);
+    data++;
+    if (withAccAndTemp) {
+        acc[X] = int16_val_little_endian(data, 0);
+        acc[Y] = int16_val_little_endian(data, 1);
+        acc[Z] = int16_val_little_endian(data, 2);
+    }
+    gyro[X] = int16_val_little_endian(data, 3);
+    gyro[Y] = int16_val_little_endian(data, 4);
+    gyro[Z] = int16_val_little_endian(data, 5);
+}
+
+static const gyroDataReadyDriver_t bmi270DataReady = {
+    .withAccAndTemp = { BMI270_REG_ACC_DATA_X_LSB | 0x80, 13 },
+    .gyroOnly = { BMI270_REG_ACC_DATA_X_LSB | 0x80, 13 },
+    .parse = bmi270DataReadyParse,
+    .registerRead = bmi270yroReadScratchpad,
+    .tested = true,
+};
+#endif
+
 static void bmi270GyroInit(gyroDev_t *gyro)
 {
     bmi270AccAndGyroInit(gyro);
+#if defined(USE_SPI_DATA_READY)
+    gyro->dataReadyDriver = &bmi270DataReady;
+#endif
 }
 
 static void bmi270AccInit(accDev_t *acc)

src/main/drivers/accgyro/accgyro_data_ready.c

@@ -179,12 +179,19 @@ void gyroDataReadySuspend(void)
     }
 }
 
+bool gyroDataReadyWanted(const gyroDev_t *gyro)
+{
+    const gyroDataReadyDriver_t *driver = gyro->dataReadyDriver;
+    const busDevice_t *dev = gyro->busDev;
+    return driver && dev && dev->irqPin && gyro->readOnDataReady != GYRO_DEV_DATA_READY_OFF &&
+        (gyro->readOnDataReady != GYRO_DEV_DATA_READY_WHERE_TESTED || driver->tested);
+}
+
 void gyroDataReadyStart(gyroDev_t *gyro)
 {
     const gyroDataReadyDriver_t *driver = gyro->dataReadyDriver;
     busDevice_t *dev = gyro->busDev;
-    if (!driver || !dev || !dev->irqPin || gyro->readOnDataReady == GYRO_DEV_DATA_READY_OFF ||
-        (gyro->readOnDataReady == GYRO_DEV_DATA_READY_WHERE_TESTED && !driver->tested)) {
+    if (!gyroDataReadyWanted(gyro)) {
         return;
     }
     // A pin already owned stays with its owner; a wrong but free pin only means polling as before
@@ -216,7 +223,11 @@ void gyroDataReadyStart(gyroDev_t *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

@@ -45,6 +45,8 @@ typedef struct gyroDataReadyDriver_s {
 typedef struct gyroDataReady_s gyroDataReady_t;
 
 #if defined(USE_SPI_DATA_READY)
+// Whether gyroDataReadyStart() will try, for a driver whose rate depends on it
+bool gyroDataReadyWanted(const gyroDev_t *gyro);
 void gyroDataReadyStart(gyroDev_t *gyro);
 void gyroDataReadySuspend(void);
 // For the driver's acc and temperature reads: true when the last data-ready read brought them

src/main/drivers/accgyro/accgyro_icm20689.c

@@ -109,6 +109,10 @@ static void icm20689AccAndGyroInit(gyroDev_t *gyro)
     busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]); // Get Divider Drops
     delay(100);
 
+#if defined(USE_SPI_DATA_READY)
+    mpuDataReadySetup(gyro);
+#endif
+
     // Switch SPI to fast speed
     busSetSpeed(busDev, BUS_SPEED_FAST);
 }

src/main/drivers/accgyro/accgyro_icm42605.c

@@ -183,9 +183,7 @@ static const gyroDataReadyDriver_t icm426xxDataReady = {
     .hasTemp = true,
     .parse = icm426xxDataReadyParse,
     .registerRead = icm42605GyroRead,
-#if defined(STM32H7)
     .tested = true,
-#endif
 };
 #endif
 

src/main/drivers/accgyro/accgyro_icm45686.c

@@ -34,6 +34,7 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_data_ready.h"
 #include "accgyro_icm45686.h"
 
 #if defined(USE_IMU_ICM45686)
@@ -309,6 +310,17 @@ static void icm45686AccInit(accDev_t *acc)
 
 static bool icm45686AccRead(accDev_t *acc)
 {
+#if defined(USE_SPI_DATA_READY)
+    // The read on data-ready brought the accelerometer too
+    int16_t v[XYZ_AXIS_COUNT];
+    if (gyroDataReadyAcc(acc->busDev, v)) {
+        acc->ADCRaw[X] = v[X];
+        acc->ADCRaw[Y] = v[Y];
+        acc->ADCRaw[Z] = v[Z];
+        return true;
+    }
+#endif
+
     uint8_t data[6];
 
     const bool ack = busReadBuf(acc->busDev, ICM456XX_ACCEL_DATA_X1_UI, data, 6);
@@ -341,6 +353,14 @@ static bool icm45686GyroRead(gyroDev_t *gyro)
 
 static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp)
 {
+#if defined(USE_SPI_DATA_READY)
+    int16_t raw;
+    if (gyroDataReadyTemperature(gyro->busDev, &raw)) {
+        *temp = (raw / 12.8f) + 250.0f; // as below, degC*10
+        return true;
+    }
+#endif
+
     uint8_t data[2];
 
     const bool ack = busReadBuf(gyro->busDev, ICM456XX_TEMP_DATA1, data, 2);
@@ -353,6 +373,34 @@ static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp)
     return true;
 }
 
+#if defined(USE_SPI_DATA_READY)
+// From ACCEL_DATA_X1_UI: accelerometer X, Y, Z, gyro X, Y, Z and temperature, little-endian
+static void icm45686DataReadyParse(const uint8_t *data, bool withAccAndTemp, int16_t *gyro, int16_t *acc, int16_t *temp)
+{
+    if (withAccAndTemp) {
+        acc[X] = int16_val_little_endian(data, 0);
+        acc[Y] = int16_val_little_endian(data, 1);
+        acc[Z] = int16_val_little_endian(data, 2);
+        data += 6;
+    }
+    gyro[X] = int16_val_little_endian(data, 0);
+    gyro[Y] = int16_val_little_endian(data, 1);
+    gyro[Z] = int16_val_little_endian(data, 2);
+    if (withAccAndTemp) {
+        *temp = int16_val_little_endian(data, 3);
+    }
+}
+
+static const gyroDataReadyDriver_t icm45686DataReady = {
+    .withAccAndTemp = { ICM456XX_ACCEL_DATA_X1_UI | 0x80, 14 },
+    .gyroOnly = { ICM456XX_GYRO_DATA_X1_UI | 0x80, 6 },
+    .hasTemp = true,
+    .parse = icm45686DataReadyParse,
+    .registerRead = icm45686GyroRead,
+    .tested = false,                    // not run on hardware yet
+};
+#endif
+
 static void icm45686AccAndGyroInit(gyroDev_t *gyro)
 {
     busDevice_t * dev = gyro->busDev;
@@ -402,6 +450,10 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro)
 
     delay(ICM456XX_INT_CONFIG_DELAY_MS);
     busSetSpeed(dev, BUS_SPEED_FAST);
+
+#if defined(USE_SPI_DATA_READY)
+    gyro->dataReadyDriver = &icm45686DataReady;
+#endif
 }
 
 static bool icm45686DeviceDetect(busDevice_t * dev)

src/main/drivers/accgyro/accgyro_lsm6dxx.c

@@ -35,6 +35,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_lsm6dxx.h"
+#include "drivers/accgyro/accgyro_data_ready.h"
 
 #if defined(USE_IMU_LSM6DXX)
 
@@ -180,6 +181,12 @@ static void lsm6dxxConfig(gyroDev_t *gyro)
     busDevice_t * dev = gyro->busDev;
     const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
+#if defined(USE_SPI_DATA_READY)
+    // On data-ready every sample reaches the gyro task and its first filter, so they run at the 6664 Hz set below
+    if (gyroDataReadyWanted(gyro)) {
+        gyro->sampleRateIntervalUs = 150;
+    }
+#endif
 
     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
     // Reset the device (wait 100ms before continuing config)
@@ -272,6 +279,17 @@ static void lsm6dxxSpiAccInit(accDev_t *acc)
 
 static bool lsm6dxxAccRead(accDev_t *acc)
 {
+#if defined(USE_SPI_DATA_READY)
+    // The read on data-ready brought the accelerometer too
+    int16_t v[XYZ_AXIS_COUNT];
+    if (gyroDataReadyAcc(acc->busDev, v)) {
+        acc->ADCRaw[X] = v[X];
+        acc->ADCRaw[Y] = v[Y];
+        acc->ADCRaw[Z] = v[Z];
+        return true;
+    }
+#endif
+
     uint8_t data[6];
     const bool ack = busReadBuf(acc->busDev, LSM6DXX_REG_OUTX_L_A, data, 6);
     if (!ack) {
@@ -296,6 +314,31 @@ static bool lsm6dxxGyroRead(gyroDev_t *gyro)
     return true;
 }
 
+#if defined(USE_SPI_DATA_READY)
+// From OUTX_L_G: gyro X, Y, Z, then accelerometer X, Y, Z, little-endian
+static void lsm6dxxDataReadyParse(const uint8_t *data, bool withAccAndTemp, int16_t *gyro, int16_t *acc, int16_t *temp)
+{
+    UNUSED(temp);
+    gyro[X] = int16_val_little_endian(data, 0);
+    gyro[Y] = int16_val_little_endian(data, 1);
+    gyro[Z] = int16_val_little_endian(data, 2);
+    if (withAccAndTemp) {
+        acc[X] = int16_val_little_endian(data, 3);
+        acc[Y] = int16_val_little_endian(data, 4);
+        acc[Z] = int16_val_little_endian(data, 5);
+    }
+}
+
+static const gyroDataReadyDriver_t lsm6dxxDataReady = {
+    .withAccAndTemp = { LSM6DXX_REG_OUTX_L_G | 0x80, 12 },
+    .gyroOnly = { LSM6DXX_REG_OUTX_L_G | 0x80, 6 },
+    .hasTemp = false,
+    .parse = lsm6dxxDataReadyParse,
+    .registerRead = lsm6dxxGyroRead,
+    .tested = true,
+};
+#endif
+
 // Init Gyro first,then Acc
 bool lsm6dGyroDetect(gyroDev_t *gyro)
 {
@@ -317,6 +360,10 @@ bool lsm6dGyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->gyroAlign = gyro->busDev->param;
     gyro->scale = 1.0f / 16.4f; // 2000 dps
+#if defined(USE_SPI_DATA_READY)
+    // Both register maps configure gyro data-ready, pulsed, on INT1
+    gyro->dataReadyDriver = &lsm6dxxDataReady;
+#endif
     return true;
 
 }