From aad27c4f94ab04689ee37023a0a409912063627d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 30 Jan 2020 01:48:20 -0500 Subject: [PATCH] invensense/icm20602: use watermarak interrupt instead of data ready and remove unnecessary perf counters --- .../imu/invensense/icm20602/ICM20602.cpp | 60 +++++++------------ .../imu/invensense/icm20602/ICM20602.hpp | 3 - .../InvenSense_ICM20602_registers.hpp | 7 ++- 3 files changed, 26 insertions(+), 44 deletions(-) diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index b5022d8fd3..25350523e2 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -70,12 +70,10 @@ ICM20602::~ICM20602() board_dma_free(_dma_data_buffer, FIFO::SIZE); } - perf_free(_interval_perf); perf_free(_transfer_perf); perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); - perf_free(_drdy_count_perf); perf_free(_drdy_interval_perf); } @@ -164,13 +162,17 @@ void ICM20602::ResetFIFO() 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); - RegisterClearBits(Register::CONFIG, CONFIG_BIT::FIFO_WM); - RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + // CONFIG: User should ensure that bit 7 of register 0x1A (Register::CONFIG) is set to 0 before using watermark feature + RegisterClearBits(Register::CONFIG, Bit7); + RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + + // FIFO Watermark + static constexpr uint8_t fifo_watermark = 8 * sizeof(FIFO::DATA); + static_assert(fifo_watermark < UINT8_MAX); + RegisterWrite(Register::FIFO_WM_TH1, 0); + RegisterWrite(Register::FIFO_WM_TH2, fifo_watermark); // FIFO_EN: enable both gyro and accel - _data_ready_count = 0; RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); up_udelay(10); } @@ -218,19 +220,12 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20602::DataReady() { - perf_count(_drdy_count_perf); perf_count(_drdy_interval_perf); - _data_ready_count++; + _time_data_ready = hrt_absolute_time(); - if (_data_ready_count >= 8) { - _time_data_ready = hrt_absolute_time(); - - _data_ready_count = 0; - - // make another measurement - ScheduleNow(); - } + // make another measurement + ScheduleNow(); } void ICM20602::Start() @@ -243,23 +238,23 @@ void ICM20602::Start() #if defined(GPIO_DRDY_PORTC_PIN14) // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY4_ICM20602) // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_DRDY_ICM_2060X) // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #else ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); #endif @@ -271,23 +266,23 @@ void ICM20602::Stop() #if defined(GPIO_DRDY_PORTC_PIN14) // Disable data ready callback px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY4_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_DRDY_ICM_2060X) // Disable data ready callback px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #else ScheduleClear(); #endif @@ -295,8 +290,6 @@ void ICM20602::Stop() void ICM20602::Run() { - perf_count(_interval_perf); - // 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 : @@ -325,13 +318,6 @@ void ICM20602::Run() return; } - // check for FIFO overflow - if (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT) { - perf_count(_fifo_overflow_perf); - ResetFIFO(); - return; - } - // Transfer data struct TransferBuffer { uint8_t cmd; @@ -416,12 +402,10 @@ void ICM20602::Run() void ICM20602::PrintInfo() { - perf_print_counter(_interval_perf); perf_print_counter(_transfer_perf); perf_print_counter(_fifo_empty_perf); perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_count_perf); perf_print_counter(_drdy_interval_perf); _px4_accel.print_status(); diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index e4ab3d8f38..f0fbc98111 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -84,14 +84,11 @@ private: PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": 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_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")}; perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; hrt_abstime _time_data_ready{0}; - int _data_ready_count{0}; }; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index 6378a0600f..dd227d53a2 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -68,12 +68,15 @@ enum class Register : uint8_t { INT_STATUS = 0x3A, - INT_PIN_CFG = 0x37, INT_ENABLE = 0x38, + FIFO_WM_INT_STATUS = 0x39, TEMP_OUT_H = 0x41, TEMP_OUT_L = 0x42, + FIFO_WM_TH1 = 0x60, + FIFO_WM_TH2 = 0x61, + USER_CTRL = 0x6A, PWR_MGMT_1 = 0x6B, @@ -85,7 +88,6 @@ enum class Register : uint8_t { // CONFIG enum CONFIG_BIT : uint8_t { - FIFO_WM = Bit7, FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0] @@ -171,7 +173,6 @@ struct DATA { uint8_t GYRO_ZOUT_H; uint8_t GYRO_ZOUT_L; }; -static_assert(sizeof(DATA) == 14); } } // namespace InvenSense_ICM20602