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
  38 #include "platform.h"
  39
  40 #ifdef USE_ACCGYRO_BMI160
  41
  42 #include "drivers/bus_spi.h"
  43 #include "drivers/exti.h"
  44 #include "drivers/io.h"
  45 #include "drivers/nvic.h"
  46 #include "drivers/sensor.h"
  47 #include "drivers/time.h"
  48
  49 #include "sensors/gyro.h"
  50
  51 #include "accgyro.h"
  52 #include "accgyro_spi_bmi160.h"
  53
  54
  55 // 10 MHz max SPI frequency
  56 #define BMI160_MAX_SPI_CLK_HZ 10000000
  57
  58 /* BMI160 Registers */
  59 #define BMI160_REG_CHIPID 0x00
  60 #define BMI160_REG_PMU_STAT 0x03
  61 #define BMI160_REG_GYR_DATA_X_LSB 0x0C
  62 #define BMI160_REG_ACC_DATA_X_LSB 0x12
  63 #define BMI160_REG_STATUS 0x1B
  64 #define BMI160_REG_TEMPERATURE_0 0x20
  65 #define BMI160_REG_ACC_CONF 0x40
  66 #define BMI160_REG_ACC_RANGE 0x41
  67 #define BMI160_REG_GYR_CONF 0x42
  68 #define BMI160_REG_GYR_RANGE 0x43
  69 #define BMI160_REG_INT_EN1 0x51
  70 #define BMI160_REG_INT_OUT_CTRL 0x53
  71 #define BMI160_REG_INT_MAP1 0x56
  72 #define BMI160_REG_FOC_CONF 0x69
  73 #define BMI160_REG_CONF 0x6A
  74 #define BMI160_REG_OFFSET_0 0x77
  75 #define BMI160_REG_CMD 0x7E
  76
  77 /* Register values */
  78 #define BMI160_PMU_CMD_PMU_ACC_NORMAL 0x11
  79 #define BMI160_PMU_CMD_PMU_GYR_NORMAL 0x15
  80 #define BMI160_INT_EN1_DRDY 0x10
  81 #define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A
  82 #define BMI160_REG_INT_MAP1_INT1_DRDY 0x80
  83 #define BMI160_CMD_START_FOC 0x03
  84 #define BMI160_CMD_PROG_NVM 0xA0
  85 #define BMI160_REG_STATUS_NVM_RDY 0x10
  86 #define BMI160_REG_STATUS_FOC_RDY 0x08
  87 #define BMI160_REG_CONF_NVM_PROG_EN 0x02
  88 #define BMI160_VAL_GYRO_CONF_BWP_OSR4 0x00
  89 #define BMI160_VAL_GYRO_CONF_BWP_OSR2 0x10
  90 #define BMI160_VAL_GYRO_CONF_BWP_NORM 0x20
  91
  92 ///* Global Variables */
  93 static volatile bool BMI160InitDone = false;
  94 static volatile bool BMI160Detected = false;
  95
  96 //! Private functions
  97 static int32_t BMI160_Config(const extDevice_t *dev);
  98 static int32_t BMI160_do_foc(const extDevice_t *dev);
  99
 100 uint8_t bmi160Detect(const extDevice_t *dev)
 101 {
 102     if (BMI160Detected) {
 103         return BMI_160_SPI;
 104     }
 105
 106     spiSetClkDivisor(dev, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ));
 107
 108     /* Read this address to activate SPI (see p. 84) */
 109     spiReadRegMsk(dev, 0x7F);
 110     delay(100); // Give SPI some time to start up
 111
 112     /* Check the chip ID */
 113     if (spiReadRegMsk(dev, BMI160_REG_CHIPID) != 0xd1) {
 114         return MPU_NONE;
 115     }
 116
 117     BMI160Detected = true;
 118     return BMI_160_SPI;
 119 }
 120
 121 /**
 122  * @brief Initialize the BMI160 6-axis sensor.
 123  * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
 124  */
 125 static void BMI160_Init(const extDevice_t *dev)
 126 {
 127     if (BMI160InitDone || !BMI160Detected) {
 128         return;
 129     }
 130
 131     /* Configure the BMI160 Sensor */
 132     if (BMI160_Config(dev) != 0) {
 133         return;
 134     }
 135
 136     bool do_foc = false;
 137
 138     /* Perform fast offset compensation if requested */
 139     if (do_foc) {
 140         BMI160_do_foc(dev);
 141     }
 142
 143     BMI160InitDone = true;
 144 }
 145
 146 static uint8_t getBmiOsrMode()
 147 {
 148     switch(gyroConfig()->gyro_hardware_lpf) {
 149         case GYRO_HARDWARE_LPF_NORMAL:
 150             return BMI160_VAL_GYRO_CONF_BWP_OSR4;
 151         case GYRO_HARDWARE_LPF_OPTION_1:
 152             return BMI160_VAL_GYRO_CONF_BWP_OSR2;
 153         case GYRO_HARDWARE_LPF_OPTION_2:
 154             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 155         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 156             return BMI160_VAL_GYRO_CONF_BWP_NORM;
 157     }
 158     return 0;
 159 }
 160
 161 /**
 162  * @brief Configure the sensor
 163  */
 164 static int32_t BMI160_Config(const extDevice_t *dev)
 165 {
 166
 167     // Set normal power mode for gyro and accelerometer
 168     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
 169     delay(100); // can take up to 80ms
 170
 171     spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
 172     delay(5); // can take up to 3.8ms
 173
 174     // Verify that normal power mode was entered
 175     uint8_t pmu_status = spiReadRegMsk(dev, BMI160_REG_PMU_STAT);
 176     if ((pmu_status & 0x3C) != 0x14) {
 177         return -3;
 178     }
 179
 180     // Set odr and ranges
 181     // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
 182     spiWriteReg(dev, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
 183     delay(1);
 184
 185     spiWriteReg(dev, BMI160_REG_GYR_CONF, getBmiOsrMode() | BMI160_ODR_3200_Hz);
 186     delay(1);
 187
 188     spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
 189     delay(1);
 190
 191     spiWriteReg(dev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
 192     delay(1);
 193
 194     // Enable offset compensation
 195     uint8_t val = spiReadRegMsk(dev, BMI160_REG_OFFSET_0);
 196     spiWriteReg(dev, BMI160_REG_OFFSET_0, val | 0xC0);
 197
 198     // Enable data ready interrupt
 199     spiWriteReg(dev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
 200     delay(1);
 201
 202     // Enable INT1 pin
 203     spiWriteReg(dev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
 204     delay(1);
 205
 206     // Map data ready interrupt to INT1 pin
 207     spiWriteReg(dev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
 208     delay(1);
 209
 210     return 0;
 211 }
 212
 213 static int32_t BMI160_do_foc(const extDevice_t *dev)
 214 {
 215     // assume sensor is mounted on top
 216     uint8_t val = 0x7D;;
 217     spiWriteReg(dev, BMI160_REG_FOC_CONF, val);
 218
 219     // Start FOC
 220     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_START_FOC);
 221
 222     // Wait for FOC to complete
 223     for (int i=0; i<50; i++) {
 224         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 225         if (val & BMI160_REG_STATUS_FOC_RDY) {
 226             break;
 227         }
 228         delay(10);
 229     }
 230     if (!(val & BMI160_REG_STATUS_FOC_RDY)) {
 231         return -3;
 232     }
 233
 234     // Program NVM
 235     val = spiReadRegMsk(dev, BMI160_REG_CONF);
 236     spiWriteReg(dev, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
 237
 238     spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
 239
 240     // Wait for NVM programming to complete
 241     for (int i=0; i<50; i++) {
 242         val = spiReadRegMsk(dev, BMI160_REG_STATUS);
 243         if (val & BMI160_REG_STATUS_NVM_RDY) {
 244             break;
 245         }
 246         delay(10);
 247     }
 248     if (!(val & BMI160_REG_STATUS_NVM_RDY)) {
 249         return -6;
 250     }
 251
 252     return 0;
 253 }
 254
 255 extiCallbackRec_t bmi160IntCallbackRec;
 256
 257 #if defined(USE_MPU_DATA_READY_SIGNAL)
 258 void bmi160ExtiHandler(extiCallbackRec_t *cb)
 259 {
 260     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
 261     gyro->dataReady = true;
 262 }
 263
 264 static void bmi160IntExtiInit(gyroDev_t *gyro)
 265 {
 266     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
 267         return;
 268     }
 269
 270     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
 271
 272     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
 273     EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler);
 274     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); // TODO - maybe pullup / pulldown ?
 275     EXTIEnable(mpuIntIO);
 276 }
 277 #endif
 278
 279 bool bmi160AccRead(accDev_t *acc)
 280 {
 281     enum {
 282         IDX_REG = 0,
 283         IDX_ACCEL_XOUT_L,
 284         IDX_ACCEL_XOUT_H,
 285         IDX_ACCEL_YOUT_L,
 286         IDX_ACCEL_YOUT_H,
 287         IDX_ACCEL_ZOUT_L,
 288         IDX_ACCEL_ZOUT_H,
 289         BUFFER_SIZE,
 290     };
 291
 292     uint8_t bmi160_rx_buf[BUFFER_SIZE];
 293     static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
 294
 295     spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE);   // receive response
 296
 297     acc->ADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_XOUT_L]);
 298     acc->ADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_YOUT_L]);
 299     acc->ADCRaw[Z] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_ZOUT_L]);
 300
 301     return true;
 302 }
 303
 304
 305 bool bmi160GyroRead(gyroDev_t *gyro)
 306 {
 307     enum {
 308         IDX_REG = 0,
 309         IDX_GYRO_XOUT_L,
 310         IDX_GYRO_XOUT_H,
 311         IDX_GYRO_YOUT_L,
 312         IDX_GYRO_YOUT_H,
 313         IDX_GYRO_ZOUT_L,
 314         IDX_GYRO_ZOUT_H,
 315         BUFFER_SIZE,
 316     };
 317
 318     uint8_t bmi160_rx_buf[BUFFER_SIZE];
 319     static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
 320
 321     spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE);   // receive response
 322
 323     gyro->gyroADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_XOUT_L]);
 324     gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_YOUT_L]);
 325     gyro->gyroADCRaw[Z] = (int16_t)((bmi160_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_ZOUT_L]);
 326
 327     return true;
 328 }
 329
 330
 331 void bmi160SpiGyroInit(gyroDev_t *gyro)
 332 {
 333     BMI160_Init(&gyro->dev);
 334 #if defined(USE_MPU_DATA_READY_SIGNAL)
 335     bmi160IntExtiInit(gyro);
 336 #endif
 337 }
 338
 339 void bmi160SpiAccInit(accDev_t *acc)
 340 {
 341     BMI160_Init(&acc->dev);
 342
 343     acc->acc_1G = 512 * 8;
 344 }
 345
 346
 347 bool bmi160SpiAccDetect(accDev_t *acc)
 348 {
 349     if (bmi160Detect(&acc->dev) == MPU_NONE) {
 350         return false;
 351     }
 352
 353     acc->initFn = bmi160SpiAccInit;
 354     acc->readFn = bmi160AccRead;
 355
 356     return true;
 357 }
 358
 359
 360 bool bmi160SpiGyroDetect(gyroDev_t *gyro)
 361 {
 362     if (bmi160Detect(&gyro->dev) == MPU_NONE) {
 363         return false;
 364     }
 365
 366     gyro->initFn = bmi160SpiGyroInit;
 367     gyro->readFn = bmi160GyroRead;
 368     gyro->scale = GYRO_SCALE_2000DPS;
 369
 370     return true;
 371 }
 372 #endif // USE_ACCGYRO_BMI160