From a9f9477d3ef9503d406d04ebf6028d0fec4f260f Mon Sep 17 00:00:00 2001 From: chrisrosser Date: Sat, 12 Feb 2022 21:40:19 +0000 Subject: [PATCH] update to BMI gyros to use OSR4 mode to reduce gyro lowpass filter cutoffs --- src/main/cli/settings.c | 5 +++- src/main/drivers/accgyro/accgyro.h | 5 +++- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 23 ++++++++++++++++--- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 23 +++++++++++++++++-- src/main/sensors/gyro.h | 1 - 5 files changed, 49 insertions(+), 8 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 84759d5f4..881a3f65d 100644 --- a/src/main/cli/settings.c +++ b/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 diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 6c77ee011..29ba307ac 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/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 { diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 34c91fdf6..ded9a219a 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/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, 0x20 | BMI160_ODR_3200_Hz); + spiWriteReg(dev, BMI160_REG_GYR_CONF, getBmiOsrMode() | BMI160_ODR_3200_Hz); delay(1); spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index b3bb27f37..4484fbdef 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/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); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b3577ba16..5ff3eb888 100644 --- a/src/main/sensors/gyro.h +++ b/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;