src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.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 #ifdef USE_ACCGYRO_LSM6DSO
  27
  28 #include "drivers/accgyro/accgyro.h"
  29 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
  30 #include "drivers/bus_spi.h"
  31 #include "drivers/exti.h"
  32 #include "drivers/io.h"
  33 #include "drivers/io_impl.h"
  34 #include "drivers/nvic.h"
  35 #include "drivers/sensor.h"
  36 #include "drivers/time.h"
  37
  38 // 10 MHz max SPI frequency
  39 #define LSM6DSO_MAX_SPI_CLK_HZ 10000000
  40
  41 #define LSM6DSO_CHIP_ID 0x6C
  42
  43 // LSM6DSO register configuration values
  44 typedef enum {
  45     LSM6DSO_VAL_INT1_CTRL = 0x02,             // enable gyro data ready interrupt pin 1
  46     LSM6DSO_VAL_INT2_CTRL = 0x02,             // enable gyro data ready interrupt pin 2
  47     LSM6DSO_VAL_CTRL1_XL_ODR833 = 0x07,       // accelerometer 833hz output data rate (gyro/8)
  48     LSM6DSO_VAL_CTRL1_XL_ODR1667 = 0x08,      // accelerometer 1666hz output data rate (gyro/4)
  49     LSM6DSO_VAL_CTRL1_XL_ODR3332 = 0x09,      // accelerometer 3332hz output data rate (gyro/2)
  50     LSM6DSO_VAL_CTRL1_XL_ODR3333 = 0x0A,      // accelerometer 6664hz output data rate (gyro/1)
  51     LSM6DSO_VAL_CTRL1_XL_8G = 0x03,           // accelerometer 8G scale
  52     LSM6DSO_VAL_CTRL1_XL_16G = 0x01,          // accelerometer 16G scale
  53     LSM6DSO_VAL_CTRL1_XL_LPF1 = 0x00,         // accelerometer output from LPF1
  54     LSM6DSO_VAL_CTRL1_XL_LPF2 = 0x01,         // accelerometer output from LPF2
  55     LSM6DSO_VAL_CTRL2_G_ODR6664 = 0x0A,       // gyro 6664hz output data rate
  56     LSM6DSO_VAL_CTRL2_G_2000DPS = 0x03,       // gyro 2000dps scale
  57     LSM6DSO_VAL_CTRL3_C_BDU = BIT(6),         // (bit 6) output registers are not updated until MSB and LSB have been read (prevents MSB from being updated while burst reading LSB/MSB)
  58     LSM6DSO_VAL_CTRL3_C_H_LACTIVE = 0,        // (bit 5) interrupt pins active high
  59     LSM6DSO_VAL_CTRL3_C_PP_OD = 0,            // (bit 4) interrupt pins push/pull
  60     LSM6DSO_VAL_CTRL3_C_SIM = 0,              // (bit 3) SPI 4-wire interface mode
  61     LSM6DSO_VAL_CTRL3_C_IF_INC = BIT(2),      // (bit 2) auto-increment address for burst reads
  62     LSM6DSO_VAL_CTRL4_C_I2C_DISABLE = BIT(2), // (bit 2) disable I2C interface
  63     LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G = BIT(1),  // (bit 1) enable gyro LPF1
  64     LSM6DSO_VAL_CTRL6_C_XL_HM_MODE = 0,       // (bit 4) enable accelerometer high performance mode
  65     LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ = 0x00,   // (bits 2:0) gyro LPF1 cutoff 335.5hz
  66     LSM6DSO_VAL_CTRL6_C_FTYPE_232HZ = 0x01,   // (bits 2:0) gyro LPF1 cutoff 232.0hz
  67     LSM6DSO_VAL_CTRL6_C_FTYPE_171HZ = 0x02,   // (bits 2:0) gyro LPF1 cutoff 171.1hz
  68     LSM6DSO_VAL_CTRL6_C_FTYPE_609HZ = 0x03,   // (bits 2:0) gyro LPF1 cutoff 609.0hz
  69     LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE = BIT(1),// (bit 1) disable I3C interface
  70 } lsm6dsoConfigValues_e;
  71
  72 // LSM6DSO register configuration bit masks
  73 typedef enum {
  74     LSM6DSO_MASK_CTRL3_C = 0x7C,         // 0b01111100
  75     LSM6DSO_MASK_CTRL3_C_RESET = BIT(0), // 0b00000001
  76     LSM6DSO_MASK_CTRL4_C = 0x06,         // 0b00000110
  77     LSM6DSO_MASK_CTRL6_C = 0x17,         // 0b00010111
  78     LSM6DSO_MASK_CTRL9_XL = 0x02,        // 0b00000010
  79 } lsm6dsoConfigMasks_e;
  80
  81 uint8_t lsm6dsoDetect(const extDevice_t *dev)
  82 {
  83     uint8_t chipID = 0;
  84     spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSO_MAX_SPI_CLK_HZ));
  85
  86     if (busReadRegisterBuffer(dev, LSM6DSO_REG_WHO_AM_I, &chipID, 1)) {
  87         if (chipID == LSM6DSO_CHIP_ID) {
  88             return LSM6DSO_SPI;
  89         }
  90     }
  91
  92     return MPU_NONE;
  93 }
  94
  95 static void lsm6dsoWriteRegister(const extDevice_t *dev, lsm6dsoRegister_e registerID, uint8_t value, unsigned delayMs)
  96 {
  97     busWriteRegister(dev, registerID, value);
  98     if (delayMs) {
  99         delay(delayMs);
 100     }
 101 }
 102
 103 static void lsm6dsoWriteRegisterBits(const extDevice_t *dev, lsm6dsoRegister_e registerID, lsm6dsoConfigMasks_e mask, uint8_t value, unsigned delayMs)
 104 {
 105     uint8_t newValue;
 106     if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) {
 107         delayMicroseconds(2);
 108         newValue = (newValue & ~mask) | value;
 109         lsm6dsoWriteRegister(dev, registerID, newValue, delayMs);
 110     }
 111 }
 112
 113 static void lsm6dsoConfig(gyroDev_t *gyro)
 114 {
 115     extDevice_t *dev = &gyro->dev;
 116
 117     // Reset the device (wait 100ms before continuing config)
 118     lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C_RESET, BIT(0), 100);
 119
 120     // Configure interrupt pin 1 for gyro data ready only
 121     lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT1_CTRL, LSM6DSO_VAL_INT1_CTRL, 1);
 122
 123     // Disable interrupt pin 2
 124     lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT2_CTRL, LSM6DSO_VAL_INT2_CTRL, 1);
 125
 126     // Configure the accelerometer
 127     // 833hz ODR, 16G scale, use LPF1 output
 128     lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL1_XL, (LSM6DSO_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSO_VAL_CTRL1_XL_16G << 2) | (LSM6DSO_VAL_CTRL1_XL_LPF1 << 1), 1);
 129
 130     // Configure the gyro
 131     // 6664hz ODR, 2000dps scale
 132     lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL2_G, (LSM6DSO_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSO_VAL_CTRL2_G_2000DPS << 2), 1);
 133
 134     // Configure control register 3
 135     // latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads
 136     lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C, (LSM6DSO_VAL_CTRL3_C_BDU | LSM6DSO_VAL_CTRL3_C_H_LACTIVE | LSM6DSO_VAL_CTRL3_C_PP_OD | LSM6DSO_VAL_CTRL3_C_SIM | LSM6DSO_VAL_CTRL3_C_IF_INC), 1);
 137
 138     // Configure control register 4
 139     // enable accelerometer high performane mode; set gyro LPF1 cutoff to 335.5hz
 140     lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL4_C, LSM6DSO_MASK_CTRL4_C, (LSM6DSO_VAL_CTRL4_C_I2C_DISABLE | LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G), 1);
 141
 142     // Configure control register 6
 143     // disable I2C interface; enable gyro LPF1
 144     lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL6_C, LSM6DSO_MASK_CTRL6_C, (LSM6DSO_VAL_CTRL6_C_XL_HM_MODE | LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ), 1);
 145
 146     // Configure control register 9
 147     // disable I3C interface
 148     lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL9_XL, LSM6DSO_MASK_CTRL9_XL, LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE, 1);
 149 }
 150
 151 #if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
 152 static void lsm6dsoIntExtiInit(gyroDev_t *gyro)
 153 {
 154     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
 155         return;
 156     }
 157
 158     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
 159
 160     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
 161     EXTIHandlerInit(&gyro->exti, lsm6dsoExtiHandler);
 162     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
 163     EXTIEnable(mpuIntIO, true);
 164 }
 165 #endif
 166
 167 static void lsm6dsoSpiGyroInit(gyroDev_t *gyro)
 168 {
 169     lsm6dsoConfig(gyro);
 170
 171 #if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
 172     lsm6dsoIntExtiInit(gyro);
 173 #endif
 174 }
 175
 176 static void lsm6dsoSpiAccInit(accDev_t *acc)
 177 {
 178     // sensor is configured during gyro init
 179     acc->acc_1G = 512 * 4;   // 16G sensor scale
 180 }
 181
 182 bool lsm6dsoSpiAccDetect(accDev_t *acc)
 183 {
 184     if (acc->mpuDetectionResult.sensor != LSM6DSO_SPI) {
 185         return false;
 186     }
 187
 188     acc->initFn = lsm6dsoSpiAccInit;
 189     acc->readFn = lsm6dsoAccRead;
 190
 191     return true;
 192 }
 193
 194 bool lsm6dsoSpiGyroDetect(gyroDev_t *gyro)
 195 {
 196     if (gyro->mpuDetectionResult.sensor != LSM6DSO_SPI) {
 197         return false;
 198     }
 199
 200     gyro->initFn = lsm6dsoSpiGyroInit;
 201     gyro->readFn = lsm6dsoGyroRead;
 202     gyro->scale = GYRO_SCALE_2000DPS;
 203
 204     return true;
 205 }
 206 #endif // USE_ACCGYRO_LSM6DSO