diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index be4ae70655..f5e5facd1e 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -194,13 +194,13 @@ void PAA3905::RunImpl() _state = STATE::READ; if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; + _motion_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(1_s); } else { - _data_ready_interrupt_enabled = false; + _motion_interrupt_enabled = false; ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } @@ -222,7 +222,7 @@ void PAA3905::RunImpl() case STATE::READ: { hrt_abstime timestamp_sample = now; - if (_data_ready_interrupt_enabled) { + if (_motion_interrupt_enabled) { // scheduled from interrupt if _drdy_timestamp_sample was set as expected const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); @@ -258,12 +258,6 @@ void PAA3905::RunImpl() PX4_ERR("invalid RawData_Sum > 0x98"); } - // Number of Features = SQUAL * 4 - // RawData_Sum maximum register value is 0x98 - bool data_valid = (buffer.data.SQUAL > 0) - && (buffer.data.RawData_Sum <= 0x98) - && (_discard_reading == 0); - // Bit [5:0] check if chip is working correctly // 0x3F: chip is working correctly if ((buffer.data.Observation & 0x3F) != 0x3F) { @@ -313,7 +307,7 @@ void PAA3905::RunImpl() if (prev_mode != _mode) { // update scheduling on mode change - if (!_data_ready_interrupt_enabled) { + if (!_motion_interrupt_enabled) { ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } } @@ -348,6 +342,14 @@ void PAA3905::RunImpl() const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower; + // Number of Features = SQUAL * 4 + // RawData_Sum maximum register value is 0x98 + bool data_valid = (buffer.data.SQUAL > 0) + && (buffer.data.RawData_Sum > 0) + && (buffer.data.RawData_Sum <= 0x98) + && (shutter > 0) + && (_discard_reading == 0); + switch (_mode) { case Mode::Bright: sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0; @@ -392,12 +394,17 @@ void PAA3905::RunImpl() // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred); - if (data_valid) { - if (motion_reported) { - // only populate flow if data valid (motion and quality > 0) - const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); - const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); + const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + if (data_valid) { + + const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0); + const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1); + + bool publish = false; + + if (motion_reported) { // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; @@ -412,15 +419,53 @@ void PAA3905::RunImpl() sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + + _last_motion = timestamp_sample; + + } else if (zero_flow && (timestamp_sample > _last_motion)) { + // no motion, but burst read looks valid and we should have seen new data by now if there was any motion + const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev) + || (shutter != _shutter_prev) + || (buffer.data.RawData_Sum != _raw_data_sum_prev) + || (buffer.data.SQUAL != _quality_prev); + + if (burst_read_changed) { + + sensor_optical_flow.pixel_flow[0] = 0; + sensor_optical_flow.pixel_flow[1] = 0; + + sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + } } - // only publish when there's motion or at least every second - if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { + // only publish when there's valid data or on timeout + if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { sensor_optical_flow.timestamp = hrt_absolute_time(); _sensor_optical_flow_pub.publish(sensor_optical_flow); - _last_publish = sensor_optical_flow.timestamp; + _last_publish = sensor_optical_flow.timestamp_sample; + } + + // backup schedule if we're reliant on the motion interrupt and there's very little flow + if (_motion_interrupt_enabled && little_to_no_flow) { + switch (_mode) { + case Mode::Bright: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_2); + break; + } } success = true; @@ -430,6 +475,12 @@ void PAA3905::RunImpl() } } + _delta_x_raw_prev = delta_x_raw; + _delta_y_raw_prev = delta_y_raw; + _shutter_prev = shutter; + _raw_data_sum_prev = buffer.data.RawData_Sum; + _quality_prev = buffer.data.SQUAL; + } 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 e27bdf0fb6..c850b0405a 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -114,14 +114,22 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; + hrt_abstime _last_motion{0}; + + int16_t _delta_x_raw_prev{0}; + int16_t _delta_y_raw_prev{0}; + uint32_t _shutter_prev{0}; + uint8_t _quality_prev{0}; + uint8_t _raw_data_sum_prev{0}; + int _failure_count{0}; int _discard_reading{0}; px4::atomic _drdy_timestamp_sample{0}; - bool _data_ready_interrupt_enabled{false}; + bool _motion_interrupt_enabled{false}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2}; - static constexpr uint32_t kBackupScheduleIntervalUs{200_ms}; + static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval Mode _mode{Mode::LowLight};