Skip to content

IMU: Add ICM42686P IMU support #11639

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
bkleiner wants to merge 1 commit into
base: release/9.1
Choose a base branch
from
+41 −13
Draft

IMU: Add ICM42686P IMU support #11639

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 40 additions & 13 deletions src/main/drivers/accgyro/accgyro_icm42605.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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 */
Expand All @@ -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);
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down
1 change: 1 addition & 0 deletions src/main/drivers/accgyro/accgyro_mpu.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
Loading