drivers/optical_flow/paa3905: backup scheduling to fetch 0 flow

- this sensor provides a MOTION interrupt used for default scheduling, but we also need to publish
   zero flow information

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) {
+				const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
-					if (motion_reported) {
+				const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
-						// 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);
 
+				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
+					// only publish when there's valid data or on timeout
-					if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+					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);
 			}

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<hrt_abstime> _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};