From ff74a2a48b907d0962c22b3d70531a67c9001b75 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 6 Apr 2020 23:06:06 -0400 Subject: [PATCH] new mpu6000/mpu6500/mpu9250 always schedule backup cycle if using data ready --- src/drivers/imu/invensense/mpu6000/MPU6000.cpp | 5 ++++- src/drivers/imu/invensense/mpu6500/MPU6500.cpp | 5 ++++- src/drivers/imu/invensense/mpu9250/MPU9250.cpp | 5 ++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index d2d0dcde9f..d2b10a69d2 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -188,10 +188,13 @@ void MPU6000::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = 0; - if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + if (_data_ready_interrupt_enabled) { // re-schedule as watchdog timeout ScheduleDelayed(10_ms); + } + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + // use timestamp from data ready interrupt if enabled and seems valid timestamp_sample = _fifo_watermark_interrupt_timestamp; } else { diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 4395330107..21287d6086 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -188,10 +188,13 @@ void MPU6500::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = 0; - if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + if (_data_ready_interrupt_enabled) { // re-schedule as watchdog timeout ScheduleDelayed(10_ms); + } + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + // use timestamp from data ready interrupt if enabled and seems valid timestamp_sample = _fifo_watermark_interrupt_timestamp; } else { diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index a47b0d1876..52e37b7ff0 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -221,10 +221,13 @@ void MPU9250::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = 0; - if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + if (_data_ready_interrupt_enabled) { // re-schedule as watchdog timeout ScheduleDelayed(10_ms); + } + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + // use timestamp from data ready interrupt if enabled and seems valid timestamp_sample = _fifo_watermark_interrupt_timestamp; } else {