diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 12fbfe7cfe..f7c115511e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -157,9 +157,9 @@ enum gyro_fsr_e { }; enum fchoice_b { - FCB_DISABLED = 0, - FCB_8800_32, - FCB_3600_32 + FCB_DISABLED = 0x00, + FCB_8800_32 = 0x01, + FCB_3600_32 = 0x02 }; enum clock_sel_e { diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index a76c0a0e27..8df14fa1b1 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -126,7 +126,16 @@ void icm20689GyroInit(gyroDev_t *gyro) // delay(100); spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); + uint8_t raGyroConfigData = gyro->gyroRateKHz = INV_FSR_2000DPS << 3; + if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) { + // use otherwise redundant LPF value to configure FCHOICE_B + // see REGISTER 27 – GYROSCOPE CONFIGURATION in datasheet + if (gyro->lpf==GYRO_LPF_NONE) { + raGyroConfigData |= FCB_8800_32; + } else { + raGyroConfigData |= FCB_3600_32; + } + } spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);