diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c index d0c4e3490aa..b4f573a06c0 100644 --- a/src/main/drivers/accgyro/accgyro_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -103,7 +103,13 @@ #define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04 // User Bank 2 #define ICM426XX_RA_ACCEL_CONFIG_STATIC4 0x05 // User Bank 2 -static bool is42688P = false; +typedef enum { + ICM42605_VARIANT_42605 = 0, + ICM42605_VARIANT_42686P, + ICM42605_VARIANT_42688P, +} icm42605Variant_e; + +static icm42605Variant_e icm42605DetectedVariant = ICM42605_VARIANT_42605; typedef struct aafConfig_s { uint16_t freq; @@ -145,7 +151,7 @@ static aafConfig_t aafLUT42605[] = { // see table in section 5.3 https://invens { 0, 0, 0, 0 } }; -static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf); +static const aafConfig_t *getGyroAafConfig(icm42605Variant_e variant, const uint16_t desiredLpf); static void setUserBank(const busDevice_t *dev, const uint8_t user_bank) { @@ -214,11 +220,16 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro) busWrite(dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN); delay(15); + uint8_t fsSel = 0; + if (icm42605DetectedVariant == ICM42605_VARIANT_42686P) { + fsSel = 1; // ICM-42686-P normal range: 2000 deg/s, 16G. + } + /* ODR and dynamic range */ - busWrite(dev, ICM42605_RA_GYRO_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 2000 deg/s */ + busWrite(dev, ICM42605_RA_GYRO_CONFIG0, fsSel << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 2000 deg/s */ delay(15); - busWrite(dev, ICM42605_RA_ACCEL_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 16 G deg/s */ + busWrite(dev, ICM42605_RA_ACCEL_CONFIG0, fsSel << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 16 G deg/s */ delay(15); /* LPF bandwidth */ @@ -228,14 +239,14 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro) if (gyro->lpf != GYRO_LPF_NONE) { // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER") - const aafConfig_t *aafConfig = getGyroAafConfig(is42688P, gyro->lpf); + const aafConfig_t *aafConfig = getGyroAafConfig(icm42605DetectedVariant, gyro->lpf); setUserBank(dev, ICM426XX_BANK_SELECT1); busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig->delt); busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig->deltSqr & 0xFF); busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig->deltSqr >> 8) | (aafConfig->bitshift << 4)); - aafConfig = getGyroAafConfig(is42688P, 256); // This was hard coded on BF + aafConfig = getGyroAafConfig(icm42605DetectedVariant, 256); // This was hard coded on BF setUserBank(dev, ICM426XX_BANK_SELECT2); busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig->delt << 1); busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig->deltSqr & 0xFF); @@ -289,12 +300,15 @@ static bool icm42605DeviceDetect(busDevice_t * dev) busRead(dev, MPU_RA_WHO_AM_I, &tmp); switch (tmp) { - /* ICM42605 and ICM42688P share the register structure*/ + /* ICM42605, ICM42686P and ICM42688P share the register structure */ case ICM42605_WHO_AM_I_CONST: - is42688P = false; + icm42605DetectedVariant = ICM42605_VARIANT_42605; + return true; + case ICM42686P_WHO_AM_I_CONST: + icm42605DetectedVariant = ICM42605_VARIANT_42686P; return true; case ICM42688P_WHO_AM_I_CONST: - is42688P = true; + icm42605DetectedVariant = ICM42605_VARIANT_42688P; return true; default: @@ -385,11 +399,24 @@ static uint16_t getAafFreq(const uint8_t gyroLpf) } } -static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf) +static const char *icm42605VariantName(icm42605Variant_e variant) +{ + switch (variant) { + case ICM42605_VARIANT_42686P: + return "42686P"; + case ICM42605_VARIANT_42688P: + return "42688P"; + case ICM42605_VARIANT_42605: + default: + return "42605"; + } +} + +static const aafConfig_t *getGyroAafConfig(icm42605Variant_e variant, const uint16_t desiredLpf) { uint16_t desiredFreq = getAafFreq(desiredLpf); const aafConfig_t *aafConfigs = NULL; - if (is42688) { + if (variant == ICM42605_VARIANT_42686P || variant == ICM42605_VARIANT_42688P) { aafConfigs = aafLUT42688; } else { aafConfigs = aafLUT42605; @@ -406,8 +433,8 @@ static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredL } } - LOG_VERBOSE(GYRO, "ICM426%s AAF CONFIG { %d, %d } -> { %d }; delt: %d deltSqr: %d, shift: %d", - (is42688P ? "88" : "05"), + LOG_VERBOSE(GYRO, "ICM%s AAF CONFIG { %d, %d } -> { %d }; delt: %d deltSqr: %d, shift: %d", + icm42605VariantName(variant), desiredLpf, desiredFreq, candidate->freq, candidate->delt, candidate->deltSqr, candidate->bitshift); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 0f59d10d3e8..4adf54e6f38 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -29,6 +29,7 @@ #define ICM20608G_WHO_AM_I_CONST (0xAF) #define ICM20689_WHO_AM_I_CONST (0x98) #define ICM42605_WHO_AM_I_CONST (0x42) +#define ICM42686P_WHO_AM_I_CONST (0x44) #define ICM42688P_WHO_AM_I_CONST (0x47) #define ICM45686_WHO_AM_I_CONST (0xE9)