UAVCAN: Add device tracking and information publishing (#25617)

* uavcan: collect node info and publish every second

* UORB: Add DeviceInformation Message

Format DeviceInformation.msg with standard comment spaces

* SENS: add getter for device_id

* UAVCAN: add publishing of DeviceInformation based on publised message type, and Node Information

* LOG: add deviceInformation

* MSG:BAT: fix comment to be inline with the max_instaces

* UAVCAN: DeviceInformation, incorporated feedback

* UAVCAN: DeviceInformation, incorporated feedback

* UAVCAN: DeviceInformation, Fixed bug with Powermonitor

---------

Co-authored-by: Beat Küng <beat-kueng@gmx.net>

msg/CMakeLists.txt

@@ -63,6 +63,7 @@ set(msg_files
 	DebugKeyValue.msg
 	DebugValue.msg
 	DebugVect.msg
+	DeviceInformation.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	DistanceSensorModeChangeRequest.msg

msg/DeviceInformation.msg

@@ -0,0 +1,33 @@
+# Device information
+#
+# Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number.
+# as well as tracking of the used firmware versions on the devices.
+
+uint64 timestamp  # time since system start (microseconds)
+
+uint8 device_type  # [@enum DEVICE_TYPE] Type of the device. Matches MAVLink DEVICE_TYPE enum
+
+uint8 DEVICE_TYPE_GENERIC = 0                 # Generic/unknown sensor
+uint8 DEVICE_TYPE_AIRSPEED = 1                # Airspeed sensor
+uint8 DEVICE_TYPE_ESC = 2                     # ESC
+uint8 DEVICE_TYPE_SERVO = 3                   # Servo
+uint8 DEVICE_TYPE_GPS = 4                     # GPS
+uint8 DEVICE_TYPE_MAGNETOMETER = 5            # Magnetometer
+uint8 DEVICE_TYPE_PARACHUTE = 6               # Parachute
+uint8 DEVICE_TYPE_RANGEFINDER = 7             # Rangefinder
+uint8 DEVICE_TYPE_WINCH = 8                   # Winch
+uint8 DEVICE_TYPE_BAROMETER = 9               # Barometer
+uint8 DEVICE_TYPE_OPTICAL_FLOW = 10           # Optical flow
+uint8 DEVICE_TYPE_ACCELEROMETER = 11          # Accelerometer
+uint8 DEVICE_TYPE_GYROSCOPE = 12              # Gyroscope
+uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13  # Differential pressure
+uint8 DEVICE_TYPE_BATTERY = 14                # Battery
+uint8 DEVICE_TYPE_HYGROMETER = 15             # Hygrometer
+
+char[32] vendor_name  # Name of the device vendor
+char[32] model_name   # Name of the device model
+
+uint32   device_id         # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles.
+char[24] firmware_version  # [-] [@invalid empty if not available] Firmware version.
+char[24] hardware_version  # [-] [@invalid empty if not available] Hardware version.
+char[33] serial_number     # [-] [@invalid empty if not available] Device serial number or unique identifier.

msg/versioned/BatteryStatus.msg

@@ -1,6 +1,6 @@
 # Battery status
 #
-# Battery status information for up to 4 battery instances.
+# Battery status information for up to 3 battery instances.
 # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink.
 # Battery instance information is also logged and streamed in MAVLink telemetry.
 

src/drivers/uavcan/CMakeLists.txt

@@ -158,6 +158,7 @@ px4_add_module(
 		# Main
 		uavcan_main.cpp
 		uavcan_servers.cpp
+		node_info.cpp
 
 		# Actuators
 		actuators/esc.cpp

src/drivers/uavcan/actuators/esc.cpp

@@ -128,6 +128,13 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 		_esc_status.timestamp = hrt_absolute_time();
 		_esc_status_pub.publish(_esc_status);
 	}
+
+	// Register device capability for each ESC channel
+	if (_node_info_publisher != nullptr) {
+		uint8_t node_id = msg.getSrcNodeID().get();
+		uint32_t device_id = msg.esc_index;
+		_node_info_publisher->registerDeviceCapability(node_id, device_id, NodeInfoPublisher::DeviceCapability::ESC);
+	}
 }
 
 uint8_t

src/drivers/uavcan/actuators/esc.hpp

@@ -52,7 +52,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
-#include <lib/mixer_module/mixer_module.hpp>
+#include <drivers/uavcan/node_info.hpp>
 #include <parameters/param.h>
 
 class UavcanEscController
@@ -78,6 +78,8 @@ public:
 	 */
 	void set_rotor_count(uint8_t count);
 
+	void set_node_info_publisher(NodeInfoPublisher *publisher) { _node_info_publisher = publisher; }
+
 	static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
 
 	esc_status_s &esc_status() { return _esc_status; }
@@ -114,4 +116,6 @@ private:
 	uavcan::INode								&_node;
 	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
+
+	NodeInfoPublisher *_node_info_publisher{nullptr};
 };

src/drivers/uavcan/node_info.cpp

@@ -0,0 +1,308 @@
+/****************************************************************************
+*
+ *   Copyright (c) 2025 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 "node_info.hpp"
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/px4_config.h>
+#include <drivers/drv_hrt.h>
+#include <cstring>
+
+using namespace time_literals;
+
+NodeInfoPublisher::NodeInfoPublisher(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever)
+	: TimerBase(node), _node_info_retriever(node_info_retriever)
+{
+	_node_info_retriever.addListener(this);
+}
+
+NodeInfoPublisher::~NodeInfoPublisher()
+{
+	_node_info_retriever.removeListener(this);
+	delete[] _device_informations;
+}
+
+void NodeInfoPublisher::handleNodeInfoRetrieved(uavcan::NodeID node_id,
+		const uavcan::protocol::GetNodeInfo_::Response &node_info)
+{
+	const NodeInfo info(node_id, node_info);
+	registerDevice(info.node_id.get(), &info, UINT32_MAX, DeviceCapability::NONE);
+
+	startTimerIfNotRunning();
+}
+
+void NodeInfoPublisher::handleNodeInfoUnavailable(uavcan::NodeID node_id)
+{
+}
+
+void NodeInfoPublisher::handleTimerEvent(const uavcan::TimerEvent &event)
+{
+	// Publish device information using round-robin approach
+	publishDeviceInformationPeriodic();
+}
+
+void NodeInfoPublisher::startTimerIfNotRunning()
+{
+	if (!TimerBase::isRunning()) {
+		TimerBase::startPeriodic(uavcan::MonotonicDuration::fromMSec(DEVICE_INFO_PUBLISH_INTERVAL_MS));
+	}
+}
+
+void NodeInfoPublisher::registerDevice(uint8_t node_id, const NodeInfo *info, uint32_t device_id,
+				       DeviceCapability capability)
+{
+	const bool is_registering_info = (info != nullptr);
+
+	int multi_capability_index = -1;
+
+	for (size_t i = 0; i < _device_informations_size; ++i) {
+		if (is_registering_info) {
+			// Case 1: Check if this entry already has node info - skip this specific entry
+			if (_device_informations[i].node_id == node_id &&
+			    _device_informations[i].has_node_info) {
+
+				continue;  // Continue to check other entries with same node_id
+			}
+
+			// Case 2: Check if node_id already exists with capability but no info - update that entry
+			if (_device_informations[i].node_id == node_id &&
+			    _device_informations[i].capability != DeviceCapability::NONE &&
+			    !_device_informations[i].has_node_info) {
+				populateDeviceInfoFields(_device_informations[i], *info);
+				publishSingleDeviceInformation(_device_informations[i]);
+				continue;
+			}
+
+		} else { // registering capabilities
+			// Case 1: Check if this exact capability already exists - skip
+			if (_device_informations[i].node_id == node_id &&
+			    _device_informations[i].device_id == device_id &&
+			    _device_informations[i].capability == capability) {
+				return;
+			}
+
+			// Case 1b: if this node has multiple capabilities, continue
+			if (_device_informations[i].node_id == node_id &&
+			    _device_informations[i].capability != DeviceCapability::NONE  &&
+			    _device_informations[i].capability != capability) {
+				multi_capability_index = i;
+				continue;
+			}
+
+			// Case 2: Check if node_id already exists with node info but no capability - update that entry
+			if (_device_informations[i].node_id == node_id &&
+			    _device_informations[i].has_node_info &&
+			    _device_informations[i].capability == DeviceCapability::NONE) {
+				_device_informations[i].device_id = device_id;
+				_device_informations[i].capability = capability;
+				publishSingleDeviceInformation(_device_informations[i]);
+				return;
+			}
+		}
+	}
+
+
+
+	// Case 3: extend array and add entry at the end
+	if (extendDeviceInformationsArray()) {
+		_device_informations[_device_informations_size - 1] = DeviceInformation();
+		_device_informations[_device_informations_size - 1].node_id = node_id;
+
+		if (multi_capability_index >= 0) {
+			_device_informations[_device_informations_size - 1] = _device_informations[multi_capability_index];
+			_device_informations[_device_informations_size - 1].node_id = node_id;
+		}
+
+		if (is_registering_info) {
+			populateDeviceInfoFields(_device_informations[_device_informations_size - 1], *info);
+
+		} else {
+			_device_informations[_device_informations_size - 1].device_id = device_id;
+			_device_informations[_device_informations_size - 1].capability = capability;
+		}
+
+	} else {
+		PX4_DEBUG("Failed to extend device informations array for %s",
+			  is_registering_info ? "node info" : "capability");
+	}
+}
+
+void NodeInfoPublisher::registerDeviceCapability(uint8_t node_id, uint32_t device_id, DeviceCapability capability)
+{
+	registerDevice(node_id, nullptr, device_id, capability);
+}
+
+void NodeInfoPublisher::publishDeviceInformationPeriodic()
+{
+	// Using round-robin approach to publish one device info per timer event
+	if (_device_informations_size == 0) {
+		return;
+	}
+
+	size_t devices_checked = 0;
+
+	while (devices_checked < _device_informations_size) {
+		if (_next_device_to_publish >= _device_informations_size) {
+			_next_device_to_publish = 0;
+		}
+
+		const auto &device_info = _device_informations[_next_device_to_publish];
+
+		if (device_info.has_node_info && device_info.capability != DeviceCapability::NONE) {
+			publishSingleDeviceInformation(device_info);
+			_next_device_to_publish++;
+			return;
+		}
+
+		_next_device_to_publish++;
+		devices_checked++;
+	}
+
+	PX4_DEBUG("No devices ready for periodic publishing");
+}
+
+void NodeInfoPublisher::publishSingleDeviceInformation(const DeviceInformation &device_info)
+{
+	const uint64_t now = hrt_absolute_time();
+
+	device_information_s msg{};
+	msg.timestamp = now;
+	msg.device_type = static_cast<uint8_t>(device_info.capability);
+	msg.device_id = device_info.device_id;
+
+	// Copy pre-populated fields directly from the struct
+	static_assert(sizeof(msg.model_name) == sizeof(device_info.model_name), "Array size mismatch");
+	static_assert(sizeof(msg.vendor_name) == sizeof(device_info.vendor_name), "Array size mismatch");
+	static_assert(sizeof(msg.firmware_version) == sizeof(device_info.firmware_version), "Array size mismatch");
+	static_assert(sizeof(msg.hardware_version) == sizeof(device_info.hardware_version), "Array size mismatch");
+	static_assert(sizeof(msg.serial_number) == sizeof(device_info.serial_number), "Array size mismatch");
+
+	memcpy(msg.model_name, device_info.model_name, sizeof(msg.model_name));
+	msg.model_name[sizeof(msg.model_name) - 1] = '\0';
+
+	memcpy(msg.vendor_name, device_info.vendor_name, sizeof(msg.vendor_name));
+	msg.vendor_name[sizeof(msg.vendor_name) - 1] = '\0';
+
+	memcpy(msg.firmware_version, device_info.firmware_version, sizeof(msg.firmware_version));
+	msg.firmware_version[sizeof(msg.firmware_version) - 1] = '\0';
+
+	memcpy(msg.hardware_version, device_info.hardware_version, sizeof(msg.hardware_version));
+	msg.hardware_version[sizeof(msg.hardware_version) - 1] = '\0';
+
+	memcpy(msg.serial_number, device_info.serial_number, sizeof(msg.serial_number));
+	msg.serial_number[sizeof(msg.serial_number) - 1] = '\0';
+
+	_device_info_pub.publish(msg);
+
+	PX4_DEBUG("Published device info for node %d, device_id %lu, type %d",
+		  device_info.node_id, static_cast<unsigned long>(device_info.device_id),
+		  static_cast<int>(device_info.capability));
+}
+
+void NodeInfoPublisher::populateDeviceInfoFields(DeviceInformation &device_info, const NodeInfo &info)
+{
+	device_info.has_node_info = true;
+
+	// Parse the node name to extract vendor and model information
+	parseNodeName(info.name, device_info);
+
+	snprintf(device_info.firmware_version, sizeof(device_info.firmware_version),
+		 "%d.%d.%lu", info.sw_major, info.sw_minor, static_cast<unsigned long>(info.vcs_commit));
+	snprintf(device_info.hardware_version, sizeof(device_info.hardware_version),
+		 "%d.%d", info.hw_major, info.hw_minor);
+	snprintf(device_info.serial_number, sizeof(device_info.serial_number),
+		 "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+		 info.unique_id[0], info.unique_id[1], info.unique_id[2], info.unique_id[3],
+		 info.unique_id[4], info.unique_id[5], info.unique_id[6], info.unique_id[7],
+		 info.unique_id[8], info.unique_id[9], info.unique_id[10], info.unique_id[11],
+		 info.unique_id[12], info.unique_id[13], info.unique_id[14], info.unique_id[15]);
+}
+
+void NodeInfoPublisher::parseNodeName(const char *name, DeviceInformation &device_info)
+{
+	if (!name || strlen(name) == 0) {
+		strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
+		strlcpy(device_info.model_name, "", sizeof(device_info.model_name));
+		return;
+	}
+
+	// Find first dot and skip everything before it
+	const char *after_first_dot = strchr(name, '.');
+
+	if (after_first_dot == nullptr) {
+		// No dot - whole string is model, vendor is -1
+		strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
+		strlcpy(device_info.model_name, name, sizeof(device_info.model_name));
+		return;
+	}
+
+	after_first_dot++;
+
+	// Find next dot in remaining string
+	const char *second_dot = strchr(after_first_dot, '.');
+
+	if (second_dot == nullptr) {
+		// Only one dot - everything after first dot is model, vendor is -1
+		strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
+		strlcpy(device_info.model_name, after_first_dot,  sizeof(device_info.model_name));
+		return;
+	}
+
+	// Copy vendor (between first and second dot)
+	size_t vendor_len = second_dot - after_first_dot;
+	size_t copy_len = (vendor_len < sizeof(device_info.vendor_name) - 1) ? vendor_len : sizeof(device_info.vendor_name) - 1;
+	strncpy(device_info.vendor_name, after_first_dot, copy_len);
+	device_info.vendor_name[copy_len] = '\0';
+
+	// Copy model (everything after second dot)
+	strlcpy(device_info.model_name, second_dot + 1, sizeof(device_info.model_name));
+}
+
+bool NodeInfoPublisher::extendDeviceInformationsArray()
+{
+	const size_t new_size = _device_informations_size + 1;
+	DeviceInformation *new_array = new DeviceInformation[new_size];
+
+	if (!new_array) {
+		return false;
+	}
+
+	if (_device_informations_size > 0 && _device_informations != nullptr) {
+		memcpy(new_array, _device_informations, _device_informations_size * sizeof(DeviceInformation));
+		delete[] _device_informations;
+	}
+
+	_device_informations = new_array;
+	_device_informations_size = new_size;
+	return true;
+}

src/drivers/uavcan/node_info.hpp

@@ -0,0 +1,164 @@
+/****************************************************************************
+*
+ *   Copyright (c) 2025 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 <uavcan/protocol/node_info_retriever.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/device_information.h>
+#include <stdint.h>  // For UINT8_MAX, UINT32_MAX
+#include <px4_platform_common/time.h>
+
+using namespace time_literals;
+
+constexpr int 		DEVICE_INFO_PUBLISH_INTERVAL_MS 	= 1000;
+constexpr hrt_abstime 	DEVICE_INFO_PUBLISH_RATE_LIMIT_US 	= 100_ms;
+
+class NodeInfoPublisher : private uavcan::INodeInfoListener, private uavcan::TimerBase
+{
+public:
+	enum class DeviceCapability : uint8_t {
+		NONE = UINT8_MAX,  // Invalid/unset capability value (255)
+		GENERIC = device_information_s::DEVICE_TYPE_GENERIC,
+		AIRSPEED = device_information_s::DEVICE_TYPE_AIRSPEED,
+		ESC = device_information_s::DEVICE_TYPE_ESC,
+		SERVO = device_information_s::DEVICE_TYPE_SERVO,
+		GPS = device_information_s::DEVICE_TYPE_GPS,
+		MAGNETOMETER = device_information_s::DEVICE_TYPE_MAGNETOMETER,
+		PARACHUTE = device_information_s::DEVICE_TYPE_PARACHUTE,
+		RANGEFINDER = device_information_s::DEVICE_TYPE_RANGEFINDER,
+		WINCH = device_information_s::DEVICE_TYPE_WINCH,
+		BAROMETER = device_information_s::DEVICE_TYPE_BAROMETER,
+		OPTICAL_FLOW = device_information_s::DEVICE_TYPE_OPTICAL_FLOW,
+		ACCELEROMETER = device_information_s::DEVICE_TYPE_ACCELEROMETER,
+		GYROSCOPE = device_information_s::DEVICE_TYPE_GYROSCOPE,
+		DIFFERENTIAL_PRESSURE = device_information_s::DEVICE_TYPE_DIFFERENTIAL_PRESSURE,
+		BATTERY = device_information_s::DEVICE_TYPE_BATTERY,
+		HYGROMETER = device_information_s::DEVICE_TYPE_HYGROMETER,
+	};
+
+	NodeInfoPublisher(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
+	~NodeInfoPublisher();
+
+	// Called by sensor bridges to register device capabilities
+	void registerDeviceCapability(uint8_t node_id, uint32_t device_id, DeviceCapability capability);
+
+private:
+	struct NodeInfo {
+		NodeInfo(uavcan::NodeID id, const uavcan::protocol::GetNodeInfo_::Response &node_info)
+			: node_id(id), sw_major(node_info.software_version.major), sw_minor(node_info.software_version.minor),
+			  vcs_commit(node_info.software_version.vcs_commit), hw_major(node_info.hardware_version.major),
+			  hw_minor(node_info.hardware_version.minor)
+		{
+			memcpy(name, node_info.name.c_str(), node_info.name.capacity());
+			name[node_info.name.capacity() - 1] = '\0';
+			memcpy(unique_id, &node_info.hardware_version.unique_id.front(), node_info.hardware_version.unique_id.size());
+		}
+		NodeInfo() = default;
+
+		uavcan::NodeID node_id{};
+
+		char name[uavcan::protocol::GetNodeInfo_::Response::FieldTypes::name::MaxSize];
+		uint8_t unique_id[uavcan::protocol::GetNodeInfo_::Response::FieldTypes::hardware_version::FieldTypes::unique_id::MaxSize];
+		uint8_t sw_major;
+		uint8_t sw_minor;
+		uint32_t vcs_commit;
+		uint8_t hw_major;
+		uint8_t hw_minor;
+	};
+
+	struct DeviceInformation {
+		uint8_t node_id{UINT8_MAX};
+		uint32_t device_id{UINT32_MAX};
+		DeviceCapability capability{DeviceCapability::NONE};
+		bool has_node_info{false};
+
+		char vendor_name[32];
+		char model_name[32];
+		char firmware_version[24];
+		char hardware_version[24];
+		char serial_number[33];
+
+		DeviceInformation() : node_id(UINT8_MAX), device_id(UINT32_MAX), capability(DeviceCapability::NONE),
+			has_node_info(false)
+		{
+			// Initialize string fields
+			vendor_name[0] = '\0';
+			model_name[0] = '\0';
+			firmware_version[0] = '\0';
+			hardware_version[0] = '\0';
+			serial_number[0] = '\0';
+		}
+
+		DeviceInformation(uint8_t nid, uint32_t did, DeviceCapability cap)
+			: node_id(nid), device_id(did), capability(cap), has_node_info(false)
+		{
+			// Initialize string fields
+			vendor_name[0] = '\0';
+			model_name[0] = '\0';
+			firmware_version[0] = '\0';
+			hardware_version[0] = '\0';
+			serial_number[0] = '\0';
+		}
+	};
+
+	void handleNodeInfoRetrieved(uavcan::NodeID node_id,
+				     const uavcan::protocol::GetNodeInfo_::Response &node_info) override;
+	void handleNodeInfoUnavailable(uavcan::NodeID node_id) override;
+
+	void handleTimerEvent(const uavcan::TimerEvent &event) override;
+
+	void startTimerIfNotRunning();
+
+	// Register device info or capability, set nodeinfo to nullptr if only registering capability
+	void registerDevice(uint8_t node_id, const NodeInfo *info, uint32_t device_id, DeviceCapability capability);
+
+	// Publishing methods
+	void publishDeviceInformationPeriodic();
+	void publishSingleDeviceInformation(const DeviceInformation &device_info);
+
+	// Helper functions
+	void populateDeviceInfoFields(DeviceInformation &device_info, const NodeInfo &info);
+	void parseNodeName(const char *name, DeviceInformation &device_info);
+	bool extendDeviceInformationsArray();
+
+	uavcan::NodeInfoRetriever &_node_info_retriever;
+
+	// Device capability tracking
+	DeviceInformation *_device_informations{nullptr};
+	size_t _device_informations_size{0};
+	uORB::Publication<device_information_s> _device_info_pub{ORB_ID(device_information)};
+	hrt_abstime _last_device_info_publish{0};
+
+	// Round-robin publishing
+	size_t _next_device_to_publish{0};
+};

src/drivers/uavcan/sensors/accel.cpp

@@ -40,8 +40,8 @@
 
 const char *const UavcanAccelBridge::NAME = "accel";
 
-UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel)),
+UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher),
 	_sub_imu_data(node)
 { }
 
@@ -77,6 +77,12 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 
 	accel->set_error_count(0);
 	accel->update(timestamp_sample, msg.accelerometer_latest[0], msg.accelerometer_latest[1], msg.accelerometer_latest[2]);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), accel->get_device_id(),
+				NodeInfoPublisher::DeviceCapability::ACCELEROMETER);
+	}
 }
 
 int UavcanAccelBridge::init_driver(uavcan_bridge::Channel *channel)

src/drivers/uavcan/sensors/accel.hpp

@@ -38,7 +38,6 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-
 #include <uavcan/equipment/ahrs/RawIMU.hpp>
 
 class UavcanAccelBridge : public UavcanSensorBridgeBase
@@ -46,7 +45,7 @@ class UavcanAccelBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanAccelBridge(uavcan::INode &node);
+	UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/airspeed.cpp

@@ -42,8 +42,8 @@
 
 const char *const UavcanAirspeedBridge::NAME = "airspeed";
 
-UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_airspeed", ORB_ID(airspeed)),
+UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_airspeed", ORB_ID(airspeed), node_info_publisher),
 	_sub_ias_data(node),
 	_sub_tas_data(node),
 	_sub_oat_data(node)
@@ -106,4 +106,10 @@ UavcanAirspeedBridge::ias_sub_cb(const
 	report.true_airspeed_m_s   	= _last_tas_m_s;
 
 	publish(msg.getSrcNodeID().get(), &report);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				0, NodeInfoPublisher::DeviceCapability::AIRSPEED); // Device ID not defined for airspeed message
+	}
 }

src/drivers/uavcan/sensors/airspeed.hpp

@@ -39,7 +39,6 @@
 
 #include "sensor_bridge.hpp"
 #include <uORB/topics/airspeed.h>
-
 #include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>
 #include <uavcan/equipment/air_data/TrueAirspeed.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
@@ -49,7 +48,7 @@ class UavcanAirspeedBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanAirspeedBridge(uavcan::INode &node);
+	UavcanAirspeedBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/baro.cpp

@@ -45,8 +45,8 @@
 
 const char *const UavcanBarometerBridge::NAME = "baro";
 
-UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro)),
+UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher),
 	_sub_air_pressure_data(node),
 	_sub_air_temperature_data(node)
 { }
@@ -112,6 +112,12 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
 	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
 
+	// Register barometer capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid,
+				NodeInfoPublisher::DeviceCapability::BAROMETER);
+	}
+
 	// publish
 	sensor_baro_s sensor_baro{};
 	sensor_baro.timestamp_sample = timestamp_sample;

src/drivers/uavcan/sensors/baro.hpp

@@ -38,7 +38,6 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-
 #include <uavcan/equipment/air_data/StaticPressure.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
 
@@ -47,7 +46,7 @@ class UavcanBarometerBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanBarometerBridge(uavcan::INode &node);
+	UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/battery.cpp

@@ -39,8 +39,8 @@
 
 const char *const UavcanBatteryBridge::NAME = "battery";
 
-UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status)),
+UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status), node_info_publisher),
 	ModuleParams(nullptr),
 	_sub_battery(node),
 	_sub_battery_aux(node),
@@ -100,6 +100,11 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 		}
 	}
 
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				msg.battery_id, NodeInfoPublisher::DeviceCapability::BATTERY);
+	}
+
 	if (instance >= battery_status_s::MAX_INSTANCES
 	    || _batt_update_mod[instance] == BatteryDataType::CBAT) {
 		return;
@@ -128,7 +133,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	_battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
 	_battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
 	_battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
-	_battery_status[instance].id = msg.getSrcNodeID().get();
+	_battery_status[instance].id = msg.battery_id;
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
 		// Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available.
@@ -150,6 +155,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 				 "%" PRIu32, msg.model_instance_id);
 			_battery_info_pub[instance].publish(_battery_info[instance]);
 		}
+
 	}
 }
 
@@ -284,6 +290,11 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
 	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu16,
 		 msg.serial_number);
 	_battery_info_pub[instance].publish(_battery_info[instance]);
+
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				_battery_status[instance].id, NodeInfoPublisher::DeviceCapability::BATTERY);
+	}
 }
 
 void

src/drivers/uavcan/sensors/battery.hpp

@@ -54,7 +54,7 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 public:
 	static const char *const NAME;
 
-	UavcanBatteryBridge(uavcan::INode &node);
+	UavcanBatteryBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -44,8 +44,9 @@
 
 const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure";
 
-UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure)),
+UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node,
+		NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher),
 	_sub_air(node)
 {
 }
@@ -88,4 +89,10 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 	report.timestamp = hrt_absolute_time();
 
 	publish(msg.getSrcNodeID().get(), &report);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				report.device_id, NodeInfoPublisher::DeviceCapability::DIFFERENTIAL_PRESSURE);
+	}
 }

src/drivers/uavcan/sensors/differential_pressure.hpp

@@ -38,7 +38,6 @@
 #pragma once
 
 #include <uORB/topics/differential_pressure.h>
-
 #include "sensor_bridge.hpp"
 
 #include <uavcan/equipment/air_data/RawAirData.hpp>
@@ -48,7 +47,7 @@ class UavcanDifferentialPressureBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanDifferentialPressureBridge(uavcan::INode &node);
+	UavcanDifferentialPressureBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 
@@ -63,4 +62,5 @@ private:
 		AirCbBinder;
 
 	uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, AirCbBinder> _sub_air;
+
 };

src/drivers/uavcan/sensors/flow.cpp

@@ -37,8 +37,8 @@
 
 const char *const UavcanFlowBridge::NAME = "flow";
 
-UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)),
+UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher),
 	_sub_flow(node)
 {
 }
@@ -95,4 +95,10 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex:
 	flow.timestamp = hrt_absolute_time();
 
 	publish(msg.getSrcNodeID().get(), &flow);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				flow.device_id, NodeInfoPublisher::DeviceCapability::OPTICAL_FLOW);
+	}
 }

src/drivers/uavcan/sensors/flow.hpp

@@ -46,7 +46,7 @@ class UavcanFlowBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanFlowBridge(uavcan::INode &node);
+	UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/fuel_tank_status.cpp

@@ -43,7 +43,7 @@
 const char *const UavcanFuelTankStatusBridge::NAME = "fuel_tank_status";
 
 UavcanFuelTankStatusBridge::UavcanFuelTankStatusBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_fuel_tank_status", ORB_ID(fuel_tank_status)),
+	UavcanSensorBridgeBase("uavcan_fuel_tank_status", ORB_ID(fuel_tank_status), nullptr),
 	_sub_fuel_tank_status_data(node)
 { }
 

src/drivers/uavcan/sensors/gnss.cpp

@@ -53,8 +53,8 @@ using namespace time_literals;
 
 const char *const UavcanGnssBridge::NAME = "gnss";
 
-UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps)),
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps), node_info_publisher),
 	_node(node),
 	_sub_auxiliary(node),
 	_sub_fix(node),
@@ -359,6 +359,13 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	sensor_gps_s sensor_gps{};
 	sensor_gps.device_id = get_device_id();
 
+	// Register GPS capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				sensor_gps.device_id,
+				NodeInfoPublisher::DeviceCapability::GPS);
+	}
+
 	/*
 	 * FIXME HACK
 	 * There used to be the following line of code:

src/drivers/uavcan/sensors/gnss.hpp

@@ -67,7 +67,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanGnssBridge(uavcan::INode &node);
+	UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 	~UavcanGnssBridge();
 
 	const char *get_name() const override { return NAME; }

src/drivers/uavcan/sensors/gnss_relative.cpp

@@ -38,8 +38,8 @@
 
 const char *const UavcanGnssRelativeBridge::NAME = "gnss_relative";
 
-UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative)),
+UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher),
 	_sub_rel_pos_heading(node)
 {
 }
@@ -72,6 +72,13 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const
 
 	sensor_gnss_relative.device_id = get_device_id();
 
+	// Register GPS capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				sensor_gnss_relative.device_id,
+				NodeInfoPublisher::DeviceCapability::GPS);
+	}
+
 	sensor_gnss_relative.timestamp = hrt_absolute_time();
 
 	publish(msg.getSrcNodeID().get(), &sensor_gnss_relative);

src/drivers/uavcan/sensors/gnss_relative.hpp

@@ -34,7 +34,6 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-
 #include <stdint.h>
 
 #include <uORB/topics/sensor_gnss_relative.h>
@@ -46,7 +45,7 @@ class UavcanGnssRelativeBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanGnssRelativeBridge(uavcan::INode &node);
+	UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/gyro.cpp

@@ -40,8 +40,8 @@
 
 const char *const UavcanGyroBridge::NAME = "gyro";
 
-UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro)),
+UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher),
 	_sub_imu_data(node)
 { }
 
@@ -77,6 +77,12 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 		     msg.rate_gyro_latest[0],
 		     msg.rate_gyro_latest[1],
 		     msg.rate_gyro_latest[2]);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				gyro->get_device_id(), NodeInfoPublisher::DeviceCapability::GYROSCOPE);
+	}
 }
 
 int UavcanGyroBridge::init_driver(uavcan_bridge::Channel *channel)

src/drivers/uavcan/sensors/gyro.hpp

@@ -46,7 +46,7 @@ class UavcanGyroBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanGyroBridge(uavcan::INode &node);
+	UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/hygrometer.cpp

@@ -33,13 +33,13 @@
 
 #include "hygrometer.hpp"
 
-#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_hygrometer.h>
 #include <lib/atmosphere/atmosphere.h>
 
 const char *const UavcanHygrometerBridge::NAME = "hygrometer_sensor";
 
-UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer)),
+UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher),
 	_sub_hygro(node)
 {
 }
@@ -70,4 +70,10 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure<dr
 	report.timestamp = hrt_absolute_time();
 
 	publish(msg.getSrcNodeID().get(), &report);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				report.device_id, NodeInfoPublisher::DeviceCapability::HYGROMETER);
+	}
 }

src/drivers/uavcan/sensors/hygrometer.hpp

@@ -43,7 +43,7 @@ class UavcanHygrometerBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanHygrometerBridge(uavcan::INode &node);
+	UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 
@@ -58,4 +58,5 @@ private:
 		HygroCbBinder;
 
 	uavcan::Subscriber<dronecan::sensors::hygrometer::Hygrometer, HygroCbBinder> _sub_hygro;
+
 };

src/drivers/uavcan/sensors/ice_status.cpp

@@ -41,7 +41,7 @@
 const char *const UavcanIceStatusBridge::NAME = "ice_status";
 
 UavcanIceStatusBridge::UavcanIceStatusBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_ice_status", ORB_ID(internal_combustion_engine_status)),
+	UavcanSensorBridgeBase("uavcan_ice_status", ORB_ID(internal_combustion_engine_status), nullptr),
 	_sub_ice_status_data(node)
 { }
 

src/drivers/uavcan/sensors/mag.cpp

@@ -44,8 +44,8 @@
 
 const char *const UavcanMagnetometerBridge::NAME = "mag";
 
-UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_mag", ORB_ID(sensor_mag)),
+UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_mag", ORB_ID(sensor_mag), node_info_publisher),
 	_sub_mag(node),
 	_sub_mag2(node)
 {
@@ -87,6 +87,12 @@ void UavcanMagnetometerBridge::mag_sub_cb(const
 		return;
 	}
 
+	// Register magnetometer capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), mag->get_device_id(),
+				NodeInfoPublisher::DeviceCapability::MAGNETOMETER);
+	}
+
 	const float x = msg.magnetic_field_ga[0];
 	const float y = msg.magnetic_field_ga[1];
 	const float z = msg.magnetic_field_ga[2];
@@ -112,6 +118,13 @@ UavcanMagnetometerBridge::mag2_sub_cb(const
 		return;
 	}
 
+	// Register magnetometer capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				mag->get_device_id(),
+				NodeInfoPublisher::DeviceCapability::MAGNETOMETER);
+	}
+
 	const float x = msg.magnetic_field_ga[0];
 	const float y = msg.magnetic_field_ga[1];
 	const float z = msg.magnetic_field_ga[2];

src/drivers/uavcan/sensors/mag.hpp

@@ -47,7 +47,7 @@ class UavcanMagnetometerBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanMagnetometerBridge(uavcan::INode &node);
+	UavcanMagnetometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/rangefinder.cpp

@@ -42,8 +42,8 @@
 
 const char *const UavcanRangefinderBridge::NAME = "rangefinder";
 
-UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor)),
+UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
+	UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher),
 	_sub_range_data(node)
 { }
 
@@ -115,6 +115,12 @@ void UavcanRangefinderBridge::range_sub_cb(const
 	}
 
 	rangefinder->update(hrt_absolute_time(), msg.range, quality);
+
+	// Register device capability if not already done
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				rangefinder->get_device_id(), NodeInfoPublisher::DeviceCapability::RANGEFINDER);
+	}
 }
 
 int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel)

src/drivers/uavcan/sensors/rangefinder.hpp

@@ -48,7 +48,7 @@ class UavcanRangefinderBridge : public UavcanSensorBridgeBase
 public:
 	static const char *const NAME;
 
-	UavcanRangefinderBridge(uavcan::INode &node);
+	UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher);
 
 	const char *get_name() const override { return NAME; }
 

src/drivers/uavcan/sensors/safety_button.cpp

@@ -41,7 +41,8 @@ const char *const UavcanSafetyButtonBridge::NAME = "safety_button";
 using namespace time_literals;
 
 UavcanSafetyButtonBridge::UavcanSafetyButtonBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_safety_button", ORB_ID(button_event)), _sub_button(node)
+	UavcanSensorBridgeBase("uavcan_safety_button", ORB_ID(button_event), nullptr),
+	_sub_button(node)
 { }
 
 int UavcanSafetyButtonBridge::init()

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -85,7 +85,8 @@
 /*
  * IUavcanSensorBridge
  */
-void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list,
+				   NodeInfoPublisher *node_info_publisher)
 {
 	// airspeed
 #if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED)
@@ -93,7 +94,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd);
 
 	if (uavcan_sub_aspd != 0) {
-		list.add(new UavcanAirspeedBridge(node));
+		list.add(new UavcanAirspeedBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -104,7 +105,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_BARO"), &uavcan_sub_baro);
 
 	if (uavcan_sub_baro != 0) {
-		list.add(new UavcanBarometerBridge(node));
+		list.add(new UavcanBarometerBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -115,7 +116,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_BAT"), &uavcan_sub_bat);
 
 	if (uavcan_sub_bat != 0) {
-		list.add(new UavcanBatteryBridge(node));
+		list.add(new UavcanBatteryBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -126,7 +127,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_DPRES"), &uavcan_sub_dpres);
 
 	if (uavcan_sub_dpres != 0) {
-		list.add(new UavcanDifferentialPressureBridge(node));
+		list.add(new UavcanDifferentialPressureBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -137,7 +138,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_FLOW"), &uavcan_sub_flow);
 
 	if (uavcan_sub_flow != 0) {
-		list.add(new UavcanFlowBridge(node));
+		list.add(new UavcanFlowBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -159,7 +160,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_GPS"), &uavcan_sub_gps);
 
 	if (uavcan_sub_gps != 0) {
-		list.add(new UavcanGnssBridge(node));
+		list.add(new UavcanGnssBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -170,7 +171,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_GPS_R"), &uavcan_sub_gps_rel);
 
 	if (uavcan_sub_gps_rel != 0) {
-		list.add(new UavcanGnssRelativeBridge(node));
+		list.add(new UavcanGnssRelativeBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -181,7 +182,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_HYGRO"), &uavcan_sub_hygro);
 
 	if (uavcan_sub_hygro != 0) {
-		list.add(new UavcanHygrometerBridge(node));
+		list.add(new UavcanHygrometerBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -203,8 +204,8 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_IMU"), &uavcan_sub_imu);
 
 	if (uavcan_sub_imu != 0) {
-		list.add(new UavcanAccelBridge(node));
-		list.add(new UavcanGyroBridge(node));
+		list.add(new UavcanAccelBridge(node, node_info_publisher));
+		list.add(new UavcanGyroBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -215,7 +216,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_MAG"), &uavcan_sub_mag);
 
 	if (uavcan_sub_mag != 0) {
-		list.add(new UavcanMagnetometerBridge(node));
+		list.add(new UavcanMagnetometerBridge(node, node_info_publisher));
 	}
 
 #endif
@@ -226,7 +227,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	param_get(param_find("UAVCAN_SUB_RNG"), &uavcan_sub_rng);
 
 	if (uavcan_sub_rng != 0) {
-		list.add(new UavcanRangefinderBridge(node));
+		list.add(new UavcanRangefinderBridge(node, node_info_publisher));
 	}
 
 #endif

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -42,6 +42,7 @@
 #include <drivers/drv_orb_dev.h>
 #include <lib/drivers/device/Device.hpp>
 #include <uORB/uORB.h>
+#include <drivers/uavcan/node_info.hpp>
 
 /**
  * A sensor bridge class must implement this interface.
@@ -80,7 +81,8 @@ public:
 	 * Sensor bridge factory.
 	 * Creates all known sensor bridges and puts them in the linked list.
 	 */
-	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
+	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list,
+			     NodeInfoPublisher *node_info_publisher);
 };
 
 namespace uavcan_bridge
@@ -106,13 +108,16 @@ class UavcanSensorBridgeBase : public IUavcanSensorBridge, public device::Device
 protected:
 	static constexpr unsigned DEFAULT_MAX_CHANNELS = 4;
 	const unsigned _max_channels;
+	NodeInfoPublisher *_node_info_publisher;
 
 	UavcanSensorBridgeBase(const char *name, const orb_id_t orb_topic_sensor,
+			       NodeInfoPublisher *node_info_publisher,
 			       const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
 		Device(name),
 		_orb_topic(orb_topic_sensor),
 		_channels(new uavcan_bridge::Channel[max_channels]),
-		_max_channels(max_channels)
+		_max_channels(max_channels),
+		_node_info_publisher(node_info_publisher)
 	{
 		set_device_bus_type(DeviceBusType_UAVCAN);
 		set_device_bus(0);

src/drivers/uavcan/uavcan_main.cpp

@@ -107,6 +107,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_time_sync_slave(_node),
 	_node_status_monitor(_node),
 	_node_info_retriever(_node),
+	_node_info_publisher(_node, _node_info_retriever),
 	_master_timer(_node),
 	_param_getset_client(_node),
 	_param_opcode_client(_node),
@@ -595,7 +596,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 	}
 
 	// Sensor bridges
-	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
+	IUavcanSensorBridge::make_all(_node, _sensor_bridges, &_node_info_publisher);
 
 	for (const auto &br : _sensor_bridges) {
 		ret = br->init();
@@ -608,6 +609,10 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		PX4_DEBUG("sensor bridge '%s' init ok", br->get_name());
 	}
 
+#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
+	_esc_controller.set_node_info_publisher(&_node_info_publisher);
+#endif
+
 	/* Set up shared service clients */
 	_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
 	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));

src/drivers/uavcan/uavcan_main.hpp

@@ -45,6 +45,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include "node_info.hpp"
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
 #include "actuators/esc.hpp"
@@ -291,6 +292,7 @@ private:
 	uavcan::NodeStatusMonitor	_node_status_monitor;
 
 	uavcan::NodeInfoRetriever   _node_info_retriever;
+	NodeInfoPublisher           _node_info_publisher;
 
 	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
 

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -45,9 +45,6 @@ public:
 	PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope();
 
-	uint32_t get_device_id() const { return _device_id; }
-
-	int32_t get_max_rate_hz() const { return math::constrain(_imu_gyro_rate_max, static_cast<int32_t>(100), static_cast<int32_t>(4000)); }
 
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
@@ -60,7 +57,9 @@ public:
 
 	void updateFIFO(sensor_gyro_fifo_s &sample);
 
+	uint32_t get_device_id() const { return _device_id; }
 	int get_instance() { return _sensor_pub.get_instance(); };
+	int32_t get_max_rate_hz() const { return math::constrain(_imu_gyro_rate_max, static_cast<int32_t>(100), static_cast<int32_t>(4000)); }
 
 private:
 	void UpdateClipLimit();

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -53,6 +53,7 @@ public:
 	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	int get_instance() { return _sensor_pub.get_instance(); };
+	uint32_t get_device_id() const { return _device_id; }
 
 private:
 	uORB::PublicationMulti<sensor_mag_s> _sensor_pub{ORB_ID(sensor_mag)};

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -65,6 +65,7 @@ public:
 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
 
 	int get_instance() { return _distance_sensor_pub.get_instance(); };
+	uint32_t get_device_id() { return _distance_sensor_pub.get().device_id; };
 
 private:
 	uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};

src/modules/logger/logged_topics.cpp

@@ -59,6 +59,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("config_overrides");
 	add_topic("cpuload");
 	add_topic("distance_sensor_mode_change_request");
+	add_topic("device_information", 900);
 	add_topic_multi("dronecan_node_status", 250);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");