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/drivers/optical_flow/CMakeLists.txt

@@ -1,37 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 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.
-#
-############################################################################
-
-add_subdirectory(paw3902)
-add_subdirectory(pmw3901)
-add_subdirectory(px4flow)
-add_subdirectory(thoneflow)

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -535,27 +535,26 @@ void PAA3905::RunImpl()
 		return;
 	}
 
-	optical_flow_s report{};
-	report.timestamp = timestamp_sample;
-	//report.device_id = get_device_id();
+	sensor_optical_flow_s report{};
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = get_device_id();
 
 	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
 	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
 
 	// rotate measurements in yaw from sensor frame to body frame
 	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
-	report.pixel_flow_x_integral = pixel_flow_rotated(0);
-	report.pixel_flow_y_integral = pixel_flow_rotated(1);
+	report.pixel_flow[0] = pixel_flow_rotated(0);
+	report.pixel_flow[1] = pixel_flow_rotated(1);
 
-	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
-	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
+	report.integration_timespan_us = _flow_dt_sum_usec;              // microseconds
 
 	report.quality = _flow_quality_sum / _flow_sample_counter;
 
 	// No gyro on this board
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
+	report.delta_angle[0] = NAN;
+	report.delta_angle[1] = NAN;
+	report.delta_angle[2] = NAN;
 
 	// set (conservative) specs according to datasheet
 	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
@@ -565,20 +564,20 @@ 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;
 	}
 
 	report.timestamp = hrt_absolute_time();
-	_optical_flow_pub.publish(report);
+	_sensor_optical_flow_pub.publish(report);
 
 	if (report.quality > 10) {
 		_last_good_publish = report.timestamp;

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> _sensor_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

@@ -788,27 +788,26 @@ void PAW3902::RunImpl()
 		return;
 	}
 
-	optical_flow_s report{};
-	report.timestamp = timestamp_sample;
-	//report.device_id = get_device_id();
+	sensor_optical_flow_s report{};
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = get_device_id();
 
 	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
 	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
 
 	// rotate measurements in yaw from sensor frame to body frame
 	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
-	report.pixel_flow_x_integral = pixel_flow_rotated(0);
-	report.pixel_flow_y_integral = pixel_flow_rotated(1);
+	report.pixel_flow[0] = pixel_flow_rotated(0);
+	report.pixel_flow[1] = pixel_flow_rotated(1);
 
-	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
-	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
+	report.integration_timespan_us = _flow_dt_sum_usec;              // microseconds
 
 	report.quality = _flow_quality_sum / _flow_sample_counter;
 
 	// No gyro on this board
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
+	report.delta_angle[0] = NAN;
+	report.delta_angle[1] = NAN;
+	report.delta_angle[2] = NAN;
 
 	// set (conservative) specs according to datasheet
 	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
@@ -818,20 +817,20 @@ 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;
 	}
 
 	report.timestamp = hrt_absolute_time();
-	_optical_flow_pub.publish(report);
+	_sensor_optical_flow_pub.publish(report);
 
 	if (report.quality > 10) {
 		_last_good_publish = report.timestamp;

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> _sensor_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

@@ -207,16 +207,6 @@ PMW3901::sensorInit()
 int
 PMW3901::init()
 {
-	// get yaw rotation from sensor frame to body frame
-	param_t rot = param_find("SENS_FLOW_ROT");
-
-	if (rot != PARAM_INVALID) {
-		int32_t val = 0;
-		param_get(rot, &val);
-
-		_yaw_rotation = (enum Rotation)val;
-	}
-
 	/* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
 	SPI::set_lockmode(LOCK_THREADS);
 
@@ -326,28 +316,24 @@ 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{};
-	report.timestamp = timestamp;
+	sensor_optical_flow_s report{};
+	report.timestamp_sample = timestamp;
 
-	report.pixel_flow_x_integral = static_cast<float>(delta_x);
-	report.pixel_flow_y_integral = static_cast<float>(delta_y);
+	report.pixel_flow[0] = static_cast<float>(delta_x);
+	report.pixel_flow[1] = static_cast<float>(delta_y);
 
-	// rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
+	// rotate measurements in yaw from sensor frame to body frame
 	float zeroval = 0.0f;
-	rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+	rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
 
-	report.frame_count_since_last_readout = _flow_sample_counter;	// number of frames
-	report.integration_timespan = _flow_dt_sum_usec; 	// microseconds
+	report.integration_timespan_us = _flow_dt_sum_usec; 	// microseconds
 
-	report.sensor_id = 0;
 	report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
 
-
 	/* No gyro on this board */
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
+	report.delta_angle[0] = NAN;
+	report.delta_angle[1] = NAN;
+	report.delta_angle[2] = NAN;
 
 	// set (conservative) specs according to datasheet
 	report.max_flow_rate = 5.0f;       // Datasheet: 7.4 rad/s
@@ -360,7 +346,8 @@ PMW3901::RunImpl()
 	_flow_sample_counter = 0;
 	_flow_quality_sum = 0;
 
-	_optical_flow_pub.publish(report);
+	report.timestamp = hrt_absolute_time();
+	_sensor_optical_flow_pub.publish(report);
 
 	perf_end(_sample_perf);
 }

src/drivers/optical_flow/pmw3901/PMW3901.hpp

@@ -45,10 +45,9 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#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,14 +83,14 @@ 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> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t _sample_perf;
 	perf_counter_t _comms_errors;
 
 	uint64_t _previous_collect_timestamp{0};
 
-	enum Rotation _yaw_rotation;
+	enum Rotation _yaw_rotation {};
 
 	int _flow_sum_x{0};
 	int _flow_sum_y{0};

src/drivers/optical_flow/px4flow/px4flow.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -41,8 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <lib/conversion/rotation.h>
-#include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -50,8 +48,7 @@
 #include <px4_platform_common/i2c_spi_buses.h>
 #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
@@ -89,28 +86,19 @@ public:
 	 * and start a new one.
 	 */
 	void				RunImpl();
-protected:
-	int			probe() override;
 
 private:
+	int			probe() override;
 
-	uint8_t _sonar_rotation;
 	bool				_sensor_ok{false};
 	bool				_collect_phase{false};
 
-	uORB::PublicationMulti<optical_flow_s>		_px4flow_topic{ORB_ID(optical_flow)};
-	uORB::PublicationMulti<distance_sensor_s>	_distance_sensor_topic{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 
-	enum Rotation       _sensor_rotation;
-	float 				_sensor_min_range{0.0f};
-	float 				_sensor_max_range{0.0f};
-	float 				_sensor_max_flow_rate{0.0f};
-
 	i2c_frame _frame;
-	i2c_integral_frame _frame_integral;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -134,15 +122,11 @@ private:
 
 };
 
-extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
-
 PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
-	_sonar_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
-	_sensor_rotation(Rotation::ROTATION_NONE)
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
 }
 
@@ -166,44 +150,6 @@ PX4FLOW::init()
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
 
-	/* get yaw rotation from sensor frame to body frame */
-	param_t rot = param_find("SENS_FLOW_ROT");
-
-	if (rot != PARAM_INVALID) {
-		int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
-		param_get(rot, &val);
-
-		_sensor_rotation = (enum Rotation)val;
-	}
-
-	/* get operational limits of the sensor */
-	param_t hmin = param_find("SENS_FLOW_MINHGT");
-
-	if (hmin != PARAM_INVALID) {
-		float val = 0.7;
-		param_get(hmin, &val);
-
-		_sensor_min_range = val;
-	}
-
-	param_t hmax = param_find("SENS_FLOW_MAXHGT");
-
-	if (hmax != PARAM_INVALID) {
-		float val = 3.0;
-		param_get(hmax, &val);
-
-		_sensor_max_range = val;
-	}
-
-	param_t ratemax = param_find("SENS_FLOW_MAXR");
-
-	if (ratemax != PARAM_INVALID) {
-		float val = 2.5;
-		param_get(ratemax, &val);
-
-		_sensor_max_flow_rate = val;
-	}
-
 	start();
 
 	return ret;
@@ -269,6 +215,8 @@ PX4FLOW::collect()
 		return ret;
 	}
 
+	i2c_integral_frame _frame_integral{};
+
 	if (PX4FLOW_REG == 0) {
 		memcpy(&_frame, val, I2C_FRAME_SIZE);
 		memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
@@ -279,52 +227,37 @@ PX4FLOW::collect()
 	}
 
 
-	optical_flow_s report{};
+	DeviceId device_id;
+	device_id.devid = get_device_id();
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
+	device_id.devid_s.address = get_i2c_address();
+
+	sensor_optical_flow_s report{};
+
+	report.timestamp_sample = hrt_absolute_time();
+	report.device_id = device_id.devid;
+
+	report.pixel_flow[0] = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians
+	report.pixel_flow[1] = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians
+
+	report.delta_angle_available = true;
+	report.delta_angle[0] = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[1] = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[2] = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
+
+	report.distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.f; // convert to meters
+	report.distance_available = true;
+
+	report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds
+
+	report.quality = _frame_integral.qual; // 0:bad ; 255 max quality
+
+	report.max_flow_rate = 2.5f;
+	report.min_ground_distance = PX4FLOW_MIN_DISTANCE;
+	report.max_ground_distance = PX4FLOW_MAX_DISTANCE;
 
 	report.timestamp = hrt_absolute_time();
-	report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
-	report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
-	report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
-	report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
-	report.quality = _frame_integral.qual; //0:bad ; 255 max quality
-	report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
-	report.integration_timespan = _frame_integral.integration_timespan; //microseconds
-	report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
-	report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
-	report.sensor_id = 0;
-	report.max_flow_rate = _sensor_max_flow_rate;
-	report.min_ground_distance = _sensor_min_range;
-	report.max_ground_distance = _sensor_max_range;
-
-	/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
-	float zeroval = 0.0f;
-
-	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
-
-	_px4flow_topic.publish(report);
-
-	/* publish to the distance_sensor topic as well */
-	if (_distance_sensor_topic.get_instance() == 0) {
-		distance_sensor_s distance_report{};
-		DeviceId device_id;
-		device_id.devid = get_device_id();
-		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
-
-		distance_report.timestamp = report.timestamp;
-		distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
-		distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
-		distance_report.current_distance = report.ground_distance_m;
-		distance_report.variance = 0.0f;
-		distance_report.signal_quality = -1;
-		distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-		distance_report.device_id = device_id.devid;
-		distance_report.orientation = _sonar_rotation;
-
-		_distance_sensor_topic.publish(distance_report);
-	}
+	_sensor_optical_flow_pub.publish(report);
 
 	perf_end(_sample_perf);
 
@@ -374,28 +307,16 @@ PX4FLOW::print_usage()
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
 	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 35, "Rotation (default=downwards)", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
-px4flow_main(int argc, char *argv[])
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[])
 {
-	int ch;
 	using ThisDriver = PX4FLOW;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED;
-	cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
 	const char *verb = cli.optArg();
 
 	if (!verb) {
@@ -415,13 +336,11 @@ px4flow_main(int argc, char *argv[])
 		}
 
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 

src/drivers/optical_flow/thoneflow/thoneflow.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 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
@@ -50,10 +50,10 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <perf/perf_counter.h>
-#include <systemlib/err.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/uORB.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include "thoneflow_parser.h"
 
@@ -71,21 +71,20 @@ public:
 	void print_info();
 
 private:
-	char                     _port[20];
-	Rotation            	 _rotation;
-	int                      _cycle_interval;
-	int                      _fd;
-	char                     _linebuf[5];
-	unsigned                 _linebuf_index;
-	THONEFLOW_PARSE_STATE    _parse_state;
+	char                     _port[20] {};
+	Rotation            	 _rotation{ROTATION_NONE};
+	int                      _cycle_interval{10526};
+	int                      _fd{-1};
+	char                     _linebuf[5] {};
+	unsigned                 _linebuf_index{0};
+	THONEFLOW_PARSE_STATE    _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC};
 
-	hrt_abstime              _last_read;
+	hrt_abstime              _last_read{0};
 
-	optical_flow_s           _report;
-	orb_advert_t             _optical_flow_pub;
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	perf_counter_t           _sample_perf;
-	perf_counter_t           _comms_errors;
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 
 	/**
 	* Initialise the automatic measurement state machine and start it.
@@ -106,23 +105,8 @@ private:
 
 };
 
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]);
-
 Thoneflow::Thoneflow(const char *port) :
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_rotation(Rotation(0)),
-	_cycle_interval(10526),
-	_fd(-1),
-	_linebuf_index(0),
-	_parse_state(THONEFLOW_PARSE_STATE0_UNSYNC),
-	_last_read(0),
-	_report(),
-	_optical_flow_pub(nullptr),
-	_sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
 {
 	/* store port name */
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -206,41 +190,6 @@ Thoneflow::init()
 			ret = PX4_ERROR;
 			break;
 		}
-
-		/* get yaw rotation from sensor frame to body frame */
-		param_t rot = param_find("SENS_FLOW_ROT");
-
-		if (rot != PARAM_INVALID) {
-			int32_t val = 0;
-			param_get(rot, &val);
-
-			_rotation = Rotation(val);
-		}
-
-		/* Initialise report structure */
-		/* No gyro on this board */
-		_report.gyro_x_rate_integral = NAN;
-		_report.gyro_y_rate_integral = NAN;
-		_report.gyro_z_rate_integral = NAN;
-
-		/* Conservative specs according to datasheet */
-		_report.max_flow_rate = 5.0f;           // Datasheet: 7.4 rad/s
-		_report.min_ground_distance = 0.1f;     // Datasheet: 80mm
-		_report.max_ground_distance = 30.0f;    // Datasheet: infinity
-
-		/* Integrated flow is sent at 66fps */
-		_report.frame_count_since_last_readout = 1;
-		_report.integration_timespan = 10526;	// microseconds
-
-		/* Get a publish handle on the optical flow topic */
-		_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report);
-
-		if (_optical_flow_pub == nullptr) {
-			PX4_ERR("Failed to create optical_flow object");
-			ret = PX4_ERROR;
-			break;
-		}
-
 	} while (0);
 
 	/* Close the fd */
@@ -299,20 +248,33 @@ Thoneflow::collect()
 
 		_last_read = hrt_absolute_time();
 
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = hrt_absolute_time();
+
 		/* Parse each byte of read buffer */
 		for (int i = 0; i < ret; i++) {
-			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report);
+			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report);
 		}
 
 		/* Publish most recent valid measurement */
 		if (valid) {
-			_report.timestamp = hrt_absolute_time();
+
+			report.device_id = 0; // TODO get_device_id();
+			report.integration_timespan_us = 10526; // microseconds
 
 			/* Rotate measurements from sensor frame to body frame */
 			float zeroval = 0.0f;
-			rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval);
+			rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
 
-			orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report);
+			// Conservative specs according to datasheet
+			report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+			report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+			report.max_ground_distance = INFINITY; // Datasheet: infinity
+
+			report.timestamp = hrt_absolute_time();
+
+			_sensor_optical_flow_pub.publish(report);
 		}
 
 		/* Bytes left to parse */
@@ -478,16 +440,15 @@ $ thoneflow stop
 
     PRINT_MODULE_USAGE_NAME("thoneflow", "driver");
     PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
     PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-    PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information");
 }
 
 } // namespace
 
-int
-thoneflow_main(int argc, char *argv[])
+extern "C" __EXPORT int thoneflow_main(int argc, char *argv[])
 {
     int ch;
     const char *device_path = "";
@@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[])
             thoneflow::usage();
             return -1;
         }
-    }
 
-    /*
-     * Stop the driver
-     */
-    if (!strcmp(argv[myoptind], "stop")) {
+    } else if (!strcmp(argv[myoptind], "stop")) {
         return thoneflow::stop();
-    }
 
-    /*
-     * Print driver information.
-     */
-    if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+    } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
         thoneflow::info();
         return 0;
     }

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;
 
@@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH
 				// Checksum valid, populate sensor report
 				int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0];
 				int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2];
-				flow->pixel_flow_x_integral = static_cast<float>(delta_x) * (3.52e-3f);
-				flow->pixel_flow_y_integral = static_cast<float>(delta_y) * (3.52e-3f);
+				flow->pixel_flow[0] = static_cast<float>(delta_x) * (3.52e-3f);
+				flow->pixel_flow[1] = static_cast<float>(delta_y) * (3.52e-3f);
 				*state = THONEFLOW_PARSE_STATE7_CHECKSUM;
 
 			} else {

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);