Skip to content
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

Jump to bottom

Add separate AAF values for ICM-42605 #12616

Merged
merged 1 commit into from Apr 3, 2023
Merged

Add separate AAF values for ICM-42605 #12616

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.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
60 changes: 43 additions & 17 deletions src/main/drivers/accgyro/accgyro_spi_icm426xx.c
View file
Open in desktop
Expand Up @@ -138,14 +138,23 @@ static uint8_t odrLUT[ODR_CONFIG_COUNT] = { // see GYRO_ODR in section 5.6
[ODR_CONFIG_1K] = 6,
};

// Possible gyro Anti-Alias Filter (AAF) cutoffs
static aafConfig_t aafLUT[AAF_CONFIG_COUNT] = { // see table in section 5.3
// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
static aafConfig_t aafLUT42688[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 },
};

// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
// actual cutoff differs slightly from those of the 42688P
static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = { // see table in section 5.3
[AAF_CONFIG_258HZ] = { 21, 440, 6 }, // actually 249 Hz
[AAF_CONFIG_536HZ] = { 39, 1536, 4 }, // actually 524 Hz
[AAF_CONFIG_997HZ] = { 63, 3968, 3 }, // actually 995 Hz
[AAF_CONFIG_1962HZ] = { 63, 3968, 3 }, // 995 Hz is the max cutoff on the 42605
};

uint8_t icm426xxSpiDetect(const extDevice_t *dev)
{
spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
Expand Down Expand Up @@ -199,7 +208,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
return true;
}

static aafConfig_t getGyroAafConfig(void);
static aafConfig_t getGyroAafConfig(const mpuSensor_e, const aafConfig_e);

static void turnGyroAccOff(const extDevice_t *dev)
{
Expand Down Expand Up @@ -234,14 +243,15 @@ void icm426xxGyroInit(gyroDev_t *gyro)
turnGyroAccOff(dev);

// Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
aafConfig_t aafConfig = getGyroAafConfig();
const mpuSensor_e gyroModel = gyro->mpuDetectionResult.sensor;
aafConfig_t aafConfig = getGyroAafConfig(gyroModel, gyroConfig()->gyro_hardware_lpf);
setUserBank(dev, ICM426XX_BANK_SELECT1);
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)
aafConfig = aafLUT[AAF_CONFIG_258HZ];
aafConfig = getGyroAafConfig(gyroModel, AAF_CONFIG_258HZ);
setUserBank(dev, ICM426XX_BANK_SELECT2);
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF);
Expand Down Expand Up @@ -305,21 +315,37 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
return true;
}

static aafConfig_t getGyroAafConfig(void)
static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig_e config)
{
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];
switch (gyroModel){
case ICM_42605_SPI:
switch (config) {
case GYRO_HARDWARE_LPF_NORMAL:
return aafLUT42605[AAF_CONFIG_258HZ];
case GYRO_HARDWARE_LPF_OPTION_1:
return aafLUT42605[AAF_CONFIG_536HZ];
case GYRO_HARDWARE_LPF_OPTION_2:
return aafLUT42605[AAF_CONFIG_997HZ];
default:
return aafLUT42605[AAF_CONFIG_258HZ];
}

case ICM_42688P_SPI:
default:
switch (config) {
case GYRO_HARDWARE_LPF_NORMAL:
return aafLUT42688[AAF_CONFIG_258HZ];
case GYRO_HARDWARE_LPF_OPTION_1:
return aafLUT42688[AAF_CONFIG_536HZ];
case GYRO_HARDWARE_LPF_OPTION_2:
return aafLUT42688[AAF_CONFIG_997HZ];
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
return aafLUT[AAF_CONFIG_1962HZ];
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
return aafLUT42688[AAF_CONFIG_1962HZ];
#endif
default:
return aafLUT[AAF_CONFIG_258HZ];
default:
return aafLUT42688[AAF_CONFIG_258HZ];
}
}
}

Expand Down