Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

ICM-426xx refactoring

Browse source
This commit is contained in:
KarateBrot 2022-10-10 05:48:39 +02:00
parent 9cafe31b31
commit cd2a259b14
1 changed files with 75 additions and 76 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -64,19 +64,6 @@
#define ICM426XX_RA_ACCEL_CONFIG_STATIC2 0x03 #define ICM426XX_RA_ACCEL_CONFIG_STATIC2 0x03
#define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04 #define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04
#define ICM426XX_RA_ACCEL_CONFIG_STATIC4 0x05 #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 -------- // --- Register & setting for gyro and acc UI Filter --------
#define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52 #define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52
#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4) #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_DISABLED (0 << 3)
#define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 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) uint8_t icm426xxSpiDetect(const extDevice_t *dev)
{ {
spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00); spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
@ -166,17 +191,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
return true; return true;
} }
typedef struct odrConfig_s { static aafConfig_t getGyroAafConfig(void);
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 },
};
void icm426xxGyroInit(gyroDev_t *gyro) 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); 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); delay(15);
uint8_t odrIdx = 0; // Get desired output data rate
bool supportedODRFound = false; uint8_t odrConfig;
const uint8_t decim = LOG2_8BIT(gyro->mpuDividerDrops + 1);
if (gyro->gyroRateKHz) { if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) {
const uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c odrConfig = odrLUT[decim];
const uint8_t desiredODR = 8 / gyroSyncDenominator; } else {
for (unsigned i = 0; i < ARRAYLEN(odrConfigLUT); i++) { odrConfig = odrLUT[ODR_CONFIG_1K];
if (odrConfigLUT[i].odr == desiredODR) {
odrIdx = odrConfigLUT[i].odr_idx;
supportedODRFound = true;
break;
}
}
}
if (!supportedODRFound) {
odrIdx = 6;
gyro->gyroRateKHz = GYRO_RATE_1_kHz; gyro->gyroRateKHz = GYRO_RATE_1_kHz;
} }
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value"); 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); delay(15);
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value"); 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); 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") // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafDelt); aafConfig_t aafConfig = getGyroAafConfig();
spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafDeltSqr & 0xFF); spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt);
spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafDeltSqr >> 8) | (aafBitshift << 4)); 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) // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c)
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, ICM426XX_AAF_258HZ_DELT << 1); aafConfig = aafLUT[AAF_CONFIG_258HZ];
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, ICM426XX_AAF_258HZ_DELTSQR & 0xFF); spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (ICM426XX_AAF_258HZ_DELTSQR >> 8) | (ICM426XX_AAF_258HZ_BITSHIFT << 4)); 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 // 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); 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; 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 #endif
default:
return aafLUT[AAF_CONFIG_258HZ];
}
}
#endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P