WIP: optical_flow sensor & vehicle

msg/CMakeLists.txt

@@ -118,7 +118,8 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
-	optical_flow.msg
+	sensor_optical_flow.msg
+	vehicle_optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
 	ping.msg

msg/sensor_optical_flow.msg

@@ -4,16 +4,24 @@
 uint64 timestamp		# time since system start (microseconds)
 
 uint8 sensor_id			# id of the sensor emitting the flow value
+
 float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
 float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
+
 float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
 float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
 float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+
 float32 ground_distance_m	# Altitude / distance to ground in meters
+
 uint32 integration_timespan	# accumulation timespan in microseconds
+
 uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
+
 uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
+
 int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
+
 uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
 
 float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably

msg/vehicle_optical_flow.msg

@@ -0,0 +1,29 @@
+# Optical flow in XYZ body frame in SI units.
+# http://en.wikipedia.org/wiki/International_System_of_Units
+
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 sensor_id			# id of the sensor emitting the flow value
+
+float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
+float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
+
+float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+
+float32 ground_distance_m	# Altitude / distance to ground in meters
+
+uint32 integration_timespan	# accumulation timespan in microseconds
+
+uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
+
+uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
+
+int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
+
+uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
+float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -535,7 +535,7 @@ void PAA3905::RunImpl()
 		return;
 	}
 
-	optical_flow_s report{};
+	sensor_optical_flow_s report{};
 	report.timestamp = timestamp_sample;
 	//report.device_id = get_device_id();
 
@@ -565,15 +565,15 @@ void PAA3905::RunImpl()
 
 	switch (_mode) {
 	case Mode::Bright:
-		report.mode = optical_flow_s::MODE_BRIGHT;
+		report.mode = sensor_optical_flow_s::MODE_BRIGHT;
 		break;
 
 	case Mode::LowLight:
-		report.mode = optical_flow_s::MODE_LOWLIGHT;
+		report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
 		break;
 
 	case Mode::SuperLowLight:
-		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
+		report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
 		break;
 	}
 

src/drivers/optical_flow/paa3905/PAA3905.hpp

@@ -51,7 +51,7 @@
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 using namespace time_literals;
 using namespace PixArt_PAA3905;
@@ -99,7 +99,7 @@ private:
 
 	void ResetAccumulatedData();
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
 	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -788,7 +788,7 @@ void PAW3902::RunImpl()
 		return;
 	}
 
-	optical_flow_s report{};
+	sensor_optical_flow_s report{};
 	report.timestamp = timestamp_sample;
 	//report.device_id = get_device_id();
 
@@ -818,15 +818,15 @@ void PAW3902::RunImpl()
 
 	switch (_mode) {
 	case Mode::Bright:
-		report.mode = optical_flow_s::MODE_BRIGHT;
+		report.mode = sensor_optical_flow_s::MODE_BRIGHT;
 		break;
 
 	case Mode::LowLight:
-		report.mode = optical_flow_s::MODE_LOWLIGHT;
+		report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
 		break;
 
 	case Mode::SuperLowLight:
-		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
+		report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
 		break;
 	}
 

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -51,7 +51,7 @@
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 using namespace time_literals;
 using namespace PixArt_PAW3902JF;
@@ -97,7 +97,7 @@ private:
 
 	void ResetAccumulatedData();
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
 	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};

src/drivers/optical_flow/pmw3901/PMW3901.cpp

@@ -326,7 +326,7 @@ PMW3901::RunImpl()
 	delta_x = (float)_flow_sum_x / 385.0f;		// proportional factor + convert from pixels to radians
 	delta_y = (float)_flow_sum_y / 385.0f;		// proportional factor + convert from pixels to radians
 
-	optical_flow_s report{};
+	sensor_optical_flow_s report{};
 	report.timestamp = timestamp;
 
 	report.pixel_flow_x_integral = static_cast<float>(delta_x);

src/drivers/optical_flow/pmw3901/PMW3901.hpp

@@ -48,7 +48,7 @@
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
 /* Configuration Constants */
@@ -84,7 +84,7 @@ private:
 
 	const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t _sample_perf;
 	perf_counter_t _comms_errors;

src/drivers/optical_flow/px4flow/px4flow.cpp

@@ -51,7 +51,7 @@
 #include <px4_platform_common/module.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 /* Configuration Constants */
 #define I2C_FLOW_ADDRESS_DEFAULT    0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
@@ -98,7 +98,7 @@ private:
 	bool				_sensor_ok{false};
 	bool				_collect_phase{false};
 
-	uORB::PublicationMulti<optical_flow_s>		_px4flow_topic{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s>		_px4flow_topic{ORB_ID(sensor_optical_flow)};
 	uORB::PublicationMulti<distance_sensor_s>	_distance_sensor_topic{ORB_ID(distance_sensor)};
 
 	perf_counter_t		_sample_perf;
@@ -279,7 +279,7 @@ PX4FLOW::collect()
 	}
 
 
-	optical_flow_s report{};
+	sensor_optical_flow_s report{};
 
 	report.timestamp = hrt_absolute_time();
 	report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians

src/drivers/optical_flow/thoneflow/thoneflow.cpp

@@ -52,7 +52,7 @@
 #include <lib/parameters/param.h>
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/uORB.h>
 
 #include "thoneflow_parser.h"
@@ -81,7 +81,7 @@ private:
 
 	hrt_abstime              _last_read;
 
-	optical_flow_s           _report;
+	sensor_optical_flow_s    _report;
 	orb_advert_t             _optical_flow_pub;
 
 	perf_counter_t           _sample_perf;
@@ -233,7 +233,7 @@ Thoneflow::init()
 		_report.integration_timespan = 10526;	// microseconds
 
 		/* Get a publish handle on the optical flow topic */
-		_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report);
+		_optical_flow_pub = orb_advertise(ORB_ID(sensor_optical_flow), &_report);
 
 		if (_optical_flow_pub == nullptr) {
 			PX4_ERR("Failed to create optical_flow object");
@@ -312,7 +312,7 @@ Thoneflow::collect()
 			float zeroval = 0.0f;
 			rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval);
 
-			orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report);
+			orb_publish(ORB_ID(sensor_optical_flow), _optical_flow_pub, &_report);
 		}
 
 		/* Bytes left to parse */

src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp

@@ -61,7 +61,7 @@ const char *parser_state[] = {
 #endif
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *flow)
+		     sensor_optical_flow_s *flow)
 {
 	bool parsed_packet = false;
 

src/drivers/optical_flow/thoneflow/thoneflow_parser.h

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 // Data Format for ThoneFlow 3901U
 // ===============================
@@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE {
 };
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *report);
+		     sensor_optical_flow_s *report);

src/drivers/uavcan/sensors/flow.cpp

@@ -38,7 +38,7 @@
 const char *const UavcanFlowBridge::NAME = "flow";
 
 UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)),
+	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)),
 	_sub_flow(node)
 {
 }
@@ -58,7 +58,7 @@ UavcanFlowBridge::init()
 
 void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex::equipment::flow::Measurement> &msg)
 {
-	optical_flow_s flow{};
+	sensor_optical_flow_s flow{};
 
 	// We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID
 	flow.sensor_id = msg.getSrcNodeID().get();

src/drivers/uavcan/sensors/flow.hpp

@@ -37,7 +37,7 @@
 
 #include <stdint.h>
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include <com/hex/equipment/flow/Measurement.hpp>
 

src/drivers/uavcannode/Publishers/FlowMeasurement.hpp

@@ -52,7 +52,7 @@ class FlowMeasurement :
 public:
 	FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_optical_flow)),
 		uavcan::Publisher<com::hex::equipment::flow::Measurement>(node)
 	{
 		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}};
@@ -80,7 +80,7 @@ public:
 	void BroadcastAnyUpdates() override
 	{
 		// optical_flow -> com::hex::equipment::flow::Measurement
-		optical_flow_s optical_flow;
+		sensor_optical_flow_s optical_flow;
 
 		if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) {
 			com::hex::equipment::flow::Measurement measurement{};

src/modules/ekf2/EKF2.cpp

@@ -1680,14 +1680,14 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF flow sample
 	bool new_optical_flow = false;
-	const unsigned last_generation = _optical_flow_sub.get_last_generation();
-	optical_flow_s optical_flow;
+	const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation();
+	vehicle_optical_flow_s optical_flow;
 
-	if (_optical_flow_sub.update(&optical_flow)) {
+	if (_vehicle_optical_flow_sub.update(&optical_flow)) {
 		if (_msg_missed_optical_flow_perf == nullptr) {
 			_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
 
-		} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
+		} else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) {
 			perf_count(_msg_missed_optical_flow_perf);
 		}
 

src/modules/ekf2/EKF2.hpp

@@ -77,7 +77,6 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_status_flags.h>
 #include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -91,6 +90,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/yaw_estimator_status.h>
@@ -252,12 +252,12 @@ private:
 	uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
 	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
-	uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -20,7 +20,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
@@ -270,7 +270,7 @@ private:
 	uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
 	uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
-	uORB::SubscriptionData<optical_flow_s> _sub_flow{ORB_ID(optical_flow)};
+	uORB::SubscriptionData<vehicle_optical_flow_s> _sub_flow{ORB_ID(vehicle_optical_flow)};
 	uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
 	uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
 	uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};

src/modules/mavlink/mavlink_receiver.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 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
@@ -799,7 +799,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	mavlink_optical_flow_rad_t flow;
 	mavlink_msg_optical_flow_rad_decode(msg, &flow);
 
-	optical_flow_s f{};
+	sensor_optical_flow_s f{};
 
 	f.timestamp = hrt_absolute_time();
 	f.time_since_last_sonar_update = flow.time_delta_distance_us;
@@ -858,7 +858,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	mavlink_hil_optical_flow_t flow;
 	mavlink_msg_hil_optical_flow_decode(msg, &flow);
 
-	optical_flow_s f{};
+	sensor_optical_flow_s f{};
 
 	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
 	f.integration_timespan = flow.integration_time_us;

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 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
@@ -86,7 +86,7 @@
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
@@ -307,7 +307,7 @@ private:
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
-	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
+	uORB::Publication<sensor_optical_flow_s>		_flow_pub{ORB_ID(sensor_optical_flow)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};

src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp

@@ -34,7 +34,7 @@
 #ifndef OPTICAL_FLOW_RAD_HPP
 #define OPTICAL_FLOW_RAD_HPP
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 
 class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
@@ -55,11 +55,11 @@ public:
 private:
 	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
+	uORB::Subscription _optical_flow_sub{ORB_ID(vehicle_optical_flow)};
 
 	bool send() override
 	{
-		optical_flow_s flow;
+		vehicle_optical_flow_s flow;
 
 		if (_optical_flow_sub.update(&flow)) {
 			mavlink_optical_flow_rad_t msg{};

src/modules/replay/ReplayEkf2.cpp

@@ -39,7 +39,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -91,7 +91,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 	} else if (sub.orb_meta == ORB_ID(distance_sensor)) {
 		_distance_sensor_msg_id = msg_id;
 
-	} else if (sub.orb_meta == ORB_ID(optical_flow)) {
+	} else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) {
 		_optical_flow_msg_id = msg_id;
 
 	} else if (sub.orb_meta == ORB_ID(vehicle_air_data)) {

src/modules/sensors/CMakeLists.txt

@@ -40,6 +40,7 @@ add_subdirectory(vehicle_air_data)
 add_subdirectory(vehicle_gps_position)
 add_subdirectory(vehicle_imu)
 add_subdirectory(vehicle_magnetometer)
+add_subdirectory(vehicle_optical_flow)
 
 px4_add_module(
 	MODULE modules__sensors
@@ -62,4 +63,5 @@ px4_add_module(
 		vehicle_gps_position
 		vehicle_imu
 		vehicle_magnetometer
+		vehicle_optical_flow
 	)

src/modules/sensors/sensors.cpp

@@ -78,6 +78,7 @@
 #include "vehicle_gps_position/VehicleGPSPosition.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
 #include "vehicle_magnetometer/VehicleMagnetometer.hpp"
+#include "vehicle_optical_flow/VehicleOpticalFlow.hpp"
 
 using namespace sensors;
 using namespace time_literals;
@@ -181,6 +182,7 @@ private:
 	VehicleAirData          *_vehicle_air_data{nullptr};
 	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
 	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
+	VehicleOpticalFlow      _vehicle_optical_flow;
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
@@ -264,6 +266,7 @@ Sensors::Sensors(bool hil_enabled) :
 	_airspeed_validator.set_equal_value_threshold(100);
 
 	parameters_update();
+	_vehicle_optical_flow.Start();
 
 	InitializeVehicleAirData();
 	InitializeVehicleGPSPosition();
@@ -280,6 +283,7 @@ Sensors::~Sensors()
 
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
+	_vehicle_optical_flow.Stop();
 
 	if (_vehicle_air_data) {
 		_vehicle_air_data->Stop();
@@ -800,6 +804,8 @@ int Sensors::print_status()
 		}
 	}
 
+	_vehicle_optical_flow.PrintStatus();
+
 	return 0;
 }
 

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/VehicleOpticalFlow.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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)
+{
+}
+
+VehicleOpticalFlow::~VehicleOpticalFlow()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleOpticalFlow::Start()
+{
+	ScheduleNow();
+	return true;
+}
+
+void VehicleOpticalFlow::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto &sub : _sensor_sub) {
+		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();
+	}
+}
+
+void VehicleOpticalFlow::Run()
+{
+	perf_begin(_cycle_perf);
+
+	ParametersUpdate();
+
+
+	// reschedule timeout
+	ScheduleDelayed(100_ms);
+
+	perf_end(_cycle_perf);
+}
+
+void VehicleOpticalFlow::PrintStatus()
+{
+
+}
+
+}; // namespace sensors

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/systemlib/mavlink_log.h>
+#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/topics/parameter_update.h>
+#include <uORB/topics/sensor_optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
+
+namespace sensors
+{
+class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	VehicleOpticalFlow();
+	~VehicleOpticalFlow() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	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_flowpub{ORB_ID(vehicle_optical_flow)};
+
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
+		{this, ORB_ID(sensor_optical_flow), 0},
+		{this, ORB_ID(sensor_optical_flow), 1},
+		{this, ORB_ID(sensor_optical_flow), 2}
+	};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	bool _advertised[MAX_SENSOR_COUNT] {};
+
+	uint8_t _priority[MAX_SENSOR_COUNT] {};
+
+	int8_t _selected_sensor_sub_index{-1};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh
+	)
+};
+}; // namespace sensors

src/modules/simulator/simulator.h

@@ -61,7 +61,7 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
@@ -188,7 +188,7 @@ private:
 
 	// uORB publisher handlers
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
-	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s>	_flow_pub{ORB_ID(sensor_optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_odometry_s>		_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};

src/modules/simulator/simulator_mavlink.cpp

@@ -1285,7 +1285,7 @@ void Simulator::check_failure_injections()
 
 int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
 {
-	optical_flow_s flow = {};
+	sensor_optical_flow_s flow{};
 	flow.sensor_id = flow_mavlink->sensor_id;
 	flow.timestamp = hrt_absolute_time();
 	flow.time_since_last_sonar_update = 0;