src/main/drivers/accgyro/accgyro_spi_bmi270.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_ACCGYRO_BMI270
  29
  30 #include "drivers/accgyro/accgyro.h"
  31 #include "drivers/accgyro/accgyro_spi_bmi270.h"
  32 #include "drivers/bus_spi.h"
  33 #include "drivers/exti.h"
  34 #include "drivers/io.h"
  35 #include "drivers/io_impl.h"
  36 #include "drivers/nvic.h"
  37 #include "drivers/sensor.h"
  38 #include "drivers/system.h"
  39 #include "drivers/time.h"
  40
  41 #include "sensors/gyro.h"
  42
  43 // 10 MHz max SPI frequency
  44 #define BMI270_MAX_SPI_CLK_HZ 10000000
  45
  46 #define BMI270_FIFO_FRAME_SIZE 6
  47
  48 #define BMI270_CONFIG_SIZE 328
  49
  50 // Declaration for the device config (microcode) that must be uploaded to the sensor
  51 extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE];
  52
  53 #define BMI270_CHIP_ID 0x24
  54
  55 // BMI270 registers (not the complete list)
  56 typedef enum {
  57     BMI270_REG_CHIP_ID = 0x00,
  58     BMI270_REG_ERR_REG = 0x02,
  59     BMI270_REG_STATUS = 0x03,
  60     BMI270_REG_ACC_DATA_X_LSB = 0x0C,
  61     BMI270_REG_GYR_DATA_X_LSB = 0x12,
  62     BMI270_REG_SENSORTIME_0 = 0x18,
  63     BMI270_REG_SENSORTIME_1 = 0x19,
  64     BMI270_REG_SENSORTIME_2 = 0x1A,
  65     BMI270_REG_EVENT = 0x1B,
  66     BMI270_REG_INT_STATUS_0 = 0x1C,
  67     BMI270_REG_INT_STATUS_1 = 0x1D,
  68     BMI270_REG_INTERNAL_STATUS = 0x21,
  69     BMI270_REG_TEMPERATURE_LSB = 0x22,
  70     BMI270_REG_TEMPERATURE_MSB = 0x23,
  71     BMI270_REG_FIFO_LENGTH_LSB = 0x24,
  72     BMI270_REG_FIFO_LENGTH_MSB = 0x25,
  73     BMI270_REG_FIFO_DATA = 0x26,
  74     BMI270_REG_ACC_CONF = 0x40,
  75     BMI270_REG_ACC_RANGE = 0x41,
  76     BMI270_REG_GYRO_CONF = 0x42,
  77     BMI270_REG_GYRO_RANGE = 0x43,
  78     BMI270_REG_AUX_CONF = 0x44,
  79     BMI270_REG_FIFO_DOWNS = 0x45,
  80     BMI270_REG_FIFO_WTM_0 = 0x46,
  81     BMI270_REG_FIFO_WTM_1 = 0x47,
  82     BMI270_REG_FIFO_CONFIG_0 = 0x48,
  83     BMI270_REG_FIFO_CONFIG_1 = 0x49,
  84     BMI270_REG_SATURATION = 0x4A,
  85     BMI270_REG_INT1_IO_CTRL = 0x53,
  86     BMI270_REG_INT2_IO_CTRL = 0x54,
  87     BMI270_REG_INT_LATCH = 0x55,
  88     BMI270_REG_INT1_MAP_FEAT = 0x56,
  89     BMI270_REG_INT2_MAP_FEAT = 0x57,
  90     BMI270_REG_INT_MAP_DATA = 0x58,
  91     BMI270_REG_INIT_CTRL = 0x59,
  92     BMI270_REG_INIT_DATA = 0x5E,
  93     BMI270_REG_ACC_SELF_TEST = 0x6D,
  94     BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
  95     BMI270_REG_PWR_CONF = 0x7C,
  96     BMI270_REG_PWR_CTRL = 0x7D,
  97     BMI270_REG_CMD = 0x7E,
  98 } bmi270Register_e;
  99
 100 // BMI270 register configuration values
 101 typedef enum {
 102     BMI270_VAL_CMD_SOFTRESET = 0xB6,
 103     BMI270_VAL_CMD_FIFOFLUSH = 0xB0,
 104     BMI270_VAL_PWR_CTRL = 0x0E,              // enable gyro, acc and temp sensors
 105     BMI270_VAL_PWR_CONF = 0x02,              // disable advanced power save, enable FIFO self-wake
 106     BMI270_VAL_ACC_CONF_ODR800 = 0x0B,       // set acc sample rate to 800hz
 107     BMI270_VAL_ACC_CONF_ODR1600 = 0x0C,      // set acc sample rate to 1600hz
 108     BMI270_VAL_ACC_CONF_BWP = 0x02,          // set acc filter in normal mode
 109     BMI270_VAL_ACC_CONF_HP = 0x01,           // set acc in high performance mode
 110     BMI270_VAL_ACC_RANGE_8G = 0x02,          // set acc to 8G full scale
 111     BMI270_VAL_ACC_RANGE_16G = 0x03,         // set acc to 16G full scale
 112     BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D,     // set gyro sample rate to 3200hz
 113     BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00,    // set gyro filter in OSR4 mode
 114     BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01,    // set gyro filter in OSR2 mode
 115     BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02,    // set gyro filter in normal mode
 116     BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01,  // set gyro in high performance noise mode
 117     BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode
 118
 119     BMI270_VAL_GYRO_RANGE_2000DPS = 0x08,    // set gyro to 2000dps full scale
 120                                              // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well
 121                                              // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!)
 122
 123     BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1
 124     BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02,  // enable the FIFO watermark interrupt pin 1
 125     BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A,  // active high, push-pull, output enabled, input disabled
 126     BMI270_VAL_FIFO_CONFIG_0 = 0x00,         // don't stop when full, disable sensortime frame
 127     BMI270_VAL_FIFO_CONFIG_1 = 0x80,         // only gyro data in FIFO, use headerless mode
 128     BMI270_VAL_FIFO_DOWNS = 0x00,            // select unfiltered gyro data with no downsampling (6.4KHz samples)
 129     BMI270_VAL_FIFO_WTM_0 = 0x06,            // set the FIFO watermark level to 1 gyro sample (6 bytes)
 130     BMI270_VAL_FIFO_WTM_1 = 0x00,            // FIFO watermark MSB
 131 } bmi270ConfigValues_e;
 132
 133 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
 134 #define GYRO_EXTI_DETECT_THRESHOLD 1000
 135
 136 // BMI270 register reads are 16bits with the first byte a "dummy" value 0
 137 // that must be ignored. The result is in the second byte.
 138 static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId)
 139 {
 140     uint8_t data[2] = { 0, 0 };
 141
 142     if (spiReadRegMskBufRB(dev, registerId, data, 2)) {
 143         return data[1];
 144     } else {
 145         return 0;
 146     }
 147 }
 148
 149 static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs)
 150 {
 151     spiWriteReg(dev, registerId, value);
 152     if (delayMs) {
 153         delay(delayMs);
 154     }
 155 }
 156
 157 // Toggle the CS to switch the device into SPI mode.
 158 // Device switches initializes as I2C and switches to SPI on a low to high CS transition
 159 static void bmi270EnableSPI(const extDevice_t *dev)
 160 {
 161     IOLo(dev->busType_u.spi.csnPin);
 162     delay(1);
 163     IOHi(dev->busType_u.spi.csnPin);
 164     delay(10);
 165 }
 166
 167 uint8_t bmi270Detect(const extDevice_t *dev)
 168 {
 169     spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ));
 170     bmi270EnableSPI(dev);
 171
 172     if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) {
 173         return BMI_270_SPI;
 174     }
 175
 176     return MPU_NONE;
 177 }
 178
 179 static void bmi270UploadConfig(const extDevice_t *dev)
 180 {
 181     bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1);
 182     bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1);
 183
 184     // Transfer the config file
 185     spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file));
 186
 187     delay(10);
 188     bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1);
 189 }
 190
 191 static uint8_t getBmiOsrMode()
 192 {
 193     switch(gyroConfig()->gyro_hardware_lpf) {
 194         case GYRO_HARDWARE_LPF_NORMAL:
 195             return BMI270_VAL_GYRO_CONF_BWP_OSR4;
 196         case GYRO_HARDWARE_LPF_OPTION_1:
 197             return BMI270_VAL_GYRO_CONF_BWP_OSR2;
 198         case GYRO_HARDWARE_LPF_OPTION_2:
 199             return BMI270_VAL_GYRO_CONF_BWP_NORM;
 200         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 201             return BMI270_VAL_GYRO_CONF_BWP_NORM;
 202     }
 203     return 0;
 204 }
 205
 206 static void bmi270Config(gyroDev_t *gyro)
 207 {
 208     extDevice_t *dev = &gyro->dev;
 209
 210     // If running in hardware_lpf experimental mode then switch to FIFO-based,
 211     // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering
 212 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 213     const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL);
 214 #else
 215     const bool fifoMode = false;
 216 #endif
 217
 218     // Perform a soft reset to set all configuration to default
 219     // Delay 100ms before continuing configuration
 220     bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100);
 221
 222     // Toggle the chip into SPI mode
 223     bmi270EnableSPI(dev);
 224
 225     bmi270UploadConfig(dev);
 226
 227     // Configure the FIFO
 228     if (fifoMode) {
 229         bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1);
 230         bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1);
 231         bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1);
 232         bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1);
 233         bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1);
 234     }
 235
 236     // Configure the accelerometer
 237     bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1);
 238
 239     // Configure the accelerometer full-scale range
 240     bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1);
 241
 242     // Configure the gyro
 243     bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1);
 244
 245     // Configure the gyro full-range scale
 246     bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1);
 247
 248     // Configure the gyro data ready interrupt
 249     if (fifoMode) {
 250         // Interrupt driven by FIFO watermark level
 251         bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1);
 252     } else {
 253         // Interrupt driven by data ready
 254         bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1);
 255     }
 256
 257     // Configure the behavior of the INT1 pin
 258     bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1);
 259
 260     // Configure the device for  performance mode
 261     bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1);
 262
 263     // Enable the gyro, accelerometer and temperature sensor - disable aux interface
 264     bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1);
 265
 266     // Flush the FIFO
 267     if (fifoMode) {
 268         bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1);
 269     }
 270 }
 271
 272 extiCallbackRec_t bmi270IntCallbackRec;
 273
 274 /*
 275  * Gyro interrupt service routine
 276  */
 277 #ifdef USE_GYRO_EXTI
 278 // Called in ISR context
 279 // Gyro read has just completed
 280 busStatus_e bmi270Intcallback(uint32_t arg)
 281 {
 282     gyroDev_t *gyro = (gyroDev_t *)arg;
 283     int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
 284
 285     if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
 286         gyro->gyroDmaMaxDuration = gyroDmaDuration;
 287     }
 288
 289     gyro->dataReady = true;
 290
 291     return BUS_READY;
 292 }
 293
 294 void bmi270ExtiHandler(extiCallbackRec_t *cb)
 295 {
 296     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 297     // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does
 298     // not have an associated timer
 299     uint32_t nowCycles = getCycleCounter();
 300     gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
 301     gyro->gyroLastEXTI = nowCycles;
 302
 303     if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
 304         spiSequence(&gyro->dev, gyro->segments);
 305     }
 306
 307     gyro->detectedEXTI++;
 308
 309 }
 310
 311 static void bmi270IntExtiInit(gyroDev_t *gyro)
 312 {
 313     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
 314         return;
 315     }
 316
 317     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
 318
 319     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
 320     EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler);
 321     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
 322     EXTIEnable(mpuIntIO);
 323 }
 324 #else
 325 void bmi270ExtiHandler(extiCallbackRec_t *cb)
 326 {
 327     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 328     gyro->dataReady = true;
 329 }
 330 #endif
 331
 332 static bool bmi270AccRead(accDev_t *acc)
 333 {
 334     switch (acc->gyro->gyroModeSPI) {
 335     case GYRO_EXTI_INT:
 336     case GYRO_EXTI_NO_INT:
 337     {
 338         acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
 339
 340         busSegment_t segments[] = {
 341                 {.u.buffers = {NULL, NULL}, 8, true, NULL},
 342                 {.u.link = {NULL, NULL}, 0, true, NULL},
 343         };
 344         segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
 345         segments[0].u.buffers.rxData = acc->gyro->dev.rxBuf;
 346
 347         spiSequence(&acc->gyro->dev, &segments[0]);
 348
 349         // Wait for completion
 350         spiWait(&acc->gyro->dev);
 351
 352         // Fall through
 353         FALLTHROUGH;
 354     }
 355
 356     case GYRO_EXTI_INT_DMA:
 357     {
 358         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 359         // up an old value.
 360
 361         // This data was read from the gyro, which is the same SPI device as the acc
 362         uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
 363         acc->ADCRaw[X] = accData[1];
 364         acc->ADCRaw[Y] = accData[2];
 365         acc->ADCRaw[Z] = accData[3];
 366         break;
 367     }
 368
 369     case GYRO_EXTI_INIT:
 370     default:
 371         break;
 372     }
 373
 374     return true;
 375 }
 376
 377 static bool bmi270GyroReadRegister(gyroDev_t *gyro)
 378 {
 379     uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
 380     switch (gyro->gyroModeSPI) {
 381     case GYRO_EXTI_INIT:
 382     {
 383         // Initialise the tx buffer to all 0x00
 384         memset(gyro->dev.txBuf, 0x00, 14);
 385 #ifdef USE_GYRO_EXTI
 386         // Check that minimum number of interrupts have been detected
 387
 388         // We need some offset from the gyro interrupts to ensure sampling after the interrupt
 389         gyro->gyroDmaMaxDuration = 5;
 390         // Using DMA for gyro access upsets the scheduler on the F4
 391         if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
 392             if (spiUseDMA(&gyro->dev)) {
 393                 gyro->dev.callbackArg = (uint32_t)gyro;
 394                 gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
 395                 gyro->segments[0].len = 14;
 396                 gyro->segments[0].callback = bmi270Intcallback;
 397                 gyro->segments[0].u.buffers.txData = gyro->dev.txBuf;
 398                 gyro->segments[0].u.buffers.rxData = gyro->dev.rxBuf;
 399                 gyro->segments[0].negateCS = true;
 400                 gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
 401             } else {
 402                 // Interrupts are present, but no DMA
 403                 gyro->gyroModeSPI = GYRO_EXTI_INT;
 404             }
 405         } else
 406 #endif
 407         {
 408             gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
 409         }
 410         break;
 411     }
 412
 413     case GYRO_EXTI_INT:
 414     case GYRO_EXTI_NO_INT:
 415     {
 416         gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80;
 417
 418         busSegment_t segments[] = {
 419                 {.u.buffers = {NULL, NULL}, 8, true, NULL},
 420                 {.u.link = {NULL, NULL}, 0, true, NULL},
 421         };
 422         segments[0].u.buffers.txData = gyro->dev.txBuf;
 423         segments[0].u.buffers.rxData = gyro->dev.rxBuf;
 424
 425         spiSequence(&gyro->dev, &segments[0]);
 426
 427         // Wait for completion
 428         spiWait(&gyro->dev);
 429
 430         gyro->gyroADCRaw[X] = gyroData[1];
 431         gyro->gyroADCRaw[Y] = gyroData[2];
 432         gyro->gyroADCRaw[Z] = gyroData[3];
 433
 434         break;
 435     }
 436
 437     case GYRO_EXTI_INT_DMA:
 438     {
 439         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 440         // up an old value.
 441         gyro->gyroADCRaw[X] = gyroData[4];
 442         gyro->gyroADCRaw[Y] = gyroData[5];
 443         gyro->gyroADCRaw[Z] = gyroData[6];
 444         break;
 445     }
 446
 447     default:
 448         break;
 449     }
 450
 451     return true;
 452 }
 453
 454 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 455 static bool bmi270GyroReadFifo(gyroDev_t *gyro)
 456 {
 457     enum {
 458         IDX_REG = 0,
 459         IDX_SKIP,
 460         IDX_FIFO_LENGTH_L,
 461         IDX_FIFO_LENGTH_H,
 462         IDX_GYRO_XOUT_L,
 463         IDX_GYRO_XOUT_H,
 464         IDX_GYRO_YOUT_L,
 465         IDX_GYRO_YOUT_H,
 466         IDX_GYRO_ZOUT_L,
 467         IDX_GYRO_ZOUT_H,
 468         BUFFER_SIZE,
 469     };
 470
 471     bool dataRead = false;
 472     STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 473     STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
 474
 475     // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for
 476     // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the
 477     // length before using the sample.
 478     spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE);   // receive response
 479
 480     int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]);
 481
 482     if (fifoLength >= BMI270_FIFO_FRAME_SIZE) {
 483
 484         const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]);
 485         const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]);
 486         const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]);
 487
 488         // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet).
 489         // This shouldn't happen since we're only using the data if the FIFO length indicates
 490         // that data is available, but this safeguard is needed to prevent bad things in
 491         // case it does happen.
 492         if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) {
 493             gyro->gyroADCRaw[X] = gyroX;
 494             gyro->gyroADCRaw[Y] = gyroY;
 495             gyro->gyroADCRaw[Z] = gyroZ;
 496             dataRead = true;
 497         }
 498         fifoLength -= BMI270_FIFO_FRAME_SIZE;
 499     }
 500
 501     // If there are additional samples in the FIFO then we don't use those for now and simply
 502     // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since
 503     // the gyro loop is running at the native sample rate of 6.4KHz.
 504     // However the way the FIFO works in the sensor is that if a frame is partially read then
 505     // it remains in the queue instead of bein removed. So if we ever got into a state where there
 506     // was a partial frame or other unexpected data in the FIFO is may never get cleared and we
 507     // would end up in a lock state of always re-reading the same partial or invalid sample.
 508     if (fifoLength > 0) {
 509         // Partial or additional frames left - flush the FIFO
 510         bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0);
 511     }
 512
 513     return dataRead;
 514 }
 515 #endif
 516
 517 static bool bmi270GyroRead(gyroDev_t *gyro)
 518 {
 519 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 520     if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) {
 521         // running in 6.4KHz FIFO mode
 522         return bmi270GyroReadFifo(gyro);
 523     } else
 524 #endif
 525     {
 526         // running in 3.2KHz register mode
 527         return bmi270GyroReadRegister(gyro);
 528     }
 529 }
 530
 531 static void bmi270SpiGyroInit(gyroDev_t *gyro)
 532 {
 533     bmi270Config(gyro);
 534
 535 #if defined(USE_GYRO_EXTI)
 536     bmi270IntExtiInit(gyro);
 537 #endif
 538 }
 539
 540 static void bmi270SpiAccInit(accDev_t *acc)
 541 {
 542     // sensor is configured during gyro init
 543     acc->acc_1G = 512 * 4;   // 16G sensor scale
 544 }
 545
 546 bool bmi270SpiAccDetect(accDev_t *acc)
 547 {
 548     if (acc->mpuDetectionResult.sensor != BMI_270_SPI) {
 549         return false;
 550     }
 551
 552     acc->initFn = bmi270SpiAccInit;
 553     acc->readFn = bmi270AccRead;
 554
 555     return true;
 556 }
 557
 558
 559 bool bmi270SpiGyroDetect(gyroDev_t *gyro)
 560 {
 561     if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) {
 562         return false;
 563     }
 564
 565     gyro->initFn = bmi270SpiGyroInit;
 566     gyro->readFn = bmi270GyroRead;
 567     gyro->scale = GYRO_SCALE_2000DPS;
 568
 569     return true;
 570 }
 571
 572 // Used to query the status register to determine what event caused the EXTI to fire.
 573 // When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready
 574 // trigger will fire for both gyro and accelerometer. So it's necessary to check this register
 575 // to determine which event caused the interrupt.
 576 // When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes.
 577 // Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO
 578 // watermark reason as an idication of gyro data ready.
 579 uint8_t bmi270InterruptStatus(gyroDev_t *gyro)
 580 {
 581     return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1);
 582 }
 583 #endif // USE_ACCGYRO_BMI270