src/main/drivers/accgyro/accgyro_spi_bmi160.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_BMI160 BMI160 Functions
   6  * @brief Hardware functions to deal with the 6DOF gyro / accel sensor
   7  * @{
   8  *
   9  * @file       pios_bmi160.c
  10  * @author     dRonin, http://dRonin.org/, Copyright (C) 2016
  11  * @brief      BMI160 Gyro / Accel Sensor Routines
  12  * @see        The GNU Public License (GPL) Version 3
  13  ******************************************************************************/
  14
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  *
  30  * Additional note on redistribution: The copyright and license notices above
  31  * must be maintained in each individual source file that is a derivative work
  32  * of this source file; otherwise redistribution is prohibited.
  33  */
  34
  35 #include <stdbool.h>
  36 #include <stdint.h>
  37 #include <stdlib.h>
  38 #include <string.h>
  39
  40 #include "platform.h"
  41
  42 #ifdef USE_ACCGYRO_BMI160
  43
  44 #include "drivers/accgyro/accgyro.h"
  45 #include "drivers/accgyro/accgyro_spi_bmi160.h"
  46 #include "drivers/bus_spi.h"
  47 #include "drivers/exti.h"
  48 #include "drivers/io.h"
  49 #include "drivers/io_impl.h"
  50 #include "drivers/nvic.h"
  51 #include "drivers/sensor.h"
  52 #include "drivers/system.h"
  53 #include "drivers/time.h"
  54
  55 #include "sensors/gyro.h"
  56
  57 // 10 MHz max SPI frequency
  58 #define BMI160_MAX_SPI_CLK_HZ 10000000
  59
  60 /* BMI160 Registers */
  61 #define BMI160_REG_CHIPID 0x00
  62 #define BMI160_REG_PMU_STAT 0x03
  63 #define BMI160_REG_GYR_DATA_X_LSB 0x0C
  64 #define BMI160_REG_ACC_DATA_X_LSB 0x12
  65 #define BMI160_REG_STATUS 0x1B
  66 #define BMI160_REG_TEMPERATURE_0 0x20
  67 #define BMI160_REG_ACC_CONF 0x40
  68 #define BMI160_REG_ACC_RANGE 0x41
  69 #define BMI160_REG_GYR_CONF 0x42
  70 #define BMI160_REG_GYR_RANGE 0x43
  71 #define BMI160_REG_INT_EN1 0x51
  72 #define BMI160_REG_INT_OUT_CTRL 0x53
  73 #define BMI160_REG_INT_MAP1 0x56
  74 #define BMI160_REG_FOC_CONF 0x69
  75 #define BMI160_REG_CONF 0x6A
  76 #define BMI160_REG_OFFSET_0 0x77
  77 #define BMI160_REG_CMD 0x7E
  78
  79 /* Register values */
  80 #define BMI160_PMU_CMD_PMU_ACC_NORMAL 0x11
  81 #define BMI160_PMU_CMD_PMU_GYR_NORMAL 0x15
  82 #define BMI160_INT_EN1_DRDY 0x10
  83 #define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A
  84 #define BMI160_REG_INT_MAP1_INT1_DRDY 0x80
  85 #define BMI160_CMD_START_FOC 0x03
  86 #define BMI160_CMD_PROG_NVM 0xA0
  87 #define BMI160_REG_STATUS_NVM_RDY 0x10
  88 #define BMI160_REG_STATUS_FOC_RDY 0x08
  89 #define BMI160_REG_CONF_NVM_PROG_EN 0x02
  90 #define BMI160_VAL_GYRO_CONF_BWP_OSR4 0x00
  91 #define BMI160_VAL_GYRO_CONF_BWP_OSR2 0x10
  92 #define BMI160_VAL_GYRO_CONF_BWP_NORM 0x20
  93 #define BMI160_VAL_ACC_CONF_BWP_OSR4 0x00
  94 #define BMI160_VAL_ACC_CONF_BWP_OSR2 0x10
  95 #define BMI160_VAL_ACC_CONF_BWP_NORM 0x20
  96 #define BMI160_VAL_ACC_CONF_US_HP 0x00
  97
  98 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
  99 #define GYRO_EXTI_DETECT_THRESHOLD 1000
 100
 101 // Global Variables
 102 static volatile bool BMI160InitDone = false;
 103 static volatile bool BMI160Detected = false;
 104
 105 //! Private functions
 106 static int32_t BMI160_Config(const extDevice_t *dev);
 107 static int32_t BMI160_do_foc(const extDevice_t *dev);
 108
 109 uint8_t bmi160Detect(const extDevice_t *dev)
 110 {
 111     if (BMI160Detected) {
 112         return BMI_160_SPI;
 113     }
 114
 115     // Toggle CS to activate SPI (see https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi160-ds000.pdf section 3.2.1)
 116     spiWrite(dev, 0xFF);
 117
 118     delay(100); // Give SPI some time to start up
 119
 120     // Check the chip ID
 121     if (spiReadRegMsk(dev, BMI160_REG_CHIPID) != 0xd1) {
 122         return MPU_NONE;
 123     }
 124
 125     BMI160Detected = true;
 126
 127     return BMI_160_SPI;
 128 }
 129
 130 /**
 131  * @brief Initialize the BMI160 6-axis sensor.
 132  * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
 133  */
 134 static void BMI160_Init(const extDevice_t *dev)
 135 {
 136     if (BMI160InitDone || !BMI160Detected) {
 137         return;
 138     }
 139
 140     /* Configure the BMI160 Sensor */
 141     if (BMI160_Config(dev) != 0) {
 142         return;
 143     }
 144
 145     bool do_foc = false;
 146
 147     /* Perform fast offset compensation if requested */
 148     if (do_foc) {
 149         BMI160_do_foc(dev);
 150     }
 151
 152     BMI160InitDone = true;
 153 }
 154
 155 static uint8_t getBmiOsrMode(void)
 156 {
 157     switch(gyroConfig()->gyro_hardware_lpf) {
 158         case GYRO_HARDWARE_LPF_NORMAL:
 159             return BMI160_VAL_GYRO_CONF_BWP_OSR4;
 160         case GYRO_HARDWARE_LPF_OPTION_1:
 161             return BMI160_VAL_GYRO_CONF_BWP_OSR2;
 162         case GYRO_HARDWARE_LPF_OPTION_2:
 163             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 164 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 165         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 166             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 167 #endif
 168         default:
 169             return BMI160_VAL_GYRO_CONF_BWP_OSR4;
 170     }
 171 }
 172
 173 /**
 174  * @brief Configure the sensor
 175  */
 176 static int32_t BMI160_Config(const extDevice_t *dev)
 177 {
 178     // Set normal power mode for gyro and accelerometer
 179     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
 180     delay(100); // can take up to 80ms
 181
 182     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
 183     delay(5); // can take up to 3.8ms
 184
 185     // Verify that normal power mode was entered
 186     uint8_t pmu_status = spiReadRegMsk(dev, BMI160_REG_PMU_STAT);
 187     if ((pmu_status & 0x3C) != 0x14) {
 188         return -3;
 189     }
 190
 191     // Set odr and ranges
 192     // Set acc_us = 0 & acc_bwp = 0b001 for high performance and OSR2 mode
 193     spiWriteReg(dev, BMI160_REG_ACC_CONF, BMI160_VAL_ACC_CONF_US_HP | BMI160_VAL_ACC_CONF_BWP_OSR2 | BMI160_ODR_800_Hz);
 194     delay(1);
 195
 196     spiWriteReg(dev, BMI160_REG_GYR_CONF, getBmiOsrMode() | BMI160_ODR_3200_Hz);
 197     delay(1);
 198
 199     spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
 200     delay(1);
 201
 202     spiWriteReg(dev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
 203     delay(1);
 204
 205     // Enable offset compensation
 206     uint8_t val = spiReadRegMsk(dev, BMI160_REG_OFFSET_0);
 207     spiWriteReg(dev, BMI160_REG_OFFSET_0, val | 0xC0);
 208
 209     // Enable data ready interrupt
 210     spiWriteReg(dev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
 211     delay(1);
 212
 213     // Enable INT1 pin
 214     spiWriteReg(dev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
 215     delay(1);
 216
 217     // Map data ready interrupt to INT1 pin
 218     spiWriteReg(dev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
 219     delay(1);
 220
 221     return 0;
 222 }
 223
 224 static int32_t BMI160_do_foc(const extDevice_t *dev)
 225 {
 226     // assume sensor is mounted on top
 227     uint8_t val = 0x7D;
 228     spiWriteReg(dev, BMI160_REG_FOC_CONF, val);
 229
 230     // Start FOC
 231     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_START_FOC);
 232
 233     // Wait for FOC to complete
 234     for (int i=0; i<50; i++) {
 235         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 236         if (val & BMI160_REG_STATUS_FOC_RDY) {
 237             break;
 238         }
 239         delay(10);
 240     }
 241     if (!(val & BMI160_REG_STATUS_FOC_RDY)) {
 242         return -3;
 243     }
 244
 245     // Program NVM
 246     val = spiReadRegMsk(dev, BMI160_REG_CONF);
 247     spiWriteReg(dev, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
 248
 249     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
 250
 251     // Wait for NVM programming to complete
 252     for (int i=0; i<50; i++) {
 253         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 254         if (val & BMI160_REG_STATUS_NVM_RDY) {
 255             break;
 256         }
 257         delay(10);
 258     }
 259     if (!(val & BMI160_REG_STATUS_NVM_RDY)) {
 260         return -6;
 261     }
 262
 263     return 0;
 264 }
 265
 266 extiCallbackRec_t bmi160IntCallbackRec;
 267
 268 // Called in ISR context
 269 // Gyro read has just completed
 270 busStatus_e bmi160Intcallback(uint32_t arg)
 271 {
 272     gyroDev_t *gyro = (gyroDev_t *)arg;
 273     int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
 274
 275     if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
 276         gyro->gyroDmaMaxDuration = gyroDmaDuration;
 277     }
 278
 279     gyro->dataReady = true;
 280
 281     return BUS_READY;
 282 }
 283
 284 void bmi160ExtiHandler(extiCallbackRec_t *cb)
 285 {
 286     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 287     extDevice_t *dev = &gyro->dev;
 288
 289     // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
 290     // not have an associated timer
 291     uint32_t nowCycles = getCycleCounter();
 292     gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
 293     gyro->gyroLastEXTI = nowCycles;
 294
 295     if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
 296         spiSequence(dev, gyro->segments);
 297     }
 298
 299     gyro->detectedEXTI++;
 300
 301 }
 302
 303 static void bmi160IntExtiInit(gyroDev_t *gyro)
 304 {
 305     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
 306         return;
 307     }
 308
 309     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
 310
 311     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
 312     EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler);
 313     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
 314     EXTIEnable(mpuIntIO);
 315 }
 316
 317 static bool bmi160AccRead(accDev_t *acc)
 318 {
 319     extDevice_t *dev = &acc->gyro->dev;
 320
 321     switch (acc->gyro->gyroModeSPI) {
 322     case GYRO_EXTI_INT:
 323     case GYRO_EXTI_NO_INT:
 324     {
 325         dev->txBuf[1] = BMI160_REG_ACC_DATA_X_LSB | 0x80;
 326
 327         busSegment_t segments[] = {
 328                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
 329                 {.u.link = {NULL, NULL}, 0, true, NULL},
 330         };
 331         segments[0].u.buffers.txData = &dev->txBuf[1];
 332         segments[0].u.buffers.rxData = &dev->rxBuf[1];
 333
 334         spiSequence(&acc->gyro->dev, &segments[0]);
 335
 336         // Wait for completion
 337         spiWait(&acc->gyro->dev);
 338
 339         int16_t *accData = (int16_t *)dev->rxBuf;
 340         acc->ADCRaw[X] = accData[1];
 341         acc->ADCRaw[Y] = accData[2];
 342         acc->ADCRaw[Z] = accData[3];
 343         break;
 344     }
 345
 346     case GYRO_EXTI_INT_DMA:
 347     {
 348         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 349         // up an old value.
 350
 351         // This data was read from the gyro, which is the same SPI device as the acc
 352         int16_t *accData = (int16_t *)dev->rxBuf;
 353         acc->ADCRaw[X] = accData[4];
 354         acc->ADCRaw[Y] = accData[5];
 355         acc->ADCRaw[Z] = accData[6];
 356         break;
 357     }
 358
 359     case GYRO_EXTI_INIT:
 360     default:
 361         break;
 362     }
 363
 364     return true;
 365 }
 366
 367 static bool bmi160GyroRead(gyroDev_t *gyro)
 368 {
 369     extDevice_t *dev = &gyro->dev;
 370     int16_t *gyroData = (int16_t *)dev->rxBuf;
 371     switch (gyro->gyroModeSPI) {
 372     case GYRO_EXTI_INIT:
 373     {
 374         // Initialise the tx buffer to all 0x00
 375         memset(dev->txBuf, 0x00, 14);
 376
 377         // Check that minimum number of interrupts have been detected
 378
 379         // We need some offset from the gyro interrupts to ensure sampling after the interrupt
 380         gyro->gyroDmaMaxDuration = 5;
 381         // Using DMA for gyro access upsets the scheduler on the F4
 382         if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
 383             if (spiUseDMA(dev)) {
 384                 dev->callbackArg = (uint32_t)gyro;
 385                 dev->txBuf[1] = BMI160_REG_GYR_DATA_X_LSB | 0x80;
 386                 gyro->segments[0].len = 13;
 387                 gyro->segments[0].callback = bmi160Intcallback;
 388                 gyro->segments[0].u.buffers.txData = &dev->txBuf[1];
 389                 gyro->segments[0].u.buffers.rxData = &dev->rxBuf[1];
 390                 gyro->segments[0].negateCS = true;
 391                 gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
 392             } else {
 393                 // Interrupts are present, but no DMA
 394                 gyro->gyroModeSPI = GYRO_EXTI_INT;
 395             }
 396         } else {
 397             gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
 398         }
 399         break;
 400     }
 401
 402     case GYRO_EXTI_INT:
 403     case GYRO_EXTI_NO_INT:
 404     {
 405         dev->txBuf[1] = BMI160_REG_GYR_DATA_X_LSB | 0x80;
 406
 407         busSegment_t segments[] = {
 408                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
 409                 {.u.link = {NULL, NULL}, 0, true, NULL},
 410         };
 411         segments[0].u.buffers.txData = &dev->txBuf[1];
 412         segments[0].u.buffers.rxData = &dev->rxBuf[1];
 413
 414         spiSequence(dev, &segments[0]);
 415
 416         // Wait for completion
 417         spiWait(dev);
 418
 419         // Fall through
 420         FALLTHROUGH;
 421     }
 422
 423     case GYRO_EXTI_INT_DMA:
 424     {
 425         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 426         // up an old value.
 427         gyro->gyroADCRaw[X] = gyroData[1];
 428         gyro->gyroADCRaw[Y] = gyroData[2];
 429         gyro->gyroADCRaw[Z] = gyroData[3];
 430         break;
 431     }
 432
 433     default:
 434         break;
 435     }
 436
 437     return true;
 438 }
 439
 440 void bmi160SpiGyroInit(gyroDev_t *gyro)
 441 {
 442     extDevice_t *dev = &gyro->dev;
 443
 444     BMI160_Init(dev);
 445     bmi160IntExtiInit(gyro);
 446
 447     spiSetClkDivisor(dev, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ));
 448 }
 449
 450 void bmi160SpiAccInit(accDev_t *acc)
 451 {
 452     acc->acc_1G = 512 * 8;
 453 }
 454
 455 bool bmi160SpiAccDetect(accDev_t *acc)
 456 {
 457     if (acc->mpuDetectionResult.sensor != BMI_160_SPI) {
 458         return false;
 459     }
 460
 461     acc->initFn = bmi160SpiAccInit;
 462     acc->readFn = bmi160AccRead;
 463
 464     return true;
 465 }
 466
 467 bool bmi160SpiGyroDetect(gyroDev_t *gyro)
 468 {
 469     if (gyro->mpuDetectionResult.sensor != BMI_160_SPI) {
 470         return false;
 471     }
 472
 473     gyro->initFn = bmi160SpiGyroInit;
 474     gyro->readFn = bmi160GyroRead;
 475     gyro->scale = GYRO_SCALE_2000DPS;
 476
 477     return true;
 478 }
 479 #endif // USE_ACCGYRO_BMI160