Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions src/drivers/optical_flow/paa3905/PAA3905.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ bool PAA3905::Reset()
_state = STATE::RESET;
DataReadyInterruptDisable();
_drdy_timestamp_sample.store(0);
_timestamp_sample_last = 0;
ScheduleClear();
ScheduleNow();
return true;
Expand Down Expand Up @@ -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<uint32_t>(dt),
static_cast<uint32_t>(1_ms), static_cast<uint32_t>(200_ms));
}

// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);

Expand Down Expand Up @@ -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);
}
Expand Down
1 change: 1 addition & 0 deletions src/drivers/optical_flow/paa3905/PAA3905.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
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};
Expand Down
14 changes: 14 additions & 0 deletions src/drivers/optical_flow/paw3902/PAW3902.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ bool PAW3902::Reset()
_state = STATE::RESET;
DataReadyInterruptDisable();
_drdy_timestamp_sample.store(0);
_timestamp_sample_last = 0;
ScheduleClear();
ScheduleNow();
return true;
Expand Down Expand Up @@ -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<uint32_t>(dt),
static_cast<uint32_t>(1_ms), static_cast<uint32_t>(200_ms));
}

// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);

Expand Down Expand Up @@ -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);
}
Expand Down
1 change: 1 addition & 0 deletions src/drivers/optical_flow/paw3902/PAW3902.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
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};
Expand Down
Loading