From 83daf648efc5e7e311a0293c3bd0ddbc233ccd96 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 May 2022 12:46:46 -0400 Subject: [PATCH] drivers/imu/invensense/icm42670p: cleanup and small fixes --- .../imu/invensense/icm42670p/CMakeLists.txt | 2 +- .../imu/invensense/icm42670p/ICM42670P.cpp | 304 +++++++----------- .../imu/invensense/icm42670p/ICM42670P.hpp | 73 +++-- .../InvenSense_ICM42670P_registers.hpp | 220 +++++-------- 4 files changed, 235 insertions(+), 364 deletions(-) diff --git a/src/drivers/imu/invensense/icm42670p/CMakeLists.txt b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt index 709c0ce2fb..b426216c30 100644 --- a/src/drivers/imu/invensense/icm42670p/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp index f66eb13e2e..a7d1244a43 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config): _px4_accel(get_device_id(), config.rotation), _px4_gyro(get_device_id(), config.rotation) { - if (_drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -123,7 +123,7 @@ void ICM42670P::RunImpl() switch (_state) { case STATE::RESET: - // DEVICE_CONFIG: Software reset configuration + // SIGNAL_PATH_RESET: Software Reset (auto clear bit) RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG); _reset_timestamp = now; _failure_count = 0; @@ -133,7 +133,7 @@ void ICM42670P::RunImpl() case STATE::WAIT_FOR_RESET: if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) - && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04) + && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00) && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { // Wakeup accel and gyro and schedule remaining configuration @@ -190,15 +190,19 @@ void ICM42670P::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + 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 @@ -209,8 +213,6 @@ void ICM42670P::RunImpl() // check current FIFO count const uint16_t fifo_count = FIFOReadCount(); - // PX4_WARN("fifo_count = %d ",fifo_count); - if (fifo_count >= FIFO::SIZE) { FIFOReset(); perf_count(_fifo_overflow_perf); @@ -222,12 +224,17 @@ void ICM42670P::RunImpl() // 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; - // PX4_WARN("samples 1 = %d ",samples); } } } @@ -235,12 +242,9 @@ void ICM42670P::RunImpl() bool success = false; if (samples >= 1) { - // PX4_WARN("samples 2 = %d ",samples); - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; - // PX4_WARN("success = %d ",success); - if (_failure_count > 0) { _failure_count--; } @@ -260,9 +264,11 @@ void ICM42670P::RunImpl() if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) + && RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1]) ) { _last_config_check_timestamp = now; _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg; } else { // register check failed, force reset @@ -283,68 +289,8 @@ void ICM42670P::RunImpl() } } -void ICM42670P::ConfigureAccel() -{ - const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL - - switch (ACCEL_FS_SEL) { - case ACCEL_FS_SEL_2G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); - _px4_accel.set_range(2.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_4G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); - _px4_accel.set_range(4.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_8G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); - _px4_accel.set_range(8.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_16G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); - _px4_accel.set_range(16.f * CONSTANTS_ONE_G); - break; - } -} - -void ICM42670P::ConfigureGyro() -{ - const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL - - float range_dps = 0.f; - - switch (GYRO_FS_SEL) { - - case GYRO_FS_SEL_250_DPS: - range_dps = 250.f; - break; - - case GYRO_FS_SEL_500_DPS: - range_dps = 500.f; - break; - - case GYRO_FS_SEL_1000_DPS: - range_dps = 1000.f; - break; - - case GYRO_FS_SEL_2000_DPS: - range_dps = 2000.f; - break; - } - - _px4_gyro.set_scale(math::radians(range_dps / 32768.f)); - _px4_gyro.set_range(math::radians(range_dps)); -} - void ICM42670P::ConfigureSampleRate(int sample_rate) { - if (sample_rate == 0) { - sample_rate = 800; // default to 800 Hz - } - // round down to nearest FIFO sample dt const float min_interval = FIFO_SAMPLE_DT; _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); @@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples) } } - - bool ICM42670P::Configure() { // first set and clear all configured register bits @@ -383,7 +327,9 @@ bool ICM42670P::Configure() RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); } - Mreg1Config(); + for (const auto ®_cfg : _register_mreg1_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } // now check that all are configured bool success = true; @@ -394,11 +340,17 @@ bool ICM42670P::Configure() } } - success = Mreg1Check(); + for (const auto ®_cfg : _register_mreg1_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); - ConfigureAccel(); - ConfigureGyro(); + _px4_gyro.set_scale(math::radians(2000.f / 32768.f)); + _px4_gyro.set_range(math::radians(2000.f)); return success; } @@ -411,11 +363,8 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42670P::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool ICM42670P::DataReadyInterruptConfigure() @@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T ®_cfg) 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; } 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; } return success; } -template -uint8_t ICM42670P::RegisterRead(T reg) +uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg) { uint8_t cmd[2] {}; cmd[0] = static_cast(reg) | DIR_READ; @@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg) return cmd[1]; } -template -void ICM42670P::RegisterWrite(T reg, uint8_t value) +uint8_t ICM42670P::RegisterRead(Register::MREG1 reg) { - uint8_t cmd[2] { (uint8_t)reg, value }; + // BLK_SEL_R must be set to 0 + RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00); + + // MADDR_R must be set to 0x14 (address of the MREG1 register being accessed) + RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg); + + // Wait for 10 µs + px4_udelay(10); + + // Read register M_R to access the value in MREG1 register 0x14 + uint8_t value = RegisterRead(Register::BANK_0::M_R); + // Wait for 10 µs + // Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off. + px4_udelay(10); + + return value; +} + +void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; transfer(cmd, cmd, sizeof(cmd)); } +void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value) +{ + // BLK_SEL_W must be set to 0 + RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00); + + // MADDR_W must be set to address of the MREG1 register being accessed + RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg); + + // M_W must be set to the desired value + RegisterWrite(Register::BANK_0::M_W, value); + + // Wait for 10 µs + // Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off. + px4_udelay(10); +} + + template void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) { @@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount() return combine(fifo_count_buf[1], fifo_count_buf[2]); } - - bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { FIFOTransferBuffer buffer{}; - const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE); + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) 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(); @@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { // gyro bit not set valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) { + // Packet does not contain a new and valid extended 20-bit data + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { + // accel ODR changed + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) { + // gyro ODR changed + valid = false; } if (valid) { @@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) } } - // PX4_WARN("valid_samples = %d ",valid_samples); if (valid_samples > 0) { ProcessGyro(timestamp_sample, buffer.f, valid_samples); ProcessAccel(timestamp_sample, buffer.f, valid_samples); @@ -577,18 +571,30 @@ void ICM42670P::FIFOReset() { perf_count(_fifo_reset_perf); + // FIFO flush requires the following programming sequence: + // Write FIFO_FLUSH = 1 + // Wait for 1.5 µs + // Read FIFO_FLUSH, it should now be 0 + // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + px4_udelay(2); // Wait for 1.5 µs + + const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET); + + if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) { + PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed"); + } // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; - accel.samples = 0; + accel.samples = samples; accel.dt = FIFO_SAMPLE_DT; for (int i = 0; i < samples; i++) { @@ -598,18 +604,15 @@ void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // 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; - accel.samples++; + accel.x[i] = accel_x; + accel.y[i] = math::negate(accel_y); + accel.z[i] = math::negate(accel_z); } _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - if (accel.samples > 0) { - _px4_accel.updateFIFO(accel); - } + _px4_accel.updateFIFO(accel); } void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) @@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // 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; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + @@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature() _px4_gyro.set_temperature(TEMP_degC); } } - - -uint8_t ICM42670P::RegisterReadBank1(uint8_t reg) -{ - uint8_t value; - - RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00); - RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg); - ScheduleDelayed(10_us); - value = RegisterRead((uint8_t)Register::BANK_0::M_R); - ScheduleDelayed(10_us); - RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00); - - return value; -} - -void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value) -{ - RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00); - RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg); - RegisterWrite((uint8_t)Register::BANK_0::M_W, value); - ScheduleDelayed(10_us); - RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00); -} - -void ICM42670P::Mreg1Config() -{ - uint8_t data; - uint8_t set_bits; - uint8_t clear_bits; - - clear_bits = Bit7 | Bit6 | Bit4 | Bit3; - set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN | - FIFO_CONFIG5_BIT::FIFO_ACCEL_EN; - - data = RegisterReadBank1(0x01); - data &= ~clear_bits; - data |= set_bits; - RegisterWriteBank1(0x01, data); - - - clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0; - set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ; - data = RegisterReadBank1(0x04); - data &= ~clear_bits; - data |= set_bits; - RegisterWriteBank1(0x04, data); -} - -bool ICM42670P::Mreg1Check() -{ - uint8_t set_bits; - uint8_t clear_bits; - uint8_t reg_value; - bool success; - success = true; - - reg_value = RegisterReadBank1(0x01); - - clear_bits = Bit7 | Bit6 | Bit4 | Bit3; - set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN | - FIFO_CONFIG5_BIT::FIFO_ACCEL_EN; - - if (set_bits && ((reg_value & set_bits) != set_bits)) { - success = false; - } - - if (clear_bits && ((reg_value & clear_bits) != 0)) { - success = false; - } - - reg_value = RegisterReadBank1(0x04); - clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0; - set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ; - - if (set_bits && ((reg_value & set_bits) != set_bits)) { - success = false; - } - - if (clear_bits && ((reg_value & clear_bits) != 0)) { - success = false; - } - - return success; -} diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp index 2db173d4a0..362418f7ae 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver public: ICM42670P(const I2CSPIDriverConfig &config); ~ICM42670P() override; + static void print_usage(); void RunImpl(); @@ -68,12 +69,12 @@ private: void exit_and_cleanup() override; // Sensor Configuration - static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f}; // 8000 Hz accel & gyro ODR configured + static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f}; // 1600 Hz accel & gyro ODR configured static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -85,10 +86,8 @@ private: uint8_t FIFO_COUNTL{0}; FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; - - // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); struct register_bank0_config_t { Register::BANK_0 reg; @@ -96,13 +95,17 @@ private: uint8_t clear_bits{0}; }; + struct register_mreg1_config_t { + Register::MREG1 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + int probe() override; bool Reset(); bool Configure(); - void ConfigureAccel(); - void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); @@ -111,9 +114,13 @@ private: bool DataReadyInterruptConfigure(); bool DataReadyInterruptDisable(); + uint8_t RegisterRead(Register::BANK_0 reg); + uint8_t RegisterRead(Register::MREG1 reg); + + void RegisterWrite(Register::BANK_0 reg, uint8_t value); + void RegisterWrite(Register::MREG1 reg, uint8_t value); + template bool RegisterCheck(const T ®_cfg); - template uint8_t RegisterRead(T reg); - template void RegisterWrite(T reg, uint8_t value); template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); template void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } template void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } @@ -126,12 +133,6 @@ private: void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void UpdateTemperature(); - uint8_t RegisterReadBank1(uint8_t reg); - void RegisterWriteBank1(uint8_t reg, uint8_t value); - - void Mreg1Config(); - bool Mreg1Check(); - const spi_drdy_gpio_t _drdy_gpio; PX4Accelerometer _px4_accel; @@ -149,9 +150,7 @@ private: hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -159,26 +158,32 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{10}; 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::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 }, - { 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_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 }, - { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 }, - { Register::BANK_0::GYRO_CONFIG1, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 }, - { Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 }, - { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime - { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime - { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + // 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::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_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR }, + { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS }, + { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime + { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime + { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + }; + + uint8_t _checked_register_mreg1{0}; + static constexpr uint8_t size_register_mreg1_cfg{2}; + register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] { + // Register | Set bits, Clear bits + { Register::MREG1::FIFO_CONFIG5, FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 }, + { Register::MREG1::INT_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp index dc611d5243..48790473bd 100644 --- a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp +++ b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -66,21 +66,13 @@ namespace Register { enum class BANK_0 : uint8_t { - DEVICE_CONFIG = 0x01, + SIGNAL_PATH_RESET = 0x02, INT_CONFIG = 0x06, - TEMP_DATA1 = 0x09, TEMP_DATA0 = 0x0A, - INT_STATUS = 0x3A, - FIFO_COUNTH = 0x3D, - FIFO_COUNTL = 0x3E, - FIFO_DATA = 0x3F, - - SIGNAL_PATH_RESET = 0x02, - PWR_MGMT0 = 0x1F, GYRO_CONFIG0 = 0x20, ACCEL_CONFIG0 = 0x21, @@ -90,203 +82,159 @@ enum class BANK_0 : uint8_t { FIFO_CONFIG1 = 0x28, FIFO_CONFIG2 = 0x29, FIFO_CONFIG3 = 0x2A, - INT_SOURCE0 = 0x2B, + INT_STATUS = 0x3A, + FIFO_COUNTH = 0x3D, + FIFO_COUNTL = 0x3E, + FIFO_DATA = 0x3F, + WHO_AM_I = 0x75, - // REG_BANK_SEL = 0x76, BLK_SEL_W = 0x79, MADDR_W = 0x7A, M_W = 0x7B, - BLK_SEL_R = 0x7C, MADDR_R = 0x7D, M_R = 0x7E, - }; -enum class MREG_1 : uint8_t { - FIFO_CONFIG5_MREG1 = 0x01, - INT_CONFIG0_MREG1 = 0x04, -}; +enum class MREG1 : uint8_t { + FIFO_CONFIG5 = 0x01, -enum class MREG_2 : uint8_t { - OTP_CTRL7_MREG2 = 0x06, + INT_CONFIG0 = 0x04, }; -enum class MREG_3 : uint8_t { - XA_ST_DATA_MREG3 = 0x00, - YA_ST_DATA_MREG3 = 0x01, - ZA_ST_DATA_MREG3 = 0x02, - XG_ST_DATA_MREG3 = 0x03, - YG_ST_DATA_MREG3 = 0x04, - ZG_ST_DATA_MREG3 = 0x05, -}; - - - }; //---------------- BANK0 Register bits // SIGNAL_PATH_RESET enum SIGNAL_PATH_RESET_BIT : uint8_t { - SOFT_RESET_DEVICE_CONFIG = Bit4, // - FIFO_FLUSH = Bit2, + SOFT_RESET_DEVICE_CONFIG = Bit4, // 1: Software reset enabled + FIFO_FLUSH = Bit2, // When set to 1, FIFO will get flushed }; // INT_CONFIG enum INT_CONFIG_BIT : uint8_t { - INT1_MODE = Bit2, - INT1_DRIVE_CIRCUIT = Bit1, - INT1_POLARITY = Bit0, -}; -// GYRO_CONFIG1 -enum GYRO_CONFIG1_BIT : uint8_t { - - // 2:0 GYRO_ODR - GYRO_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz - GYRO_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz - GYRO_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz - GYRO_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz - GYRO_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz - GYRO_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz - GYRO_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz -}; - -// ACCEL_CONFIG1 -enum ACCEL_CONFIG1_BIT : uint8_t { - - // 2:0 ACCEL_ODR - ACCEL_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz - ACCEL_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz - ACCEL_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz - ACCEL_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz - ACCEL_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz - ACCEL_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz - ACCEL_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz -}; -// FIFO_CONFIG1 -enum FIFO_CONFIG1_BIT : uint8_t { - // 1 FIFO_MODE - FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode -}; - -// INT_STATUS -enum INT_STATUS_BIT : uint8_t { - RESET_DONE_INT = Bit4, - FIFO_THS_INT = Bit2, - FIFO_FULL_INT = Bit1, + INT1_MODE = Bit2, // INT1 interrupt mode 1: Latched mode + INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull + INT1_POLARITY = Bit0, // INT1 interrupt polarity 0: Active low }; // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { + // 3:2 GYRO_MODE GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode + // 1:0 ACCEL_MODE ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode }; // GYRO_CONFIG0 enum GYRO_CONFIG0_BIT : uint8_t { // 6:5 GYRO_FS_SEL - GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps - GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps - GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps - GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps + GYRO_FS_SEL_2000_DPS_SET = 0, // 0b000 = ±2000dps + GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5, // 3:0 GYRO_ODR - GYRO_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz - GYRO_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz - GYRO_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz - GYRO_ODR_200Hz = Bit3, // 1000: 200Hz + GYRO_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz + GYRO_ODR_1600HZ_CLEAR = Bit3 | Bit1, }; // ACCEL_CONFIG0 enum ACCEL_CONFIG0_BIT : uint8_t { - // 6:5 ACCEL_FS_SEL - ACCEL_FS_SEL_16G = 0, // 000: ±16g - ACCEL_FS_SEL_8G = Bit5, // 001: ±8g - ACCEL_FS_SEL_4G = Bit6, // 010: ±4g - ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g + // 6:5 ACCEL_UI_FS_SEL + ACCEL_UI_FS_SEL_16G_SET = 0, // 000: ±16g + ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5, // 3:0 ACCEL_ODR - ACCEL_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz - ACCEL_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz - ACCEL_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz - ACCEL_ODR_200Hz = Bit3, // 1000: 200Hz + ACCEL_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz + ACCEL_ODR_1600HZ_CLEAR = Bit3 | Bit1, }; -// FIFO_CONFIG5 -enum FIFO_CONFIG5_BIT : uint8_t { - FIFO_RESUME_PARTIAL_RD = Bit4, - FIFO_WM_GT_TH = Bit5, - FIFO_HIRES_EN = Bit3, - FIFO_TMST_FSYNC_EN = Bit2, - FIFO_GYRO_EN = Bit1, - FIFO_ACCEL_EN = Bit0, +// GYRO_CONFIG1 +enum GYRO_CONFIG1_BIT : uint8_t { + // 2:0 GYRO_UI_FILT_BW + GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed }; -// INT_CONFIG0 -enum INT_CONFIG0_BIT : uint8_t { - // 3:2 FIFO_THS_INT_CLEAR - CLEAR_ON_FIFO_READ = Bit3, +// ACCEL_CONFIG1 +enum ACCEL_CONFIG1_BIT : uint8_t { + // 2:0 ACCEL_UI_FILT_BW + ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed +}; + +// FIFO_CONFIG1 +enum FIFO_CONFIG1_BIT : uint8_t { + // FIFO_MODE + FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode + FIFO_BYPASS = Bit0, // 0: FIFO is not bypassed +}; + +// INT_STATUS +enum INT_STATUS_BIT : uint8_t { + RESET_DONE_INT = Bit4, + FIFO_THS_INT = Bit2, + FIFO_FULL_INT = Bit1, }; // INT_SOURCE0 enum INT_SOURCE0_BIT : uint8_t { - ST_INT1_END = Bit7, - FSYNC_INT1_EN = Bit6, - PLL_RDY_INT1_EN = Bit5, - RESET_DONE_INT1_EN = Bit4, - DRDY_INT1_EN = Bit3, + RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 - FIFO_FULL_INT1_EN = Bit1, - AGC_RDY_INT1_EN = Bit0, }; -// REG_BANK_SEL -enum REG_BANK_SEL_BIT : uint8_t { - USER_BANK_0 = 0, // 0: Select USER BANK 0. - USER_BANK_1 = Bit4, // 1: Select USER BANK 1. - USER_BANK_2 = Bit5, // 2: Select USER BANK 2. - USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3. + +//---------------- USER BANK MREG1 Register bits + +// FIFO_CONFIG5 +enum FIFO_CONFIG5_BIT : uint8_t { + FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM + FIFO_GYRO_EN = Bit1, // 1: Enables Gyro packets to go to FIFO + FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO +}; + +// INT_CONFIG0 +enum INT_CONFIG0_BIT : uint8_t { + // 3:2 FIFO_THS_INT_CLEAR + FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read }; namespace FIFO { -static constexpr size_t SIZE = 2048; +static constexpr size_t SIZE = 1024; // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set // Packet 3 struct DATA { uint8_t FIFO_Header; - uint8_t ACCEL_DATA_X1; - uint8_t ACCEL_DATA_X0; - uint8_t ACCEL_DATA_Y1; - uint8_t ACCEL_DATA_Y0; - uint8_t ACCEL_DATA_Z1; - uint8_t ACCEL_DATA_Z0; - uint8_t GYRO_DATA_X1; - uint8_t GYRO_DATA_X0; - uint8_t GYRO_DATA_Y1; - uint8_t GYRO_DATA_Y0; - uint8_t GYRO_DATA_Z1; - uint8_t GYRO_DATA_Z0; - uint8_t temperature; // Temperature[7:0] - uint8_t timestamp_l; - uint8_t timestamp_h; + uint8_t ACCEL_DATA_X1; // Accel X [19:12] + uint8_t ACCEL_DATA_X0; // Accel X [11:4] + uint8_t ACCEL_DATA_Y1; // Accel Y [19:12] + uint8_t ACCEL_DATA_Y0; // Accel Y [11:4] + uint8_t ACCEL_DATA_Z1; // Accel Z [19:12] + uint8_t ACCEL_DATA_Z0; // Accel Z [11:4] + uint8_t GYRO_DATA_X1; // Gyro X [19:12] + uint8_t GYRO_DATA_X0; // Gyro X [11:4] + uint8_t GYRO_DATA_Y1; // Gyro Y [19:12] + uint8_t GYRO_DATA_Y0; // Gyro Y [11:4] + uint8_t GYRO_DATA_Z1; // Gyro Z [19:12] + uint8_t GYRO_DATA_Z0; // Gyro Z [11:4] + uint8_t temperature; // Temperature[7:0] + uint8_t TimeStamp_h; // TimeStamp[15:8] + uint8_t TimeStamp_l; // TimeStamp[7:0] }; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx enum FIFO_HEADER_BIT : uint8_t { HEADER_MSG = Bit7, // 1: FIFO is empty - HEADER_ACCEL = Bit6, - HEADER_GYRO = Bit5, - HEADER_20 = Bit4, + HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 + HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 + HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, - HEADER_ODR_ACCEL = Bit1, - HEADER_ODR_GYRO = Bit0, + HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet + HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet }; }