Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <stdbool.h>
- #include <stdint.h>
- #include <stdlib.h>
- #include "spi.h"
- #include "task.h"
- // 24 MHz max SPI frequency
- #define ICM426XX_MAX_SPI_CLK_HZ 24000000
- #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)
- #define ICM426XX_RA_GYRO_CONFIG0 0x4F
- #define ICM426XX_RA_ACCEL_CONFIG0 0x50
- // --- Registers for gyro and acc Anti-Alias Filter ---------
- #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 // 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 // User Bank 0
- #define ICM426XX_RA_ACCEL_DATA_X1 0x1F // User Bank 0
- #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)
- #define ICM426XX_INT1_DRIVE_CIRCUIT_PP (1 << 1)
- #define ICM426XX_INT1_POLARITY_ACTIVE_LOW (0 << 0)
- #define ICM426XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
- #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 // 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)
- #define ICM426XX_INT_TDEASSERT_DISABLED (1 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
- #define ICM426XX_INT_TPULSE_DURATION_BIT 6
- #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 // User Bank 0
- #define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3)
- #define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3)
- enum gyro_fsr_e {
- INV_FSR_250DPS = 0,
- INV_FSR_500DPS,
- INV_FSR_1000DPS,
- INV_FSR_2000DPS,
- NUM_GYRO_FSR
- };
- enum accel_fsr_e {
- INV_FSR_2G = 0,
- INV_FSR_4G,
- INV_FSR_8G,
- INV_FSR_16G,
- NUM_ACCEL_FSR
- };
- 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 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 },
- };
- void spiWriteReg(uint8_t addr, uint8_t data)
- {
- spi_select_chip();
- spi_send_byte(addr);
- spi_send_byte(data);
- spi_deselect_chip();
- }
- uint8_t spiReadReg(uint8_t addr)
- {
- uint8_t t;
- spi_select_chip();
- spi_send_byte(addr);
- t = spi_send_byte(0);
- spi_deselect_chip();
- return t;
- }
- static void turnAGOff(void)
- {
- spiWriteReg(ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF);
- }
- // Turn on gyro and acc on in Low Noise mode
- static void turnAGOn(void)
- {
- spiWriteReg(ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
- task_wait(1);
- }
- static void setUserBank(const uint8_t user_bank)
- {
- spiWriteReg(ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
- }
- void icm426xxAGInit(void)
- {
- SPI_Configuration();
- //accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
- //gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;
- // Turn off ACC and GYRO so they can be configured
- // See section 12.9 in ICM-42688-P datasheet v1.7
- setUserBank(ICM426XX_BANK_SELECT0);
- turnAGOff();
- // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
- aafConfig_t aafConfig = aafLUT42688[AAF_CONFIG_258HZ];
- setUserBank(ICM426XX_BANK_SELECT1);
- spiWriteReg(ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt);
- spiWriteReg(ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig.deltSqr & 0xFF);
- spiWriteReg(ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
- // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c)
- aafConfig = aafLUT42688[AAF_CONFIG_258HZ];
- setUserBank(ICM426XX_BANK_SELECT2);
- spiWriteReg(ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
- spiWriteReg(ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF);
- spiWriteReg(ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
- // Configure gyro and acc UI Filters
- setUserBank(ICM426XX_BANK_SELECT0);
- spiWriteReg(ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
- // Configure interrupt pin
- spiWriteReg(ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
- spiWriteReg(ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
- spiWriteReg(ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
- uint8_t intConfig1Value = spiReadReg(ICM426XX_RA_INT_CONFIG1 | 0x80);
- // 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(ICM426XX_RA_INT_CONFIG1, intConfig1Value);
- // Turn on gyro and acc on again so ODR and FSR can be configured
- turnAGOn();
- uint8_t odrConfig = odrLUT[ODR_CONFIG_1K];
- spiWriteReg(ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
- task_wait(15);
- spiWriteReg(ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
- task_wait(15);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement