Add hardware LPT options for lsm6dsv16x gyro

betaflight · Dec 19, 2023 · b3309e8 · b3309e8
1 parent 58933f9
commit b3309e8
Showing 1 changed file with 25 additions and 2 deletions.
diff --git a/src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c b/src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c
@@ -28,6 +28,8 @@
 
 #include "accgyro_spi_lsm6dsv16x.h"
 
+#include "sensors/gyro.h"
+
 /* See datasheet
  *
  *     https://www.st.com/content/ccc/resource/technical/document/datasheet/group3/47/03/b2/44/47/32/4b/76/DM00741844/files/DM00741844.pdf/jcr:content/translations/en.DM00741844.pdf
@@ -943,6 +945,7 @@ bool lsm6dsv16xSpiAccDetect(accDev_t *acc)
 void lsm6dsv16xGyroInit(gyroDev_t *gyro)
 {
     const extDevice_t *dev = &gyro->dev;
+    uint8_t gyro_lpf;
 
     spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSV16X_MAX_SPI_CLK_HZ));
 
@@ -964,9 +967,30 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
                                     LSM6DSV_CTRL2_ODR_G_MASK,
                                     LSM6DSV_CTRL2_ODR_G_SHIFT));
 
+    switch (gyroConfig()->gyro_hardware_lpf) {
+        default:
+        case GYRO_HARDWARE_LPF_NORMAL:
+            gyro_lpf = LSM6DSV_CTRL6_FS_G_BW_407HZ;
+            break;
+
+        case GYRO_HARDWARE_LPF_OPTION_1:
+            gyro_lpf = LSM6DSV_CTRL6_FS_G_BW_281HZ;
+            break;
+
+        case GYRO_HARDWARE_LPF_OPTION_2:
+            gyro_lpf = LSM6DSV_CTRL6_FS_G_BW_213HZ;
+            break;
+
+#ifdef USE_GYRO_DLPF_EXPERIMENTAL
+        case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+            gyro_lpf = LSM6DSV_CTRL6_FS_G_BW_156HZ;
+            break;
+#endif
+        }
+
     // Enable 2000 deg/s sensitivity
     spiWriteReg(dev, LSM6DSV_CTRL6,
-                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL6_FS_G_BW_407HZ,
+                LSM6DSV_ENCODE_BITS(gyro_lpf,
                                     LSM6DSV_CTRL6_LPF1_G_BW_MASK,
                                     LSM6DSV_CTRL6_LPF1_G_BW_SHIFT) |
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL6_FS_G_2000DPS,
@@ -977,7 +1001,6 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
     spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC);
 
     // Generate pulse on interrupt line, not requiring a read to clear
-    // TODO this pulse lasts 66us followed by a low of 66us, so we get 132us cycle time, not 125us
     spiWriteReg(dev, LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED);
 
     // From section 4.1, Mechanical characteristics, of the datasheet, G_So is 70mdps/LSB for FS = ±2000 dps.