forked from Archive/PX4-Autopilot
drivers: icm42688p and iim42652 enable notch and AAF
This commit is contained in:
Alex Klimaj
GitHub
parent
a18e07e525
commit
3ed1c688bf
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 },
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 },
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue