Merge pull request #19 from flightng/add-support-for-LSM6DSL-IMU

Add support for LSM6DSL IMU & restore  LSM6DSO gyroSampleRateHz = 1666

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
 
 

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)

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;

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

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;

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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);

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#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

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

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;

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:

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_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_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSO)
+ || 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);

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

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 \

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
 
 

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 \

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

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 \