diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 3aa2326c0c..d2e0a2d5e9 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -36,17 +36,16 @@ #include using namespace time_literals; -using namespace InvenSense_ICM20689; -static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } - -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_INTERVAL{1000}; // 1000 us / 1000 Hz interval +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} -static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)}; -static constexpr uint32_t FIFO_ACCEL_SAMPLES{FIFO_INTERVAL / (1000000 / ACCEL_RATE)}; +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); +} ICM20689::ICM20689(int bus, uint32_t device, enum Rotation rotation) : SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), @@ -57,9 +56,6 @@ ICM20689::ICM20689(int bus, uint32_t device, enum Rotation rotation) : set_device_type(DRV_ACC_DEVTYPE_ICM20689); _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20689); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20689); - - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); - _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } ICM20689::~ICM20689() @@ -71,12 +67,34 @@ ICM20689::~ICM20689() } perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); perf_free(_drdy_interval_perf); } +void ICM20689::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1 kHz + } + + sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz + + _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); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); + + _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); +} + int ICM20689::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -96,11 +114,6 @@ bool ICM20689::Init() return false; } - if (!Reset()) { - PX4_ERR("reset failed"); - return false; - } - // allocate DMA capable buffer _dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE); @@ -109,6 +122,11 @@ bool ICM20689::Init() return false; } + if (!Reset()) { + PX4_ERR("reset failed"); + return false; + } + Start(); return true; @@ -117,61 +135,146 @@ bool ICM20689::Init() bool ICM20689::Reset() { // PWR_MGMT_1: Device Reset - // CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); - usleep(1000); - // PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. - RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); - usleep(1000); + for (int i = 0; i < 10; i++) { + // The reset value is 0x00 for all registers other than the registers below + // Document Number: RM-000030 Page 5 of 23 + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { + return true; + } - // ACCEL_CONFIG: Accel 16 G range - RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G); - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); - _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + usleep(1); + } - // GYRO_CONFIG: Gyro 2000 degrees/second - RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS); - _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + return false; +} - // reset done once data is ready - const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::DEVICE_RESET); - const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0); - const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT); +void ICM20689::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] + + switch (ACCEL_FS_SEL) { + case ACCEL_FS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _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_range(4 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _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_range(16 * CONSTANTS_ONE_G); + break; + } +} - return reset_done && clksel_done && data_ready; +void ICM20689::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0] + + switch (GYRO_FS_SEL) { + case FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 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_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)); + break; + + case FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + break; + } } void ICM20689::ResetFIFO() { perf_count(_fifo_reset_perf); - // ACCEL_CONFIG2: Accel DLPF disabled for full rate (4 kHz), Clear FIFO_SIZE bits for 512 B FIFO - RegisterSetBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF); - RegisterClearBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::FIFO_SIZE); - - // GYRO_CONFIG: Gyro DLPF disabled for full rate (8 kHz) - RegisterClearBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF); - - // FIFO_EN: disable FIFO - RegisterWrite(Register::FIFO_EN, 0); - RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RST); + // 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 then re-enable - RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST); - up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock - RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN); - - // CONFIG: should ensure that bit 7 of register 0x1A is set to 0 before using FIFO watermark feature - RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE); - RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + _data_ready_count.store(0); // FIFO_EN: enable both gyro and accel - _data_ready_count = 0; RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); - up_udelay(10); + + // 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); +} + +bool ICM20689::Configure(bool notify) +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!CheckRegister(reg, notify)) { + success = false; + } + } + + return success; +} + +bool ICM20689::CheckRegister(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); + } + + 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); + } + + success = false; + } + + 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); + } + } + + return success; } uint8_t ICM20689::RegisterRead(Register reg) @@ -188,24 +291,30 @@ void ICM20689::RegisterWrite(Register reg, uint8_t value) transfer(cmd, cmd, sizeof(cmd)); } -void ICM20689::RegisterSetBits(Register reg, uint8_t setbits) +void ICM20689::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) { - uint8_t val = RegisterRead(reg); + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = orig_val; - if (!(val & setbits)) { + if (setbits) { val |= setbits; - RegisterWrite(reg, val); } + + if (clearbits) { + val &= ~clearbits; + } + + RegisterWrite(reg, val); } -void ICM20689::RegisterClearBits(Register reg, uint8_t clearbits) +void ICM20689::RegisterSetBits(Register reg, uint8_t setbits) { - uint8_t val = RegisterRead(reg); + RegisterSetAndClearBits(reg, setbits, 0); +} - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); - } +void ICM20689::RegisterClearBits(Register reg, uint8_t clearbits) +{ + RegisterSetAndClearBits(reg, 0, clearbits); } int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -219,81 +328,112 @@ void ICM20689::DataReady() { perf_count(_drdy_interval_perf); - _data_ready_count++; - - if (_data_ready_count >= 8) { - _time_data_ready = hrt_absolute_time(); - - _data_ready_count = 0; - + if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) { // make another measurement ScheduleNow(); + _data_ready_count.store(0); } } void ICM20689::Start() { - Stop(); + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); - ResetFIFO(); + // 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_SPI1_DRDY1_ICM20689) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20689, true, false, true, &ICM20689::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); #else + _using_data_ready_interrupt_enabled = false; ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); #endif + + ResetFIFO(); + + // schedule as watchdog + if (_using_data_ready_interrupt_enabled) { + ScheduleDelayed(100_ms); + } } void ICM20689::Stop() { + Reset(); + // TODO: cleanup horrible DRDY define mess #if defined(GPIO_SPI1_DRDY1_ICM20689) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20689, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); -#else - ScheduleClear(); #endif + + ScheduleClear(); } void ICM20689::Run() { - // use timestamp from the data ready interrupt if available, - // otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO - const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready : - hrt_absolute_time(); + // use the time now roughly corresponding with the last sample we'll pull from the FIFO + const hrt_abstime timestamp_sample = hrt_absolute_time(); // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { - return; + perf_count(_bad_transfer_perf); + } + + if (_using_data_ready_interrupt_enabled) { + // re-schedule as watchdog + ScheduleDelayed(100_ms); } - const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); - const int samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 + // check registers + if (hrt_elapsed_time(&_last_config_check) > 100_ms) { + _checked_register = (_checked_register + 1) % size_register_cfg; + + if (CheckRegister(_register_cfg[_checked_register])) { + // delay next register check if current succeeded + _last_config_check = hrt_absolute_time(); + + } else { + // if register check failed reconfigure all + Configure(); + ResetFIFO(); + return; + } + } + + // FIFO_COUNTH (FIFO_COUNT[12:8]) + FIFO_COUNTL (FIFO_COUNT[7:0]) + const uint16_t fifo_count = combine(fifo_count_buf[1] & 0x1F, fifo_count_buf[2]); + const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 if (samples < 2) { perf_count(_fifo_empty_perf); return; - } else if (samples > 16) { - // not technically an overflow, but more samples than we expected + } 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; } // Transfer data struct TransferBuffer { uint8_t cmd; - FIFO::DATA f[16]; // max 16 samples + FIFO::DATA f[FIFO_MAX_SAMPLES]; }; - static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding + // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA))); TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); @@ -304,43 +444,73 @@ void ICM20689::Run() if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); return; } perf_end(_transfer_perf); + PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; - accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES; + accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + + // accel data is doubled in FIFO, but might be shifted + int accel_first_sample = 0; + + if (samples >= 3) { + if (fifo_accel_equal(report->f[0], report->f[1])) { + // [A0, A1, A2, A3] + // A0==A1, A2==A3 + accel_first_sample = 1; + + } else if (fifo_accel_equal(report->f[1], report->f[2])) { + // [A0, A1, A2, A3] + // A0, A1==A2, A3 + accel_first_sample = 0; + + } else { + perf_count(_bad_transfer_perf); + return; + } + } + + int accel_samples = 0; + + for (int i = accel_first_sample; i < samples; i = i + 2) { + const FIFO::DATA &fifo_sample = report->f[i]; + int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); + 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) + 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; + accel_samples++; + } + + accel.samples = accel_samples; + PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; - gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; - - int accel_samples = 0; + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = report->f[i]; - // accel data is doubled - if (i % 2) { - // coordinate convention (x forward, y right, z down) - accel.x[accel_samples] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); - accel.y[accel_samples] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); - accel.z[accel_samples] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_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); - accel_samples++; - } - - // coordinate convention (x forward, y right, z down) - gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); - gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); - gyro.z[i] = -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) + 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; } - accel.samples = accel_samples; - // Temperature if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { // read current temperature @@ -352,11 +522,7 @@ void ICM20689::Run() } const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); - - static constexpr float RoomTemp_Offset = 25.0f; // Room Temperature Offset 25°C - static constexpr float Temp_Sensitivity = 326.8f; // Sensitivity 326.8 LSB/°C - - const float TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 25.0f; + const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET; _px4_accel.set_temperature(TEMP_degC); _px4_gyro.set_temperature(TEMP_degC); @@ -368,7 +534,12 @@ void ICM20689::Run() void ICM20689::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); diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index 70551acf8f..0ffdb59603 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -48,9 +48,10 @@ #include #include #include +#include #include -using InvenSense_ICM20689::Register; +using namespace InvenSense_ICM20689; class ICM20689 : public device::SPI, public px4::ScheduledWorkItem { @@ -65,6 +66,13 @@ public: void PrintInfo(); private: + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + int probe() override; static int DataReadyInterruptCallback(int irq, void *context, void *arg); @@ -72,10 +80,17 @@ private: void Run() override; + bool CheckRegister(const register_config_t ®_cfg, bool notify = true); + bool Configure(bool notify = true); + void ConfigureAccel(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + uint8_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint8_t value); - void RegisterSetBits(Register reg, uint8_t setbits); void RegisterClearBits(Register reg, uint8_t clearbits); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + void RegisterSetBits(Register reg, uint8_t setbits); + void RegisterWrite(Register reg, uint8_t value); void ResetFIFO(); @@ -85,12 +100,41 @@ private: PX4Gyroscope _px4_gyro; 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 _time_data_ready{0}; + hrt_abstime _last_config_check{0}; hrt_abstime _time_last_temperature_update{0}; - int _data_ready_count{0}; + + px4::atomic _data_ready_count{0}; + + uint8_t _checked_register{0}; + + bool _using_data_ready_interrupt_enabled{false}; + + // 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]))}; + + 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))}; + uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + + static constexpr uint8_t size_register_cfg{11}; + 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 }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, + { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B, ACCEL_CONFIG2_BIT::FIFO_SIZE }, + { 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::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, + { Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN }, + { Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN | INT_ENABLE_BIT::DATA_RDY_INT_EN } + }; }; diff --git a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp index c3b7673e0b..bb80143df6 100644 --- a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp +++ b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp @@ -40,6 +40,8 @@ #pragma once +#include + // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); static constexpr uint8_t Bit1 = (1 << 1); @@ -57,6 +59,9 @@ static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x98; +static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C +static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C + enum class Register : uint8_t { CONFIG = 0x1A, GYRO_CONFIG = 0x1B, @@ -65,8 +70,6 @@ enum class Register : uint8_t { FIFO_EN = 0x23, - INT_STATUS = 0x3A, - INT_ENABLE = 0x38, TEMP_OUT_H = 0x41, @@ -97,7 +100,7 @@ enum GYRO_CONFIG_BIT : uint8_t { FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 // FCHOICE_B [1:0] - FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz + FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz }; // ACCEL_CONFIG @@ -112,7 +115,7 @@ enum ACCEL_CONFIG_BIT : uint8_t { // ACCEL_CONFIG2 enum ACCEL_CONFIG2_BIT : uint8_t { FIFO_SIZE = Bit7 | Bit6, // 0=512bytes, - ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3, + ACCEL_FCHOICE_B = Bit3, // Used to bypass DLPF as shown in the table below. (DS-000114 Page 40 of 53) }; // FIFO_EN @@ -130,30 +133,27 @@ enum INT_ENABLE_BIT : uint8_t { DATA_RDY_INT_EN = Bit0 }; -// INT_STATUS -enum INT_STATUS_BIT : uint8_t { - FIFO_OFLOW_INT = Bit4, - DATA_RDY_INT = Bit0, -}; - // USER_CTRL enum USER_CTRL_BIT : uint8_t { - FIFO_EN = Bit6, - FIFO_RST = Bit2, + FIFO_EN = Bit6, + I2C_IF_DIS = Bit4, + FIFO_RST = Bit2, + SIG_COND_RST = Bit0, }; // PWR_MGMT_1 enum PWR_MGMT_1_BIT : uint8_t { DEVICE_RESET = Bit7, + SLEEP = Bit6, + CLKSEL_2 = Bit2, CLKSEL_1 = Bit1, CLKSEL_0 = Bit0, }; - namespace FIFO { -static constexpr size_t SIZE = 512; // max is 4 KB, but limited in software to 512bytes via ACCEL_CONFIG2 +static constexpr size_t SIZE = 512; // max is 4 KB, but limited in software to 512 bytes via ACCEL_CONFIG2 // FIFO_DATA layout when FIFO_EN has both {X, Y, Z}G_FIFO_EN and ACCEL_FIFO_EN set struct DATA {