src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #if defined(USE_ACCGYRO_LSM6DSV16X)
  30
  31 #include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h"
  32
  33 // 10 MHz max SPI frequency
  34 #define LSM6DSV16X_MAX_SPI_CLK_HZ 10000000
  35
  36 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
  37 #define GYRO_EXTI_DETECT_THRESHOLD 1000
  38
  39 uint8_t lsm6dsv16xSpiDetect(const extDevice_t *dev)
  40 {
  41     const uint8_t whoAmI = spiReadRegMsk(dev, LSM6DSV_WHO_AM_I);
  42
  43     if (whoAmI != LSM6DSV16X_WHO_AM_I_CONST) {
  44         return MPU_NONE;
  45     }
  46
  47     return LSM6DSV16X_SPI;
  48 }
  49
  50 void lsm6dsv16xAccInit(accDev_t *acc)
  51 {
  52     const extDevice_t *dev = &acc->gyro->dev;
  53
  54     // Enable the accelerometer in high accuracy mode at 1kHz
  55     spiWriteReg(dev, LSM6DSV_CTRL1,
  56                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
  57                                     LSM6DSV_CTRL1_OP_MODE_XL_MASK,
  58                                     LSM6DSV_CTRL1_OP_MODE_XL_SHIFT) |
  59                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
  60                                     LSM6DSV_CTRL1_ODR_XL_MASK,
  61                                     LSM6DSV_CTRL1_ODR_XL_SHIFT));
  62
  63     // Enable 16G sensitivity
  64     spiWriteReg(dev, LSM6DSV_CTRL8,
  65                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
  66                                     LSM6DSV_CTRL8_FS_XL_MASK,
  67                                     LSM6DSV_CTRL8_FS_XL_SHIFT));
  68
  69     // ±16G mode
  70     acc->acc_1G = 512 * 4;
  71 }
  72
  73 static bool lsm6dsv16xAccReadSPI(accDev_t *acc)
  74 {
  75     switch (acc->gyro->gyroModeSPI) {
  76     case GYRO_EXTI_INT:
  77     case GYRO_EXTI_NO_INT:
  78     {
  79         acc->gyro->dev.txBuf[0] = LSM6DSV_OUTX_L_A | 0x80;
  80
  81         busSegment_t segments[] = {
  82                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
  83                 {.u.link = {NULL, NULL}, 0, true, NULL},
  84         };
  85         segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
  86         segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
  87
  88         spiSequence(&acc->gyro->dev, &segments[0]);
  89
  90         // Wait for completion
  91         spiWait(&acc->gyro->dev);
  92
  93         int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf;
  94
  95         acc->ADCRaw[X] = accData[1];
  96         acc->ADCRaw[Y] = accData[2];
  97         acc->ADCRaw[Z] = accData[3];
  98         break;
  99     }
 100
 101     case GYRO_EXTI_INT_DMA:
 102     {
 103         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 104         // up an old value.
 105
 106         // This data was read from the gyro, which is the same SPI device as the acc
 107         int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf;
 108
 109         acc->ADCRaw[X] = accData[4];
 110         acc->ADCRaw[Y] = accData[5];
 111         acc->ADCRaw[Z] = accData[6];
 112         break;
 113     }
 114
 115     case GYRO_EXTI_INIT:
 116     default:
 117         break;
 118     }
 119
 120     return true;
 121 }
 122
 123 bool lsm6dsv16xSpiAccDetect(accDev_t *acc)
 124 {
 125     if (acc->mpuDetectionResult.sensor != LSM6DSV16X_SPI) {
 126         return false;
 127     }
 128
 129     acc->initFn = lsm6dsv16xAccInit;
 130     acc->readFn = lsm6dsv16xAccReadSPI;
 131
 132     return true;
 133 }
 134
 135 void lsm6dsv16xGyroInit(gyroDev_t *gyro)
 136 {
 137     const extDevice_t *dev = &gyro->dev;
 138
 139     spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSV16X_MAX_SPI_CLK_HZ));
 140
 141     // Perform a software reset
 142     spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_SW_RESET);
 143
 144     // Select high-accuracy ODR mode 1 before leaving power-off mode
 145     spiWriteReg(dev, LSM6DSV_HAODR_CFG,
 146                 LSM6DSV_ENCODE_BITS(LSM6DSV_HAODR_MODE1,
 147                                     LSM6DSV_HAODR_CFG_HAODR_SEL_MASK,
 148                                     LSM6DSV_HAODR_CFG_HAODR_SEL_SHIFT));
 149
 150     // Enable the gyro in high accuracy mode at 8kHz
 151     spiWriteReg(dev, LSM6DSV_CTRL2,
 152                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_OP_MODE_G_HIGH_ACCURACY,
 153                                     LSM6DSV_CTRL2_OP_MODE_G_MASK,
 154                                     LSM6DSV_CTRL2_OP_MODE_G_SHIFT) |
 155                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_ODR_G_8000HZ,
 156                                     LSM6DSV_CTRL2_ODR_G_MASK,
 157                                     LSM6DSV_CTRL2_ODR_G_SHIFT));
 158
 159     // Enable 2000 deg/s sensitivity
 160     spiWriteReg(dev, LSM6DSV_CTRL6,
 161                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL6_FS_G_BW_407HZ,
 162                                     LSM6DSV_CTRL6_LPF1_G_BW_MASK,
 163                                     LSM6DSV_CTRL6_LPF1_G_BW_SHIFT) |
 164                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL6_FS_G_2000DPS,
 165                                     LSM6DSV_CTRL6_FS_G_MASK,
 166                                     LSM6DSV_CTRL6_FS_G_SHIFT));
 167
 168     // Autoincrement register address when doing block SPI reads and update continuously
 169     spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC);
 170
 171     // Generate pulse on interrupt line, not requiring a read to clear
 172     // TODO this pulse lasts 66us followed by a low of 66us, so we get 132us cycle time, not 125us
 173     spiWriteReg(dev, LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED);
 174
 175     // From section 4.1, Mechanical characteristics, of the datasheet, G_So is 70mdps/LSB for FS = ±2000 dps.
 176     gyro->scale = 0.070f;
 177
 178     // Enable the INT1 output to interrupt when new gyro data is ready
 179     spiWriteReg(dev, LSM6DSV_INT1_CTRL, LSM6DSV_INT1_CTRL_INT1_DRDY_G);
 180
 181     mpuGyroInit(gyro);
 182 }
 183
 184 bool lsm6dsv16xGyroReadSPI(gyroDev_t *gyro)
 185 {
 186     int16_t *gyroData = (int16_t *)gyro->dev.rxBuf;
 187     switch (gyro->gyroModeSPI) {
 188     case GYRO_EXTI_INIT:
 189     {
 190         // Initialise the tx buffer to all 0xff
 191         memset(gyro->dev.txBuf, 0xff, 16);
 192
 193         // Check that minimum number of interrupts have been detected
 194
 195         // We need some offset from the gyro interrupts to ensure sampling after the interrupt
 196         gyro->gyroDmaMaxDuration = 5;
 197         if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
 198             if (spiUseDMA(&gyro->dev)) {
 199                 gyro->dev.callbackArg = (uint32_t)gyro;
 200                 gyro->dev.txBuf[0] = LSM6DSV_OUTX_L_G | 0x80;
 201                 // Read three words of gyro data immediately followed by three bytes of acc data
 202                 gyro->segments[0].len = sizeof(uint8_t) + 6 * sizeof(int16_t);
 203                 gyro->segments[0].callback = mpuIntcallback;
 204                 gyro->segments[0].u.buffers.txData = gyro->dev.txBuf;
 205                 gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
 206                 gyro->segments[0].negateCS = true;
 207                 gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
 208             } else {
 209                 // Interrupts are present, but no DMA
 210                 gyro->gyroModeSPI = GYRO_EXTI_INT;
 211             }
 212         } else {
 213             gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
 214         }
 215         break;
 216     }
 217
 218     case GYRO_EXTI_INT:
 219     case GYRO_EXTI_NO_INT:
 220     {
 221         gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;
 222
 223         busSegment_t segments[] = {
 224                 {.u.buffers = {NULL, NULL}, 7, true, NULL},
 225                 {.u.link = {NULL, NULL}, 0, true, NULL},
 226         };
 227         segments[0].u.buffers.txData = gyro->dev.txBuf;
 228         segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
 229
 230         spiSequence(&gyro->dev, &segments[0]);
 231
 232         // Wait for completion
 233         spiWait(&gyro->dev);
 234
 235         gyro->gyroADCRaw[X] = gyroData[1];
 236         gyro->gyroADCRaw[Y] = gyroData[2];
 237         gyro->gyroADCRaw[Z] = gyroData[3];
 238         break;
 239     }
 240
 241     case GYRO_EXTI_INT_DMA:
 242     {
 243         // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
 244         // up an old value.
 245         gyro->gyroADCRaw[X] = gyroData[1];
 246         gyro->gyroADCRaw[Y] = gyroData[2];
 247         gyro->gyroADCRaw[Z] = gyroData[3];
 248         break;
 249     }
 250
 251     default:
 252         break;
 253     }
 254
 255     return true;
 256 }
 257
 258 bool lsm6dsv16xSpiGyroDetect(gyroDev_t *gyro)
 259 {
 260     if (gyro->mpuDetectionResult.sensor != LSM6DSV16X_SPI) {
 261         return false;
 262     }
 263
 264     gyro->initFn = lsm6dsv16xGyroInit;
 265     gyro->readFn = lsm6dsv16xGyroReadSPI;
 266
 267     return true;
 268 }
 269 #endif // USE_ACCGYRO_LSM6DSV16X