src/main/drivers/accgyro/accgyro_bmi088.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_bmi088.h"
  43
  44
  45 #if defined(USE_IMU_BMI088)
  46
  47 /*
  48   device registers, names follow datasheet conventions, with REGA_
  49   prefix for accel, and REGG_ prefix for gyro
  50  */
  51 #define REGA_CHIPID        0x00
  52 #define REGA_ERR_REG       0x02
  53 #define REGA_STATUS        0x03
  54 #define REGA_X_LSB         0x12
  55 #define REGA_INT_STATUS_1  0x1D
  56 #define REGA_TEMP_LSB      0x22
  57 #define REGA_TEMP_MSB      0x23
  58 #define REGA_CONF          0x40
  59 #define REGA_RANGE         0x41
  60 #define REGA_PWR_CONF      0x7C
  61 #define REGA_PWR_CTRL      0x7D
  62 #define REGA_SOFTRESET     0x7E
  63 #define REGA_FIFO_CONFIG0  0x48
  64 #define REGA_FIFO_CONFIG1  0x49
  65 #define REGA_FIFO_DOWNS    0x45
  66 #define REGA_FIFO_DATA     0x26
  67 #define REGA_FIFO_LEN0     0x24
  68 #define REGA_FIFO_LEN1     0x25
  69
  70 #define REGG_CHIPID        0x00
  71 #define REGG_RATE_X_LSB    0x02
  72 #define REGG_INT_CTRL      0x15
  73 #define REGG_INT_STATUS_1  0x0A
  74 #define REGG_INT_STATUS_2  0x0B
  75 #define REGG_INT_STATUS_3  0x0C
  76 #define REGG_FIFO_STATUS   0x0E
  77 #define REGG_RANGE         0x0F
  78 #define REGG_BW            0x10
  79 #define REGG_LPM1          0x11
  80 #define REGG_RATE_HBW      0x13
  81 #define REGG_BGW_SOFTRESET 0x14
  82 #define REGG_FIFO_CONFIG_1 0x3E
  83 #define REGG_FIFO_DATA     0x3F
  84
  85
  86 static void bmi088GyroInit(gyroDev_t *gyro)
  87 {
  88     busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
  89
  90     // Soft reset
  91     busWrite(gyro->busDev, REGG_BGW_SOFTRESET, 0xB6);
  92     delay(100);
  93
  94     // ODR 2kHz, BW 532Hz
  95     busWrite(gyro->busDev, REGG_BW, 0x81);
  96     delay(1);
  97
  98     // Enable sampling
  99     busWrite(gyro->busDev, REGG_INT_CTRL, 0x80);
 100     delay(1);
 101
 102     busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
 103 }
 104
 105 static void bmi088AccInit(accDev_t *acc)
 106 {
 107     busSetSpeed(acc->busDev, BUS_SPEED_INITIALIZATION);
 108
 109     // Soft reset
 110     busWrite(acc->busDev, REGA_SOFTRESET, 0xB6);
 111     delay(100);
 112
 113     // Active mode
 114     busWrite(acc->busDev, REGA_PWR_CONF, 0);
 115     delay(100);
 116
 117     // ACC ON
 118     busWrite(acc->busDev, REGA_PWR_CTRL, 0x04);
 119     delay(100);
 120
 121     // OSR4, ODR 1600Hz
 122     busWrite(acc->busDev, REGA_CONF, 0x8C);
 123     delay(1);
 124
 125     // Range 12g
 126     busWrite(acc->busDev, REGA_RANGE, 0x02);
 127     delay(1);
 128
 129     busSetSpeed(acc->busDev, BUS_SPEED_STANDARD);
 130
 131     acc->acc_1G = 2048;
 132 }
 133
 134 static bool bmi088GyroRead(gyroDev_t *gyro)
 135 {
 136     uint8_t gyroRaw[6];
 137
 138     if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
 139         gyro->gyroADCRaw[X] = (float) int16_val_little_endian(gyroRaw, 0);
 140         gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(gyroRaw, 1);
 141         gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(gyroRaw, 2);
 142         return true;
 143     }
 144
 145     return false;
 146 }
 147
 148 static bool bmi088AccRead(accDev_t *acc)
 149 {
 150     uint8_t buffer[7];
 151
 152     if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
 153         // first byte is discarded, see datasheet
 154         acc->ADCRaw[X] = (float)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
 155         acc->ADCRaw[Y] = (float)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
 156         acc->ADCRaw[Z] = (float)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
 157         return true;
 158     }
 159
 160     return false;
 161 }
 162
 163 static bool gyroDeviceDetect(busDevice_t * busDev)
 164 {
 165     uint8_t attempts;
 166
 167     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
 168
 169     for (attempts = 0; attempts < 5; attempts++) {
 170         uint8_t chipId;
 171
 172         delay(100);
 173         busRead(busDev, REGG_CHIPID, &chipId);
 174
 175         if (chipId == 0x0F) {
 176             return true;
 177         }
 178     }
 179
 180     return false;
 181 }
 182
 183 static bool accDeviceDetect(busDevice_t * busDev)
 184 {
 185     uint8_t attempts;
 186
 187     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
 188
 189     for (attempts = 0; attempts < 5; attempts++) {
 190         uint8_t chipId;
 191
 192         delay(100);
 193         busRead(busDev, REGA_CHIPID, &chipId);
 194
 195         if (chipId == 0x1E) {
 196             return true;
 197         }
 198     }
 199
 200     return false;
 201 }
 202
 203 bool bmi088AccDetect(accDev_t *acc)
 204 {
 205     acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_ACC, acc->imuSensorToUse, OWNER_MPU);
 206     if (acc->busDev == NULL) {
 207         return false;
 208     }
 209
 210     if (!accDeviceDetect(acc->busDev)) {
 211         busDeviceDeInit(acc->busDev);
 212         return false;
 213     }
 214
 215     acc->initFn = bmi088AccInit;
 216     acc->readFn = bmi088AccRead;
 217     acc->accAlign = acc->busDev->param;
 218
 219     return true;
 220 }
 221
 222 bool bmi088GyroDetect(gyroDev_t *gyro)
 223 {
 224     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_GYRO, gyro->imuSensorToUse, OWNER_MPU);
 225     if (gyro->busDev == NULL) {
 226         return false;
 227     }
 228
 229     if (!gyroDeviceDetect(gyro->busDev)) {
 230         busDeviceDeInit(gyro->busDev);
 231         return false;
 232     }
 233
 234     gyro->initFn = bmi088GyroInit;
 235     gyro->readFn = bmi088GyroRead;
 236     gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb
 237     gyro->intStatusFn = gyroCheckDataReady;
 238     gyro->gyroAlign = gyro->busDev->param;
 239
 240     return true;
 241 }
 242
 243 #endif /* USE_IMU_BMI088 */