From 3ed1c688bfe74963aedbd69df16da683d94e4461 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Mon, 13 Feb 2023 19:08:37 -0700 Subject: [PATCH] drivers: icm42688p and iim42652 enable notch and AAF --- .../imu/invensense/icm42688p/ICM42688P.cpp | 4 +- .../imu/invensense/icm42688p/ICM42688P.hpp | 13 ++- .../InvenSense_ICM42688P_registers.hpp | 84 +++++++++++++++++-- .../imu/invensense/iim42652/IIM42652.cpp | 4 +- .../imu/invensense/iim42652/IIM42652.hpp | 13 ++- .../InvenSense_IIM42652_registers.hpp | 84 +++++++++++++++++-- 6 files changed, 172 insertions(+), 30 deletions(-) diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index fa23ca4afd..e4ad620f89 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -359,8 +359,8 @@ void ICM42688P::ConfigureCLKIN() for (auto &r1 : _register_bank1_cfg) { if (r1.reg == Register::BANK_1::INTF_CONFIG5) { - r1.set_bits = PIN9_FUNCTION_BIT::CLKIN; - r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9; + r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET; + r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR; } } } diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index 0e10b17d6d..6eeb83e4a3 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -201,17 +201,22 @@ private: }; uint8_t _checked_register_bank1{0}; - static constexpr uint8_t size_register_bank1_cfg{2}; + static constexpr uint8_t size_register_bank1_cfg{5}; register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { // Register | Set bits, Clear bits - { Register::BANK_1::GYRO_CONFIG_STATIC2, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 }, + { Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS }, + { Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR}, { Register::BANK_1::INTF_CONFIG5, 0, 0 }, }; uint8_t _checked_register_bank2{0}; - static constexpr uint8_t size_register_bank2_cfg{1}; + static constexpr uint8_t size_register_bank2_cfg{3}; register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { // Register | Set bits, Clear bits - { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS, 0 }, + { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, + { Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR }, + { Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR }, }; }; diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 3874d6e470..21a6464092 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -108,12 +108,18 @@ enum class BANK_0 : uint8_t { }; enum class BANK_1 : uint8_t { - GYRO_CONFIG_STATIC2 = 0x0B, - INTF_CONFIG5 = 0x7B, + GYRO_CONFIG_STATIC2 = 0x0B, + GYRO_CONFIG_STATIC3 = 0x0C, + GYRO_CONFIG_STATIC4 = 0x0D, + GYRO_CONFIG_STATIC5 = 0x0E, + + INTF_CONFIG5 = 0x7B, }; enum class BANK_2 : uint8_t { ACCEL_CONFIG_STATIC2 = 0x03, + ACCEL_CONFIG_STATIC3 = 0x04, + ACCEL_CONFIG_STATIC4 = 0x05, }; }; @@ -289,21 +295,81 @@ enum REG_BANK_SEL_BIT : uint8_t { // GYRO_CONFIG_STATIC2 enum GYRO_CONFIG_STATIC2_BIT : uint8_t { - GYRO_AAF_DIS = Bit1, - GYRO_NF_DIS = Bit0, + GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter + GYRO_NF_DIS = Bit0, // 1: Disable Notch Filter }; -// PIN9_FUNCTION -enum PIN9_FUNCTION_BIT : uint8_t { - FSYNC_PIN9 = Bit1, - CLKIN = Bit2, +// GYRO_CONFIG_STATIC3 +enum GYRO_CONFIG_STATIC3_BIT : uint8_t { + // 5:0 GYRO_AAF_DELT + // 585 Hz = 13 (0b00'1101) + GYRO_AAF_DELT_585HZ_SET = Bit3 | Bit2 | Bit0, + GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1, +}; + +// GYRO_CONFIG_STATIC4 +enum GYRO_CONFIG_STATIC4_BIT : uint8_t { + // 7:0 GYRO_AAF_DELTSQR + // 585 Hz = 170 (0b1010'1010) + GYRO_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// GYRO_CONFIG_STATIC5 +enum GYRO_CONFIG_STATIC5_BIT : uint8_t { + // 7:4 GYRO_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + GYRO_AAF_BITSHIFT_585HZ_SET = Bit7, + GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 GYRO_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + GYRO_AAF_DELTSQR_MSB_585HZ_SET = 0, + GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// INTF_CONFIG5 +enum INTF_CONFIG5_BIT : uint8_t { + // 2:1 PIN9_FUNCTION + PIN9_FUNCTION_CLKIN_SET = Bit2, // 0b10: CLKIN + PIN9_FUNCTION_CLKIN_CLEAR = Bit1, + + PIN9_FUNCTION_RESET_SET = 0, + PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1, }; //---------------- BANK2 Register bits // ACCEL_CONFIG_STATIC2 enum ACCEL_CONFIG_STATIC2_BIT : uint8_t { - ACCEL_AAF_DIS = Bit0, + // 6:1 ACCEL_AAF_DELT + // 585 Hz = 13 (0b00'1101) + ACCEL_AAF_DELT_585HZ_SET = Bit4 | Bit3 | Bit1, + ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2, + + // 0 ACCEL_AAF_DIS + ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default) +}; + +// ACCEL_CONFIG_STATIC3 +enum ACCEL_CONFIG_STATIC3_BIT : uint8_t { + // 7:0 ACCEL_AAF_DELTSQR[7:0] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// ACCEL_CONFIG_STATIC4 +enum ACCEL_CONFIG_STATIC4_BIT : uint8_t { + // 7:4 ACCEL_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + ACCEL_AAF_BITSHIFT_585HZ_SET = Bit7, + ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 ACCEL_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_MSB_SET = 0, + ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, }; namespace FIFO diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.cpp b/src/drivers/imu/invensense/iim42652/IIM42652.cpp index d7a2172a88..69e3075ea2 100644 --- a/src/drivers/imu/invensense/iim42652/IIM42652.cpp +++ b/src/drivers/imu/invensense/iim42652/IIM42652.cpp @@ -359,8 +359,8 @@ void IIM42652::ConfigureCLKIN() for (auto &r1 : _register_bank1_cfg) { if (r1.reg == Register::BANK_1::INTF_CONFIG5) { - r1.set_bits = PIN9_FUNCTION_BIT::CLKIN; - r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9; + r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET; + r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR; } } } diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.hpp b/src/drivers/imu/invensense/iim42652/IIM42652.hpp index fc7927a34c..65e286e3ed 100644 --- a/src/drivers/imu/invensense/iim42652/IIM42652.hpp +++ b/src/drivers/imu/invensense/iim42652/IIM42652.hpp @@ -201,17 +201,22 @@ private: }; uint8_t _checked_register_bank1{0}; - static constexpr uint8_t size_register_bank1_cfg{2}; + static constexpr uint8_t size_register_bank1_cfg{5}; register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { // Register | Set bits, Clear bits - { Register::BANK_1::GYRO_CONFIG_STATIC2, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 }, + { Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS }, + { Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR}, { Register::BANK_1::INTF_CONFIG5, 0, 0 }, }; uint8_t _checked_register_bank2{0}; - static constexpr uint8_t size_register_bank2_cfg{1}; + static constexpr uint8_t size_register_bank2_cfg{3}; register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { // Register | Set bits, Clear bits - { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS, 0 }, + { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, + { Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR }, + { Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR }, }; }; diff --git a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp index 79bd024101..cd328745ee 100644 --- a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp +++ b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp @@ -108,12 +108,18 @@ enum class BANK_0 : uint8_t { }; enum class BANK_1 : uint8_t { - GYRO_CONFIG_STATIC2 = 0x0B, - INTF_CONFIG5 = 0x7B, + GYRO_CONFIG_STATIC2 = 0x0B, + GYRO_CONFIG_STATIC3 = 0x0C, + GYRO_CONFIG_STATIC4 = 0x0D, + GYRO_CONFIG_STATIC5 = 0x0E, + + INTF_CONFIG5 = 0x7B, }; enum class BANK_2 : uint8_t { ACCEL_CONFIG_STATIC2 = 0x03, + ACCEL_CONFIG_STATIC3 = 0x04, + ACCEL_CONFIG_STATIC4 = 0x05, }; }; @@ -289,21 +295,81 @@ enum REG_BANK_SEL_BIT : uint8_t { // GYRO_CONFIG_STATIC2 enum GYRO_CONFIG_STATIC2_BIT : uint8_t { - GYRO_AAF_DIS = Bit1, - GYRO_NF_DIS = Bit0, + GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter + GYRO_NF_DIS = Bit0, // 1: Disable Notch Filter }; -// PIN9_FUNCTION -enum PIN9_FUNCTION_BIT : uint8_t { - FSYNC_PIN9 = Bit1, - CLKIN = Bit2, +// GYRO_CONFIG_STATIC3 +enum GYRO_CONFIG_STATIC3_BIT : uint8_t { + // 5:0 GYRO_AAF_DELT + // 585 Hz = 13 (0b00'1101) + GYRO_AAF_DELT_585HZ_SET = Bit3 | Bit2 | Bit0, + GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1, +}; + +// GYRO_CONFIG_STATIC4 +enum GYRO_CONFIG_STATIC4_BIT : uint8_t { + // 7:0 GYRO_AAF_DELTSQR + // 585 Hz = 170 (0b1010'1010) + GYRO_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// GYRO_CONFIG_STATIC5 +enum GYRO_CONFIG_STATIC5_BIT : uint8_t { + // 7:4 GYRO_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + GYRO_AAF_BITSHIFT_585HZ_SET = Bit7, + GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 GYRO_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + GYRO_AAF_DELTSQR_MSB_585HZ_SET = 0, + GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// INTF_CONFIG5 +enum INTF_CONFIG5_BIT : uint8_t { + // 2:1 PIN9_FUNCTION + PIN9_FUNCTION_CLKIN_SET = Bit2, // 0b10: CLKIN + PIN9_FUNCTION_CLKIN_CLEAR = Bit1, + + PIN9_FUNCTION_RESET_SET = 0, + PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1, }; //---------------- BANK2 Register bits // ACCEL_CONFIG_STATIC2 enum ACCEL_CONFIG_STATIC2_BIT : uint8_t { - ACCEL_AAF_DIS = Bit0, + // 6:1 ACCEL_AAF_DELT + // 585 Hz = 13 (0b00'1101) + ACCEL_AAF_DELT_585HZ_SET = Bit4 | Bit3 | Bit1, + ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2, + + // 0 ACCEL_AAF_DIS + ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default) +}; + +// ACCEL_CONFIG_STATIC3 +enum ACCEL_CONFIG_STATIC3_BIT : uint8_t { + // 7:0 ACCEL_AAF_DELTSQR[7:0] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// ACCEL_CONFIG_STATIC4 +enum ACCEL_CONFIG_STATIC4_BIT : uint8_t { + // 7:4 ACCEL_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + ACCEL_AAF_BITSHIFT_585HZ_SET = Bit7, + ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 ACCEL_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_MSB_SET = 0, + ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, }; namespace FIFO