From 669d2ec10b64294ea5ba568a3c196b09225deee1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 25 Mar 2020 00:02:46 -0400 Subject: [PATCH] invensense/icm40609d driver minor improvements - change default FIFO empty rate to 1 kHz (consistent with other drivers) - relax retry timeout if configure failed - improve configured empty rate (sample rate) rounding - check interupt status and FIFO count as part of full transfer and reset or adjust timing if necessary --- .../imu/invensense/icm40609d/ICM40609D.cpp | 83 ++++++++++--------- .../imu/invensense/icm40609d/ICM40609D.hpp | 30 ++++--- 2 files changed, 59 insertions(+), 54 deletions(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 6c63872abc..c5aa4441e6 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -45,8 +45,8 @@ ICM40609D::ICM40609D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) + _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) { set_device_type(DRV_IMU_DEVTYPE_ICM40609D); @@ -142,10 +142,10 @@ void ICM40609D::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { - PX4_ERR("Reset failed, retrying"); + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } else { PX4_DEBUG("Reset not complete, check again in 10 ms"); @@ -203,13 +203,7 @@ void ICM40609D::RunImpl() // use the time now roughly corresponding with the last sample we'll pull from the FIFO timestamp_sample = hrt_absolute_time(); const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count == 0) { - failure = true; - perf_count(_fifo_empty_perf); - } - - samples = fifo_count / sizeof(FIFO::DATA); + samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest } if (samples > FIFO_MAX_SAMPLES) { @@ -218,18 +212,22 @@ void ICM40609D::RunImpl() failure = true; FIFOReset(); - } else if (samples >= 1) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + // require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer) if (!FIFORead(timestamp_sample, samples)) { failure = true; _px4_accel.increase_error_count(); _px4_gyro.increase_error_count(); } + + } else if (samples == 0) { + failure = true; + perf_count(_fifo_empty_perf); } if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { // check BANK_0 registers incrementally if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) { - _last_config_check_timestamp = timestamp_sample; _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; @@ -307,7 +305,7 @@ void ICM40609D::ConfigureGyro() case GYRO_FS_SEL_2000_DPS: _px4_gyro.set_scale(math::radians(1.f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + _px4_gyro.set_range(math::radians(2000.f)); break; } } @@ -315,26 +313,30 @@ void ICM40609D::ConfigureGyro() void ICM40609D::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 2000; // default to 2 kHz + sample_rate = 1000; // default to 1 kHz } - static constexpr float sample_min_interval = 1e6f / GYRO_RATE; + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); - _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / sample_min_interval) * sample_min_interval, - sample_min_interval); // round down to nearest sample_min_interval - - _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); + _fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES); // recompute FIFO empty interval (us) with actual gyro sample limit - _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); - _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); + _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) +{ // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { @@ -372,10 +374,10 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM40609D::DataReady() { + perf_count(_drdy_interval_perf); _fifo_watermark_interrupt_timestamp = hrt_absolute_time(); _fifo_read_samples.store(_fifo_gyro_samples); ScheduleNow(); - perf_count(_drdy_interval_perf); } bool ICM40609D::DataReadyInterruptConfigure() @@ -399,9 +401,10 @@ bool ICM40609D::DataReadyInterruptDisable() bool ICM40609D::RegisterCheck(const register_bank0_config_t ®_cfg, bool notify) { - const uint8_t reg_value = RegisterRead(reg_cfg.reg); bool success = true; + 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; @@ -455,16 +458,6 @@ void ICM40609D::RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, u RegisterWrite(reg, val); } -void ICM40609D::RegisterSetBits(Register::BANK_0 reg, uint8_t setbits) -{ - RegisterSetAndClearBits(reg, setbits, 0); -} - -void ICM40609D::RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits) -{ - RegisterSetAndClearBits(reg, 0, clearbits); -} - uint16_t ICM40609D::FIFOReadCount() { // read FIFO count @@ -506,8 +499,14 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) return false; } + if (fifo_count_bytes >= FIFO::SIZE) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + return false; + } + // check FIFO header in every sample - int valid_samples = 0; + uint16_t valid_samples = 0; for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { bool valid = true; @@ -558,7 +557,8 @@ void ICM40609D::FIFOReset() _fifo_read_samples.store(0); } -void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) +void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, + const uint8_t samples) { PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; @@ -585,7 +585,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTran _px4_accel.updateFIFO(accel); } -void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) +void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, + const uint8_t samples) { PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 074005dc0b..55c25be5dc 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -76,22 +76,24 @@ protected: void custom_method(const BusCLIArguments &cli) override; void exit_and_cleanup() override; private: - // Sensor Configuration - static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro - static constexpr uint32_t ACCEL_RATE{8000}; // 8 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + static constexpr float FIFO_SAMPLE_DT{125.f}; + static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer + static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro + static constexpr float ACCEL_RATE{GYRO_RATE}; // 8 kHz accel + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; // Transfer data struct FIFOTransferBuffer { uint8_t cmd{static_cast(Register::BANK_0::INT_STATUS) | DIR_READ}; - uint8_t INT_STATUS; - uint8_t FIFO_COUNTH; - uint8_t FIFO_COUNTL; + uint8_t INT_STATUS{0}; + uint8_t FIFO_COUNTH{0}; + uint8_t FIFO_COUNTL{0}; FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); struct register_bank0_config_t { Register::BANK_0 reg; @@ -105,6 +107,7 @@ private: void ConfigureAccel(); void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); + void ConfigureFIFOWatermark(uint8_t samples); static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); @@ -112,18 +115,19 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_bank0_config_t ®_cfg, bool notify = false); + uint8_t RegisterRead(Register::BANK_0 reg); void RegisterWrite(Register::BANK_0 reg, uint8_t value); void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits); - void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits); - void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits); + void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } uint16_t FIFOReadCount(); bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); void FIFOReset(); - void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); - void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); void UpdateTemperature(); const spi_drdy_gpio_t _drdy_gpio; @@ -156,7 +160,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};