From 79dc313260db86fdfea428d115024c6cfc142143 Mon Sep 17 00:00:00 2001
From: JacobCrabill <jacob@flyvoly.com>
Date: Fri, 28 Feb 2020 11:08:04 -0800
Subject: [PATCH] uavcan: Add device IDs to baro, flow publishers

Can now see proper UAVCAN bus and unique device ID for barometer
instances; optical flow will also have the UAVCAN node ID assigned as
sensor ID.
---
 src/drivers/uavcan/sensors/baro.cpp | 6 ++++--
 src/drivers/uavcan/sensors/flow.cpp | 3 +++
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp
index 22236e9e1f..7e6f6b0a90 100644
--- a/src/drivers/uavcan/sensors/baro.cpp
+++ b/src/drivers/uavcan/sensors/baro.cpp
@@ -85,6 +85,9 @@ UavcanBarometerBridge::air_pressure_sub_cb(const
 {
 	sensor_baro_s report{};
 
+	// Set the devid address to the UAVCAN node ID (so we get a unique address)
+	_device_id.devid_s.address = (uint8_t)(msg.getSrcNodeID().get() & 0xFF);
+
 	/*
 	 * FIXME HACK
 	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
@@ -97,8 +100,7 @@ UavcanBarometerBridge::air_pressure_sub_cb(const
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 	report.error_count = 0;
 
-	/* TODO get device ID for sensor */
-	report.device_id = 0;
+	report.device_id = _device_id.devid;
 
 	publish(msg.getSrcNodeID().get(), &report);
 }
diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp
index 33b32ab4df..5ec09f779f 100644
--- a/src/drivers/uavcan/sensors/flow.cpp
+++ b/src/drivers/uavcan/sensors/flow.cpp
@@ -67,6 +67,9 @@ UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex::equi
 {
 	optical_flow_s flow{};
 
+	// We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID
+	flow.sensor_id = (uint8_t)(msg.getSrcNodeID().get() & 0xFF);
+
 	flow.timestamp = hrt_absolute_time();
 	flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros
 	flow.pixel_flow_x_integral = msg.flow_integral[0];