diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 54d3f8e478..044bc3a69c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -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)