dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

invensense/icm42688p: use full 20 bit data, increase ODR, disable all filters

This commit is contained in:
Daniel Agar 2021-01-06 17:03:39 -05:00 committed by GitHub
parent d2b0f63e1f
commit 6c9072720e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 375 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/imu/invensense/icm42688p/CMakeLists.txt
View File

@ -34,6 +34,7 @@ px4_add_module(
MODULE drivers__imu__invensense__icm42688p MODULE drivers__imu__invensense__icm42688p
MAIN icm42688p MAIN icm42688p
COMPILE_FLAGS COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS SRCS
icm42688p_main.cpp icm42688p_main.cpp
ICM42688P.cpp ICM42688P.cpp

341
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -252,25 +252,20 @@ void ICM42688P::RunImpl()
} }
} }
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure
// check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
) { && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
_last_config_check_timestamp = now; ) {
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; _last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
} else { _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
// register check failed, force reset _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
perf_count(_bad_register_perf);
Reset();
}
} else { } else {
// periodically update temperature (~1 Hz) // register check failed, force reset
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { perf_count(_bad_register_perf);
UpdateTemperature(); Reset();
_temperature_update_timestamp = now;
}
} }
} }
@ -278,65 +273,6 @@ void ICM42688P::RunImpl()
} }
} }
void ICM42688P::ConfigureAccel()
{
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
switch (ACCEL_FS_SEL) {
case ACCEL_FS_SEL_2G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_4G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_8G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_16G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
break;
}
}
void ICM42688P::ConfigureGyro()
{
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_125_DPS:
range_dps = 125.f;
break;
case GYRO_FS_SEL_250_DPS:
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
range_dps = 2000.f;
break;
}
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
}
void ICM42688P::ConfigureSampleRate(int sample_rate) void ICM42688P::ConfigureSampleRate(int sample_rate)
{ {
if (sample_rate == 0) { if (sample_rate == 0) {
@ -392,6 +328,14 @@ bool ICM42688P::Configure()
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
} }
for (const auto &reg_cfg : _register_bank1_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank2_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured // now check that all are configured
bool success = true; bool success = true;
@ -401,8 +345,22 @@ bool ICM42688P::Configure()
} }
} }
ConfigureAccel(); for (const auto &reg_cfg : _register_bank1_cfg) {
ConfigureGyro(); if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg_cfg : _register_bank2_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
// 20-bits data format used
// the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_gyro.set_range(math::radians(2000.f));
return success; return success;
} }
@ -558,6 +516,18 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
// gyro bit not set // gyro bit not set
valid = false; valid = false;
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
// Packet does not contain a new and valid extended 20-bit data
valid = false;
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
// accel ODR changed
valid = false;
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
// gyro ODR changed
valid = false;
} }
if (valid) { if (valid) {
@ -570,9 +540,11 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
} }
if (valid_samples > 0) { if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples); if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessAccel(timestamp_sample, buffer.f, valid_samples); ProcessGyro(timestamp_sample, buffer.f, valid_samples);
return true; ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
}
} }
return false; return false;
@ -589,6 +561,23 @@ void ICM42688P::FIFOReset()
_drdy_fifo_read_samples.store(0); _drdy_fifo_read_samples.store(0);
} }
static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
{
// 0xXXXAABBC
uint32_t high = ((a << 12) & 0x000FF000);
uint32_t low = ((b << 4) & 0x00000FF0);
uint32_t lowest = (c & 0x0000000F);
uint32_t x = high | low | lowest;
if (a & Bit7) {
// sign extend
x |= 0xFFF00000u;
}
return static_cast<int32_t>(x);
}
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples) void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{ {
sensor_accel_fifo_s accel{}; sensor_accel_fifo_s accel{};
@ -596,17 +585,76 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
accel.samples = 0; accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT; accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { // 18-bits of accelerometer data
int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0); bool scale_20bit = false;
int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
// first pass
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
// Accel data is 18 bit ()
int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
// sample invalid if -524288
if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
// check if any values are going to exceed int16 limits
static constexpr int16_t max_accel = INT16_MAX;
static constexpr int16_t min_accel = INT16_MIN;
if (accel_x >= max_accel || accel_x <= min_accel) {
scale_20bit = true;
}
if (accel_y >= max_accel || accel_y <= min_accel) {
scale_20bit = true;
}
if (accel_z >= max_accel || accel_z <= min_accel) {
scale_20bit = true;
}
// shift by 2 (2 least significant bits are always 0)
accel.x[accel.samples] = accel_x / 4;
accel.y[accel.samples] = accel_y / 4;
accel.z[accel.samples] = accel_z / 4;
accel.samples++;
}
}
if (!scale_20bit) {
// if highres enabled accel data is always 8192 LSB/g
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
} else {
// 20 bit data scaled to 16 bit (2^4)
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
// Accel data is 18 bit ()
int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
accel.x[i] = accel_x;
accel.y[i] = accel_y;
accel.z[i] = accel_z;
}
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
}
// correct frame for publication
for (int i = 0; i < accel.samples; i++) {
// sensor's frame is +x forward, +y left, +z up // sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down) // flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel.samples] = accel_x; accel.x[i] = accel.x[i];
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
accel.samples++;
} }
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@ -621,46 +669,115 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
{ {
sensor_gyro_fifo_s gyro{}; sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample; gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples; gyro.samples = 0;
gyro.dt = FIFO_SAMPLE_DT; gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { // 20-bits of gyroscope data
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); bool scale_20bit = false;
const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// first pass
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
// check if any values are going to exceed int16 limits
static constexpr int16_t max_gyro = INT16_MAX;
static constexpr int16_t min_gyro = INT16_MIN;
if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
scale_20bit = true;
}
if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
scale_20bit = true;
}
if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
scale_20bit = true;
}
gyro.x[gyro.samples] = gyro_x / 2;
gyro.y[gyro.samples] = gyro_y / 2;
gyro.z[gyro.samples] = gyro_z / 2;
gyro.samples++;
}
if (!scale_20bit) {
// if highres enabled gyro data is always 131 LSB/dps
_px4_gyro.set_scale(math::radians(1.f / 131.f));
} else {
// 20 bit data scaled to 16 bit (2^4)
for (int i = 0; i < samples; i++) {
gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
}
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
}
// correct frame for publication
for (int i = 0; i < gyro.samples; i++) {
// sensor's frame is +x forward, +y left, +z up // sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down) // flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro_x; gyro.x[i] = gyro.x[i];
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
} }
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro); if (gyro.samples > 0) {
_px4_gyro.updateFIFO(gyro);
}
} }
void ICM42688P::UpdateTemperature() bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{ {
// read current temperature int16_t temperature[FIFO_MAX_SAMPLES];
uint8_t temperature_buf[3] {}; float temperature_sum{0};
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { int valid_samples = 0;
perf_count(_bad_transfer_perf);
return; for (int i = 0; i < samples; i++) {
const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
// sample invalid if -32768
if (t != -32768) {
temperature_sum += t;
temperature[valid_samples] = t;
valid_samples++;
}
} }
const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]); if (valid_samples > 0) {
const float temperature_avg = temperature_sum / valid_samples;
// Temperature in Degrees Centigrade for (int i = 0; i < valid_samples; i++) {
const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; // temperature changing wildly is an indication of a transfer error
if (fabsf(temperature[i] - temperature_avg) > 1000) {
perf_count(_bad_transfer_perf);
return false;
}
}
if (PX4_ISFINITE(TEMP_degC)) { // use average temperature reading
_px4_accel.set_temperature(TEMP_degC); const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
_px4_gyro.set_temperature(TEMP_degC);
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
return true;
} else {
perf_count(_bad_transfer_perf);
}
} }
return false;
} }

65
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
View File

@ -73,7 +73,7 @@ private:
void exit_and_cleanup() override; void exit_and_cleanup() override;
// Sensor Configuration // Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured static constexpr float FIFO_SAMPLE_DT{1e6f / 16000.f}; // 16000 Hz accel & gyro ODR configured
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
@ -97,18 +97,30 @@ private:
uint8_t clear_bits{0}; uint8_t clear_bits{0};
}; };
struct register_bank1_config_t {
Register::BANK_1 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct register_bank2_config_t {
Register::BANK_2 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override; int probe() override;
bool Reset(); bool Reset();
bool Configure(); bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate); void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples); void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg); static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady(); void DataReady();
@ -128,7 +140,7 @@ private:
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature(); bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
const spi_drdy_gpio_t _drdy_gpio; const spi_drdy_gpio_t _drdy_gpio;
@ -157,26 +169,41 @@ private:
WAIT_FOR_RESET, WAIT_FOR_RESET,
CONFIGURE, CONFIGURE,
FIFO_READ, FIFO_READ,
}; } _state{STATE::RESET};
STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0}; uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10}; static constexpr uint8_t size_register_bank0_cfg{13};
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits // Register | Set bits, Clear bits
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
{ Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_16kHz, Bit3 | Bit2 | Bit0 },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_16kHz, Bit3 | Bit2 | Bit0 },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN }, { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
};
uint8_t _checked_register_bank1{0};
static constexpr uint8_t size_register_bank1_cfg{1};
register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS },
};
uint8_t _checked_register_bank2{0};
static constexpr uint8_t size_register_bank2_cfg{1};
register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_2::ACCEL_CONFIG_STATIC2, 0, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
}; };
}; };

154
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
View File

@ -66,36 +66,52 @@ namespace Register
{ {
enum class BANK_0 : uint8_t { enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x11, DEVICE_CONFIG = 0x11,
INT_CONFIG = 0x14, INT_CONFIG = 0x14,
FIFO_CONFIG = 0x16, FIFO_CONFIG = 0x16,
TEMP_DATA1 = 0x1D, TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E, TEMP_DATA0 = 0x1E,
INT_STATUS = 0x2D, INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E, FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F, FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30, FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B, SIGNAL_PATH_RESET = 0x4B,
INTF_CONFIG0 = 0x4C,
INTF_CONFIG1 = 0x4D,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
GYRO_ACCEL_CONFIG0 = 0x52,
ACCEL_CONFIG1 = 0x53,
PWR_MGMT0 = 0x4E, FIFO_CONFIG1 = 0x5F,
GYRO_CONFIG0 = 0x4F, FIFO_CONFIG2 = 0x60,
ACCEL_CONFIG0 = 0x50, FIFO_CONFIG3 = 0x61,
FIFO_CONFIG1 = 0x5F, INT_CONFIG0 = 0x63,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63, INT_SOURCE0 = 0x65,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65,
WHO_AM_I = 0x75, SELF_TEST_CONFIG = 0x70,
REG_BANK_SEL = 0x76,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
};
enum class BANK_1 : uint8_t {
GYRO_CONFIG_STATIC2 = 0x0B,
INTF_CONFIG5 = 0x7B,
};
enum class BANK_2 : uint8_t {
ACCEL_CONFIG_STATIC2 = 0x03,
}; };
}; };
@ -144,10 +160,6 @@ enum PWR_MGMT0_BIT : uint8_t {
enum GYRO_CONFIG0_BIT : uint8_t { enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL // 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps
GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps
GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps
GYRO_FS_SEL_125_DPS = Bit7, // 0b100 = ±125 dps
// 3:0 GYRO_ODR // 3:0 GYRO_ODR
GYRO_ODR_32kHz = Bit0, // 0001: 32kHz GYRO_ODR_32kHz = Bit0, // 0001: 32kHz
@ -162,9 +174,6 @@ enum GYRO_CONFIG0_BIT : uint8_t {
enum ACCEL_CONFIG0_BIT : uint8_t { enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL // 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5, // 001: ±8g
ACCEL_FS_SEL_4G = Bit6, // 010: ±4g
ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g
// 3:0 ACCEL_ODR // 3:0 ACCEL_ODR
ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz
@ -175,11 +184,30 @@ enum ACCEL_CONFIG0_BIT : uint8_t {
ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default)
}; };
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
};
// GYRO_ACCEL_CONFIG0
enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
// 7:4 ACCEL_UI_FILT_BW
ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
// 3:0 GYRO_UI_FILT_BW
GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
};
// FIFO_CONFIG1 // FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t { enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5, FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit4,
FIFO_TEMP_EN = Bit2, FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1, FIFO_GYRO_EN = Bit1,
FIFO_ACCEL_EN = Bit0, FIFO_ACCEL_EN = Bit0,
@ -191,11 +219,6 @@ enum INT_CONFIG0_BIT : uint8_t {
CLEAR_ON_FIFO_READ = Bit3, CLEAR_ON_FIFO_READ = Bit3,
}; };
// INT_CONFIG1
enum INT_CONFIG1_BIT : uint8_t {
INT_TPULSE_DURATION = Bit6, // 0: Interrupt pulse duration is 100μs
};
// INT_SOURCE0 // INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t { enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6, UI_FSYNC_INT1_EN = Bit6,
@ -215,41 +238,62 @@ enum REG_BANK_SEL_BIT : uint8_t {
USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3. USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
}; };
//---------------- BANK1 Register bits
// GYRO_CONFIG_STATIC2
enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
GYRO_AAF_DIS = Bit1,
GYRO_NF_DIS = Bit0,
};
//---------------- BANK2 Register bits
// ACCEL_CONFIG_STATIC2
enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
ACCEL_AAF_DIS = Bit0,
};
namespace FIFO namespace FIFO
{ {
static constexpr size_t SIZE = 2048; static constexpr size_t SIZE = 2048;
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
// Packet 3 // Packet 4
struct DATA { struct DATA {
uint8_t FIFO_Header; uint8_t FIFO_Header;
uint8_t ACCEL_DATA_X1; uint8_t ACCEL_DATA_X1; // Accel X [19:12]
uint8_t ACCEL_DATA_X0; uint8_t ACCEL_DATA_X0; // Accel X [11:4]
uint8_t ACCEL_DATA_Y1; uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
uint8_t ACCEL_DATA_Y0; uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
uint8_t ACCEL_DATA_Z1; uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
uint8_t ACCEL_DATA_Z0; uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
uint8_t GYRO_DATA_X1; uint8_t GYRO_DATA_X1; // Gyro X [19:12]
uint8_t GYRO_DATA_X0; uint8_t GYRO_DATA_X0; // Gyro X [11:4]
uint8_t GYRO_DATA_Y1; uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
uint8_t GYRO_DATA_Y0; uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
uint8_t GYRO_DATA_Z1; uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
uint8_t GYRO_DATA_Z0; uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
uint8_t temperature; // Temperature[7:0] uint8_t TEMP_DATA1; // Temperature[15:8]
uint8_t timestamp_l; uint8_t TEMP_DATA0; // Temperature[7:0]
uint8_t timestamp_h; uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t TimeStamp_l; // TimeStamp[7:0]
uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
}; };
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
enum FIFO_HEADER_BIT : uint8_t { enum FIFO_HEADER_BIT : uint8_t {
HEADER_MSG = Bit7, // 1: FIFO is empty HEADER_MSG = Bit7, // 1: FIFO is empty
HEADER_ACCEL = Bit6, HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
HEADER_GYRO = Bit5, HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
HEADER_20 = Bit4, HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
HEADER_ODR_ACCEL = Bit1, HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
HEADER_ODR_GYRO = Bit0, HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
}; };
} }