From 0459481cb4e4e22912fa9d29bea2a1385bf05ea6 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 | 95 ++++++------------- .../imu/invensense/icm40609d/ICM40609D.hpp | 2 +- 2 files changed, 32 insertions(+), 65 deletions(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 55db372d5d..28fce0691c 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; - - 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; - - } else { - perf_count(_drdy_missed_perf); - } - - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } + uint8_t samples = FIFOReadCount(); if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); + perf_count(_fifo_empty_perf); - if (fifo_count >= FIFO::SIZE) { + } 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--; + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); - - } else { - // 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(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; - } + samples = 0; } } bool success = false; - if (samples >= 1) { + 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); + + 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 + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + 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 882e11a91e..8dee6c2fb7 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -164,7 +164,7 @@ private: 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 },