UAVCAN: fix and improve device_id logic

src/drivers/drv_sensor.h

@@ -184,6 +184,7 @@
 #define DRV_IMU_DEVTYPE_UAVCAN	0x87
 #define DRV_MAG_DEVTYPE_UAVCAN	0x88
 #define DRV_DIST_DEVTYPE_UAVCAN	0x89
+#define DRV_HYGRO_DEVTYPE_UAVCAN	0x8A
 
 #define DRV_ADC_DEVTYPE_ADS1115        0x90
 

src/drivers/uavcan/sensors/accel.cpp

@@ -43,7 +43,9 @@ const char *const UavcanAccelBridge::NAME = "accel";
 UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher),
 	_sub_imu_data(node)
-{ }
+{
+	set_device_type(DRV_ACC_DEVTYPE_UAVCAN);
+}
 
 int UavcanAccelBridge::init()
 {
@@ -59,7 +61,7 @@ int UavcanAccelBridge::init()
 
 void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
@@ -87,13 +89,10 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 
 int UavcanAccelBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
-
-	device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
 
-	channel->h_driver = new PX4Accelerometer(device_id.devid);
+	channel->h_driver = new PX4Accelerometer(device_id);
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;

src/drivers/uavcan/sensors/baro.cpp

@@ -49,7 +49,9 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublis
 	UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher),
 	_sub_air_pressure_data(node),
 	_sub_air_temperature_data(node)
-{ }
+{
+	set_device_type(DRV_BARO_DEVTYPE_UAVCAN);
+}
 
 int UavcanBarometerBridge::init()
 {
@@ -91,7 +93,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 {
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -105,23 +107,18 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 		return;
 	}
 
-	DeviceId device_id{};
-	device_id.devid_s.bus = 0;
-	device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
-
-	device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	uint32_t device_id = make_uavcan_device_id(msg);
 
 	// Register barometer capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {
-		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid,
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id,
 				NodeInfoPublisher::DeviceCapability::BAROMETER);
 	}
 
 	// publish
 	sensor_baro_s sensor_baro{};
 	sensor_baro.timestamp_sample = timestamp_sample;
-	sensor_baro.device_id = device_id.devid;
+	sensor_baro.device_id = device_id;
 	sensor_baro.pressure = msg.static_pressure;
 
 	if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) {

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -49,6 +49,7 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode
 	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher),
 	_sub_air(node)
 {
+	set_device_type(DRV_DIFF_PRESS_DEVTYPE_UAVCAN);
 }
 
 int UavcanDifferentialPressureBridge::init()
@@ -68,8 +69,6 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 {
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
-	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
-	_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
 	float diff_press_pa = msg.differential_pressure;
 	int32_t differential_press_rev = 0;
 	param_get(param_find("SENS_DPRES_REV"), &differential_press_rev);
@@ -83,7 +82,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 
 	differential_pressure_s report{};
 	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id.devid;
+	report.device_id = make_uavcan_device_id(msg);
 	report.differential_pressure_pa = diff_press_pa;
 	report.temperature = temperature_c;
 	report.timestamp = hrt_absolute_time();

src/drivers/uavcan/sensors/flow.cpp

@@ -41,6 +41,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_
 	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher),
 	_sub_flow(node)
 {
+	set_device_type(DRV_FLOW_DEVTYPE_UAVCAN);
 }
 
 int
@@ -61,13 +62,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex:
 	sensor_optical_flow_s flow{};
 	flow.timestamp_sample = hrt_absolute_time(); // TODO
 
-	device::Device::DeviceId device_id;
-	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
-	device_id.devid_s.bus = 0;
-	device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
-
-	flow.device_id = device_id.devid;
+	flow.device_id = make_uavcan_device_id(msg);
 
 	flow.pixel_flow[0] = msg.flow_integral[0];
 	flow.pixel_flow[1] = msg.flow_integral[1];

src/drivers/uavcan/sensors/gnss.cpp

@@ -405,12 +405,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 {
 	sensor_gps_s sensor_gps{};
 
-	device::Device::DeviceId device_id{};
-	device_id.devid_s.devtype = get_device_type();
-	device_id.devid_s.address = static_cast<uint8_t>(msg.getSrcNodeID().get());
-	device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN;
-	device_id.devid_s.bus = 0;
-	sensor_gps.device_id = device_id.devid;
+	sensor_gps.device_id = make_uavcan_device_id(msg);
 
 	// Register GPS capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {

src/drivers/uavcan/sensors/gnss_relative.cpp

@@ -42,6 +42,7 @@ UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfo
 	UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher),
 	_sub_rel_pos_heading(node)
 {
+	set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
 }
 
 int
@@ -70,7 +71,7 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const
 	sensor_gnss_relative.position_length = msg.relative_distance_m;
 	sensor_gnss_relative.position[2] = msg.relative_down_pos_m;
 
-	sensor_gnss_relative.device_id = get_device_id();
+	sensor_gnss_relative.device_id = make_uavcan_device_id(msg);
 
 	// Register GPS capability with NodeInfoPublisher after first successful message
 	if (_node_info_publisher != nullptr) {

src/drivers/uavcan/sensors/gyro.cpp

@@ -43,7 +43,9 @@ const char *const UavcanGyroBridge::NAME = "gyro";
 UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher),
 	_sub_imu_data(node)
-{ }
+{
+	set_device_type(DRV_GYR_DEVTYPE_UAVCAN);
+}
 
 int UavcanGyroBridge::init()
 {
@@ -59,7 +61,7 @@ int UavcanGyroBridge::init()
 
 void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -87,13 +89,10 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 
 int UavcanGyroBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
-
-	device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
 
-	channel->h_driver = new PX4Gyroscope(device_id.devid);
+	channel->h_driver = new PX4Gyroscope(device_id);
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;

src/drivers/uavcan/sensors/hygrometer.cpp

@@ -42,6 +42,7 @@ UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPubl
 	UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher),
 	_sub_hygro(node)
 {
+	set_device_type(DRV_HYGRO_DEVTYPE_UAVCAN);
 }
 
 int UavcanHygrometerBridge::init()
@@ -64,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure<dr
 
 	sensor_hygrometer_s report{};
 	report.timestamp_sample = timestamp_sample;
-	report.device_id = 0; // TODO
+	report.device_id = make_uavcan_device_id(msg);
 	report.temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius;
 	report.humidity = msg.humidity;
 	report.timestamp = hrt_absolute_time();

src/drivers/uavcan/sensors/mag.cpp

@@ -49,6 +49,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node, NodeInfo
 	_sub_mag(node),
 	_sub_mag2(node)
 {
+	set_device_type(DRV_MAG_DEVTYPE_UAVCAN);
 }
 
 int UavcanMagnetometerBridge::init()
@@ -73,7 +74,7 @@ int UavcanMagnetometerBridge::init()
 void UavcanMagnetometerBridge::mag_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -104,7 +105,7 @@ void
 UavcanMagnetometerBridge::mag2_sub_cb(const
 				      uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	if (channel == nullptr || channel->instance < 0) {
 		// Something went wrong - no channel to publish on; return
@@ -134,13 +135,10 @@ UavcanMagnetometerBridge::mag2_sub_cb(const
 
 int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
 
-	device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
-
-	channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE);
+	channel->h_driver = new PX4Magnetometer(device_id, ROTATION_NONE);
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;

src/drivers/uavcan/sensors/rangefinder.cpp

@@ -45,7 +45,9 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder";
 UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher),
 	_sub_range_data(node)
-{ }
+{
+	set_device_type(DRV_DIST_DEVTYPE_UAVCAN);
+}
 
 int UavcanRangefinderBridge::init()
 {
@@ -66,7 +68,7 @@ int UavcanRangefinderBridge::init()
 void UavcanRangefinderBridge::range_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::range_sensor::Measurement> &msg)
 {
-	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
 	if (channel == nullptr || channel->instance < 0) {
 		// Something went wrong - no channel to publish on; return
@@ -125,13 +127,10 @@ void UavcanRangefinderBridge::range_sub_cb(const
 
 int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
-
-	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	// Build device ID using node_id and interface index
+	uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
 
-	channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	channel->h_driver = new PX4Rangefinder(device_id, distance_sensor_s::ROTATION_DOWNWARD_FACING);
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -316,7 +316,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report)
 	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }
 
-uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id)
+uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id, uint8_t iface_index)
 {
 	uavcan_bridge::Channel *channel = nullptr;
 
@@ -354,6 +354,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id
 
 		// initialize the driver, which registers the class device name and uORB publisher
 		channel->node_id = node_id;
+		channel->iface_index = iface_index;
 		int ret = init_driver(channel);
 
 		if (ret != PX4_OK) {

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -92,6 +92,7 @@ struct Channel {
 	orb_advert_t orb_advert{nullptr};
 	int instance{-1};
 	void *h_driver{nullptr};
+	uint8_t iface_index{0};
 };
 } // namespace uavcan_bridge
 
@@ -138,7 +139,34 @@ class UavcanSensorBridgeBase : public IUavcanSensorBridge, public device::Device
 	 */
 	virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; };
 
-	uavcan_bridge::Channel *get_channel_for_node(int node_id);
+	uavcan_bridge::Channel *get_channel_for_node(int node_id, uint8_t iface_index);
+
+	/**
+	 * Builds a unique device ID from a UAVCAN message
+	 * @param msg UAVCAN message (must have getSrcNodeID() and getIfaceIndex() methods)
+	 * @return Complete device ID with node address and interface encoded
+	 */
+	template<typename T>
+	uint32_t make_uavcan_device_id(const uavcan::ReceivedDataStructure<T> &msg) const
+	{
+		return make_uavcan_device_id(msg.getSrcNodeID().get(), msg.getIfaceIndex());
+	}
+
+	/**
+	 * Builds a unique device ID from node ID and interface index
+	 * @param node_id UAVCAN node ID
+	 * @param iface_index CAN interface index (0 = CAN1, 1 = CAN2, etc.)
+	 * @return Complete device ID with node address and interface encoded
+	 */
+	uint32_t make_uavcan_device_id(uint8_t node_id, uint8_t iface_index) const
+	{
+		device::Device::DeviceId device_id{};
+		device_id.devid_s.devtype = get_device_type();
+		device_id.devid_s.address = node_id;
+		device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN;
+		device_id.devid_s.bus = iface_index;
+		return device_id.devid;
+	}
 
 public:
 	virtual ~UavcanSensorBridgeBase();