From 949730bda2fcad64eb433b7403c34c485e7fd5e9 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <1483569698@qq.com> Date: Mon, 12 Dec 2022 16:37:01 +0800 Subject: [PATCH] add support for lsm6dsl imu --- make/source.mk | 2 + src/main/cli/settings.c | 4 +- src/main/drivers/accgyro/accgyro.h | 1 + src/main/drivers/accgyro/accgyro_mpu.c | 4 + src/main/drivers/accgyro/accgyro_mpu.h | 1 + .../drivers/accgyro/accgyro_spi_lsm6dsl.c | 206 +++++++++++++++ .../drivers/accgyro/accgyro_spi_lsm6dsl.h | 67 +++++ .../accgyro/accgyro_spi_lsm6dsl_init.c | 239 ++++++++++++++++++ src/main/drivers/accgyro/gyro_sync.c | 7 +- src/main/sensors/acceleration.h | 1 + src/main/sensors/acceleration_init.c | 9 + src/main/sensors/gyro_init.c | 17 +- src/main/target/AT32F437DEV/target.h | 1 + src/main/target/AT32F437DEV/target.mk | 2 + src/main/target/EMSRPROTO2/target.h | 1 + src/main/target/EMSRPROTO2/target.mk | 2 + src/main/target/NEUTRONRCF435AIO/target.h | 1 + src/main/target/NEUTRONRCF435AIO/target.mk | 2 + 18 files changed, 561 insertions(+), 6 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_lsm6dsl.h create mode 100644 src/main/drivers/accgyro/accgyro_spi_lsm6dsl_init.c diff --git a/make/source.mk b/make/source.mk index 9891bada2..913218ffe 100644 --- a/make/source.mk +++ b/make/source.mk @@ -235,6 +235,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/accgyro/accgyro_spi_bmi160.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_lsm6ds3.c \ + drivers/accgyro/accgyro_spi_lsm6dsl.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ drivers/accgyro_legacy/accgyro_adxl345.c \ drivers/accgyro_legacy/accgyro_bma280.c \ @@ -374,6 +375,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/accgyro/accgyro_spi_icm20689.c \ drivers/accgyro/accgyro_spi_icm426xx.c \ drivers/accgyro/accgyro_spi_lsm6ds3_init.c \ + drivers/accgyro/accgyro_spi_lsm6dsl_init.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 493a23c7b..ed6887ff4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -136,14 +136,14 @@ const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P", - "BMI160", "BMI270", "LSM6DS3", "LSM6DSO", "FAKE" + "BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6DSO", "FAKE" }; // sync with gyroHardware_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P", - "BMI160", "BMI270", "LSM6DS3", "LSM6SDO", "FAKE" + "BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6SDO", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 66e0a09db..ec4cf9ce2 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -59,6 +59,7 @@ typedef enum { GYRO_BMI160, GYRO_BMI270, GYRO_LSM6DS3, + GYRO_LSM6DSL, GYRO_LSM6DSO, GYRO_FAKE } gyroHardware_e; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 83bfda14f..1bcbe9f50 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -52,6 +52,7 @@ #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" #include "drivers/accgyro/accgyro_spi_lsm6ds3.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsl.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -359,6 +360,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { #ifdef USE_ACCGYRO_LSM6DS3 lsm6ds3Detect, #endif +#ifdef USE_ACCGYRO_LSM6DSL + lsm6dslDetect, +#endif #ifdef USE_ACCGYRO_LSM6DSO lsm6dsoDetect, #endif diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 674d8f96d..8d230a02c 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -203,6 +203,7 @@ typedef enum { BMI_160_SPI, BMI_270_SPI, LSM6DS3_SPI, + LSM6DSL_SPI, LSM6DSO_SPI, L3GD20_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c new file mode 100644 index 000000000..609f1751c --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.c @@ -0,0 +1,206 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_LSM6DSL + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsl.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/time.h" + +// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity +#define GYRO_EXTI_DETECT_THRESHOLD 1000 + +#ifdef USE_GYRO_EXTI +// Called in ISR context +// Gyro read has just completed +busStatus_e lsm6dslIntcallback(uint32_t arg) +{ + gyroDev_t *gyro = (gyroDev_t *)arg; + int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); + + if (gyroDmaDuration > gyro->gyroDmaMaxDuration) { + gyro->gyroDmaMaxDuration = gyroDmaDuration; + } + + gyro->dataReady = true; + + return BUS_READY; +} + +void lsm6dslExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does + // not have an associated timer + uint32_t nowCycles = getCycleCounter(); + gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration; + gyro->gyroLastEXTI = nowCycles; + + if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { + spiSequence(&gyro->dev, gyro->segments); + } + + gyro->detectedEXTI++; + +} +#else +void lsm6dslExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} +#endif + +bool lsm6dslAccRead(accDev_t *acc) +{ + switch (acc->gyro->gyroModeSPI) { + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + acc->gyro->dev.txBuf[1] = LSM6DSL_REG_OUTX_L_A | 0x80; + + busSegment_t segments[] = { + {.u.buffers = {NULL, NULL}, 8, true, NULL}, + {.u.link = {NULL, NULL}, 0, true, NULL}, + }; + segments[0].u.buffers.txData = &acc->gyro->dev.txBuf[1]; + segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1]; + + spiSequence(&acc->gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&acc->gyro->dev); + + uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + acc->ADCRaw[X] = accData[1]; + acc->ADCRaw[Y] = accData[2]; + acc->ADCRaw[Z] = accData[3]; + break; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + + // This data was read from the gyro, which is the same SPI device as the acc + uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + acc->ADCRaw[X] = accData[4]; + acc->ADCRaw[Y] = accData[5]; + acc->ADCRaw[Z] = accData[6]; + break; + } + + case GYRO_EXTI_INIT: + default: + break; + } + + return true; +} + +bool lsm6dslGyroRead(gyroDev_t *gyro) +{ + uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + switch (gyro->gyroModeSPI) { + case GYRO_EXTI_INIT: + { + // Initialise the tx buffer to all 0x00 + memset(gyro->dev.txBuf, 0x00, 14); +#ifdef USE_GYRO_EXTI + // Check that minimum number of interrupts have been detected + + // We need some offset from the gyro interrupts to ensure sampling after the interrupt + gyro->gyroDmaMaxDuration = 5; + // Using DMA for gyro access upsets the scheduler on the F4 + if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { + if (spiUseDMA(&gyro->dev)) { + gyro->dev.callbackArg = (uint32_t)gyro; + gyro->dev.txBuf[1] = LSM6DSL_REG_OUTX_L_G | 0x80; + gyro->segments[0].len = 13; + gyro->segments[0].callback = lsm6dslIntcallback; + gyro->segments[0].u.buffers.txData = &gyro->dev.txBuf[1]; + gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1]; + gyro->segments[0].negateCS = true; + gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; + } else { + // Interrupts are present, but no DMA + gyro->gyroModeSPI = GYRO_EXTI_INT; + } + } else +#endif + { + gyro->gyroModeSPI = GYRO_EXTI_NO_INT; + } + break; + } + + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + gyro->dev.txBuf[1] = LSM6DSL_REG_OUTX_L_G | 0x80; + + busSegment_t segments[] = { + {.u.buffers = {NULL, NULL}, 7, true, NULL}, + {.u.link = {NULL, NULL}, 0, true, NULL}, + }; + segments[0].u.buffers.txData = &gyro->dev.txBuf[1]; + segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1]; + + spiSequence(&gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&gyro->dev); + + // Fall through + FALLTHROUGH; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + gyro->gyroADCRaw[X] = gyroData[1]; + gyro->gyroADCRaw[Y] = gyroData[2]; + gyro->gyroADCRaw[Z] = gyroData[3]; + break; + } + + default: + break; + } + + return true; +} +#endif diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.h b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.h new file mode 100644 index 000000000..017746ab5 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl.h @@ -0,0 +1,67 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" +#include "drivers/exti.h" + +// LSM6DSL registers (not the complete list) +typedef enum { + LSM6DSL_REG_DRDY_PULSED_CFG_G = 0x0B,// DataReady configuration register + LSM6DSL_REG_INT1_CTRL = 0x0D, // int pin 1 control + LSM6DSL_REG_INT2_CTRL = 0x0E, // int pin 2 control + LSM6DSL_REG_WHO_AM_I = 0x0F, // chip ID + LSM6DSL_REG_CTRL1_XL = 0x10, // accelerometer control + LSM6DSL_REG_CTRL2_G = 0x11, // gyro control + LSM6DSL_REG_CTRL3_C = 0x12, // control register 3 + LSM6DSL_REG_CTRL4_C = 0x13, // control register 4 + LSM6DSL_REG_CTRL5_C = 0x14, // control register 5MSB + LSM6DSL_REG_CTRL6_C = 0x15, // control register 6 + LSM6DSL_REG_CTRL7_G = 0x16, // control register 7 + LSM6DSL_REG_CTRL8_XL = 0x17, // control register 8 + LSM6DSL_REG_CTRL9_XL = 0x18, // control register 9 + LSM6DSL_REG_CTRL10_C = 0x19, // control register 10 + LSM6DSL_REG_STATUS = 0x1E, // status register + LSM6DSL_REG_OUT_TEMP_L = 0x20, // temperature LSB + LSM6DSL_REG_OUT_TEMP_H = 0x21, // temperature MSB + LSM6DSL_REG_OUTX_L_G = 0x22, // gyro X axis LSB + LSM6DSL_REG_OUTX_H_G = 0x23, // gyro X axis MSB + LSM6DSL_REG_OUTY_L_G = 0x24, // gyro Y axis LSB + LSM6DSL_REG_OUTY_H_G = 0x25, // gyro Y axis MSB + LSM6DSL_REG_OUTZ_L_G = 0x26, // gyro Z axis LSB + LSM6DSL_REG_OUTZ_H_G = 0x27, // gyro Z axis MSB + LSM6DSL_REG_OUTX_L_A = 0x28, // acc X axis LSB + LSM6DSL_REG_OUTX_H_A = 0x29, // acc X axis MSB + LSM6DSL_REG_OUTY_L_A = 0x2A, // acc Y axis LSB + LSM6DSL_REG_OUTY_H_A = 0x2B, // acc Y axis MSB + LSM6DSL_REG_OUTZ_L_A = 0x2C, // acc Z axis LSB + LSM6DSL_REG_OUTZ_H_A = 0x2D, // acc Z axis MSB +} lsm6dslRegister_e; + +// Contained in accgyro_spi_lsm6dsl_init.c which is size-optimized +uint8_t lsm6dslDetect(const extDevice_t *dev); +bool lsm6dslSpiAccDetect(accDev_t *acc); +bool lsm6dslSpiGyroDetect(gyroDev_t *gyro); + +// Contained in accgyro_spi_lsm6dsl.c which is speed-optimized +void lsm6dslExtiHandler(extiCallbackRec_t *cb); +bool lsm6dslAccRead(accDev_t *acc); +bool lsm6dslGyroRead(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dsl_init.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl_init.c new file mode 100644 index 000000000..6f7f5af10 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_lsm6dsl_init.c @@ -0,0 +1,239 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_LSM6DSL + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsl.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "sensors/gyro.h" + +// 10 MHz max SPI frequency +#define LSM6DSL_MAX_SPI_CLK_HZ 10000000 + +#define LSM6DSL_CHIP_ID 0x6A + +// LSM6DSL register configuration values +typedef enum { + LSM6DSL_VAL_DRDY_PULSED_CFG_G_DRDY_PULSED = BIT(7),// (bit 7) enable data ready pulsed mode + LSM6DSL_VAL_INT1_CTRL = 0x02, // enable gyro data ready interrupt pin 1 + LSM6DSL_VAL_INT2_CTRL = 0x00, // disable gyro data ready interrupt pin 2 + LSM6DSL_VAL_CTRL1_XL_ODR833 = 0x07, // accelerometer 833hz output data rate (gyro/8) + LSM6DSL_VAL_CTRL1_XL_ODR1667 = 0x08, // accelerometer 1666hz output data rate (gyro/4) + LSM6DSL_VAL_CTRL1_XL_ODR3332 = 0x09, // accelerometer 3332hz output data rate (gyro/2) + LSM6DSL_VAL_CTRL1_XL_ODR6664 = 0x0A, // accelerometer 6664hz output data rate (gyro/1) + LSM6DSL_VAL_CTRL1_XL_8G = 0x03, // accelerometer 8G scale + LSM6DSL_VAL_CTRL1_XL_16G = 0x01, // accelerometer 16G scale + LSM6DSL_VAL_CTRL1_XL_LPF1 = 0x00, // accelerometer output from LPF1 + LSM6DSL_VAL_CTRL1_XL_LPF2 = 0x01, // accelerometer output from LPF2 + LSM6DSL_VAL_CTRL2_G_ODR6664 = 0x0A, // gyro 6664hz output data rate + LSM6DSL_VAL_CTRL2_G_2000DPS = 0x03, // gyro 2000dps scale + // LSM6DSL_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) + LSM6DSL_VAL_CTRL3_C_H_LACTIVE = 0, // (bit 5) interrupt pins active high + LSM6DSL_VAL_CTRL3_C_PP_OD = 0, // (bit 4) interrupt pins push/pull + LSM6DSL_VAL_CTRL3_C_SIM = 0, // (bit 3) SPI 4-wire interface mode + LSM6DSL_VAL_CTRL3_C_IF_INC = BIT(2), // (bit 2) auto-increment address for burst reads + LSM6DSL_VAL_CTRL4_C_DRDY_MASK = BIT(3), // (bit 3) data ready interrupt mask + LSM6DSL_VAL_CTRL4_C_I2C_DISABLE = BIT(2), // (bit 2) disable I2C interface + LSM6DSL_VAL_CTRL4_C_LPF1_SEL_G = BIT(1), // (bit 1) enable gyro LPF1 + LSM6DSL_VAL_CTRL6_C_XL_HM_MODE = 0, // (bit 4) enable accelerometer high performance mode + LSM6DSL_VAL_CTRL6_C_FTYPE_315HZ = 0x00, // (bits 1:0) gyro LPF1 cutoff 315Hz + LSM6DSL_VAL_CTRL6_C_FTYPE_237HZ = 0x01, // (bits 1:0) gyro LPF1 cutoff 237Hz + LSM6DSL_VAL_CTRL6_C_FTYPE_173HZ = 0x02, // (bits 1:0) gyro LPF1 cutoff 173Hz + LSM6DSL_VAL_CTRL6_C_FTYPE_937HZ = 0x03, // (bits 1:0) gyro LPF1 cutoff 937Hz + LSM6DSL_VAL_CTRL7_G_HP_EN_G = BIT(6), // (bit 6) enable gyro high-pass filter + LSM6DSL_VAL_CTRL7_G_HPM_G_16 = 0x00, // (bits 5:4) gyro HPF cutoff 16mHz + LSM6DSL_VAL_CTRL7_G_HPM_G_65 = 0x01, // (bits 5:4) gyro HPF cutoff 65mHz + LSM6DSL_VAL_CTRL7_G_HPM_G_260 = 0x02, // (bits 5:4) gyro HPF cutoff 260mHz + LSM6DSL_VAL_CTRL7_G_HPM_G_1040 = 0x03, // (bits 5:4) gyro HPF cutoff 1.04Hz +} lsm6dslConfigValues_e; + +// LSM6DSL register configuration bit masks +typedef enum { + LSM6DSL_MASK_DRDY_PULSED_CFG_G = 0x80, // 0b10000000 + LSM6DSL_MASK_CTRL3_C = 0x3C, // 0b00111100 + LSM6DSL_MASK_CTRL3_C_RESET = BIT(0), // 0b00000001 + LSM6DSL_MASK_CTRL4_C = 0x0E, // 0b00001110 + LSM6DSL_MASK_CTRL6_C = 0x13, // 0b00010011 + LSM6DSL_MASK_CTRL7_G = 0x70, // 0b01110000 +} lsm6dslConfigMasks_e; + +uint8_t lsm6dslDetect(const extDevice_t *dev) +{ + uint8_t chipID = 0; + + if (busReadRegisterBuffer(dev, LSM6DSL_REG_WHO_AM_I, &chipID, 1)) { + if (chipID == LSM6DSL_CHIP_ID) { + return LSM6DSL_SPI; + } + } + + return MPU_NONE; +} + +static void lsm6dslWriteRegister(const extDevice_t *dev, lsm6dslRegister_e registerID, uint8_t value, unsigned delayMs) +{ + busWriteRegister(dev, registerID, value); + if (delayMs) { + delay(delayMs); + } +} + +static void lsm6dslWriteRegisterBits(const extDevice_t *dev, lsm6dslRegister_e registerID, lsm6dslConfigMasks_e mask, uint8_t value, unsigned delayMs) +{ + uint8_t newValue; + if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) { + delayMicroseconds(2); + newValue = (newValue & ~mask) | value; + lsm6dslWriteRegister(dev, registerID, newValue, delayMs); + } +} + +static uint8_t getLsmDlpfBandwidth() +{ + switch(gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return LSM6DSL_VAL_CTRL6_C_FTYPE_237HZ; + case GYRO_HARDWARE_LPF_OPTION_1: + return LSM6DSL_VAL_CTRL6_C_FTYPE_315HZ; + case GYRO_HARDWARE_LPF_OPTION_2: + return LSM6DSL_VAL_CTRL6_C_FTYPE_937HZ; + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return LSM6DSL_VAL_CTRL6_C_FTYPE_937HZ; + } + return 0; +} + +static void lsm6dslConfig(gyroDev_t *gyro) +{ + extDevice_t *dev = &gyro->dev; + + // Reset the device (wait 100ms before continuing config) + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_CTRL3_C, LSM6DSL_MASK_CTRL3_C_RESET, BIT(0), 100); + + // Configure data ready pulsed mode + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_DRDY_PULSED_CFG_G, LSM6DSL_MASK_DRDY_PULSED_CFG_G, LSM6DSL_VAL_DRDY_PULSED_CFG_G_DRDY_PULSED, 0); + + // Configure interrupt pin 1 for gyro data ready only + lsm6dslWriteRegister(dev, LSM6DSL_REG_INT1_CTRL, LSM6DSL_VAL_INT1_CTRL, 1); + + // Disable interrupt pin 2 + lsm6dslWriteRegister(dev, LSM6DSL_REG_INT2_CTRL, LSM6DSL_VAL_INT2_CTRL, 1); + + // Configure the accelerometer + // 833hz ODR, 16G scale, use LPF2 output (default with ODR/4 cutoff) + lsm6dslWriteRegister(dev, LSM6DSL_REG_CTRL1_XL, (LSM6DSL_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSL_VAL_CTRL1_XL_16G << 2) | (LSM6DSL_VAL_CTRL1_XL_LPF2 << 1), 1); + + // Configure the gyro + // 6664hz ODR, 2000dps scale + lsm6dslWriteRegister(dev, LSM6DSL_REG_CTRL2_G, (LSM6DSL_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSL_VAL_CTRL2_G_2000DPS << 2), 1); + + // Configure control register 3 + // latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_CTRL3_C, LSM6DSL_MASK_CTRL3_C, (LSM6DSL_VAL_CTRL3_C_H_LACTIVE | LSM6DSL_VAL_CTRL3_C_PP_OD | LSM6DSL_VAL_CTRL3_C_SIM | LSM6DSL_VAL_CTRL3_C_IF_INC), 1); + + // Configure control register 4 + // enable accelerometer high performane mode; enable gyro LPF1 + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_CTRL4_C, LSM6DSL_MASK_CTRL4_C, (LSM6DSL_VAL_CTRL4_C_DRDY_MASK | LSM6DSL_VAL_CTRL4_C_I2C_DISABLE | LSM6DSL_VAL_CTRL4_C_LPF1_SEL_G), 1); + + // Configure control register 6 + // disable I2C interface; set gyro LPF1 cutoff according to gyro_hardware_lpf setting + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_CTRL6_C, LSM6DSL_MASK_CTRL6_C, (LSM6DSL_VAL_CTRL6_C_XL_HM_MODE | getLsmDlpfBandwidth()), 1); + + // Configure control register 7 + lsm6dslWriteRegisterBits(dev, LSM6DSL_REG_CTRL7_G, LSM6DSL_MASK_CTRL7_G, (LSM6DSL_VAL_CTRL7_G_HP_EN_G | LSM6DSL_VAL_CTRL7_G_HPM_G_16), 1); + +} + +#ifdef USE_GYRO_EXTI +static void lsm6dslIntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + EXTIHandlerInit(&gyro->exti, lsm6dslExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); + EXTIEnable(mpuIntIO); +} +#endif + +static void lsm6dslSpiGyroInit(gyroDev_t *gyro) +{ + extDevice_t *dev = &gyro->dev; + + lsm6dslConfig(gyro); + +#ifdef USE_GYRO_EXTI + lsm6dslIntExtiInit(gyro); +#endif + + spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSL_MAX_SPI_CLK_HZ)); +} + +static void lsm6dslSpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool lsm6dslSpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != LSM6DSL_SPI) { + return false; + } + + acc->initFn = lsm6dslSpiAccInit; + acc->readFn = lsm6dslAccRead; + + return true; +} + +bool lsm6dslSpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != LSM6DSL_SPI) { + return false; + } + + gyro->initFn = lsm6dslSpiGyroInit; + gyro->readFn = lsm6dslGyroRead; + gyro->scale = GYRO_SCALE_2000DPS; + + return true; +} +#endif // USE_ACCGYRO_LSM6DSL diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index 60d268f0a..1b0b39358 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -82,10 +82,15 @@ uint16_t gyroSetSampleRate(gyroDev_t *gyro) gyroSampleRateHz = 1667; accSampleRateHz = 833; break; + case LSM6DSL_SPI: + gyro->gyroRateKHz = GYRO_RATE_6664_Hz; + gyroSampleRateHz = 6664; + accSampleRateHz = 833; + break; #ifdef USE_ACCGYRO_LSM6DSO case LSM6DSO_SPI: gyro->gyroRateKHz = GYRO_RATE_6664_Hz; - gyroSampleRateHz = 1666; // Yes, this is correct per the datasheet. Will effectively round to 150us and 6.67KHz. + gyroSampleRateHz = 6664; // Yes, this is correct per the datasheet. Will effectively round to 150us and 6.67KHz. accSampleRateHz = 833; break; #endif diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index f3da696e1..498564d42 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -47,6 +47,7 @@ typedef enum { ACC_BMI160, ACC_BMI270, ACC_LSM6DS3, + ACC_LSM6DSL, ACC_LSM6DSO, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index d257f5617..2e16a28fd 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -48,6 +48,7 @@ #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" #include "drivers/accgyro/accgyro_spi_lsm6ds3.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsl.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -316,6 +317,14 @@ retry: FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_LSM6DSL + case ACC_LSM6DSL: + if (lsm6dslSpiAccDetect(dev)) { + accHardware = ACC_LSM6DSL; + break; + } + FALLTHROUGH; +#endif #ifdef USE_ACCGYRO_LSM6DSO case ACC_LSM6DSO: diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 89ba7d9cc..4ea4a537e 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -47,6 +47,7 @@ #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" #include "drivers/accgyro/accgyro_spi_lsm6ds3.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsl.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -315,6 +316,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) case GYRO_MPU6500: case GYRO_MPU9250: case GYRO_LSM6DS3: + case GYRO_LSM6DSL: case GYRO_LSM6DSO: gyroSensor->gyroDev.gyroHasOverflowProtection = true; break; @@ -493,6 +495,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_LSM6DSL + case GYRO_LSM6DSL: + if (lsm6dslSpiGyroDetect(dev)) { + gyroHardware = GYRO_LSM6DSL; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_ACCGYRO_LSM6DSO case GYRO_LSM6DSO: if (lsm6dsoSpiGyroDetect(dev)) { @@ -528,7 +539,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \ - || defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSL) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config); @@ -552,8 +563,8 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t static void gyroPreInitSensor(const gyroDeviceConfig_t *config) { #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSO) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) \ + || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSL) ||defined(USE_ACCGRYO_LSM6DSO) mpuPreInit(config); #else UNUSED(config); diff --git a/src/main/target/AT32F437DEV/target.h b/src/main/target/AT32F437DEV/target.h index dfa0d23e1..c69f4a313 100644 --- a/src/main/target/AT32F437DEV/target.h +++ b/src/main/target/AT32F437DEV/target.h @@ -110,6 +110,7 @@ #define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_ICM42688P #define USE_ACCGYRO_LSM6DS3 +#define USE_ACCGYRO_LSM6DSL #define USE_ACCGYRO_LSM6DSO #define USE_ACC diff --git a/src/main/target/AT32F437DEV/target.mk b/src/main/target/AT32F437DEV/target.mk index e7e3f2aaf..164d123b9 100644 --- a/src/main/target/AT32F437DEV/target.mk +++ b/src/main/target/AT32F437DEV/target.mk @@ -13,6 +13,8 @@ TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c\ drivers/accgyro/accgyro_spi_lsm6ds3_init.c \ drivers/accgyro/accgyro_spi_lsm6ds3.c \ + drivers/accgyro/accgyro_spi_lsm6dsl_init.c \ + drivers/accgyro/accgyro_spi_lsm6dsl.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ drivers/max7456.c \ diff --git a/src/main/target/EMSRPROTO2/target.h b/src/main/target/EMSRPROTO2/target.h index cd3132813..1138e4513 100644 --- a/src/main/target/EMSRPROTO2/target.h +++ b/src/main/target/EMSRPROTO2/target.h @@ -103,6 +103,7 @@ #define USE_GYRO #define USE_GYRO_SPI_ICM42688P #define USE_ACCGYRO_BMI270 +#define USE_ACCGYRO_LSM6DSL #define USE_ACCGYRO_LSM6DSO diff --git a/src/main/target/EMSRPROTO2/target.mk b/src/main/target/EMSRPROTO2/target.mk index e50564bdf..ffece14c1 100644 --- a/src/main/target/EMSRPROTO2/target.mk +++ b/src/main/target/EMSRPROTO2/target.mk @@ -10,6 +10,8 @@ TARGET_SRC = \ drivers/compass/compass_qmc5883l.c\ $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \ drivers/accgyro/accgyro_spi_bmi270.c\ + drivers/accgyro/accgyro_spi_lsm6dsl_init.c \ + drivers/accgyro/accgyro_spi_lsm6dsl.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ drivers/max7456.c \ diff --git a/src/main/target/NEUTRONRCF435AIO/target.h b/src/main/target/NEUTRONRCF435AIO/target.h index d503cf7ab..7ebc90d61 100644 --- a/src/main/target/NEUTRONRCF435AIO/target.h +++ b/src/main/target/NEUTRONRCF435AIO/target.h @@ -93,6 +93,7 @@ #define USE_ACC_SPI_MPU6500 //debug only #define USE_GYRO_SPI_ICM42688P #define USE_ACCGYRO_BMI270 +#define USE_ACCGYRO_LSM6DSL #define USE_ACCGYRO_LSM6DSO #define USE_ACC diff --git a/src/main/target/NEUTRONRCF435AIO/target.mk b/src/main/target/NEUTRONRCF435AIO/target.mk index d668a254b..2dc835b39 100644 --- a/src/main/target/NEUTRONRCF435AIO/target.mk +++ b/src/main/target/NEUTRONRCF435AIO/target.mk @@ -15,6 +15,8 @@ TARGET_SRC = \ drivers/compass/compass_qmc5883l.c\ $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \ drivers/accgyro/accgyro_spi_bmi270.c\ + drivers/accgyro/accgyro_spi_lsm6dsl_init.c \ + drivers/accgyro/accgyro_spi_lsm6dsl.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ drivers/max7456.c \