src/main/drivers/accgyro/accgyro_lsm6dxx.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  * from atbetaflight https://github.com/flightng/atbetaflight
  17  */
  18
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21 #include <stdlib.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #include "drivers/system.h"
  32 #include "drivers/time.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro/accgyro.h"
  36 #include "drivers/accgyro/accgyro_mpu.h"
  37 #include "drivers/accgyro/accgyro_lsm6dxx.h"
  38
  39 #if defined(USE_IMU_LSM6DXX)
  40
  41 typedef struct __attribute__ ((__packed__)) lsm6DContextData_s {
  42     uint16_t    chipMagicNumber;
  43     uint8_t     lastReadStatus;
  44     uint8_t     __padding_dummy;
  45     uint8_t     accRaw[6];
  46     uint8_t     gyroRaw[6];
  47 } lsm6DContextData_t;
  48
  49 #define LSM6DSO_CHIP_ID 0x6C
  50 #define LSM6DSL_CHIP_ID 0x6A
  51 #define LSM6DS3_CHIP_ID 0x69
  52
  53 static uint8_t lsm6dID = 0x6C;
  54
  55 static void lsm6dxxWriteRegister(const  busDevice_t *dev, lsm6dxxRegister_e registerID, uint8_t value, unsigned delayMs)
  56 {
  57     busWrite(dev, registerID, value);
  58     if (delayMs) {
  59         delay(delayMs);
  60     }
  61 }
  62
  63 static void lsm6dxxWriteRegisterBits(const  busDevice_t *dev, lsm6dxxRegister_e registerID, lsm6dxxConfigMasks_e mask, uint8_t value, unsigned delayMs)
  64 {
  65     uint8_t newValue;
  66     if (busRead(dev, registerID, &newValue)) {
  67         delayMicroseconds(2);
  68         newValue = (newValue & ~mask) | value;
  69         lsm6dxxWriteRegister(dev, registerID, newValue, delayMs);
  70     }
  71 }
  72
  73 static uint8_t getLsmDlpfBandwidth(gyroDev_t *gyro)
  74 {
  75     switch(gyro->lpf) {
  76         case GYRO_HARDWARE_LPF_NORMAL:
  77             return LSM6DXX_VAL_CTRL6_C_FTYPE_201HZ;
  78         case GYRO_HARDWARE_LPF_OPTION_1:
  79             return LSM6DXX_VAL_CTRL6_C_FTYPE_300HZ;
  80         case GYRO_HARDWARE_LPF_OPTION_2:
  81             return LSM6DXX_VAL_CTRL6_C_FTYPE_603HZ;
  82         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
  83             return LSM6DXX_VAL_CTRL6_C_FTYPE_603HZ;
  84     }
  85     return 0;
  86 }
  87
  88 static void lsm6dxxConfig(gyroDev_t *gyro)
  89 {
  90     busDevice_t * dev = gyro->busDev;
  91     const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
  92     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
  93
  94     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
  95     // Reset the device (wait 100ms before continuing config)
  96     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL3_C, LSM6DXX_MASK_CTRL3_C_RESET, BIT(0), 100);
  97
  98     // Configure data ready pulsed mode
  99     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_COUNTER_BDR1, LSM6DXX_MASK_COUNTER_BDR1, LSM6DXX_VAL_COUNTER_BDR1_DDRY_PM, 0);
 100
 101     // Configure interrupt pin 1 for gyro data ready only
 102     lsm6dxxWriteRegister(dev, LSM6DXX_REG_INT1_CTRL, LSM6DXX_VAL_INT1_CTRL, 1);
 103
 104     // Disable interrupt pin 2
 105     lsm6dxxWriteRegister(dev, LSM6DXX_REG_INT2_CTRL, LSM6DXX_VAL_INT2_CTRL, 1);
 106
 107     // Configure the accelerometer
 108     // 833hz ODR, 16G scale, use LPF2 output (default with ODR/4 cutoff)
 109     lsm6dxxWriteRegister(dev, LSM6DXX_REG_CTRL1_XL, (LSM6DXX_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DXX_VAL_CTRL1_XL_16G << 2) | (LSM6DXX_VAL_CTRL1_XL_LPF2 << 1), 1);
 110
 111     // Configure the gyro
 112     // 6664hz ODR, 2000dps scale
 113     lsm6dxxWriteRegister(dev, LSM6DXX_REG_CTRL2_G, (LSM6DXX_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DXX_VAL_CTRL2_G_2000DPS << 2), 1);
 114
 115     // Configure control register 3
 116     // latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads
 117     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL3_C, LSM6DXX_MASK_CTRL3_C, (LSM6DXX_VAL_CTRL3_C_H_LACTIVE | LSM6DXX_VAL_CTRL3_C_PP_OD | LSM6DXX_VAL_CTRL3_C_SIM | LSM6DXX_VAL_CTRL3_C_IF_INC), 1);
 118
 119     // Configure control register 4
 120     // enable accelerometer high performane mode; enable gyro LPF1
 121     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL4_C, LSM6DXX_MASK_CTRL4_C, (LSM6DXX_VAL_CTRL4_C_DRDY_MASK | LSM6DXX_VAL_CTRL4_C_I2C_DISABLE | LSM6DXX_VAL_CTRL4_C_LPF1_SEL_G), 1);
 122
 123
 124
 125     // Configure control register 6
 126     // disable I2C interface; set gyro LPF1 cutoff according to gyro_hardware_lpf setting
 127     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL6_C, (lsm6dID == LSM6DSO_CHIP_ID? LSM6DXX_MASK_CTRL6_C:LSM6DSL_MASK_CTRL6_C), (LSM6DXX_VAL_CTRL6_C_XL_HM_MODE | getLsmDlpfBandwidth(gyro)), 1);
 128
 129     // Configure control register 7
 130     lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL7_G, LSM6DXX_MASK_CTRL7_G, (LSM6DXX_VAL_CTRL7_G_HP_EN_G | LSM6DXX_VAL_CTRL7_G_HPM_G_16), 1);
 131
 132     // Configure control register 9
 133     // disable I3C interface
 134     if(lsm6dID == LSM6DSO_CHIP_ID)
 135     {
 136         lsm6dxxWriteRegisterBits(dev, LSM6DXX_REG_CTRL9_XL, LSM6DXX_MASK_CTRL9_XL, LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE, 1);
 137     }
 138
 139     busSetSpeed(dev, BUS_SPEED_FAST);
 140 }
 141
 142
 143
 144 static bool lsm6dxxDetect(busDevice_t * dev)
 145 {
 146     uint8_t tmp;
 147     uint8_t attemptsRemaining = 5;
 148     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
 149     do {
 150         delay(150);
 151
 152         busRead(dev, LSM6DXX_REG_WHO_AM_I, &tmp);
 153
 154         switch (tmp) {
 155             case LSM6DSO_CHIP_ID:
 156             case LSM6DSL_CHIP_ID:
 157                  lsm6dID = tmp;
 158                 // Compatible chip detected
 159                 return true;
 160             default:
 161                 // Retry detection
 162                 break;
 163         }
 164     } while (attemptsRemaining--);
 165
 166     return false;
 167 }
 168
 169 static void lsm6dxxSpiGyroInit(gyroDev_t *gyro)
 170 {
 171     lsm6dxxConfig(gyro);
 172 }
 173
 174 static void lsm6dxxSpiAccInit(accDev_t *acc)
 175 {
 176     // sensor is configured during gyro init
 177     acc->acc_1G = 512 * 4;   // 16G sensor scale
 178 }
 179
 180 static bool lsm6dxxAccRead(accDev_t *acc)
 181 {
 182     uint8_t data[6];
 183     const bool ack = busReadBuf(acc->busDev, LSM6DXX_REG_OUTX_L_A, data, 6);
 184     if (!ack) {
 185         return false;
 186     }
 187     acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0);
 188     acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1);
 189     acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2);
 190     return true;
 191 }
 192
 193 static bool lsm6dxxGyroRead(gyroDev_t *gyro)
 194 {
 195     uint8_t data[6];
 196     const bool ack = busReadBuf(gyro->busDev, LSM6DXX_REG_OUTX_L_G, data, 6);
 197     if (!ack) {
 198         return false;
 199     }
 200     gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0);
 201     gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1);
 202     gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2);
 203     return true;
 204 }
 205
 206 // Init Gyro first,then Acc
 207 bool lsm6dGyroDetect(gyroDev_t *gyro)
 208 {
 209     gyro->busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_LSM6D, gyro->imuSensorToUse, OWNER_MPU);
 210     if (gyro->busDev == NULL) {
 211         return false;
 212     }
 213
 214     if (!lsm6dxxDetect(gyro->busDev)) {
 215         busDeviceDeInit(gyro->busDev);
 216         return false;
 217     }
 218
 219     lsm6DContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 220     ctx->chipMagicNumber = 0xD6;
 221
 222     gyro->initFn = lsm6dxxSpiGyroInit;
 223     gyro->readFn = lsm6dxxGyroRead;
 224     gyro->intStatusFn = gyroCheckDataReady;
 225     gyro->scale = 1.0f / 16.4f; // 2000 dps
 226     return true;
 227
 228 }
 229 bool lsm6dAccDetect(accDev_t *acc)
 230 {
 231     acc->busDev = busDeviceOpen(BUSTYPE_SPI, DEVHW_LSM6D, acc->imuSensorToUse);
 232     if (acc->busDev == NULL) {
 233         return false;
 234     }
 235
 236     lsm6DContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 237     if (ctx->chipMagicNumber != 0xD6) {
 238         return false;
 239     }
 240     acc->initFn = lsm6dxxSpiAccInit;
 241     acc->readFn = lsm6dxxAccRead;
 242     acc->accAlign = acc->busDev->param;
 243
 244     return true;
 245 }
 246
 247
 248
 249 #endif