Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Add separate AAF values for ICM-42605 (#12616)

Browse source 
Co-authored-by: Tobias Bolin <tobias_bolin@hotmail.com>
This commit is contained in:
SugarK 2023-04-04 00:37:37 +10:00 • committed by GitHub
parent 64f48611aa
commit e43d591b2d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 44 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

62
src/main/drivers/accgyro/accgyro_spi_icm426xx.c
View file

@ -138,14 +138,23 @@ static uint8_t odrLUT[ODR_CONFIG_COUNT] = { // see GYRO_ODR in section 5.6
[ODR_CONFIG_1K] = 6, [ODR_CONFIG_1K] = 6,
}; };
// Possible gyro Anti-Alias Filter (AAF) cutoffs // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
static aafConfig_t aafLUT[AAF_CONFIG_COUNT] = { // see table in section 5.3 static aafConfig_t aafLUT42688[AAF_CONFIG_COUNT] = { // see table in section 5.3
[AAF_CONFIG_258HZ] = { 6, 36, 10 }, [AAF_CONFIG_258HZ] = { 6, 36, 10 },
[AAF_CONFIG_536HZ] = { 12, 144, 8 }, [AAF_CONFIG_536HZ] = { 12, 144, 8 },
[AAF_CONFIG_997HZ] = { 21, 440, 6 }, [AAF_CONFIG_997HZ] = { 21, 440, 6 },
[AAF_CONFIG_1962HZ] = { 37, 1376, 4 }, [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) uint8_t icm426xxSpiDetect(const extDevice_t *dev)
{ {
spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00); spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
@ -199,7 +208,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
return true; return true;
} }
static aafConfig_t getGyroAafConfig(void); static aafConfig_t getGyroAafConfig(const mpuSensor_e, const aafConfig_e);
static void turnGyroAccOff(const extDevice_t *dev) static void turnGyroAccOff(const extDevice_t *dev)
{ {
@ -234,14 +243,15 @@ void icm426xxGyroInit(gyroDev_t *gyro)
turnGyroAccOff(dev); turnGyroAccOff(dev);
// Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER") // 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); setUserBank(dev, ICM426XX_BANK_SELECT1);
spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt); 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_STATIC4, aafConfig.deltSqr & 0xFF);
spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4)); 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) // 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); setUserBank(dev, ICM426XX_BANK_SELECT2);
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1); 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_STATIC3, aafConfig.deltSqr & 0xFF);
@ -305,21 +315,37 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
return true; return true;
} }
static aafConfig_t getGyroAafConfig(void) static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig_e config)
{ {
switch (gyroConfig()->gyro_hardware_lpf) { switch (gyroModel){
case GYRO_HARDWARE_LPF_NORMAL: case ICM_42605_SPI:
return aafLUT[AAF_CONFIG_258HZ]; switch (config) {
case GYRO_HARDWARE_LPF_OPTION_1: case GYRO_HARDWARE_LPF_NORMAL:
return aafLUT[AAF_CONFIG_536HZ]; return aafLUT42605[AAF_CONFIG_258HZ];
case GYRO_HARDWARE_LPF_OPTION_2: case GYRO_HARDWARE_LPF_OPTION_1:
return aafLUT[AAF_CONFIG_997HZ]; return aafLUT42605[AAF_CONFIG_536HZ];
#ifdef USE_GYRO_DLPF_EXPERIMENTAL case GYRO_HARDWARE_LPF_OPTION_2:
case GYRO_HARDWARE_LPF_EXPERIMENTAL: return aafLUT42605[AAF_CONFIG_997HZ];
return aafLUT[AAF_CONFIG_1962HZ]; default:
#endif return aafLUT42605[AAF_CONFIG_258HZ];
}
case ICM_42688P_SPI:
default: default:
return aafLUT[AAF_CONFIG_258HZ]; 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 aafLUT42688[AAF_CONFIG_1962HZ];
#endif
default:
return aafLUT42688[AAF_CONFIG_258HZ];
}
} }
} }