From 74d55173100d6c7352ef99bb9637080b3f93fc0a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 20 May 2026 14:14:19 -0600 Subject: [PATCH 1/2] fix(optical_flow): report actual integration timespan in PAW3902/PAA3905 The chip accumulates delta_x/delta_y in its burst registers until the Motion_Burst register is read, which clears the accumulator. The drivers previously reported a hardcoded mode-dependent SAMPLE_INTERVAL_MODE_X as the integration timespan regardless of how long ago the last burst read happened. This is wrong any time reads span more than one frame period - most commonly when the Motion DRDY pin doesn't fire (chip detected no motion) and the backup watchdog fires the read kBackupScheduleIntervalUs (20 ms) later instead of every frame. Downstream, VehicleOpticalFlow uses integration_timespan_us to size the gyro integration window used for body-rate compensation, and ekf2's _flow_gyro_bias estimator compares that gyro against the IMU at the matched delayed time. A wrong window length misaligns the gyro compensation with what the chip actually saw, and slowly biases _flow_gyro_bias - contaminating motion-reported fusions on the correctly-timed DRDY path too. Replace the hardcoded value with the actual interval between consecutive successful burst reads (clamped to a sane range). _timestamp_sample_last is reset on driver Reset() and updated on every successful burst read regardless of publish, since the chip clears its accumulator on every read. --- src/drivers/optical_flow/paa3905/PAA3905.cpp | 14 ++++++++++++++ src/drivers/optical_flow/paa3905/PAA3905.hpp | 1 + src/drivers/optical_flow/paw3902/PAW3902.cpp | 14 ++++++++++++++ src/drivers/optical_flow/paw3902/PAW3902.hpp | 1 + 4 files changed, 30 insertions(+) diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index e99621f27bca..954a58610630 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(1000), static_cast(200000)); + } + // 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..54dde4f80845 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(1000), static_cast(200000)); + } + // 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}; From 2797a5bc416c4f9a6011be130caf0faae8af7f35 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 20 May 2026 14:25:06 -0600 Subject: [PATCH 2/2] refactor(optical_flow): use _ms literals for integration_timespan_us clamp Replace raw 1000/200000 microsecond bounds with 1_ms/200_ms to match the existing time_literals convention already used elsewhere in these drivers. No behavior change. Signed-off-by: Jacob Dahl --- src/drivers/optical_flow/paa3905/PAA3905.cpp | 2 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index 954a58610630..e488b92d2e5e 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -398,7 +398,7 @@ void PAA3905::RunImpl() 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(1000), static_cast(200000)); + static_cast(1_ms), static_cast(200_ms)); } // motion in burst transfer diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 54dde4f80845..001fa078641a 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -401,7 +401,7 @@ void PAW3902::RunImpl() 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(1000), static_cast(200000)); + static_cast(1_ms), static_cast(200_ms)); } // motion in burst transfer