From 00e542591e0087961d9d21f688510ddb0a186936 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Mon, 12 Aug 2024 16:08:10 +0300 Subject: [PATCH] icm40609d: Change FIFO count to samples instead of bytes As the sensor can directly report the amount of samples in the fifo, use it to simplify the logic. Also combine the fifo empty/fifo overflow checks for interrupt and polling modes. Signed-off-by: Jukka Laitinen --- .../imu/invensense/icm40609d/ICM40609D.cpp | 89 ++++++------------- .../imu/invensense/icm40609d/ICM40609D.hpp | 2 +- 2 files changed, 29 insertions(+), 62 deletions(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 55db372d5d39..28fce0691ce9 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -203,57 +203,45 @@ void ICM40609D::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; - uint8_t samples = 0; + uint8_t samples = FIFOReadCount(); - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_timestamp_sample was set as expected - const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - - if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { - timestamp_sample = drdy_timestamp_sample; - samples = _fifo_gyro_samples; + if (samples == 0) { + perf_count(_fifo_empty_perf); - } else { - perf_count(_drdy_missed_perf); + } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; } - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { + 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; + } + } - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); + bool success = false; - } else { - // FIFO count (size in bytes) - samples = (fifo_count / sizeof(FIFO::DATA)); + if (samples > 0) { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - // tolerate minor jitter, leave sample to next iteration if behind by only 1 - if (samples == _fifo_gyro_samples + 1) { - timestamp_sample -= static_cast(FIFO_SAMPLE_DT); - samples--; - } + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_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; + } else { + perf_count(_drdy_missed_perf); } - } - } - bool success = false; + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } - if (samples >= 1) { if (FIFORead(timestamp_sample, samples)) { success = true; @@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) { - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); - for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { // FIFO_WM[7:0] FIFO_CONFIG2 - r.set_bits = fifo_watermark_threshold & 0xFF; + r.set_bits = samples & 0xFF; - } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { - // FIFO_WM[11:8] FIFO_CONFIG3 - r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; } } } @@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - 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(); - return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - - if (fifo_count_samples == 0) { - perf_count(_fifo_empty_perf); - return false; - } - // check FIFO header in every sample uint8_t valid_samples = 0; - for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + for (int i = 0; i < samples; i++) { bool valid = true; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 882e11a91ee8..8dee6c2fb793 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -164,7 +164,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver 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::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 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::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },