optical flow sensor pipeline overhaul

- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>

src/modules/sensors/CMakeLists.txt

@@ -51,13 +51,20 @@ if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 	add_subdirectory(vehicle_magnetometer)
 endif()
 
+if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	add_subdirectory(vehicle_optical_flow)
+endif()
+
 px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
 		sensors.cpp
 		voted_sensors_update.cpp
 		voted_sensors_update.h
+		Integrator.hpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -85,3 +92,7 @@ endif()
 if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 	target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer)
 endif()
+
+if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	target_link_libraries(modules__sensors PRIVATE vehicle_optical_flow)
+endif()

src/modules/sensors/Integrator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -45,6 +45,9 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+namespace sensors
+{
+
 class Integrator
 {
 public:
@@ -94,6 +97,8 @@ public:
 	 */
 	inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); }
 
+	float integral_dt() const { return _integral_dt; }
+
 	void reset()
 	{
 		_alpha.zero();
@@ -137,7 +142,7 @@ protected:
 	matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
 	float _integral_dt{0};
 
-	float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */
+	float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */
 	uint8_t _reset_samples_min{1};
 
 	uint8_t _integrated_samples{0};
@@ -214,3 +219,5 @@ private:
 	matrix::Vector3f _last_alpha{0.f, 0.f, 0.f};       /**< previous value of _alpha */
 
 };
+
+}; // namespace sensors

src/modules/sensors/Kconfig

@@ -28,4 +28,8 @@ if MODULES_SENSORS
         bool "Include vehicle magnetometer"
         default y
 
+    config SENSORS_VEHICLE_OPTICAL_FLOW
+        bool "Include vehicle optical flow"
+        default y
+
 endif #MODULES_SENSORS

src/modules/sensors/sensor_params_flow.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -32,11 +32,10 @@
  ****************************************************************************/
 
 /**
- * PX4Flow board rotation
+ * Optical flow rotation
  *
- * This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame.
+ * This parameter defines the yaw rotation of the optical flow relative to the vehicle body frame.
  * Zero rotation is defined as X on flow board pointing towards front of vehicle.
- * The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw).
  *
  * @value 0 No rotation
  * @value 1 Yaw 45°
@@ -47,11 +46,9 @@
  * @value 6 Yaw 270°
  * @value 7 Yaw 315°
  *
- * @reboot_required true
- *
  * @group Sensors
  */
-PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
+PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
 
 /**
  * Minimum height above ground when reliant on optical flow.
@@ -66,7 +63,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
  * @decimal 1
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f);
 
 /**
  * Maximum height above ground when reliant on optical flow.
@@ -78,12 +75,12 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f);
  *
  * @unit m
  * @min 1.0
- * @max 25.0
+ * @max 100.0
  * @increment 0.1
  * @decimal 1
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f);
 
 /**
  * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor.
@@ -97,4 +94,20 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f);
  * @decimal 2
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 2.5f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f);
+
+/**
+ * Optical flow max rate.
+ *
+ * Optical flow data maximum publication rate. This is an upper bound,
+ * actual optical flow data rate is still dependant on the sensor.
+ *
+ * @min 1
+ * @max 200
+ * @group Sensors
+ * @unit Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f);

src/modules/sensors/sensors.cpp

@@ -93,6 +93,10 @@
 # include <uORB/topics/sensor_mag.h>
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+# include "vehicle_optical_flow/VehicleOpticalFlow.hpp"
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 using namespace sensors;
 using namespace time_literals;
 
@@ -210,6 +214,11 @@ private:
 	uint8_t _n_gps{0};
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	VehicleOpticalFlow *_vehicle_optical_flow {nullptr};
+	uint8_t _n_optical_flow{0};
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 	VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
 	uint8_t _n_accel{0};
@@ -242,6 +251,7 @@ private:
 	void		InitializeVehicleGPSPosition();
 	void		InitializeVehicleIMU();
 	void		InitializeVehicleMagnetometer();
+	void		InitializeVehicleOpticalFlow();
 
 	DEFINE_PARAMETERS(
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
@@ -335,6 +345,15 @@ Sensors::~Sensors()
 
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+
+	if (_vehicle_optical_flow) {
+		_vehicle_optical_flow->Stop();
+		delete _vehicle_optical_flow;
+	}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 	for (auto &vehicle_imu : _vehicle_imu_list) {
 		if (vehicle_imu) {
 			vehicle_imu->Stop();
@@ -445,6 +464,10 @@ int Sensors::parameters_update()
 	InitializeVehicleMagnetometer();
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	InitializeVehicleOpticalFlow();
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 	return PX4_OK;
 }
 
@@ -688,6 +711,23 @@ void Sensors::InitializeVehicleMagnetometer()
 }
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+void Sensors::InitializeVehicleOpticalFlow()
+{
+	if (_vehicle_optical_flow == nullptr) {
+		uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)};
+
+		if (sensor_optical_flow_sub.advertised()) {
+			_vehicle_optical_flow = new VehicleOpticalFlow();
+
+			if (_vehicle_optical_flow) {
+				_vehicle_optical_flow->Start();
+			}
+		}
+	}
+}
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 void Sensors::Run()
 {
 	if (should_exit()) {
@@ -747,6 +787,16 @@ void Sensors::Run()
 
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+		const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow));
+
+		if (n_optical_flow != _n_optical_flow) {
+			_n_optical_flow = n_optical_flow;
+			updated = true;
+		}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 
 		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
 		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
@@ -877,6 +927,15 @@ int Sensors::print_status()
 	_airspeed_validator.print();
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+
+	if (_vehicle_optical_flow) {
+		PX4_INFO_RAW("\n");
+		_vehicle_optical_flow->PrintStatus();
+	}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 	PX4_INFO_RAW("\n");
 	_vehicle_acceleration.PrintStatus();
 

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 px4_add_library(vehicle_imu
-	Integrator.hpp
 	VehicleIMU.cpp
 	VehicleIMU.hpp
 )

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "Integrator.hpp"
+#include <Integrator.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/mathlib/math/WelfordMean.hpp>
@@ -108,8 +108,8 @@ private:
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};
 
-	Integrator       _accel_integrator{};
-	IntegratorConing _gyro_integrator{};
+	sensors::Integrator       _accel_integrator{};
+	sensors::IntegratorConing _gyro_integrator{};
 
 	uint32_t _imu_integration_interval_us{5000};
 

src/modules/sensors/vehicle_optical_flow/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2020 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(vehicle_optical_flow
+	VehicleOpticalFlow.cpp
+	VehicleOpticalFlow.hpp
+)
+target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue)

src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RingBuffer.hpp
+ * Template RingBuffer
+ */
+
+#include <inttypes.h>
+#include <cstdio>
+#include <cstring>
+
+template <typename T, size_t SIZE>
+class RingBuffer
+{
+public:
+	void push(const T &sample)
+	{
+		uint8_t head_new = _head;
+
+		if (!_first_write) {
+			head_new = (_head + 1) % SIZE;
+		}
+
+		_buffer[head_new] = sample;
+		_head = head_new;
+
+		// move tail if we overwrite it
+		if (_head == _tail && !_first_write) {
+			_tail = (_tail + 1) % SIZE;
+
+		} else {
+			_first_write = false;
+		}
+	}
+
+	uint8_t get_length() const { return SIZE; }
+
+	T &operator[](const uint8_t index) { return _buffer[index]; }
+
+	const T &get_newest() const { return _buffer[_head]; }
+	const T &get_oldest() const { return _buffer[_tail]; }
+
+	uint8_t get_oldest_index() const { return _tail; }
+
+	bool peak_first_older_than(const uint64_t &timestamp, T *sample)
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < SIZE; i++) {
+			int index = (_head - i);
+			index = index < 0 ? SIZE + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
+				*sample = _buffer[index];
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a
+				// match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
+	bool pop_first_older_than(const uint64_t &timestamp, T *sample)
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < SIZE; i++) {
+			int index = (_head - i);
+			index = index < 0 ? SIZE + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
+				*sample = _buffer[index];
+
+				// Now we can set the tail to the item which
+				// comes after the one we removed since we don't
+				// want to have any older data in the buffer
+				if (index == _head) {
+					_tail = _head;
+					_first_write = true;
+
+				} else {
+					_tail = (index + 1) % SIZE;
+				}
+
+				_buffer[index].time_us = 0;
+
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a
+				// match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
+	bool pop_oldest(const uint64_t &timestamp_oldest, const uint64_t &timestamp_newest, T *sample)
+	{
+		if (timestamp_oldest >= timestamp_newest) {
+			return false;
+		}
+
+		for (uint8_t i = 0; i < SIZE; i++) {
+
+			uint8_t index = (_tail + i) % SIZE;
+
+			if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) {
+				*sample = _buffer[index];
+
+				// Now we can set the tail to the item which
+				// comes after the one we removed since we don't
+				// want to have any older data in the buffer
+				if (index == _head) {
+					_tail = _head;
+					_first_write = true;
+
+				} else {
+					_tail = (index + 1) % SIZE;
+				}
+
+				_buffer[index] = {};
+
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	bool pop_oldest(T *sample)
+	{
+		if (_tail == _head) {
+			return false;
+		}
+
+		*sample = _buffer[_tail];
+		_buffer[_tail].time_us = 0;
+
+		_tail = (_tail + 1) % SIZE;
+
+		return true;
+	}
+
+	bool pop_newest(T *sample)
+	{
+		if (_tail == _head) {
+			return false;
+		}
+
+		*sample = _buffer[_head];
+		_buffer[_head].time_us = 0;
+
+		_head = (_head - 1) % SIZE;
+
+		return true;
+	}
+
+	int get_total_size() const { return sizeof(*this) + sizeof(T) * SIZE; }
+
+	uint8_t entries() const
+	{
+		int count = 0;
+
+		for (uint8_t i = 0; i < SIZE; i++) {
+			if (_buffer[i].time_us != 0) {
+				count++;
+			}
+		}
+
+		return count;
+	}
+
+private:
+	T _buffer[SIZE] {};
+
+	uint8_t _head{0};
+	uint8_t _tail{0};
+
+	bool _first_write{true};
+};

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp

@@ -0,0 +1,481 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleOpticalFlow.hpp"
+
+#include <px4_platform_common/log.h>
+
+namespace sensors
+{
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
+
+VehicleOpticalFlow::VehicleOpticalFlow() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	_vehicle_optical_flow_pub.advertise();
+
+	_gyro_integrator.set_reset_samples(1);
+}
+
+VehicleOpticalFlow::~VehicleOpticalFlow()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleOpticalFlow::Start()
+{
+	_sensor_flow_sub.registerCallback();
+
+	_sensor_gyro_sub.registerCallback();
+	_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
+
+	_sensor_selection_sub.registerCallback();
+
+	ScheduleNow();
+	return true;
+}
+
+void VehicleOpticalFlow::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	_sensor_flow_sub.unregisterCallback();
+	_sensor_gyro_sub.unregisterCallback();
+	_sensor_selection_sub.unregisterCallback();
+}
+
+void VehicleOpticalFlow::ParametersUpdate()
+{
+	// Check if parameters have changed
+	if (_params_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		_flow_rotation = get_rot_matrix((enum Rotation)_param_sens_flow_rot.get());
+	}
+}
+
+void VehicleOpticalFlow::Run()
+{
+	perf_begin(_cycle_perf);
+
+	ParametersUpdate();
+
+	UpdateDistanceSensor();
+	UpdateSensorGyro();
+
+	sensor_optical_flow_s sensor_optical_flow;
+
+	if (_sensor_flow_sub.update(&sensor_optical_flow)) {
+
+		// clear data accumulation if there's a gap in data
+		if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last)
+		     > sensor_optical_flow.integration_timespan_us * 1.5f)
+		    || (_accumulated_count > 0 && _quality_sum == 0)) {
+			ClearAccumulatedData();
+		}
+
+
+		const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf(
+				sensor_optical_flow.integration_timespan_us);
+		const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp;
+
+		// delta angle
+		//  - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available
+		if (sensor_optical_flow.delta_angle_available
+		    && PX4_ISFINITE(sensor_optical_flow.delta_angle[0])
+		    && PX4_ISFINITE(sensor_optical_flow.delta_angle[1])
+		    && PX4_ISFINITE(sensor_optical_flow.delta_angle[2])
+		   ) {
+			// passthrough integrated gyro if available
+			_delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle};
+
+		} else {
+			// integrate synchronized gyro
+			gyroSample gyro_sample;
+
+			while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) {
+
+				_gyro_integrator.put(gyro_sample.data, gyro_sample.dt);
+
+				float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f;
+
+				if (_gyro_integrator.integral_dt() > min_interval_s) {
+					//PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s);
+					break;
+				}
+			}
+
+			Vector3f delta_angle{NAN, NAN, NAN};
+			uint16_t delta_angle_dt;
+
+			if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
+				_delta_angle += delta_angle;
+
+			} else {
+				// force integrator reset
+				_gyro_integrator.reset();
+			}
+		}
+
+		// distance
+		//  - from sensor_optical_flow if available, otherwise use downward distance_sensor if available
+		if (sensor_optical_flow.distance_available && PX4_ISFINITE(sensor_optical_flow.distance_m)) {
+			if (!PX4_ISFINITE(_distance_sum)) {
+				_distance_sum = sensor_optical_flow.distance_m;
+				_distance_sum_count = 1;
+
+			} else {
+				_distance_sum += sensor_optical_flow.distance_m;
+				_distance_sum_count += 1;
+			}
+
+		} else {
+			// otherwise use buffered downward facing distance_sensor if available
+			rangeSample range_sample;
+
+			if (_range_buffer.peak_first_older_than(sensor_optical_flow.timestamp_sample, &range_sample)) {
+				if (!PX4_ISFINITE(_distance_sum)) {
+					_distance_sum = range_sample.data;
+					_distance_sum_count = 1;
+
+				} else {
+					_distance_sum += range_sample.data;
+					_distance_sum_count += 1;
+				}
+			}
+		}
+
+		_flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample;
+		_flow_integral(0) += sensor_optical_flow.pixel_flow[0];
+		_flow_integral(1) += sensor_optical_flow.pixel_flow[1];
+
+		_integration_timespan_us += sensor_optical_flow.integration_timespan_us;
+
+		_quality_sum += sensor_optical_flow.quality;
+		_accumulated_count++;
+
+		bool publish = true;
+
+		if (_param_sens_flow_rate.get() > 0) {
+			const float interval_us = 1e6f / _param_sens_flow_rate.get();
+
+			// don't allow publishing faster than SENS_FLOW_RATE
+			if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) {
+				publish = false;
+			}
+
+			// integrate for full interval unless we haven't published recently
+			if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) {
+				publish = false;
+			}
+		}
+
+		if (publish) {
+			vehicle_optical_flow_s vehicle_optical_flow{};
+
+			vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample;
+			vehicle_optical_flow.device_id = sensor_optical_flow.device_id;
+
+			_flow_integral.copyTo(vehicle_optical_flow.pixel_flow);
+			_delta_angle.copyTo(vehicle_optical_flow.delta_angle);
+
+			vehicle_optical_flow.integration_timespan_us = _integration_timespan_us;
+
+			vehicle_optical_flow.quality = _quality_sum / _accumulated_count;
+
+			if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
+				vehicle_optical_flow.distance_m = _distance_sum / _distance_sum_count;
+
+			} else {
+				vehicle_optical_flow.distance_m = NAN;
+			}
+
+			// SENS_FLOW_MAXR
+			if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate)
+			    && (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) {
+
+				vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate;
+
+			} else {
+				vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get();
+			}
+
+			// SENS_FLOW_MINHGT
+			if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance)
+			    && (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) {
+
+				vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance;
+
+			} else {
+				vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get();
+			}
+
+			// SENS_FLOW_MAXHGT
+			if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance)
+			    && (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) {
+
+				vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance;
+
+			} else {
+				vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get();
+			}
+
+
+			// rotate (SENS_FLOW_ROT)
+			float zeroval = 0.f;
+			rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0],
+				  vehicle_optical_flow.pixel_flow[1], zeroval);
+
+			vehicle_optical_flow.timestamp = hrt_absolute_time();
+			_vehicle_optical_flow_pub.publish(vehicle_optical_flow);
+			_last_publication_timestamp = vehicle_optical_flow.timestamp_sample;
+
+
+			// vehicle_optical_flow_vel if distance is available (for logging)
+			if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
+				const float range = _distance_sum / _distance_sum_count;
+
+				vehicle_optical_flow_vel_s flow_vel{};
+
+				flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample;
+
+				// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
+				// is produced by a RH rotation of the image about the sensor axis.
+				const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
+				const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
+
+				const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
+
+				// compensate for body motion to give a LOS rate
+				const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
+
+				Vector3f vel_optflow_body;
+				vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
+				vel_optflow_body(1) =   range * flow_compensated_XY_rad(0) / flow_dt;
+				vel_optflow_body(2) = 0.f;
+
+				// vel_body
+				flow_vel.vel_body[0] = vel_optflow_body(0);
+				flow_vel.vel_body[1] = vel_optflow_body(1);
+
+				// vel_ne
+				flow_vel.vel_ne[0] = NAN;
+				flow_vel.vel_ne[1] = NAN;
+
+				vehicle_attitude_s vehicle_attitude{};
+
+				if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+					const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q);
+					const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body;
+
+					flow_vel.vel_ne[0] = flow_vel_ne(0);
+					flow_vel.vel_ne[1] = flow_vel_ne(1);
+				}
+
+				// flow_uncompensated_integral
+				flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
+
+				// flow_compensated_integral
+				flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
+
+				const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
+
+				// gyro_rate
+				flow_vel.gyro_rate[0] = measured_body_rate(0);
+				flow_vel.gyro_rate[1] = measured_body_rate(1);
+
+				// gyro_rate_integral
+				flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
+				flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
+
+				flow_vel.timestamp = hrt_absolute_time();
+
+				_vehicle_optical_flow_vel_pub.publish(flow_vel);
+			}
+
+			ClearAccumulatedData();
+		}
+	}
+
+	// reschedule backup
+	ScheduleDelayed(10_ms);
+
+	perf_end(_cycle_perf);
+}
+
+void VehicleOpticalFlow::UpdateDistanceSensor()
+{
+	// update range finder buffer
+	distance_sensor_s distance_sensor;
+
+	if ((_distance_sensor_selected < 0) && _distance_sensor_subs.advertised()) {
+		for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
+
+			if (_distance_sensor_subs[i].update(&distance_sensor)) {
+				// only use the first instace which has the correct orientation
+				if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
+				    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
+
+					int ndist = orb_group_count(ORB_ID(distance_sensor));
+
+					if (ndist > 1) {
+						PX4_INFO("selected distance_sensor:%d (%d advertised)", i, ndist);
+					}
+
+					_distance_sensor_selected = i;
+					_last_range_sensor_update = distance_sensor.timestamp;
+					break;
+				}
+			}
+		}
+	}
+
+	if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
+		// range sample
+		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
+
+			if ((distance_sensor.current_distance >= distance_sensor.min_distance)
+			    && (distance_sensor.current_distance <= distance_sensor.max_distance)) {
+
+				rangeSample sample;
+				sample.time_us = distance_sensor.timestamp;
+				sample.data = distance_sensor.current_distance;
+
+				_range_buffer.push(sample);
+
+				_last_range_sensor_update = distance_sensor.timestamp;
+
+				return;
+			}
+
+		} else {
+			_distance_sensor_selected = -1;
+		}
+	}
+
+	if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
+		_distance_sensor_selected = -1;
+	}
+}
+
+void VehicleOpticalFlow::UpdateSensorGyro()
+{
+	if (_sensor_selection_sub.updated()) {
+		sensor_selection_s sensor_selection{};
+		_sensor_selection_sub.copy(&sensor_selection);
+
+		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+			uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
+
+			if (sensor_gyro_sub.advertised()
+			    && (sensor_gyro_sub.get().timestamp != 0)
+			    && (sensor_gyro_sub.get().device_id != 0)
+			    && (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) {
+
+				if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
+					if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
+
+						_gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id);
+						PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
+						break;
+
+					} else {
+						PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
+					}
+				}
+			}
+		}
+	}
+
+	// buffer
+	while (_sensor_gyro_sub.updated()) {
+		const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
+
+		sensor_gyro_s sensor_gyro;
+
+		if (_sensor_gyro_sub.copy(&sensor_gyro)) {
+
+			if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
+			}
+
+			_gyro_calibration.set_device_id(sensor_gyro.device_id);
+			_gyro_calibration.SensorCorrectionsUpdate();
+
+			const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
+			_gyro_timestamp_sample_last = sensor_gyro.timestamp_sample;
+
+			gyroSample gyro_sample;
+			gyro_sample.time_us = sensor_gyro.timestamp_sample;
+			gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z});
+			gyro_sample.dt = dt_s;
+
+			_gyro_buffer.push(gyro_sample);
+		}
+	}
+}
+
+void VehicleOpticalFlow::ClearAccumulatedData()
+{
+	// clear accumulated data
+	_flow_integral.zero();
+	_integration_timespan_us = 0;
+
+	_delta_angle.zero();
+
+	_distance_sum = NAN;
+	_distance_sum_count = 0;
+
+	_quality_sum = 0;
+	_accumulated_count = 0;
+
+	_gyro_integrator.reset();
+}
+
+void VehicleOpticalFlow::PrintStatus()
+{
+
+}
+
+}; // namespace sensors

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "data_validator/DataValidatorGroup.hpp"
+#include "RingBuffer.hpp"
+
+#include <Integrator.hpp>
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_optical_flow.h>
+#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow_vel.h>
+
+namespace sensors
+{
+
+class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	VehicleOpticalFlow();
+	~VehicleOpticalFlow() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void ClearAccumulatedData();
+	void UpdateDistanceSensor();
+	void UpdateSensorGyro();
+
+	void Run() override;
+
+	void ParametersUpdate();
+	void SensorCorrectionsUpdate(bool force = false);
+
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	uORB::Publication<vehicle_optical_flow_s> _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)};
+	uORB::Publication<vehicle_optical_flow_vel_s> _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)};
+
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
+
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)};
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};
+	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
+
+	sensors::IntegratorConing _gyro_integrator{};
+
+	hrt_abstime _gyro_timestamp_sample_last{0};
+
+	calibration::Gyroscope _gyro_calibration{};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	matrix::Dcmf _flow_rotation{matrix::eye<float, 3>()};
+
+	hrt_abstime _flow_timestamp_sample_last{0};
+	matrix::Vector2f _flow_integral{};
+	matrix::Vector3f _delta_angle{};
+	uint32_t _integration_timespan_us{};
+	float _distance_sum{NAN};
+	uint8_t _distance_sum_count{0};
+	uint16_t _quality_sum{0};
+	uint8_t _accumulated_count{0};
+
+	hrt_abstime _last_publication_timestamp{0};
+
+	int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
+	hrt_abstime _last_range_sensor_update{0};
+
+	struct gyroSample {
+		uint64_t time_us{}; ///< timestamp of the measurement (uSec)
+		matrix::Vector3f data{};
+		float dt{0.f};
+	};
+
+	struct rangeSample {
+		uint64_t time_us{}; ///< timestamp of the measurement (uSec)
+		float data{};
+	};
+
+	RingBuffer<gyroSample, 32> _gyro_buffer{};
+	RingBuffer<rangeSample, 5> _range_buffer{};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot,
+		(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
+		(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
+		(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
+		(ParamFloat<px4::params::SENS_FLOW_RATE>) _param_sens_flow_rate
+	)
+};
+}; // namespace sensors