src/main/drivers/accgyro/accgyro_bmi160.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34
  35 #include "drivers/system.h"
  36 #include "drivers/time.h"
  37 #include "drivers/io.h"
  38 #include "drivers/bus.h"
  39
  40 #include "drivers/sensor.h"
  41 #include "drivers/accgyro/accgyro.h"
  42 #include "drivers/accgyro/accgyro_bmi160.h"
  43
  44 #if defined(USE_IMU_BMI160)
  45
  46 /* BMI160 Registers */
  47 #define BMI160_REG_CHIPID           0x00
  48 #define BMI160_REG_PMU_STAT         0x03
  49 #define BMI160_REG_GYR_DATA_X_LSB   0x0C
  50 #define BMI160_REG_ACC_DATA_X_LSB   0x12
  51 #define BMI160_REG_STATUS           0x1B
  52 #define BMI160_REG_TEMPERATURE_0    0x20
  53 #define BMI160_REG_ACC_CONF         0x40
  54 #define BMI160_REG_ACC_RANGE        0x41
  55 #define BMI160_REG_GYR_CONF         0x42
  56 #define BMI160_REG_GYR_RANGE        0x43
  57 #define BMI160_REG_INT_EN1          0x51
  58 #define BMI160_REG_INT_OUT_CTRL     0x53
  59 #define BMI160_REG_INT_MAP1         0x56
  60 #define BMI160_REG_FOC_CONF         0x69
  61 #define BMI160_REG_CONF             0x6A
  62 #define BMI160_REG_OFFSET_0         0x77
  63 #define BMI160_REG_CMD              0x7E
  64
  65 /* Register values */
  66 #define BMI160_PMU_CMD_PMU_ACC_NORMAL   0x11
  67 #define BMI160_PMU_CMD_PMU_GYR_NORMAL   0x15
  68 #define BMI160_INT_EN1_DRDY             0x10
  69 #define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A
  70 #define BMI160_REG_INT_MAP1_INT1_DRDY   0x80
  71 #define BMI160_CMD_START_FOC            0x03
  72 #define BMI160_CMD_PROG_NVM             0xA0
  73 #define BMI160_REG_STATUS_NVM_RDY       0x10
  74 #define BMI160_REG_STATUS_FOC_RDY       0x08
  75 #define BMI160_REG_CONF_NVM_PROG_EN     0x02
  76
  77 #define BMI160_BWP_NORMAL               0x20
  78 #define BMI160_BWP_OSR2                 0x10
  79 #define BMI160_BWP_OSR4                 0x00
  80
  81 #define BMI160_ODR_400_Hz               0x0A
  82 #define BMI160_ODR_800_Hz               0x0B
  83 #define BMI160_ODR_1600_Hz              0x0C
  84 #define BMI160_ODR_3200_Hz              0x0D
  85
  86 #define BMI160_RANGE_2G                 0x03
  87 #define BMI160_RANGE_4G                 0x05
  88 #define BMI160_RANGE_8G                 0x08
  89 #define BMI160_RANGE_16G                0x0C
  90
  91 #define BMI160_RANGE_125DPS             0x04
  92 #define BMI160_RANGE_250DPS             0x03
  93 #define BMI160_RANGE_500DPS             0x02
  94 #define BMI160_RANGE_1000DPS            0x01
  95 #define BMI160_RANGE_2000DPS            0x00
  96
  97 typedef struct __attribute__ ((__packed__)) bmi160ContextData_s {
  98     uint16_t    chipMagicNumber;
  99     uint8_t     lastReadStatus;
 100     uint8_t     __padding;
 101     uint8_t     gyroRaw[6];
 102     uint8_t     accRaw[6];
 103 } bmi160ContextData_t;
 104
 105 STATIC_ASSERT(sizeof(bmi160ContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
 106
 107 static const gyroFilterAndRateConfig_t gyroConfigs[] = {
 108     { GYRO_LPF_256HZ,   3200,   { BMI160_BWP_OSR4 | BMI160_ODR_3200_Hz} },
 109     { GYRO_LPF_256HZ,   1600,   { BMI160_BWP_OSR2 | BMI160_ODR_1600_Hz} },
 110     { GYRO_LPF_256HZ,    800,   { BMI160_BWP_NORMAL | BMI160_ODR_800_Hz } },
 111 };
 112
 113 static void bmi160AccAndGyroInit(gyroDev_t *gyro)
 114 {
 115     uint8_t value;
 116
 117     busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
 118
 119     // Normal power mode, can take up to 80+3.8ms
 120     busWrite(gyro->busDev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
 121     delay(100);
 122
 123     busWrite(gyro->busDev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
 124     delay(5);
 125
 126     // Verify that normal power mode was entered
 127     busRead(gyro->busDev, BMI160_REG_PMU_STAT, &value);
 128     if ((value & 0x3C) != 0x14) {
 129         failureMode(FAILURE_GYRO_INIT_FAILED);
 130     }
 131
 132     // Set ranges and ODR
 133     busWrite(gyro->busDev, BMI160_REG_ACC_CONF, BMI160_BWP_OSR4 | BMI160_ODR_1600_Hz);
 134     delay(1);
 135
 136     // Figure out suitable filter configuration
 137     const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, &gyroConfigs[0], ARRAYLEN(gyroConfigs));
 138
 139     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
 140     busWrite(gyro->busDev, BMI160_REG_GYR_CONF, config->gyroConfigValues[0]);
 141     delay(1);
 142
 143     busWrite(gyro->busDev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
 144     delay(1);
 145
 146     busWrite(gyro->busDev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
 147     delay(1);
 148
 149     // Enable offset compensation
 150     // uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0);
 151     // busWrite(gyro->busDev, BMI160_REG_OFFSET_0, val | 0xC0);
 152
 153     // Enable data ready interrupt
 154     busWrite(gyro->busDev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
 155     delay(1);
 156
 157     // Enable INT1 pin
 158     busWrite(gyro->busDev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
 159     delay(1);
 160
 161     // Map data ready interrupt to INT1 pin
 162     busWrite(gyro->busDev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
 163     delay(1);
 164
 165     busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
 166 }
 167
 168 bool bmi160GyroReadScratchpad(gyroDev_t *gyro)
 169 {
 170     bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 171     ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI160_REG_GYR_DATA_X_LSB, ctx->gyroRaw, 6 + 6);
 172
 173     if (ctx->lastReadStatus) {
 174         gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
 175         gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
 176         gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
 177
 178         return true;
 179     }
 180
 181     return false;
 182 }
 183
 184 bool bmi160AccReadScratchpad(accDev_t *acc)
 185 {
 186     bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 187
 188     if (ctx->lastReadStatus) {
 189         acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0);
 190         acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1);
 191         acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2);
 192         return true;
 193     }
 194
 195     return false;
 196 }
 197
 198 static void bmi160AccInit(accDev_t *acc)
 199 {
 200     acc->acc_1G = 4096;
 201 }
 202
 203 bool bmi160AccDetect(accDev_t *acc)
 204 {
 205     acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_BMI160, acc->imuSensorToUse);
 206     if (acc->busDev == NULL) {
 207         return false;
 208     }
 209
 210     bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 211     if (ctx->chipMagicNumber != 0xB160) {
 212         return false;
 213     }
 214
 215     acc->initFn = bmi160AccInit;
 216     acc->readFn = bmi160AccReadScratchpad;
 217
 218     return true;
 219 }
 220
 221 static bool deviceDetect(busDevice_t * busDev)
 222 {
 223     uint8_t attempts;
 224
 225     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
 226
 227     for (attempts = 0; attempts < 5; attempts++) {
 228         uint8_t chipId;
 229
 230         delay(100);
 231         busRead(busDev, BMI160_REG_CHIPID, &chipId);
 232
 233         if (chipId == 0xD1) {
 234             return true;
 235         }
 236     }
 237
 238     return false;
 239 }
 240
 241 bool bmi160GyroDetect(gyroDev_t *gyro)
 242 {
 243     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI160, gyro->imuSensorToUse, OWNER_MPU);
 244     if (gyro->busDev == NULL) {
 245         return false;
 246     }
 247
 248     if (!deviceDetect(gyro->busDev)) {
 249         busDeviceDeInit(gyro->busDev);
 250         return false;
 251     }
 252
 253     // Magic number for ACC detection to indicate that we have detected BMI160 gyro
 254     bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 255     ctx->chipMagicNumber = 0xB160;
 256
 257     gyro->initFn = bmi160AccAndGyroInit;
 258     gyro->readFn = bmi160GyroReadScratchpad;
 259     gyro->intStatusFn = gyroCheckDataReady;
 260     gyro->temperatureFn = NULL;
 261     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 262     gyro->gyroAlign = gyro->busDev->param;
 263
 264     return true;
 265 }
 266
 267 #endif