From afc59f843c94d1c811c6cc9bce111d11ecbd3e00 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 22 Feb 2020 01:53:00 -0500 Subject: [PATCH] invensense icm20602 improvements - refactor Run() into simple state machine - perform reset and configuration in sensor bus thread - when using data ready interrupt skip checking FIFO count --- boards/px4/fmu-v5/init/rc.board_sensors | 2 + .../imu/invensense/icm20602/CMakeLists.txt | 1 + .../imu/invensense/icm20602/ICM20602.cpp | 587 +++++++++++------- .../imu/invensense/icm20602/ICM20602.hpp | 73 ++- .../InvenSense_ICM20602_registers.hpp | 3 +- .../accelerometer/PX4Accelerometer.hpp | 3 +- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 3 +- 7 files changed, 409 insertions(+), 263 deletions(-) diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index c3f18514cb..6f122d3e2d 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -7,9 +7,11 @@ adc start # Internal SPI bus ICM-20602 mpu6000 -R 8 -s -T 20602 start +#icm20602 # Internal SPI bus ICM-20689 mpu6000 -R 8 -z -T 20689 start +#icm20689 start # Internal SPI bus BMI055 accel bmi055 -A -R 10 start diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt index 8b3fcf4050..cd14e15674 100644 --- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( ICM20602.cpp ICM20602.hpp icm20602_main.cpp + InvenSense_ICM20602_registers.hpp DEPENDS drivers_accelerometer drivers_gyroscope diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index e2117cc871..9e6508935c 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) -{ - return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); -} - ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), @@ -54,8 +49,11 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) { set_device_type(DRV_ACC_DEVTYPE_ICM20602); + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602); + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); } ICM20602::~ICM20602() @@ -75,36 +73,57 @@ ICM20602::~ICM20602() perf_free(_drdy_interval_perf); } -void ICM20602::ConfigureSampleRate(int sample_rate) +bool ICM20602::Init() { - if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init failed"); + return false; } - sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz + // allocate DMA capable buffer + _dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE); - _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us - _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); + if (_dma_data_buffer == nullptr) { + PX4_ERR("DMA alloc failed"); + return false; + } - // recompute FIFO empty interval (us) with actual gyro sample limit - _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); + return Reset(); +} - _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); +void ICM20602::Stop() +{ + // wait until stopped + while (_state.load() != STATE::STOPPED) { + _state.store(STATE::REQUEST_STOP); + ScheduleNow(); + px4_usleep(10); + } +} - _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); +bool ICM20602::Reset() +{ + _state.store(STATE::RESET); + ScheduleClear(); + ScheduleNow(); + return true; +} - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); +void ICM20602::PrintInfo() +{ + PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, + static_cast(1000000 / _fifo_empty_interval_us)); - for (auto &r : _register_cfg) { - if (r.reg == Register::FIFO_WM_TH1) { - r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011; + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_interval_perf); - } else if (r.reg == Register::FIFO_WM_TH2) { - r.set_bits = fifo_watermark_threshold & 0xFF; - } - } + _px4_accel.print_status(); + _px4_gyro.print_status(); } int ICM20602::probe() @@ -119,47 +138,143 @@ int ICM20602::probe() return PX4_OK; } -bool ICM20602::Init() +void ICM20602::Run() { - if (SPI::init() != PX4_OK) { - PX4_ERR("SPI::init failed"); - return false; - } - - // allocate DMA capable buffer - _dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE); - - if (_dma_data_buffer == nullptr) { - PX4_ERR("DMA alloc failed"); - return false; - } - - if (!Reset()) { - PX4_ERR("reset failed"); - return false; - } - - Start(); - - return true; -} + switch (_state.load()) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + _reset_timestamp = hrt_absolute_time(); + _state.store(STATE::WAIT_FOR_RESET); + ScheduleDelayed(100); + break; -bool ICM20602::Reset() -{ - // PWR_MGMT_1: Device Reset - RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + case STATE::WAIT_FOR_RESET: - for (int i = 0; i < 100; i++) { // The reset value is 0x00 for all registers other than the registers below // Document Number: DS-000176 Page 31 of 57 if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x41) && (RegisterRead(Register::CONFIG) == 0x80)) { - return true; + + // if reset succeeded then configure + _state.store(STATE::CONFIGURE); + ScheduleNow(); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { + PX4_ERR("Reset failed, retrying"); + _state.store(STATE::RESET); + ScheduleDelayed(10_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 1 ms"); + ScheduleDelayed(1_ms); + } } - } - return false; + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state.store(STATE::FIFO_READ); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + PX4_DEBUG("Configure failed, retrying"); + // try again in 1 ms + ScheduleDelayed(1_ms); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // re-schedule as watchdog timeout + ScheduleDelayed(10_ms); + + // timestamp set in data ready interrupt + samples = _fifo_read_samples.load(); + timestamp_sample = _fifo_watermark_interrupt_timestamp; + } + + bool failure = false; + + // manually check FIFO count if no samples from DRDY or timestamp looks bogus + if (!_data_ready_interrupt_enabled || (samples == 0) + || (hrt_elapsed_time(×tamp_sample) > (_fifo_empty_interval_us / 2))) { + + // 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) / 2) * 2; // round down to nearest 2 + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + failure = true; + FIFOReset(); + + } else if (samples >= 2) { + // require at least 2 samples (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(); + } + } + + if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + // check registers incrementally + if (RegisterCheck(_register_cfg[_checked_register], true)) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reconfigure + PX4_DEBUG("Health check failed, reconfiguring"); + _state.store(STATE::CONFIGURE); + ScheduleNow(); + } + } + } + + break; + + case STATE::REQUEST_STOP: + DataReadyInterruptDisable(); + ScheduleClear(); + _state.store(STATE::STOPPED); + break; + + case STATE::STOPPED: + // DO NOTHING + break; + } } void ICM20602::ConfigureAccel() @@ -216,54 +331,126 @@ void ICM20602::ConfigureGyro() } } -void ICM20602::ResetFIFO() +void ICM20602::ConfigureSampleRate(int sample_rate) { - perf_count(_fifo_reset_perf); + if (sample_rate == 0) { + sample_rate = 1000; // default to 1 kHz + } - // USER_CTRL: disable FIFO and reset all signal paths - RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST, - USER_CTRL_BIT::FIFO_EN); + _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us + _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); - // FIFO_EN: enable both gyro and accel - RegisterSetBits(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); - // USER_CTRL: re-enable FIFO - RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, - USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST); + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); + + _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); + + // FIFO watermark threshold in number of bytes + const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_WM_TH1) { + r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011; + + } else if (r.reg == Register::FIFO_WM_TH2) { + r.set_bits = fifo_watermark_threshold & 0xFF; + } + } } -bool ICM20602::Configure(bool notify) +bool ICM20602::Configure() { bool success = true; for (const auto ® : _register_cfg) { - if (!CheckRegister(reg, notify)) { + if (!RegisterCheck(reg)) { success = false; } } + ConfigureAccel(); + ConfigureGyro(); + return success; } -bool ICM20602::CheckRegister(const register_config_t ®_cfg, bool notify) +int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ICM20602::DataReady() +{ + _fifo_watermark_interrupt_timestamp = hrt_absolute_time(); + _fifo_read_samples.store(_fifo_gyro_samples); + ScheduleNow(); + perf_count(_drdy_interval_perf); +} + +bool ICM20602::DataReadyInterruptConfigure() +{ + int ret_setevent = -1; + + // Setup data ready on rising edge + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, + this); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, + this); +#elif defined(GPIO_SPI1_DRDY4_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, + this); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, + this); +#elif defined(GPIO_DRDY_ICM_2060X) + ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, + this); +#endif + + return (ret_setevent == 0); +} + +bool ICM20602::DataReadyInterruptDisable() +{ + int ret_setevent = -1; + + // Disable data ready callback + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); +#elif defined(GPIO_SPI1_DRDY4_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); +#elif defined(GPIO_DRDY_ICM_2060X) + ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); +#endif + + return (ret_setevent == 0); +} + +bool ICM20602::RegisterCheck(const register_config_t ®_cfg, bool notify) { bool success = true; const uint8_t reg_value = RegisterRead(reg_cfg.reg); if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) { - if (notify) { - PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, 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)) { - if (notify) { - PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); - } - + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); success = false; } @@ -279,6 +466,8 @@ bool ICM20602::CheckRegister(const register_config_t ®_cfg, bool notify) if (notify) { perf_count(_bad_register_perf); + _px4_accel.increase_error_count(); + _px4_gyro.increase_error_count(); } } @@ -325,170 +514,96 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits) RegisterSetAndClearBits(reg, 0, clearbits); } -int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) -{ - ICM20602 *dev = reinterpret_cast(arg); - dev->DataReady(); - return 0; -} - -void ICM20602::DataReady() -{ - perf_count(_drdy_interval_perf); - - // make another measurement - ScheduleNow(); -} - -void ICM20602::Start() +uint16_t ICM20602::FIFOReadCount() { - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); - - // attempt to configure 3 times - for (int i = 0; i < 3; i++) { - if (Configure(false)) { - break; - } - } - - // TODO: cleanup horrible DRDY define mess -#if defined(GPIO_DRDY_PORTC_PIN14) - _using_data_ready_interrupt_enabled = true; - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - _using_data_ready_interrupt_enabled = true; - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); -#elif defined(GPIO_SPI1_DRDY4_ICM20602) - _using_data_ready_interrupt_enabled = true; - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - _using_data_ready_interrupt_enabled = true; - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); -#elif defined(GPIO_DRDY_ICM_2060X) - _using_data_ready_interrupt_enabled = true; - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this); -#else - _using_data_ready_interrupt_enabled = false; - ScheduleOnInterval(FIFO_EMPTY_INTERVAL_US, FIFO_EMPTY_INTERVAL_US); -#endif - - ResetFIFO(); + // read FIFO count + uint8_t fifo_count_buf[3] {}; + fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; - // schedule as watchdog - if (_using_data_ready_interrupt_enabled) { - ScheduleDelayed(100_ms); + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; } -} - -void ICM20602::Stop() -{ - Reset(); - // TODO: cleanup horrible DRDY define mess -#if defined(GPIO_DRDY_PORTC_PIN14) - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY4_ICM20602) - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_DRDY_ICM_2060X) - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); -#endif - - ScheduleClear(); + return combine(fifo_count_buf[1], fifo_count_buf[2]); } -void ICM20602::Run() +bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) { - // use the time now roughly corresponding with the last sample we'll pull from the FIFO - const hrt_abstime timestamp_sample = hrt_absolute_time(); + TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + memset(report, 0, transfer_size); + report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; - // read FIFO count - uint8_t fifo_count_buf[3] {}; - fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; + perf_begin(_transfer_perf); - if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); perf_count(_bad_transfer_perf); + return false; } - if (_using_data_ready_interrupt_enabled) { - // re-schedule as watchdog - ScheduleDelayed(100_ms); - } + perf_end(_transfer_perf); - // check registers - if (hrt_elapsed_time(&_last_config_check) > 100_ms) { - _checked_register = (_checked_register + 1) % size_register_cfg; + bool bad_data = false; - if (CheckRegister(_register_cfg[_checked_register])) { - // delay next register check if current succeeded - _last_config_check = hrt_absolute_time(); + ProcessGyro(timestamp_sample, report, samples); - } else { - // if register check failed reconfigure all - Configure(); - ResetFIFO(); - return; - } + if (!ProcessAccel(timestamp_sample, report, samples)) { + bad_data = true; } - const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 + // limit temperature updates to 1 Hz + if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) { + _temperature_update_timestamp = timestamp_sample; - if (samples < 2) { - perf_count(_fifo_empty_perf); - return; + if (!ProcessTemperature(report, samples)) { + bad_data = true; + } + } - } else if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - perf_count(_fifo_overflow_perf); - ResetFIFO(); + return !bad_data; +} - return; - } +void ICM20602::FIFOReset() +{ + perf_count(_fifo_reset_perf); - // Transfer data - struct TransferBuffer { - uint8_t cmd; - FIFO::DATA f[FIFO_MAX_SAMPLES]; - }; - // ensure no struct padding - static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA))); + // FIFO_EN: disable FIFO + RegisterWrite(Register::FIFO_EN, 0); - TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; - const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); - memset(report, 0, transfer_size); - report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; + // USER_CTRL: disable FIFO and reset all signal paths + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST, + USER_CTRL_BIT::FIFO_EN); - perf_begin(_transfer_perf); + // reset while FIFO is disabled + _fifo_watermark_interrupt_timestamp = 0; + _fifo_read_samples.store(0); - if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { - perf_end(_transfer_perf); - perf_count(_bad_transfer_perf); - return; + // FIFO_EN: enable both gyro and accel + // USER_CTRL: re-enable FIFO + for (const auto &r : _register_cfg) { + if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) { + RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); + } } +} - perf_end(_transfer_perf); - +static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) +{ + return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); +} +bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples) +{ PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + bool bad_data = false; + // accel data is doubled in FIFO, but might be shifted - int accel_first_sample = 0; + int accel_first_sample = 1; if (samples >= 3) { if (fifo_accel_equal(report->f[0], report->f[1])) { @@ -503,7 +618,7 @@ void ICM20602::Run() } else { perf_count(_bad_transfer_perf); - return; + bad_data = true; } } @@ -515,7 +630,8 @@ void ICM20602::Run() int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); - // sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down) + // 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; @@ -524,30 +640,44 @@ void ICM20602::Run() accel.samples = accel_samples; + _px4_accel.updateFIFO(accel); + + return !bad_data; +} +void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples) +{ PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; - int16_t temperature[samples]; - for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = report->f[i]; - temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L); - const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); - // sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down) + // 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; } - // Temperature + _px4_gyro.updateFIFO(gyro); +} + +bool ICM20602::ProcessTemperature(const TransferBuffer *const report, uint8_t samples) +{ + int16_t temperature[samples]; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L); + } + int32_t temperature_sum{0}; for (auto t : temperature) { @@ -560,7 +690,7 @@ void ICM20602::Run() // temperature changing wildly is an indication of a transfer error if (fabsf(t - temperature_avg) > 1000) { perf_count(_bad_transfer_perf); - return; + return false; } } @@ -569,24 +699,5 @@ void ICM20602::Run() _px4_accel.set_temperature(temperature_C); _px4_gyro.set_temperature(temperature_C); - - _px4_gyro.updateFIFO(gyro); - _px4_accel.updateFIFO(accel); -} - -void ICM20602::PrintInfo() -{ - PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, - static_cast(1000000 / _fifo_empty_interval_us)); - - perf_print_counter(_transfer_perf); - perf_print_counter(_bad_register_perf); - perf_print_counter(_bad_transfer_perf); - perf_print_counter(_fifo_empty_perf); - perf_print_counter(_fifo_overflow_perf); - perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_interval_perf); - - _px4_accel.print_status(); - _px4_gyro.print_status(); + return true; } diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index 5ed72d95f9..d3dc2e4041 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include using namespace InvenSense_ICM20602; @@ -66,6 +67,19 @@ public: private: + // Sensor Configuration + static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro + static constexpr uint32_t ACCEL_RATE{4000}; // 4 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]))}; + + // Transfer data + struct TransferBuffer { + uint8_t cmd; + FIFO::DATA f[FIFO_MAX_SAMPLES]; + }; + // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + struct register_config_t { Register reg; uint8_t set_bits{0}; @@ -74,17 +88,19 @@ private: int probe() override; - static int DataReadyInterruptCallback(int irq, void *context, void *arg); - void DataReady(); - void Run() override; - void ConfigureSampleRate(int sample_rate); - bool CheckRegister(const register_config_t ®_cfg, bool notify = true); - bool Configure(bool notify = true); - + bool Configure(); void ConfigureAccel(); void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -92,7 +108,13 @@ private: void RegisterSetBits(Register reg, uint8_t setbits); void RegisterClearBits(Register reg, uint8_t clearbits); - void ResetFIFO(); + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); + void FIFOReset(); + + bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples); + bool ProcessTemperature(const TransferBuffer *const report, uint8_t samples); uint8_t *_dma_data_buffer{nullptr}; @@ -102,21 +124,30 @@ private: perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; - perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")}; - perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")}; - perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")}; - perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; - - hrt_abstime _last_config_check{0}; - + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; + perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _fifo_watermark_interrupt_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + + px4::atomic _fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; uint8_t _checked_register{0}; - bool _using_data_ready_interrupt_enabled{false}; + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + REQUEST_STOP, + STOPPED, + }; - // Sensor Configuration - static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro - static constexpr uint32_t ACCEL_RATE{4000}; // 4 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]))}; + px4::atomic _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; @@ -135,6 +166,6 @@ private: { Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0] { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, { Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 }, - { Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN } + { Register::INT_ENABLE, 0, INT_ENABLE_BIT::DATA_RDY_INT_EN } }; }; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index 9391c5c1df..24e504dd13 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -54,8 +54,7 @@ static constexpr uint8_t Bit7 = (1 << 7); namespace InvenSense_ICM20602 { -static constexpr uint32_t SPI_SPEED = 10 * 1000 * - 1000; // 10MHz SPI serial interface for communicating with all registers +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x12; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index ceb7ac9844..e4d6b06786 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -58,7 +58,8 @@ public: void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); - void set_error_count(uint64_t error_count) { _error_count += error_count; } + void set_error_count(uint64_t error_count) { _error_count = error_count; } + void increase_error_count() { _error_count++; } void set_range(float range) { _range = range; UpdateClipLimit(); } void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_temperature(float temperature) { _temperature = temperature; } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 22975c2d16..12198f49c3 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -60,7 +60,8 @@ public: void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); - void set_error_count(uint64_t error_count) { _error_count += error_count; } + void set_error_count(uint64_t error_count) { _error_count = error_count; } + void increase_error_count() { _error_count++; } void set_range(float range) { _range = range; UpdateClipLimit(); } void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_temperature(float temperature) { _temperature = temperature; }