fix(optical_flow): report actual integration timespan in PAW3902/PAA3905#27418
Open
dakejahl wants to merge 2 commits into
Open
fix(optical_flow): report actual integration timespan in PAW3902/PAA3905#27418dakejahl wants to merge 2 commits into
dakejahl wants to merge 2 commits into
Conversation
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.
github-actions
Bot
added
kind:bug
…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 <dahl.jakejacob@gmail.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
PAW3902 and PAA3905 drivers hardcode
sensor_optical_flow.integration_timespan_usto the nominal mode frame period (e.g.1/126 s). The chips actually accumulate delta_x/delta_y until the Motion_Burst register is read, so when reads happen less often than every frame the reported timespan no longer matches reality. Derive it from the actual interval between consecutive burst reads.Problem
The Motion pin only asserts when the chip detects motion since the last burst read. In a hover with no chip-detectable flow (textureless patch, sub-pixel translation, slow drift), the pin never falls and the driver's backup watchdog reads every
kBackupScheduleIntervalUs(20 ms in mode 2) instead of every frame. The chip has been integrating that whole interval, but the driver still reports the hardcodedSAMPLE_INTERVAL_MODE_X.Downstream consequences:
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp:124,151usesintegration_timespan_usto size the gyro integration window for body-rate compensation. A wrong length means the gyro integral does not cover the same physical interval the chip integrated over, so the rotational-LOS compensationflow_compensated = flow_rate - gyro_rate.xy()does not cleanly cancel rotation from the observation.src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp:293-298updates_flow_gyro_biasvia an LPF onflow_sample.gyro_rate - _ref_body_rate. With a persistently misaligned window the difference is no longer just a real bias - it contains the timing skew, which the LPF accumulates. That bias is then applied to every fusion including the motion-reported ones on the clean DRDY path.The practical magnitude is small for steady hover (gyro ~= 0), but non-zero whenever the body rate is non-zero during a zero-flow publish (e.g. yaw scanning in hover, attitude wobble below the chip's pixel resolution, vibration). It is also a message-contract violation -
msg/SensorOpticalFlow.msgdefines the field as "accumulation timespan".Solution
Track the timestamp of the previous successful Motion_Burst read in
_timestamp_sample_last, and override the per-mode default withtimestamp_sample - _timestamp_sample_last(clamped to[1 ms, 200 ms]). Update_timestamp_sample_laston every successful burst read since the chip clears its accumulator on each read, and reset it to 0 inReset(). The first read after reset falls back to the per-mode default; subsequent reads use the measured interval.Builds verified:
make ark_can-flow_default,make ark_can-flow-mr_default.Marking draft for review of the approach before flight testing.