diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index a57461aa0c..34965c351b 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -45,8 +45,8 @@ ICM20602::ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro 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_ICM20602); @@ -131,7 +131,7 @@ void ICM20602::RunImpl() RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); _reset_timestamp = hrt_absolute_time(); _state = STATE::WAIT_FOR_RESET; - ScheduleDelayed(100); + ScheduleDelayed(1_ms); break; case STATE::WAIT_FOR_RESET: @@ -148,14 +148,14 @@ void ICM20602::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 1 ms"); - ScheduleDelayed(1_ms); + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); } } @@ -181,8 +181,8 @@ void ICM20602::RunImpl() } else { PX4_DEBUG("Configure failed, retrying"); - // try again in 1 ms - ScheduleDelayed(1_ms); + // try again in 10 ms + ScheduleDelayed(10_ms); } break; @@ -209,13 +209,7 @@ void ICM20602::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) / 2) * 2; // round down to nearest 2 + samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest } if (samples > FIFO_MAX_SAMPLES) { @@ -224,13 +218,17 @@ void ICM20602::RunImpl() failure = true; FIFOReset(); - } else if (samples >= 2) { - // require at least 2 samples (we want at least 1 new accel sample per transfer) + } 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) { @@ -258,22 +256,22 @@ void ICM20602::ConfigureAccel() switch (ACCEL_FS_SEL) { case ACCEL_FS_SEL_2G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); _px4_accel.set_range(2 * CONSTANTS_ONE_G); break; case ACCEL_FS_SEL_4G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192); + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); _px4_accel.set_range(4 * CONSTANTS_ONE_G); break; case ACCEL_FS_SEL_8G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); _px4_accel.set_range(8 * CONSTANTS_ONE_G); break; case ACCEL_FS_SEL_16G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); _px4_accel.set_range(16 * CONSTANTS_ONE_G); break; } @@ -285,23 +283,23 @@ void ICM20602::ConfigureGyro() switch (FS_SEL) { case FS_SEL_250_DPS: - _px4_gyro.set_scale(math::radians(1.0f / 131.f)); + _px4_gyro.set_scale(math::radians(1.f / 131.f)); _px4_gyro.set_range(math::radians(250.f)); break; case FS_SEL_500_DPS: - _px4_gyro.set_scale(math::radians(1.0f / 65.5f)); + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); _px4_gyro.set_range(math::radians(500.f)); break; case FS_SEL_1000_DPS: - _px4_gyro.set_scale(math::radians(1.0f / 32.8f)); - _px4_gyro.set_range(math::radians(1000.0f)); + _px4_gyro.set_scale(math::radians(1.f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.f)); break; case FS_SEL_2000_DPS: - _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + _px4_gyro.set_scale(math::radians(1.f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.f)); break; } } @@ -312,19 +310,27 @@ void ICM20602::ConfigureSampleRate(int sample_rate) sample_rate = 1000; // default to 1 kHz } - _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); + // 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_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 ICM20602::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_cfg) { if (r.reg == Register::FIFO_WM_TH1) { @@ -360,10 +366,10 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20602::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 ICM20602::DataReadyInterruptConfigure() @@ -372,8 +378,8 @@ bool ICM20602::DataReadyInterruptConfigure() return false; } - // Setup data ready on rising edge - return px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &ICM20602::DataReadyInterruptCallback, this) == 0; + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM20602::DataReadyInterruptCallback, this) == 0; } bool ICM20602::DataReadyInterruptDisable() @@ -391,12 +397,12 @@ bool ICM20602::RegisterCheck(const register_config_t ®_cfg, bool notify) const uint8_t reg_value = RegisterRead(reg_cfg.reg); - if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) { + 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)) { + 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; } @@ -404,13 +410,6 @@ bool ICM20602::RegisterCheck(const register_config_t ®_cfg, bool notify) if (!success) { RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); - if (reg_cfg.reg == Register::ACCEL_CONFIG) { - ConfigureAccel(); - - } else if (reg_cfg.reg == Register::GYRO_CONFIG) { - ConfigureGyro(); - } - if (notify) { perf_count(_bad_register_perf); _px4_accel.increase_error_count(); @@ -451,16 +450,6 @@ void ICM20602::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cl RegisterWrite(reg, val); } -void ICM20602::RegisterSetBits(Register reg, uint8_t setbits) -{ - RegisterSetAndClearBits(reg, setbits, 0); -} - -void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits) -{ - RegisterSetAndClearBits(reg, 0, clearbits); -} - uint16_t ICM20602::FIFOReadCount() { // read FIFO count @@ -516,9 +505,8 @@ void ICM20602::FIFOReset() // FIFO_EN: disable FIFO RegisterWrite(Register::FIFO_EN, 0); - // 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); + // USER_CTRL: reset FIFO + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled _fifo_watermark_interrupt_timestamp = 0; @@ -538,7 +526,8 @@ 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 FIFOTransferBuffer &buffer, uint8_t samples) +bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, + const uint8_t samples) { PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; @@ -549,8 +538,8 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans // accel data is doubled in FIFO, but might be shifted int accel_first_sample = 1; - if (samples >= 3) { - if (fifo_accel_equal(buffer.f[0], buffer.f[1])) { + if (samples >= 4) { + if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) { // [A0, A1, A2, A3] // A0==A1, A2==A3 accel_first_sample = 1; @@ -589,7 +578,7 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans return !bad_data; } -void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) +void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; @@ -613,7 +602,7 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransf _px4_gyro.updateFIFO(gyro); } -bool ICM20602::ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples) +bool ICM20602::ProcessTemperature(const FIFOTransferBuffer &buffer, const uint8_t samples) { int16_t temperature[samples]; diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index edbcd6a8e1..563af1fbda 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -76,11 +76,13 @@ 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{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]))}; + static constexpr float FIFO_SAMPLE_DT{125.f}; + static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro + static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 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 { @@ -88,7 +90,7 @@ private: FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); struct register_config_t { Register reg; @@ -102,6 +104,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(); @@ -113,16 +116,16 @@ private: uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); - void RegisterSetBits(Register reg, uint8_t setbits); - void RegisterClearBits(Register reg, uint8_t clearbits); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } uint16_t FIFOReadCount(); bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); void FIFOReset(); - bool 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); - bool ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples); + bool 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); + bool ProcessTemperature(const FIFOTransferBuffer &buffer, const uint8_t samples); const spi_drdy_gpio_t _drdy_gpio; @@ -144,7 +147,6 @@ private: px4::atomic _fifo_read_samples{0}; bool _data_ready_interrupt_enabled{false}; - uint8_t _checked_register{0}; enum class STATE : uint8_t { RESET, @@ -155,11 +157,12 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 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))}; - static constexpr uint8_t size_register_cfg{11}; + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{12}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, @@ -167,11 +170,12 @@ private: { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 }, { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF }, - { Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE }, + { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 }, { Register::FIFO_WM_TH1, 0, 0 }, // FIFO_WM_TH[9:8] { Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0] - { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST }, { Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 }, + { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL | INT_PIN_CFG_BIT::LATCH_INT_EN | INT_PIN_CFG_BIT::INT_RD_CLEAR, 0 }, { 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 24e504dd13..96738d5bd7 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -70,6 +70,7 @@ enum class Register : uint8_t { FIFO_EN = 0x23, + INT_PIN_CFG = 0x37, INT_ENABLE = 0x38, TEMP_OUT_H = 0x41, @@ -128,6 +129,14 @@ enum FIFO_EN_BIT : uint8_t { ACCEL_FIFO_EN = Bit3, }; +// INT_PIN_CFG +enum INT_PIN_CFG_BIT : uint8_t { + INT_LEVEL = Bit7, + + LATCH_INT_EN = Bit5, + INT_RD_CLEAR = Bit4, +}; + // INT_ENABLE enum INT_ENABLE_BIT : uint8_t { FIFO_OFLOW_EN = Bit4,