From 751b3497a049925a1230786706b2ab0a4b6cda3f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 11 Apr 2020 12:17:40 -0400 Subject: [PATCH] icm20608g/icm20689: force FIFO count check sooner The InvenSense icm20608g and icm20689 don't have a FIFO watermark interrupt, but they do have a data ready interrupt and the ability to get the current FIFO count in the same large transfer as the actual FIFO data. So instead of manually checking the FIFO count before every transfer (costs ~ 1-3% cpu) we trust the data ready counts, verify things are in sync after the large transfer (fifo count + fifo contents), and force a manual check before the next transfer if necessary. As a precaution this change lowers the threshold for forcing a manual FIFO count check before the large transfer. Forcing the check if out of sync by 2 (or more) samples makes sense because we always do transfers in multiples of 2 (gyro samples per accel). The other small cosmetic changes are keeping the icm20608g and icm20689 in sync (they're nearly identical). --- .../imu/invensense/icm20608g/ICM20608G.cpp | 22 +++++++++----- .../imu/invensense/icm20689/ICM20689.cpp | 30 +++++++++++-------- .../imu/invensense/icm20689/ICM20689.hpp | 6 ++-- 3 files changed, 35 insertions(+), 23 deletions(-) diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 59cb8b5cfe..2aa96bcc44 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -265,22 +265,22 @@ void ICM20608G::ConfigureAccel() switch (ACCEL_FS_SEL) { case ACCEL_FS_SEL_2G: _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); - _px4_accel.set_range(2 * CONSTANTS_ONE_G); + _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 * CONSTANTS_ONE_G); + _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 * CONSTANTS_ONE_G); + _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 * CONSTANTS_ONE_G); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); break; } } @@ -374,7 +374,7 @@ bool ICM20608G::DataReadyInterruptConfigure() } // Setup data ready on falling edge - return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM20608G::DataReadyInterruptCallback, this) == 0; + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; } bool ICM20608G::DataReadyInterruptDisable() @@ -493,8 +493,8 @@ bool ICM20608G::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) // force check if there is somehow fewer samples actually in the FIFO (potentially a serious error) _force_fifo_count_check = true; - } else if (fifo_count_samples > samples + 4) { - // if we're more than a few samples behind force FIFO_COUNT check + } else if (fifo_count_samples >= samples + 2) { + // if we're more than a couple samples behind force FIFO_COUNT check _force_fifo_count_check = true; } else { @@ -504,9 +504,15 @@ bool ICM20608G::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) if (valid_samples > 0) { ProcessGyro(timestamp_sample, buffer, valid_samples); - return ProcessAccel(timestamp_sample, buffer, valid_samples); + + if (ProcessAccel(timestamp_sample, buffer, valid_samples)) { + return true; + } } + // force FIFO count check if there was any other error + _force_fifo_count_check = true; + return false; } diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 5b343ea73a..85ea248cca 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -264,23 +264,23 @@ void ICM20689::ConfigureAccel() 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); + _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); - _px4_accel.set_range(4 * CONSTANTS_ONE_G); + _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); - _px4_accel.set_range(8 * CONSTANTS_ONE_G); + _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); - _px4_accel.set_range(16 * CONSTANTS_ONE_G); + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); break; } } @@ -374,7 +374,7 @@ bool ICM20689::DataReadyInterruptConfigure() } // Setup data ready on falling edge - return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM20689::DataReadyInterruptCallback, this) == 0; + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; } bool ICM20689::DataReadyInterruptDisable() @@ -493,8 +493,8 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) // force check if there is somehow fewer samples actually in the FIFO (potentially a serious error) _force_fifo_count_check = true; - } else if (fifo_count_samples > samples + 4) { - // if we're more than a few samples behind force FIFO_COUNT check + } else if (fifo_count_samples >= samples + 2) { + // if we're more than a couple samples behind force FIFO_COUNT check _force_fifo_count_check = true; } else { @@ -504,9 +504,15 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) if (valid_samples > 0) { ProcessGyro(timestamp_sample, buffer, valid_samples); - return ProcessAccel(timestamp_sample, buffer, valid_samples); + + if (ProcessAccel(timestamp_sample, buffer, valid_samples)) { + return true; + } } + // force FIFO count check if there was any other error + _force_fifo_count_check = true; + return false; } diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index b64071d5ba..5be6cbba5d 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -74,9 +74,9 @@ private: // Sensor Configuration 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{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro - static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel + 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]))};