diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 0e9aa15a97..4da8b8dcee 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -64,19 +64,6 @@ #define ICM426XX_RA_ACCEL_CONFIG_STATIC2 0x03 #define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04 #define ICM426XX_RA_ACCEL_CONFIG_STATIC4 0x05 -// --- Settings for AAF (section 5.3) ----------------------- -#define ICM426XX_AAF_258HZ_DELT 6 -#define ICM426XX_AAF_258HZ_DELTSQR 36 -#define ICM426XX_AAF_258HZ_BITSHIFT 10 -#define ICM426XX_AAF_536HZ_DELT 12 -#define ICM426XX_AAF_536HZ_DELTSQR 144 -#define ICM426XX_AAF_536HZ_BITSHIFT 8 -#define ICM426XX_AAF_997HZ_DELT 21 -#define ICM426XX_AAF_997HZ_DELTSQR 440 -#define ICM426XX_AAF_997HZ_BITSHIFT 6 -#define ICM426XX_AAF_1962HZ_DELT 37 -#define ICM426XX_AAF_1962HZ_DELTSQR 1376 -#define ICM426XX_AAF_1962HZ_BITSHIFT 4 // --- Register & setting for gyro and acc UI Filter -------- #define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52 #define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4) @@ -113,6 +100,44 @@ #define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3) #define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3) +typedef enum { + ODR_CONFIG_8K = 0, + ODR_CONFIG_4K, + ODR_CONFIG_2K, + ODR_CONFIG_1K, + ODR_CONFIG_COUNT +} odrConfig_e; + +typedef enum { + AAF_CONFIG_258HZ = 0, + AAF_CONFIG_536HZ, + AAF_CONFIG_997HZ, + AAF_CONFIG_1962HZ, + AAF_CONFIG_COUNT +} aafConfig_e; + +typedef struct aafConfig_s { + uint8_t delt; + uint16_t deltSqr; + uint8_t bitshift; +} aafConfig_t; + +// Possible output data rates (ODRs) +static uint8_t odrLUT[ODR_CONFIG_COUNT] = { // see GYRO_ODR in section 5.6 + [ODR_CONFIG_8K] = 3, + [ODR_CONFIG_4K] = 4, + [ODR_CONFIG_2K] = 5, + [ODR_CONFIG_1K] = 6, +}; + +// Possible gyro Anti-Alias Filter (AAF) cutoffs +static aafConfig_t aafLUT[AAF_CONFIG_COUNT] = { // see table in section 5.3 + [AAF_CONFIG_258HZ] = { 6, 36, 10 }, + [AAF_CONFIG_536HZ] = { 12, 144, 8 }, + [AAF_CONFIG_997HZ] = { 21, 440, 6 }, + [AAF_CONFIG_1962HZ] = { 37, 1376, 4 }, +}; + uint8_t icm426xxSpiDetect(const extDevice_t *dev) { spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00); @@ -166,17 +191,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc) return true; } -typedef struct odrConfig_s { - uint8_t odr; // ODR: Output data rate (gyro sample rate in kHz) - uint8_t odr_idx; // See GYRO_ODR in datasheet -} odrConfig_t; - -static odrConfig_t odrConfigLUT[] = { - { 8, 3 }, - { 4, 4 }, - { 2, 5 }, - { 1, 6 }, -}; +static aafConfig_t getGyroAafConfig(void); void icm426xxGyroInit(gyroDev_t *gyro) { @@ -191,70 +206,35 @@ void icm426xxGyroInit(gyroDev_t *gyro) spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN); delay(15); - uint8_t odrIdx = 0; - bool supportedODRFound = false; - - if (gyro->gyroRateKHz) { - const uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c - const uint8_t desiredODR = 8 / gyroSyncDenominator; - for (unsigned i = 0; i < ARRAYLEN(odrConfigLUT); i++) { - if (odrConfigLUT[i].odr == desiredODR) { - odrIdx = odrConfigLUT[i].odr_idx; - supportedODRFound = true; - break; - } - } - } - - if (!supportedODRFound) { - odrIdx = 6; + // Get desired output data rate + uint8_t odrConfig; + const uint8_t decim = LOG2_8BIT(gyro->mpuDividerDrops + 1); + if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) { + odrConfig = odrLUT[decim]; + } else { + odrConfig = odrLUT[ODR_CONFIG_1K]; gyro->gyroRateKHz = GYRO_RATE_1_kHz; } STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value"); - spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrIdx & 0x0F)); + spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F)); delay(15); STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value"); - spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrIdx & 0x0F)); + spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F)); delay(15); - // Get desired settings for the gyro Anti-Alias Filter - uint8_t aafDelt = 0; - uint16_t aafDeltSqr = 0; - uint8_t aafBitshift = 0; - switch (gyroConfig()->gyro_hardware_lpf) { - case GYRO_HARDWARE_LPF_NORMAL: // 258 Hz - aafDelt = ICM426XX_AAF_258HZ_DELT; - aafDeltSqr = ICM426XX_AAF_258HZ_DELTSQR; - aafBitshift = ICM426XX_AAF_258HZ_BITSHIFT; - break; - case GYRO_HARDWARE_LPF_OPTION_1: // 536 Hz - aafDelt = ICM426XX_AAF_536HZ_DELT; - aafDeltSqr = ICM426XX_AAF_536HZ_DELTSQR; - aafBitshift = ICM426XX_AAF_536HZ_BITSHIFT; - break; - case GYRO_HARDWARE_LPF_OPTION_2: // 997 Hz - aafDelt = ICM426XX_AAF_997HZ_DELT; - aafDeltSqr = ICM426XX_AAF_997HZ_DELTSQR; - aafBitshift = ICM426XX_AAF_997HZ_BITSHIFT; - break; - case GYRO_HARDWARE_LPF_EXPERIMENTAL: // 1962 Hz - aafDelt = ICM426XX_AAF_1962HZ_DELT; - aafDeltSqr = ICM426XX_AAF_1962HZ_DELTSQR; - aafBitshift = ICM426XX_AAF_1962HZ_BITSHIFT; - break; - } - // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER") - spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafDelt); - spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafDeltSqr & 0xFF); - spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafDeltSqr >> 8) | (aafBitshift << 4)); + aafConfig_t aafConfig = getGyroAafConfig(); + spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt); + spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig.deltSqr & 0xFF); + spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4)); // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c) - spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, ICM426XX_AAF_258HZ_DELT << 1); - spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, ICM426XX_AAF_258HZ_DELTSQR & 0xFF); - spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (ICM426XX_AAF_258HZ_DELTSQR >> 8) | (ICM426XX_AAF_258HZ_BITSHIFT << 4)); + aafConfig = aafLUT[AAF_CONFIG_258HZ]; + spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1); + spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF); + spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4)); // Configure gyro and acc UI Filters spiWriteReg(dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY); @@ -290,4 +270,23 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro) return true; } + +static aafConfig_t getGyroAafConfig(void) +{ + switch (gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return aafLUT[AAF_CONFIG_258HZ]; + case GYRO_HARDWARE_LPF_OPTION_1: + return aafLUT[AAF_CONFIG_536HZ]; + case GYRO_HARDWARE_LPF_OPTION_2: + return aafLUT[AAF_CONFIG_997HZ]; +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return aafLUT[AAF_CONFIG_1962HZ]; #endif + default: + return aafLUT[AAF_CONFIG_258HZ]; + } +} + +#endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P