mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
Fix ICM426XX AA filter (#12444)
* Change ICM426XX gyro initialization - Shut down Acc and Gyro before setting non-GYRO_ODR, ACCEL_ODR, GYRO_FS_SEL, ACCEL_FS_SEL, GYRO_MODE, ACCEL_MODE registers - Set correct User Bank before writing to registers * Change ICM426XX GYRO_ACCEL_CONFIG0 to 15 from 14 * Remove unneeded delay commands
This commit is contained in:
tbolin
GitHub
parent
58346417a1
commit
9ce8d9c8e5
1 changed files with 79 additions and 45 deletions
|
|
@ -48,9 +48,17 @@
|
|||
// 24 MHz max SPI frequency
|
||||
#define ICM426XX_MAX_SPI_CLK_HZ 24000000
|
||||
|
||||
#define ICM426XX_RA_PWR_MGMT0 0x4E
|
||||
#define ICM426XX_RA_REG_BANK_SEL 0x76
|
||||
#define ICM426XX_BANK_SELECT0 0x00
|
||||
#define ICM426XX_BANK_SELECT1 0x01
|
||||
#define ICM426XX_BANK_SELECT2 0x02
|
||||
#define ICM426XX_BANK_SELECT3 0x03
|
||||
#define ICM426XX_BANK_SELECT4 0x04
|
||||
|
||||
#define ICM426XX_RA_PWR_MGMT0 0x4E // User Bank 0
|
||||
#define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
|
||||
#define ICM426XX_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
|
||||
#define ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF ((0 << 0) | (0 << 2))
|
||||
#define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
|
||||
#define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
|
||||
|
||||
|
|
@ -58,22 +66,22 @@
|
|||
#define ICM426XX_RA_ACCEL_CONFIG0 0x50
|
||||
|
||||
// --- Registers for gyro and acc Anti-Alias Filter ---------
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC3 0x0C
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC4 0x0D
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC5 0x0E
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC2 0x03
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC4 0x05
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC3 0x0C // User Bank 1
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC4 0x0D // User Bank 1
|
||||
#define ICM426XX_RA_GYRO_CONFIG_STATIC5 0x0E // User Bank 1
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC2 0x03 // User Bank 2
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC3 0x04 // User Bank 2
|
||||
#define ICM426XX_RA_ACCEL_CONFIG_STATIC4 0x05 // User Bank 2
|
||||
// --- Register & setting for gyro and acc UI Filter --------
|
||||
#define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52
|
||||
#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
|
||||
#define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
|
||||
#define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52 // User Bank 0
|
||||
#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (15 << 4)
|
||||
#define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY (15 << 0)
|
||||
// ----------------------------------------------------------
|
||||
|
||||
#define ICM426XX_RA_GYRO_DATA_X1 0x25
|
||||
#define ICM426XX_RA_ACCEL_DATA_X1 0x1F
|
||||
#define ICM426XX_RA_GYRO_DATA_X1 0x25 // User Bank 0
|
||||
#define ICM426XX_RA_ACCEL_DATA_X1 0x1F // User Bank 0
|
||||
|
||||
#define ICM426XX_RA_INT_CONFIG 0x14
|
||||
#define ICM426XX_RA_INT_CONFIG 0x14 // User Bank 0
|
||||
#define ICM426XX_INT1_MODE_PULSED (0 << 2)
|
||||
#define ICM426XX_INT1_MODE_LATCHED (1 << 2)
|
||||
#define ICM426XX_INT1_DRIVE_CIRCUIT_OD (0 << 1)
|
||||
|
|
@ -81,13 +89,13 @@
|
|||
#define ICM426XX_INT1_POLARITY_ACTIVE_LOW (0 << 0)
|
||||
#define ICM426XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
|
||||
|
||||
#define ICM426XX_RA_INT_CONFIG0 0x63
|
||||
#define ICM426XX_RA_INT_CONFIG0 0x63 // User Bank 0
|
||||
#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
|
||||
#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
|
||||
#define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
|
||||
#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
|
||||
|
||||
#define ICM426XX_RA_INT_CONFIG1 0x64
|
||||
#define ICM426XX_RA_INT_CONFIG1 0x64 // User Bank 0
|
||||
#define ICM426XX_INT_ASYNC_RESET_BIT 4
|
||||
#define ICM426XX_INT_TDEASSERT_DISABLE_BIT 5
|
||||
#define ICM426XX_INT_TDEASSERT_ENABLED (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
|
||||
|
|
@ -96,7 +104,7 @@
|
|||
#define ICM426XX_INT_TPULSE_DURATION_100 (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
|
||||
#define ICM426XX_INT_TPULSE_DURATION_8 (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
|
||||
|
||||
#define ICM426XX_RA_INT_SOURCE0 0x65
|
||||
#define ICM426XX_RA_INT_SOURCE0 0x65 // User Bank 0
|
||||
#define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3)
|
||||
#define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3)
|
||||
|
||||
|
|
@ -193,6 +201,23 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
|
|||
|
||||
static aafConfig_t getGyroAafConfig(void);
|
||||
|
||||
static void turnGyroAccOff(const extDevice_t *dev)
|
||||
{
|
||||
spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF);
|
||||
}
|
||||
|
||||
// Turn on gyro and acc on in Low Noise mode
|
||||
static void turnGyroAccOn(const extDevice_t *dev)
|
||||
{
|
||||
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(1);
|
||||
}
|
||||
|
||||
static void setUserBank(const extDevice_t *dev, const uint8_t user_bank)
|
||||
{
|
||||
spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
|
||||
}
|
||||
|
||||
void icm426xxGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
const extDevice_t *dev = &gyro->dev;
|
||||
|
|
@ -203,8 +228,44 @@ void icm426xxGyroInit(gyroDev_t *gyro)
|
|||
gyro->accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
|
||||
gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;
|
||||
|
||||
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);
|
||||
// Turn off ACC and GYRO so they can be configured
|
||||
// See section 12.9 in ICM-42688-P datasheet v1.7
|
||||
setUserBank(dev, ICM426XX_BANK_SELECT0);
|
||||
turnGyroAccOff(dev);
|
||||
|
||||
// Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
|
||||
aafConfig_t aafConfig = getGyroAafConfig();
|
||||
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];
|
||||
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);
|
||||
spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
|
||||
|
||||
// Configure gyro and acc UI Filters
|
||||
setUserBank(dev, ICM426XX_BANK_SELECT0);
|
||||
spiWriteReg(dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
|
||||
|
||||
// Configure interrupt pin
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
|
||||
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
|
||||
|
||||
uint8_t intConfig1Value = spiReadRegMsk(dev, ICM426XX_RA_INT_CONFIG1);
|
||||
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
|
||||
intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
|
||||
intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
|
||||
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
|
||||
|
||||
// Turn on gyro and acc on again so ODR and FSR can be configured
|
||||
turnGyroAccOn(dev);
|
||||
|
||||
// Get desired output data rate
|
||||
uint8_t odrConfig;
|
||||
|
|
@ -223,33 +284,6 @@ void icm426xxGyroInit(gyroDev_t *gyro)
|
|||
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 | (odrConfig & 0x0F));
|
||||
delay(15);
|
||||
|
||||
// Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
|
||||
aafConfig_t aafConfig = getGyroAafConfig();
|
||||
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];
|
||||
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_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
|
||||
|
||||
// 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_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
|
||||
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
|
||||
|
||||
uint8_t intConfig1Value = spiReadRegMsk(dev, ICM426XX_RA_INT_CONFIG1);
|
||||
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
|
||||
intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
|
||||
intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
|
||||
|
||||
spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
|
||||
}
|
||||
|
||||
bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue