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