zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

drivers/imu/invensense/icm42670p: cleanup and small fixes

main
Daniel Agar 3 years ago committed by David Sidrane
parent
3ea492b7a2
commit
83daf648ef
4 changed files with 235 additions and 364 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/drivers/imu/invensense/icm42670p/CMakeLists.txt
  2. 300
      src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
  3. 61
      src/drivers/imu/invensense/icm42670p/ICM42670P.hpp
  4. 218
      src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp

2
src/drivers/imu/invensense/icm42670p/CMakeLists.txt
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
# Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions

300
src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config): @@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (_drdy_gpio != 0) {
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
@ -123,7 +123,7 @@ void ICM42670P::RunImpl() @@ -123,7 +123,7 @@ void ICM42670P::RunImpl()
switch (_state) {
case STATE::RESET:
// DEVICE_CONFIG: Software reset configuration
// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG);
_reset_timestamp = now;
_failure_count = 0;
@ -133,7 +133,7 @@ void ICM42670P::RunImpl() @@ -133,7 +133,7 @@ void ICM42670P::RunImpl()
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
&& (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
// Wakeup accel and gyro and schedule remaining configuration
@ -190,15 +190,19 @@ void ICM42670P::RunImpl() @@ -190,15 +190,19 @@ void ICM42670P::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
} else {
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -209,8 +213,6 @@ void ICM42670P::RunImpl() @@ -209,8 +213,6 @@ void ICM42670P::RunImpl()
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();
// PX4_WARN("fifo_count = %d ",fifo_count);
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
@ -222,12 +224,17 @@ void ICM42670P::RunImpl() @@ -222,12 +224,17 @@ void ICM42670P::RunImpl()
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
samples = 0;
// PX4_WARN("samples 1 = %d ",samples);
}
}
}
@ -235,12 +242,9 @@ void ICM42670P::RunImpl() @@ -235,12 +242,9 @@ void ICM42670P::RunImpl()
bool success = false;
if (samples >= 1) {
// PX4_WARN("samples 2 = %d ",samples);
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
// PX4_WARN("success = %d ",success);
if (_failure_count > 0) {
_failure_count--;
}
@ -260,9 +264,11 @@ void ICM42670P::RunImpl() @@ -260,9 +264,11 @@ void ICM42670P::RunImpl()
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg;
} else {
// register check failed, force reset
@ -283,68 +289,8 @@ void ICM42670P::RunImpl() @@ -283,68 +289,8 @@ void ICM42670P::RunImpl()
}
}
void ICM42670P::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 ICM42670P::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_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 ICM42670P::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples) @@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
}
}
bool ICM42670P::Configure()
{
// first set and clear all configured register bits
@ -383,7 +327,9 @@ bool ICM42670P::Configure() @@ -383,7 +327,9 @@ bool ICM42670P::Configure()
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
Mreg1Config();
for (const auto &reg_cfg : _register_mreg1_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
@ -394,11 +340,17 @@ bool ICM42670P::Configure() @@ -394,11 +340,17 @@ bool ICM42670P::Configure()
}
}
success = Mreg1Check();
for (const auto &reg_cfg : _register_mreg1_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
ConfigureAccel();
ConfigureGyro();
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
_px4_gyro.set_range(math::radians(2000.f));
return success;
}
@ -411,12 +363,9 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -411,12 +363,9 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM42670P::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
}
bool ICM42670P::DataReadyInterruptConfigure()
{
@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T &reg_cfg) @@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T &reg_cfg)
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
template <typename T>
uint8_t ICM42670P::RegisterRead(T reg)
uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
{
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg) @@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg)
return cmd[1];
}
template <typename T>
void ICM42670P::RegisterWrite(T reg, uint8_t value)
uint8_t ICM42670P::RegisterRead(Register::MREG1 reg)
{
uint8_t cmd[2] { (uint8_t)reg, value };
// BLK_SEL_R must be set to 0
RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00);
// MADDR_R must be set to 0x14 (address of the MREG1 register being accessed)
RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg);
// Wait for 10 µs
px4_udelay(10);
// Read register M_R to access the value in MREG1 register 0x14
uint8_t value = RegisterRead(Register::BANK_0::M_R);
// Wait for 10 µs
// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
px4_udelay(10);
return value;
}
void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
{
uint8_t cmd[2] { (uint8_t)reg, value };
transfer(cmd, cmd, sizeof(cmd));
}
void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
{
// BLK_SEL_W must be set to 0
RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00);
// MADDR_W must be set to address of the MREG1 register being accessed
RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg);
// M_W must be set to the desired value
RegisterWrite(Register::BANK_0::M_W, value);
// Wait for 10 µs
// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
px4_udelay(10);
}
template <typename T>
void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
{
@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount() @@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount()
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_count(_bad_transfer_perf);
@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) @@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) @@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
// gyro bit not set
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) {
@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) @@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
}
}
// PX4_WARN("valid_samples = %d ",valid_samples);
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
@ -577,18 +571,30 @@ void ICM42670P::FIFOReset() @@ -577,18 +571,30 @@ void ICM42670P::FIFOReset()
{
perf_count(_fifo_reset_perf);
// FIFO flush requires the following programming sequence:
// Write FIFO_FLUSH = 1
// Wait for 1.5 µs
// Read FIFO_FLUSH, it should now be 0
// SIGNAL_PATH_RESET: FIFO flush
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
px4_udelay(2); // Wait for 1.5 µs
const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
}
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.samples = 0;
accel.samples = samples;
accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
@ -598,19 +604,16 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA @@ -598,19 +604,16 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
// 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)
accel.x[accel.samples] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel.samples++;
accel.x[i] = accel_x;
accel.y[i] = math::negate(accel_y);
accel.z[i] = math::negate(accel_z);
}
_px4_accel.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));
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
}
}
void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT @@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
// 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)
gyro.x[i] = gyro_x;
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
gyro.y[i] = math::negate(gyro_y);
gyro.z[i] = math::negate(gyro_z);
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature() @@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature()
_px4_gyro.set_temperature(TEMP_degC);
}
}
uint8_t ICM42670P::RegisterReadBank1(uint8_t reg)
{
uint8_t value;
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg);
ScheduleDelayed(10_us);
value = RegisterRead((uint8_t)Register::BANK_0::M_R);
ScheduleDelayed(10_us);
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
return value;
}
void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value)
{
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg);
RegisterWrite((uint8_t)Register::BANK_0::M_W, value);
ScheduleDelayed(10_us);
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
}
void ICM42670P::Mreg1Config()
{
uint8_t data;
uint8_t set_bits;
uint8_t clear_bits;
clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
data = RegisterReadBank1(0x01);
data &= ~clear_bits;
data |= set_bits;
RegisterWriteBank1(0x01, data);
clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
data = RegisterReadBank1(0x04);
data &= ~clear_bits;
data |= set_bits;
RegisterWriteBank1(0x04, data);
}
bool ICM42670P::Mreg1Check()
{
uint8_t set_bits;
uint8_t clear_bits;
uint8_t reg_value;
bool success;
success = true;
reg_value = RegisterReadBank1(0x01);
clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
if (set_bits && ((reg_value & set_bits) != set_bits)) {
success = false;
}
if (clear_bits && ((reg_value & clear_bits) != 0)) {
success = false;
}
reg_value = RegisterReadBank1(0x04);
clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
if (set_bits && ((reg_value & set_bits) != set_bits)) {
success = false;
}
if (clear_bits && ((reg_value & clear_bits) != 0)) {
success = false;
}
return success;
}

61
src/drivers/imu/invensense/icm42670p/ICM42670P.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P> @@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
public:
ICM42670P(const I2CSPIDriverConfig &config);
~ICM42670P() override;
static void print_usage();
void RunImpl();
@ -68,12 +69,12 @@ private: @@ -68,12 +69,12 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f}; // 8000 Hz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f}; // 1600 Hz accel & gyro ODR configured
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
// Transfer data
struct FIFOTransferBuffer {
@ -85,10 +86,8 @@ private: @@ -85,10 +86,8 @@ private:
uint8_t FIFO_COUNTL{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
struct register_bank0_config_t {
Register::BANK_0 reg;
@ -96,13 +95,17 @@ private: @@ -96,13 +95,17 @@ private:
uint8_t clear_bits{0};
};
struct register_mreg1_config_t {
Register::MREG1 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
bool Reset();
bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
@ -111,9 +114,13 @@ private: @@ -111,9 +114,13 @@ private:
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
uint8_t RegisterRead(Register::BANK_0 reg);
uint8_t RegisterRead(Register::MREG1 reg);
void RegisterWrite(Register::BANK_0 reg, uint8_t value);
void RegisterWrite(Register::MREG1 reg, uint8_t value);
template <typename T> bool RegisterCheck(const T &reg_cfg);
template <typename T> uint8_t RegisterRead(T reg);
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
@ -126,12 +133,6 @@ private: @@ -126,12 +133,6 @@ private:
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
uint8_t RegisterReadBank1(uint8_t reg);
void RegisterWriteBank1(uint8_t reg, uint8_t value);
void Mreg1Config();
bool Mreg1Check();
const spi_drdy_gpio_t _drdy_gpio;
PX4Accelerometer _px4_accel;
@ -149,9 +150,7 @@ private: @@ -149,9 +150,7 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -159,26 +158,32 @@ private: @@ -159,26 +158,32 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
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))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10};
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// 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::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 },
{ 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_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 },
{ Register::BANK_0::GYRO_CONFIG1, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS },
{ 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_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
};
uint8_t _checked_register_mreg1{0};
static constexpr uint8_t size_register_mreg1_cfg{2};
register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] {
// Register | Set bits, Clear bits
{ Register::MREG1::FIFO_CONFIG5, FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 },
{ Register::MREG1::INT_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
};
};

218
src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -66,21 +66,13 @@ namespace Register @@ -66,21 +66,13 @@ namespace Register
{
enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x01,
SIGNAL_PATH_RESET = 0x02,
INT_CONFIG = 0x06,
TEMP_DATA1 = 0x09,
TEMP_DATA0 = 0x0A,
INT_STATUS = 0x3A,
FIFO_COUNTH = 0x3D,
FIFO_COUNTL = 0x3E,
FIFO_DATA = 0x3F,
SIGNAL_PATH_RESET = 0x02,
PWR_MGMT0 = 0x1F,
GYRO_CONFIG0 = 0x20,
ACCEL_CONFIG0 = 0x21,
@ -90,203 +82,159 @@ enum class BANK_0 : uint8_t { @@ -90,203 +82,159 @@ enum class BANK_0 : uint8_t {
FIFO_CONFIG1 = 0x28,
FIFO_CONFIG2 = 0x29,
FIFO_CONFIG3 = 0x2A,
INT_SOURCE0 = 0x2B,
INT_STATUS = 0x3A,
FIFO_COUNTH = 0x3D,
FIFO_COUNTL = 0x3E,
FIFO_DATA = 0x3F,
WHO_AM_I = 0x75,
// REG_BANK_SEL = 0x76,
BLK_SEL_W = 0x79,
MADDR_W = 0x7A,
M_W = 0x7B,
BLK_SEL_R = 0x7C,
MADDR_R = 0x7D,
M_R = 0x7E,
};
enum class MREG_1 : uint8_t {
FIFO_CONFIG5_MREG1 = 0x01,
INT_CONFIG0_MREG1 = 0x04,
};
enum class MREG1 : uint8_t {
FIFO_CONFIG5 = 0x01,
enum class MREG_2 : uint8_t {
OTP_CTRL7_MREG2 = 0x06,
INT_CONFIG0 = 0x04,
};
enum class MREG_3 : uint8_t {
XA_ST_DATA_MREG3 = 0x00,
YA_ST_DATA_MREG3 = 0x01,
ZA_ST_DATA_MREG3 = 0x02,
XG_ST_DATA_MREG3 = 0x03,
YG_ST_DATA_MREG3 = 0x04,
ZG_ST_DATA_MREG3 = 0x05,
};
};
//---------------- BANK0 Register bits
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
SOFT_RESET_DEVICE_CONFIG = Bit4, //
FIFO_FLUSH = Bit2,
SOFT_RESET_DEVICE_CONFIG = Bit4, // 1: Software reset enabled
FIFO_FLUSH = Bit2, // When set to 1, FIFO will get flushed
};
// INT_CONFIG
enum INT_CONFIG_BIT : uint8_t {
INT1_MODE = Bit2,
INT1_DRIVE_CIRCUIT = Bit1,
INT1_POLARITY = Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
// 2:0 GYRO_ODR
GYRO_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz
GYRO_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz
GYRO_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz
GYRO_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz
GYRO_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz
GYRO_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz
GYRO_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
// 2:0 ACCEL_ODR
ACCEL_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz
ACCEL_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz
ACCEL_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz
ACCEL_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz
ACCEL_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz
ACCEL_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz
ACCEL_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
// 1 FIFO_MODE
FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
INT1_MODE = Bit2, // INT1 interrupt mode 1: Latched mode
INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull
INT1_POLARITY = Bit0, // INT1 interrupt polarity 0: Active low
};
// PWR_MGMT0
enum PWR_MGMT0_BIT : uint8_t {
// 3:2 GYRO_MODE
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
// 1:0 ACCEL_MODE
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
};
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 6:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps
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_2000_DPS_SET = 0, // 0b000 = ±2000dps
GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5,
// 3:0 GYRO_ODR
GYRO_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz
GYRO_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz
GYRO_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz
GYRO_ODR_200Hz = Bit3, // 1000: 200Hz
GYRO_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz
GYRO_ODR_1600HZ_CLEAR = Bit3 | Bit1,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 6:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g
ACCEL_FS_SEL_8G = Bit5, // 001: ±8g
ACCEL_FS_SEL_4G = Bit6, // 010: ±4g
ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g
// 6:5 ACCEL_UI_FS_SEL
ACCEL_UI_FS_SEL_16G_SET = 0, // 000: ±16g
ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5,
// 3:0 ACCEL_ODR
ACCEL_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz
ACCEL_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz
ACCEL_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz
ACCEL_ODR_200Hz = Bit3, // 1000: 200Hz
ACCEL_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz
ACCEL_ODR_1600HZ_CLEAR = Bit3 | Bit1,
};
// FIFO_CONFIG5
enum FIFO_CONFIG5_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit4,
FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit3,
FIFO_TMST_FSYNC_EN = Bit2,
FIFO_GYRO_EN = Bit1,
FIFO_ACCEL_EN = Bit0,
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
// 2:0 GYRO_UI_FILT_BW
GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
};
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
// 2:0 ACCEL_UI_FILT_BW
ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
// FIFO_MODE
FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
FIFO_BYPASS = Bit0, // 0: FIFO is not bypassed
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
ST_INT1_END = Bit7,
FSYNC_INT1_EN = Bit6,
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
DRDY_INT1_EN = Bit3,
RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
enum REG_BANK_SEL_BIT : uint8_t {
USER_BANK_0 = 0, // 0: Select USER BANK 0.
USER_BANK_1 = Bit4, // 1: Select USER BANK 1.
USER_BANK_2 = Bit5, // 2: Select USER BANK 2.
USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
//---------------- USER BANK MREG1 Register bits
// FIFO_CONFIG5
enum FIFO_CONFIG5_BIT : uint8_t {
FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM
FIFO_GYRO_EN = Bit1, // 1: Enables Gyro packets to go to FIFO
FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO
};
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
};
namespace FIFO
{
static constexpr size_t SIZE = 2048;
static constexpr size_t SIZE = 1024;
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
// Packet 3
struct DATA {
uint8_t FIFO_Header;
uint8_t ACCEL_DATA_X1;
uint8_t ACCEL_DATA_X0;
uint8_t ACCEL_DATA_Y1;
uint8_t ACCEL_DATA_Y0;
uint8_t ACCEL_DATA_Z1;
uint8_t ACCEL_DATA_Z0;
uint8_t GYRO_DATA_X1;
uint8_t GYRO_DATA_X0;
uint8_t GYRO_DATA_Y1;
uint8_t GYRO_DATA_Y0;
uint8_t GYRO_DATA_Z1;
uint8_t GYRO_DATA_Z0;
uint8_t ACCEL_DATA_X1; // Accel X [19:12]
uint8_t ACCEL_DATA_X0; // Accel X [11:4]
uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
uint8_t GYRO_DATA_X1; // Gyro X [19:12]
uint8_t GYRO_DATA_X0; // Gyro X [11:4]
uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
uint8_t temperature; // Temperature[7:0]
uint8_t timestamp_l;
uint8_t timestamp_h;
uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t TimeStamp_l; // TimeStamp[7:0]
};
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
enum FIFO_HEADER_BIT : uint8_t {
HEADER_MSG = Bit7, // 1: FIFO is empty
HEADER_ACCEL = Bit6,
HEADER_GYRO = Bit5,
HEADER_20 = Bit4,
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, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
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_ODR_ACCEL = Bit1,
HEADER_ODR_GYRO = Bit0,
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, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
};
}

Write Preview
Loading…
Cancel
Save