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
  94 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
  95 #define GYRO_EXTI_DETECT_THRESHOLD 1000
  96
  97 // Global Variables
  98 static volatile bool BMI160InitDone = false;
  99 static volatile bool BMI160Detected = false;
 100
 101 //! Private functions
 102 static int32_t BMI160_Config(const extDevice_t *dev);
 103 static int32_t BMI160_do_foc(const extDevice_t *dev);
 104
 105 uint8_t bmi160Detect(const extDevice_t *dev)
 106 {
 107     if (BMI160Detected) {
 108         return BMI_160_SPI;
 109     }
 110
 111     /* Toggle CS to activate SPI (see https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi160-ds000.pdf section 3.2.1) */
 112     spiWrite(dev, 0xFF);
 113     delay(100); // Give SPI some time to start up
 114
 115     /* Check the chip ID */
 116     if (spiReadRegMsk(dev, BMI160_REG_CHIPID) != 0xd1) {
 117         return MPU_NONE;
 118     }
 119
 120     BMI160Detected = true;
 121
 122     spiSetClkDivisor(dev, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ));
 123
 124     return BMI_160_SPI;
 125 }
 126
 127 /**
 128  * @brief Initialize the BMI160 6-axis sensor.
 129  * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
 130  */
 131 static void BMI160_Init(const extDevice_t *dev)
 132 {
 133     if (BMI160InitDone || !BMI160Detected) {
 134         return;
 135     }
 136
 137     /* Configure the BMI160 Sensor */
 138     if (BMI160_Config(dev) != 0) {
 139         return;
 140     }
 141
 142     bool do_foc = false;
 143
 144     /* Perform fast offset compensation if requested */
 145     if (do_foc) {
 146         BMI160_do_foc(dev);
 147     }
 148
 149     BMI160InitDone = true;
 150 }
 151
 152 static uint8_t getBmiOsrMode()
 153 {
 154     switch(gyroConfig()->gyro_hardware_lpf) {
 155         case GYRO_HARDWARE_LPF_NORMAL:
 156             return BMI160_VAL_GYRO_CONF_BWP_OSR4;
 157         case GYRO_HARDWARE_LPF_OPTION_1:
 158             return BMI160_VAL_GYRO_CONF_BWP_OSR2;
 159         case GYRO_HARDWARE_LPF_OPTION_2:
 160             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 161         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 162             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 163     }
 164     return 0;
 165 }
 166
 167 /**
 168  * @brief Configure the sensor
 169  */
 170 static int32_t BMI160_Config(const extDevice_t *dev)
 171 {
 172
 173     // Set normal power mode for gyro and accelerometer
 174     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
 175     delay(100); // can take up to 80ms
 176
 177     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
 178     delay(5); // can take up to 3.8ms
 179
 180     // Verify that normal power mode was entered
 181     uint8_t pmu_status = spiReadRegMsk(dev, BMI160_REG_PMU_STAT);
 182     if ((pmu_status & 0x3C) != 0x14) {
 183         return -3;
 184     }
 185
 186     // Set odr and ranges
 187     // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
 188     spiWriteReg(dev, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
 189     delay(1);
 190
 191     spiWriteReg(dev, BMI160_REG_GYR_CONF, getBmiOsrMode() | BMI160_ODR_3200_Hz);
 192     delay(1);
 193
 194     spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
 195     delay(1);
 196
 197     spiWriteReg(dev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
 198     delay(1);
 199
 200     // Enable offset compensation
 201     uint8_t val = spiReadRegMsk(dev, BMI160_REG_OFFSET_0);
 202     spiWriteReg(dev, BMI160_REG_OFFSET_0, val | 0xC0);
 203
 204     // Enable data ready interrupt
 205     spiWriteReg(dev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
 206     delay(1);
 207
 208     // Enable INT1 pin
 209     spiWriteReg(dev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
 210     delay(1);
 211
 212     // Map data ready interrupt to INT1 pin
 213     spiWriteReg(dev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
 214     delay(1);
 215
 216     return 0;
 217 }
 218
 219 static int32_t BMI160_do_foc(const extDevice_t *dev)
 220 {
 221     // assume sensor is mounted on top
 222     uint8_t val = 0x7D;;
 223     spiWriteReg(dev, BMI160_REG_FOC_CONF, val);
 224
 225     // Start FOC
 226     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_START_FOC);
 227
 228     // Wait for FOC to complete
 229     for (int i=0; i<50; i++) {
 230         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 231         if (val & BMI160_REG_STATUS_FOC_RDY) {
 232             break;
 233         }
 234         delay(10);
 235     }
 236     if (!(val & BMI160_REG_STATUS_FOC_RDY)) {
 237         return -3;
 238     }
 239
 240     // Program NVM
 241     val = spiReadRegMsk(dev, BMI160_REG_CONF);
 242     spiWriteReg(dev, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
 243
 244     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
 245
 246     // Wait for NVM programming to complete
 247     for (int i=0; i<50; i++) {
 248         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 249         if (val & BMI160_REG_STATUS_NVM_RDY) {
 250             break;
 251         }
 252         delay(10);
 253     }
 254     if (!(val & BMI160_REG_STATUS_NVM_RDY)) {
 255         return -6;
 256     }
 257
 258     return 0;
 259 }
 260
 261 extiCallbackRec_t bmi160IntCallbackRec;
 262
 263 #ifdef USE_GYRO_EXTI
 264 // Called in ISR context
 265 // Gyro read has just completed
 266 busStatus_e bmi160Intcallback(uint32_t arg)
 267 {
 268     gyroDev_t *gyro = (gyroDev_t *)arg;
 269     int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
 270
 271     if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
 272         gyro->gyroDmaMaxDuration = gyroDmaDuration;
 273     }
 274
 275     gyro->dataReady = true;
 276
 277     return BUS_READY;
 278 }
 279
 280 void bmi160ExtiHandler(extiCallbackRec_t *cb)
 281 {
 282     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 283     // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
 284     // not have an associated timer
 285     uint32_t nowCycles = getCycleCounter();
 286     gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
 287     gyro->gyroLastEXTI = nowCycles;
 288
 289     if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
 290         spiSequence(&gyro->dev, gyro->segments);
 291     }
 292
 293     gyro->detectedEXTI++;
 294
 295 }
 296
 297 static void bmi160IntExtiInit(gyroDev_t *gyro)
 298 {
 299     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
 300         return;
 301     }
 302
 303     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
 304
 305     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
 306     EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler);
 307     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
 308     EXTIEnable(mpuIntIO);
 309 }
 310 #else
 311 void bmi160ExtiHandler(extiCallbackRec_t *cb)
 312 {
 313     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 314     gyro->dataReady = true;
 315 }
 316 #endif
 317
 318 static bool bmi160AccRead(accDev_t *acc)
 319 {
 320     switch (acc->gyro->gyroModeSPI) {
 321     case GYRO_EXTI_INT:
 322     case GYRO_EXTI_NO_INT:
 323     {
 324         acc->gyro->dev.txBuf[0] = BMI160_REG_ACC_DATA_X_LSB | 0x80;
 325
 326         busSegment_t segments[] = {
 327                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
 328                 {.u.link = {NULL, NULL}, 0, true, NULL},
 329         };
 330         segments[0].u.buffers.txData = &acc->gyro->dev.txBuf[1];
 331         segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
 332
 333         spiSequence(&acc->gyro->dev, &segments[0]);
 334
 335         // Wait for completion
 336         spiWait(&acc->gyro->dev);
 337
 338         uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
 339         acc->ADCRaw[X] = accData[1];
 340         acc->ADCRaw[Y] = accData[2];
 341         acc->ADCRaw[Z] = accData[3];
 342         break;
 343     }
 344
 345     case GYRO_EXTI_INT_DMA:
 346     {
 347         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 348         // up an old value.
 349
 350         // This data was read from the gyro, which is the same SPI device as the acc
 351         uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
 352         acc->ADCRaw[X] = accData[4];
 353         acc->ADCRaw[Y] = accData[5];
 354         acc->ADCRaw[Z] = accData[6];
 355         break;
 356     }
 357
 358     case GYRO_EXTI_INIT:
 359     default:
 360         break;
 361     }
 362
 363     return true;
 364 }
 365
 366
 367 static bool bmi160GyroRead(gyroDev_t *gyro)
 368 {
 369     uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
 370     switch (gyro->gyroModeSPI) {
 371     case GYRO_EXTI_INIT:
 372     {
 373         // Initialise the tx buffer to all 0x00
 374         memset(gyro->dev.txBuf, 0x00, 14);
 375 #ifdef USE_GYRO_EXTI
 376         // Check that minimum number of interrupts have been detected
 377
 378         // We need some offset from the gyro interrupts to ensure sampling after the interrupt
 379         gyro->gyroDmaMaxDuration = 5;
 380         // Using DMA for gyro access upsets the scheduler on the F4
 381         if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
 382             if (spiUseDMA(&gyro->dev)) {
 383                 gyro->dev.callbackArg = (uint32_t)gyro;
 384                 gyro->dev.txBuf[1] = BMI160_REG_GYR_DATA_X_LSB | 0x80;
 385                 gyro->segments[0].len = 14;
 386                 gyro->segments[0].callback = bmi160Intcallback;
 387                 gyro->segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
 388                 gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
 389                 gyro->segments[0].negateCS = true;
 390                 gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
 391             } else {
 392                 // Interrupts are present, but no DMA
 393                 gyro->gyroModeSPI = GYRO_EXTI_INT;
 394             }
 395         } else
 396 #endif
 397         {
 398             gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
 399         }
 400         break;
 401     }
 402
 403     case GYRO_EXTI_INT:
 404     case GYRO_EXTI_NO_INT:
 405     {
 406         gyro->dev.txBuf[1] = BMI160_REG_GYR_DATA_X_LSB | 0x80;
 407
 408         busSegment_t segments[] = {
 409                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
 410                 {.u.link = {NULL, NULL}, 0, true, NULL},
 411         };
 412         segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
 413         segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
 414
 415         spiSequence(&gyro->dev, &segments[0]);
 416
 417         // Wait for completion
 418         spiWait(&gyro->dev);
 419
 420         // Fall through
 421         FALLTHROUGH;
 422     }
 423
 424     case GYRO_EXTI_INT_DMA:
 425     {
 426         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 427         // up an old value.
 428         gyro->gyroADCRaw[X] = gyroData[1];
 429         gyro->gyroADCRaw[Y] = gyroData[2];
 430         gyro->gyroADCRaw[Z] = gyroData[3];
 431         break;
 432     }
 433
 434     default:
 435         break;
 436     }
 437
 438     return true;
 439 }
 440
 441
 442 void bmi160SpiGyroInit(gyroDev_t *gyro)
 443 {
 444     BMI160_Init(&gyro->dev);
 445 #if defined(USE_GYRO_EXTI)
 446     bmi160IntExtiInit(gyro);
 447 #endif
 448 }
 449
 450 void bmi160SpiAccInit(accDev_t *acc)
 451 {
 452     acc->acc_1G = 512 * 8;
 453 }
 454
 455
 456 bool bmi160SpiAccDetect(accDev_t *acc)
 457 {
 458     if (acc->mpuDetectionResult.sensor != BMI_160_SPI) {
 459         return false;
 460     }
 461
 462     acc->initFn = bmi160SpiAccInit;
 463     acc->readFn = bmi160AccRead;
 464
 465     return true;
 466 }
 467
 468
 469 bool bmi160SpiGyroDetect(gyroDev_t *gyro)
 470 {
 471     if (gyro->mpuDetectionResult.sensor != BMI_160_SPI) {
 472         return false;
 473     }
 474
 475     gyro->initFn = bmi160SpiGyroInit;
 476     gyro->readFn = bmi160GyroRead;
 477     gyro->scale = GYRO_SCALE_2000DPS;
 478
 479     return true;
 480 }
 481 #endif // USE_ACCGYRO_BMI160