src/main/drivers/accgyro/accgyro_bmi270.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #if defined(USE_IMU_BMI270)
  27
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/system.h"
  35 #include "drivers/time.h"
  36 #include "drivers/io.h"
  37 #include "drivers/bus.h"
  38
  39 #include "drivers/sensor.h"
  40 #include "drivers/accgyro/accgyro.h"
  41 #include "drivers/accgyro/accgyro_bmi270.h"
  42
  43 #define BMI270_CONFIG_SIZE 328
  44
  45 // Declaration for the device config (microcode) that must be uploaded to the sensor
  46 extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE];
  47
  48 // BMI270 registers (not the complete list)
  49 typedef enum {
  50     BMI270_REG_CHIP_ID = 0x00,
  51     BMI270_REG_ERR_REG = 0x02,
  52     BMI270_REG_STATUS = 0x03,
  53     BMI270_REG_ACC_DATA_X_LSB = 0x0C,
  54     BMI270_REG_GYR_DATA_X_LSB = 0x12,
  55     BMI270_REG_SENSORTIME_0 = 0x18,
  56     BMI270_REG_SENSORTIME_1 = 0x19,
  57     BMI270_REG_SENSORTIME_2 = 0x1A,
  58     BMI270_REG_EVENT = 0x1B,
  59     BMI270_REG_INT_STATUS_0 = 0x1C,
  60     BMI270_REG_INT_STATUS_1 = 0x1D,
  61     BMI270_REG_INTERNAL_STATUS = 0x21,
  62     BMI270_REG_TEMPERATURE_LSB = 0x22,
  63     BMI270_REG_TEMPERATURE_MSB = 0x23,
  64     BMI270_REG_FIFO_LENGTH_LSB = 0x24,
  65     BMI270_REG_FIFO_LENGTH_MSB = 0x25,
  66     BMI270_REG_FIFO_DATA = 0x26,
  67     BMI270_REG_ACC_CONF = 0x40,
  68     BMI270_REG_ACC_RANGE = 0x41,
  69     BMI270_REG_GYRO_CONF = 0x42,
  70     BMI270_REG_GYRO_RANGE = 0x43,
  71     BMI270_REG_AUX_CONF = 0x44,
  72     BMI270_REG_FIFO_DOWNS = 0x45,
  73     BMI270_REG_FIFO_WTM_0 = 0x46,
  74     BMI270_REG_FIFO_WTM_1 = 0x47,
  75     BMI270_REG_FIFO_CONFIG_0 = 0x48,
  76     BMI270_REG_FIFO_CONFIG_1 = 0x49,
  77     BMI270_REG_SATURATION = 0x4A,
  78     BMI270_REG_INT1_IO_CTRL = 0x53,
  79     BMI270_REG_INT2_IO_CTRL = 0x54,
  80     BMI270_REG_INT_LATCH = 0x55,
  81     BMI270_REG_INT1_MAP_FEAT = 0x56,
  82     BMI270_REG_INT2_MAP_FEAT = 0x57,
  83     BMI270_REG_INT_MAP_DATA = 0x58,
  84     BMI270_REG_INIT_CTRL = 0x59,
  85     BMI270_REG_INIT_DATA = 0x5E,
  86     BMI270_REG_ACC_SELF_TEST = 0x6D,
  87     BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
  88     BMI270_REG_PWR_CONF = 0x7C,
  89     BMI270_REG_PWR_CTRL = 0x7D,
  90     BMI270_REG_CMD = 0x7E,
  91 } bmi270Register_e;
  92
  93 #define BMI270_CHIP_ID 0x24
  94
  95 #define BMI270_CMD_SOFTRESET 0xB6
  96
  97 #define BMI270_PWR_CONF_HP 0x00
  98 #define BMI270_PWR_CTRL_GYR_EN 0x02
  99 #define BMI270_PWR_CTRL_ACC_EN 0x04
 100 #define BMI270_PWR_CTRL_TEMP_EN 0x08
 101
 102 #define BMI270_ACC_CONF_HP 0x80
 103 #define BMI270_ACC_RANGE_8G 0x02
 104 #define BMI270_ACC_RANGE_16G 0x03
 105
 106 #define BMI270_GYRO_CONF_NOISE_PERF 0x40
 107 #define BMI270_GYRO_CONF_FILTER_PERF 0x80
 108 #define BMI270_GYRO_RANGE_2000DPS 0x08
 109
 110 #define BMI270_INT_MAP_DATA_DRDY_INT1 0x04
 111 #define BMI270_INT1_IO_CTRL_ACTIVE_HIGH 0x02
 112 #define BMI270_INT1_IO_CTRL_OUTPUT_EN 0x08
 113
 114 #define BMI270_ODR_400 0x0A
 115 #define BMI270_ODR_800 0x0B
 116 #define BMI270_ODR_1600 0x0C
 117 #define BMI270_ODR_3200 0x0D
 118
 119 #define BMI270_BWP_OSR4 0x00
 120 #define BMI270_BWP_OSR2 0x10
 121 #define BMI270_BWP_NORM 0x20
 122
 123 typedef struct __attribute__ ((__packed__)) bmi270ContextData_s {
 124     uint16_t    chipMagicNumber;
 125     uint8_t     lastReadStatus;
 126     uint8_t     __padding_dummy;
 127     uint8_t     accRaw[6];
 128     uint8_t     gyroRaw[6];
 129 } bmi270ContextData_t;
 130
 131 STATIC_ASSERT(sizeof(bmi270ContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
 132
 133 static const gyroFilterAndRateConfig_t gyroConfigs[] = {
 134     { GYRO_LPF_256HZ,   3200,   { BMI270_BWP_OSR4 | BMI270_ODR_3200} },
 135     { GYRO_LPF_256HZ,   1600,   { BMI270_BWP_OSR2 | BMI270_ODR_1600} },
 136     { GYRO_LPF_256HZ,    800,   { BMI270_BWP_NORM | BMI270_ODR_800 } },
 137 };
 138
 139 // Toggle the CS to switch the device into SPI mode.
 140 // Device switches initializes as I2C and switches to SPI on a low to high CS transition
 141 static void bmi270EnableSPI(busDevice_t *dev)
 142 {
 143     IOLo(dev->busdev.spi.csnPin);
 144     delay(1);
 145     IOHi(dev->busdev.spi.csnPin);
 146     delay(10);
 147 }
 148
 149
 150 static bool bmi270DeviceDetect(busDevice_t * busDev)
 151 {
 152     uint8_t id[2];
 153
 154     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
 155     bmi270EnableSPI(busDev);
 156
 157     busReadBuf(busDev, BMI270_REG_CHIP_ID, &id[0], 2);
 158     if (id[1] == BMI270_CHIP_ID) {
 159         return true;
 160     }
 161
 162     return false;
 163 }
 164
 165 static void bmi270UploadConfig(busDevice_t * busDev)
 166 {
 167     busWrite(busDev, BMI270_REG_PWR_CONF, 0);
 168     delay(1);
 169     busWrite(busDev, BMI270_REG_INIT_CTRL, 0);
 170     delay(1);
 171
 172     // Transfer the config file
 173     uint8_t reg = BMI270_REG_INIT_DATA;
 174     busTransferDescriptor_t tfDesc[] = {
 175        {.rxBuf = NULL, .txBuf=&reg, .length=1},
 176        {.rxBuf = NULL, .txBuf=(uint8_t *)bmi270_maximum_fifo_config_file, .length=sizeof(bmi270_maximum_fifo_config_file)}
 177     };
 178
 179     spiBusTransferMultiple(busDev, tfDesc, 2);
 180
 181     delay(10);
 182     busWrite(busDev, BMI270_REG_INIT_CTRL, 1);
 183     delay(1);
 184 }
 185
 186 static void bmi270AccAndGyroInit(gyroDev_t *gyro)
 187 {
 188     busDevice_t * busDev = gyro->busDev;
 189
 190     // Perform a soft reset to set all configuration to default
 191     // Delay 100ms before continuing configuration
 192     busWrite(busDev, BMI270_REG_CMD, BMI270_CMD_SOFTRESET);
 193     delay(100);
 194
 195     // Use standard bus speed
 196     busSetSpeed(busDev, BUS_SPEED_STANDARD);
 197
 198     // Toggle the chip into SPI mode
 199     bmi270EnableSPI(busDev);
 200
 201     bmi270UploadConfig(busDev);
 202
 203     // Configure the accelerometer
 204     busWrite(busDev, BMI270_REG_ACC_CONF, BMI270_ODR_1600 | BMI270_BWP_OSR4 | BMI270_ACC_CONF_HP);
 205     delay(1);
 206
 207     // Configure the accelerometer full-scale range
 208     busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_16G);
 209     delay(1);
 210
 211     // Configure the gyro
 212     // Figure out suitable filter configuration
 213     const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, &gyroConfigs[0], ARRAYLEN(gyroConfigs));
 214
 215     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
 216
 217     busWrite(busDev, BMI270_REG_GYRO_CONF, config->gyroConfigValues[0] | BMI270_GYRO_CONF_NOISE_PERF | BMI270_GYRO_CONF_FILTER_PERF);
 218     delay(1);
 219
 220     // Configure the gyro full-range scale
 221     busWrite(busDev, BMI270_REG_GYRO_RANGE, BMI270_GYRO_RANGE_2000DPS);
 222     delay(1);
 223
 224     // Configure the gyro data ready interrupt
 225     busWrite(busDev, BMI270_REG_INT_MAP_DATA, BMI270_INT_MAP_DATA_DRDY_INT1);
 226     delay(1);
 227
 228     // Configure the behavior of the INT1 pin
 229     busWrite(busDev, BMI270_REG_INT1_IO_CTRL, BMI270_INT1_IO_CTRL_ACTIVE_HIGH | BMI270_INT1_IO_CTRL_OUTPUT_EN);
 230     delay(1);
 231
 232     // Configure the device for  performance mode
 233     busWrite(busDev, BMI270_REG_PWR_CONF, BMI270_PWR_CONF_HP);
 234     delay(1);
 235
 236     // Enable the gyro and accelerometer
 237     busWrite(busDev, BMI270_REG_PWR_CTRL, BMI270_PWR_CTRL_GYR_EN | BMI270_PWR_CTRL_ACC_EN);
 238     delay(1);
 239 }
 240
 241
 242 static bool bmi270yroReadScratchpad(gyroDev_t *gyro)
 243 {
 244     bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 245     ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI270_REG_ACC_DATA_X_LSB, &ctx->__padding_dummy, 6 + 6 + 1);
 246
 247     if (ctx->lastReadStatus) {
 248         gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
 249         gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
 250         gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
 251
 252         return true;
 253     }
 254
 255     return false;
 256 }
 257
 258 static bool bmi270AccReadScratchpad(accDev_t *acc)
 259 {
 260     bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 261
 262     if (ctx->lastReadStatus) {
 263         acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0);
 264         acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1);
 265         acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2);
 266         return true;
 267     }
 268
 269     return false;
 270 }
 271
 272 static bool bmi270TemperatureRead(gyroDev_t *gyro, int16_t * data)
 273 {
 274     uint8_t buffer[3];
 275
 276     bool readStatus = busReadBuf(gyro->busDev, BMI270_REG_TEMPERATURE_LSB, &buffer[0], sizeof(buffer));
 277
 278     if (readStatus) {
 279         // Convert to degC*10: degC = raw / 512 + 23
 280         *data = 230 + ((10 * (int32_t)((int16_t)((buffer[2] << 8) | buffer[1]))) >> 9);
 281         return true;
 282     }
 283
 284     return false;
 285 }
 286
 287 static void bmi270GyroInit(gyroDev_t *gyro)
 288 {
 289     bmi270AccAndGyroInit(gyro);
 290 }
 291
 292 static void bmi270AccInit(accDev_t *acc)
 293 {
 294     // sensor is configured during gyro init
 295     acc->acc_1G = 2048;   // 16G sensor scale
 296 }
 297
 298 bool bmi270AccDetect(accDev_t *acc)
 299 {
 300     acc->busDev = busDeviceOpen(BUSTYPE_SPI, DEVHW_BMI270, acc->imuSensorToUse);
 301     if (acc->busDev == NULL) {
 302         return false;
 303     }
 304
 305     bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 306     if (ctx->chipMagicNumber != 0xB270) {
 307         return false;
 308     }
 309
 310     acc->initFn = bmi270AccInit;
 311     acc->readFn = bmi270AccReadScratchpad;
 312     acc->accAlign = acc->busDev->param;
 313
 314     return true;
 315 }
 316
 317
 318 bool bmi270GyroDetect(gyroDev_t *gyro)
 319 {
 320     gyro->busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_BMI270, gyro->imuSensorToUse, OWNER_MPU);
 321     if (gyro->busDev == NULL) {
 322         return false;
 323     }
 324
 325     if (!bmi270DeviceDetect(gyro->busDev)) {
 326         busDeviceDeInit(gyro->busDev);
 327         return false;
 328     }
 329
 330     // Magic number for ACC detection to indicate that we have detected BMI270 gyro
 331     bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 332     ctx->chipMagicNumber = 0xB270;
 333
 334     gyro->initFn = bmi270GyroInit;
 335     gyro->readFn = bmi270yroReadScratchpad;
 336     gyro->temperatureFn = bmi270TemperatureRead;
 337     gyro->intStatusFn = gyroCheckDataReady;
 338     gyro->scale = 1.0f / 16.4f; // 2000 dps
 339     gyro->gyroAlign = gyro->busDev->param;
 340     return true;
 341 }
 342 #endif // USE_IMU_BMI270