diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index e99621f27bca..e488b92d2e5e 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -89,6 +89,7 @@ bool PAA3905::Reset() _state = STATE::RESET; DataReadyInterruptDisable(); _drdy_timestamp_sample.store(0); + _timestamp_sample_last = 0; ScheduleClear(); ScheduleNow(); return true; @@ -391,6 +392,15 @@ void PAA3905::RunImpl() break; } + // override the per-mode default with the actual interval between burst reads + // (the chip accumulates delta_x/delta_y until Motion_Burst is read), so the + // gyro integration window downstream lines up with what the chip actually saw. + if (_timestamp_sample_last != 0 && timestamp_sample > _timestamp_sample_last) { + const hrt_abstime dt = timestamp_sample - _timestamp_sample_last; + sensor_optical_flow.integration_timespan_us = math::constrain(static_cast(dt), + static_cast(1_ms), static_cast(200_ms)); + } + // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred); @@ -481,6 +491,10 @@ void PAA3905::RunImpl() _raw_data_sum_prev = buffer.data.RawData_Sum; _quality_prev = buffer.data.SQUAL; + // chip clears its delta accumulator on every Motion_Burst read, + // regardless of whether we publish, so track every successful read. + _timestamp_sample_last = timestamp_sample; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index c850b0405afa..077b6c5ac07d 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -115,6 +115,7 @@ class PAA3905 : public device::SPI, public I2CSPIDriver hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; hrt_abstime _last_motion{0}; + hrt_abstime _timestamp_sample_last{0}; int16_t _delta_x_raw_prev{0}; int16_t _delta_y_raw_prev{0}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index af70f9a5e32a..001fa078641a 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -89,6 +89,7 @@ bool PAW3902::Reset() _state = STATE::RESET; DataReadyInterruptDisable(); _drdy_timestamp_sample.store(0); + _timestamp_sample_last = 0; ScheduleClear(); ScheduleNow(); return true; @@ -394,6 +395,15 @@ void PAW3902::RunImpl() break; } + // override the per-mode default with the actual interval between burst reads + // (the chip accumulates delta_x/delta_y until Motion_Burst is read), so the + // gyro integration window downstream lines up with what the chip actually saw. + if (_timestamp_sample_last != 0 && timestamp_sample > _timestamp_sample_last) { + const hrt_abstime dt = timestamp_sample - _timestamp_sample_last; + sensor_optical_flow.integration_timespan_us = math::constrain(static_cast(dt), + static_cast(1_ms), static_cast(200_ms)); + } + // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT); @@ -484,6 +494,10 @@ void PAW3902::RunImpl() _raw_data_sum_prev = buffer.data.RawData_Sum; _quality_prev = buffer.data.SQUAL; + // chip clears its delta accumulator on every Motion_Burst read, + // regardless of whether we publish, so track every successful read. + _timestamp_sample_last = timestamp_sample; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index b2e568d6197f..9e9ff17c32d0 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -115,6 +115,7 @@ class PAW3902 : public device::SPI, public I2CSPIDriver hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; hrt_abstime _last_motion{0}; + hrt_abstime _timestamp_sample_last{0}; int16_t _delta_x_raw_prev{0}; int16_t _delta_y_raw_prev{0};