update to BMI gyros to use OSR4 mode to reduce gyro lowpass filter cutoffs

src/main/cli/settings.c

@@ -277,8 +277,10 @@ static const char * const lookupTableRxSpi[] = {
 
 static const char * const lookupTableGyroHardwareLpf[] = {
     "NORMAL",
+    "OPTION_1",
+    "OPTION_2",
 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
-    "EXPERIMENTAL"
+    "EXPERIMENTAL",
 #endif
 };
 
@@ -658,6 +660,7 @@ const lookupTableEntry_t lookupTables[] = {
 const clivalue_t valueTable[] = {
 // PG_GYRO_CONFIG
     { PARAM_NAME_GYRO_HARDWARE_LPF, VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_hardware_lpf) },
+    
 #if defined(USE_GYRO_SPI_ICM20649)
     { "gyro_high_range",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
 #endif

src/main/drivers/accgyro/accgyro.h

@@ -64,9 +64,12 @@ typedef enum {
 
 typedef enum {
     GYRO_HARDWARE_LPF_NORMAL,
+    GYRO_HARDWARE_LPF_OPTION_1,
+    GYRO_HARDWARE_LPF_OPTION_2,
 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
-    GYRO_HARDWARE_LPF_EXPERIMENTAL
+    GYRO_HARDWARE_LPF_EXPERIMENTAL,
 #endif
+    GYRO_HARDWARE_LPF_COUNT
 } gyroHardwareLpf_e;
 
 typedef enum {

src/main/drivers/accgyro/accgyro_spi_bmi160.c

@@ -46,6 +46,8 @@
 #include "drivers/sensor.h"
 #include "drivers/time.h"
 
+#include "sensors/gyro.h"
+
 #include "accgyro.h"
 #include "accgyro_spi_bmi160.h"
 
@@ -83,6 +85,9 @@
 #define BMI160_REG_STATUS_NVM_RDY 0x10
 #define BMI160_REG_STATUS_FOC_RDY 0x08
 #define BMI160_REG_CONF_NVM_PROG_EN 0x02
+#define BMI160_VAL_GYRO_CONF_BWP_OSR4 0x00
+#define BMI160_VAL_GYRO_CONF_BWP_OSR2 0x10
+#define BMI160_VAL_GYRO_CONF_BWP_NORM 0x20
 
 ///* Global Variables */
 static volatile bool BMI160InitDone = false;
@@ -113,7 +118,6 @@ uint8_t bmi160Detect(const extDevice_t *dev)
     return BMI_160_SPI;
 }
 
-
 /**
  * @brief Initialize the BMI160 6-axis sensor.
  * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
@@ -139,6 +143,20 @@ static void BMI160_Init(const extDevice_t *dev)
     BMI160InitDone = true;
 }
 
+static uint8_t getBmiOsrMode()
+{
+    switch(gyroConfig()->gyro_hardware_lpf) {
+        case GYRO_HARDWARE_LPF_NORMAL:
+            return BMI160_VAL_GYRO_CONF_BWP_OSR4;
+        case GYRO_HARDWARE_LPF_OPTION_1:
+            return BMI160_VAL_GYRO_CONF_BWP_OSR2;
+        case GYRO_HARDWARE_LPF_OPTION_2:
+            return BMI160_VAL_GYRO_CONF_BWP_NORM;
+        case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+            return BMI160_VAL_GYRO_CONF_BWP_NORM;
+    }
+    return 0;
+}
 
 /**
  * @brief Configure the sensor
@@ -164,8 +182,7 @@ static int32_t BMI160_Config(const extDevice_t *dev)
     spiWriteReg(dev, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
     delay(1);
 
-    // Set gyr_bwp = 0b010 so only the first filter stage is used
+    spiWriteReg(dev, BMI160_REG_GYR_CONF, getBmiOsrMode() | BMI160_ODR_3200_Hz);
-    spiWriteReg(dev, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz);
     delay(1);
 
     spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);

src/main/drivers/accgyro/accgyro_spi_bmi270.c

@@ -38,6 +38,8 @@
 #include "drivers/system.h"
 #include "drivers/time.h"
 
+#include "sensors/gyro.h"
+
 // 10 MHz max SPI frequency
 #define BMI270_MAX_SPI_CLK_HZ 10000000
 
@@ -108,7 +110,9 @@ typedef enum {
     BMI270_VAL_ACC_RANGE_8G = 0x02,          // set acc to 8G full scale
     BMI270_VAL_ACC_RANGE_16G = 0x03,         // set acc to 16G full scale
     BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D,     // set gyro sample rate to 3200hz
-    BMI270_VAL_GYRO_CONF_BWP = 0x02,         // set gyro filter in normal mode
+    BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00,    // set gyro filter in OSR4 mode
+    BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01,    // set gyro filter in OSR2 mode
+    BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02,    // set gyro filter in normal mode
     BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01,  // set gyro in high performance noise mode
     BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode
 
@@ -184,6 +188,21 @@ static void bmi270UploadConfig(const extDevice_t *dev)
     bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1);
 }
 
+static uint8_t getBmiOsrMode()
+{
+    switch(gyroConfig()->gyro_hardware_lpf) {
+        case GYRO_HARDWARE_LPF_NORMAL:
+            return BMI270_VAL_GYRO_CONF_BWP_OSR4;
+        case GYRO_HARDWARE_LPF_OPTION_1:
+            return BMI270_VAL_GYRO_CONF_BWP_OSR2;
+        case GYRO_HARDWARE_LPF_OPTION_2:
+            return BMI270_VAL_GYRO_CONF_BWP_NORM;
+        case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+            return BMI270_VAL_GYRO_CONF_BWP_NORM;
+    }
+    return 0;
+}
+
 static void bmi270Config(gyroDev_t *gyro)
 {
     extDevice_t *dev = &gyro->dev;
@@ -221,7 +240,7 @@ static void bmi270Config(gyroDev_t *gyro)
     bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1);
 
     // Configure the gyro
-    bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1);
+    bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1);
 
     // Configure the gyro full-range scale
     bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1);

src/main/sensors/gyro.h

@@ -165,7 +165,6 @@ enum {
 typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
     uint8_t gyro_hardware_lpf;                // gyro DLPF setting
-
     uint8_t gyro_high_fsr;
     uint8_t gyro_to_use;