From bc8ec5d1f5bd6ef88aeb263688b123cba4c538fd Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 13 Jan 2021 18:36:48 -0500
Subject: [PATCH] uavcannode: support optical_flow

---
 src/drivers/uavcannode/CMakeLists.txt |  2 +-
 src/drivers/uavcannode/UavcanNode.cpp | 20 ++++++++++++++++++++
 src/drivers/uavcannode/UavcanNode.hpp |  5 +++++
 3 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt
index a7071b082a..6e843bf641 100644
--- a/src/drivers/uavcannode/CMakeLists.txt
+++ b/src/drivers/uavcannode/CMakeLists.txt
@@ -94,7 +94,7 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC
 
 
 # generated DSDL
-set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
+set(DSDLC_INPUTS "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
 set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")
 
 set(DSDLC_INPUT_FILES)
diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp
index a9c478ee5b..dbbc443798 100644
--- a/src/drivers/uavcannode/UavcanNode.cpp
+++ b/src/drivers/uavcannode/UavcanNode.cpp
@@ -81,6 +81,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_air_data_static_temperature_publisher(_node),
 	_raw_air_data_publisher(_node),
 	_range_sensor_measurement(_node),
+	_flow_measurement_publisher(_node),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")),
 	_reset_timer(_node)
@@ -307,6 +308,7 @@ void UavcanNode::Run()
 
 		_sensor_baro_sub.registerCallback();
 		_sensor_mag_sub.registerCallback();
+		_optical_flow_sub.registerCallback();
 		_vehicle_gps_position_sub.registerCallback();
 
 		_initialized = true;
@@ -495,6 +497,24 @@ void UavcanNode::Run()
 		}
 	}
 
+	// optical_flow -> com::hex::equipment::flow::Measurement
+	if (_optical_flow_sub.updated()) {
+		optical_flow_s optical_flow;
+
+		if (_optical_flow_sub.copy(&optical_flow)) {
+			com::hex::equipment::flow::Measurement measurement{};
+
+			measurement.integration_interval  = optical_flow.integration_timespan * 1e-6f; // us -> s
+			measurement.rate_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
+			measurement.rate_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
+			measurement.flow_integral[0] = optical_flow.pixel_flow_x_integral;
+			measurement.flow_integral[1] = optical_flow.pixel_flow_y_integral;
+			measurement.quality = optical_flow.quality;
+
+			_flow_measurement_publisher.broadcast(measurement);
+		}
+	}
+
 	perf_end(_cycle_perf);
 
 	pthread_mutex_unlock(&_node_mutex);
diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp
index 2053b84c03..fae0a5c38d 100644
--- a/src/drivers/uavcannode/UavcanNode.hpp
+++ b/src/drivers/uavcannode/UavcanNode.hpp
@@ -65,6 +65,8 @@
 #include <uavcan/equipment/power/BatteryInfo.hpp>
 #include <uavcan/equipment/range_sensor/Measurement.hpp>
 
+#include <com/hex/equipment/flow/Measurement.hpp>
+
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 
@@ -74,6 +76,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -177,6 +180,7 @@ private:
 	uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature> _air_data_static_temperature_publisher;
 	uavcan::Publisher<uavcan::equipment::air_data::RawAirData> _raw_air_data_publisher;
 	uavcan::Publisher<uavcan::equipment::range_sensor::Measurement> _range_sensor_measurement;
+	uavcan::Publisher<com::hex::equipment::flow::Measurement> _flow_measurement_publisher;
 
 	hrt_abstime _last_static_temperature_publish{0};
 
@@ -191,6 +195,7 @@ private:
 		{this, ORB_ID(distance_sensor), 2},
 		{this, ORB_ID(distance_sensor), 3},
 	};
+	uORB::SubscriptionCallbackWorkItem _optical_flow_sub{this, ORB_ID(optical_flow)};
 	uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)};
 	uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};