uavcan: add node status logging to uORB (#23890)

Co-authored-by: Jordan Leiber <jordan.leiber@vertiq.co>

msg/CMakeLists.txt

@@ -66,6 +66,7 @@ set(msg_files
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	DistanceSensorModeChangeRequest.msg
+	DronecanNodeStatus.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg

msg/DronecanNodeStatus.msg

@@ -0,0 +1,41 @@
+uint64 timestamp					# time since system start (microseconds)
+
+uint16 node_id						# The node ID which this data comes from
+
+# From the uavcan.protocol.NodeStatus message
+uint32 uptime_sec					# Node uptime
+
+#
+# Abstract node health.
+#
+uint8 HEALTH_OK         = 0     # The node is functioning properly.
+uint8 HEALTH_WARNING    = 1     # A critical parameter went out of range or the node encountered a minor failure.
+uint8 HEALTH_ERROR      = 2     # The node encountered a major failure.
+uint8 HEALTH_CRITICAL   = 3     # The node suffered a fatal malfunction.
+uint8 health
+
+#
+# Current mode.
+#
+# Mode OFFLINE can be actually reported by the node to explicitly inform other network
+# participants that the sending node is about to shutdown. In this case other nodes will not
+# have to wait OFFLINE_TIMEOUT_MS before they detect that the node is no longer available.
+#
+# Reserved values can be used in future revisions of the specification.
+#
+uint8 MODE_OPERATIONAL      = 0         # Normal operating mode.
+uint8 MODE_INITIALIZATION   = 1         # Initialization is in progress; this mode is entered immediately after startup.
+uint8 MODE_MAINTENANCE      = 2         # E.g. calibration, the bootloader is running, etc.
+uint8 MODE_SOFTWARE_UPDATE  = 3         # New software/firmware is being loaded.
+uint8 MODE_OFFLINE          = 7         # The node is no longer available.
+uint8 mode
+
+#
+# Not used currently, keep zero when publishing, ignore when receiving.
+#
+uint8 sub_mode
+
+#
+# Optional, vendor-specific node status code, e.g. a fault code or a status bitmask.
+#
+uint16 vendor_specific_status_code

src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp

@@ -24,14 +24,18 @@ class UAVCAN_EXPORT NodeStatusMonitor
 public:
     struct NodeStatus
     {
+        uint32_t uptime_sec;
         uint8_t health   : 2;
         uint8_t mode     : 3;
         uint8_t sub_mode : 3;
+        uint16_t vendor_specific_status_code;
 
         NodeStatus() :
+            uptime_sec(0),
             health(protocol::NodeStatus::HEALTH_CRITICAL),
             mode(protocol::NodeStatus::MODE_OFFLINE),
-            sub_mode(0)
+            sub_mode(0),
+            vendor_specific_status_code(0)
         {
             StaticAssert<protocol::NodeStatus::FieldTypes::health::BitLen   == 2>::check();
             StaticAssert<protocol::NodeStatus::FieldTypes::mode::BitLen     == 3>::check();
@@ -123,9 +127,11 @@ private:
     {
         Entry new_entry;
         new_entry.time_since_last_update_ms100 = 0;
+        new_entry.status.uptime_sec = msg.uptime_sec;
         new_entry.status.health   = msg.health   & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1);
         new_entry.status.mode     = msg.mode     & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1);
         new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1);
+        new_entry.status.vendor_specific_status_code = msg.vendor_specific_status_code;
 
         changeNodeStatus(msg.getSrcNodeID(), new_entry);
 

src/drivers/uavcan/uavcan_main.cpp

@@ -746,40 +746,9 @@ UavcanNode::Run()
 
 	_node.spinOnce(); // expected to be non-blocking
 
-	// Publish status
-	constexpr hrt_abstime status_pub_interval = 100_ms;
+	publish_can_interface_statuses();
 
-	if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
-		_last_can_status_pub = hrt_absolute_time();
-
-		for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
-			if (i > UAVCAN_NUM_IFACES) {
-				break;
-			}
-
-			auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
-
-			if (!iface) {
-				continue;
-			}
-
-			auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
-			can_interface_status_s status{
-				.timestamp = hrt_absolute_time(),
-				.io_errors = iface_perf_cnt.errors,
-				.frames_tx = iface_perf_cnt.frames_tx,
-				.frames_rx = iface_perf_cnt.frames_rx,
-				.interface = static_cast<uint8_t>(i),
-			};
-
-			if (_can_status_pub_handles[i] == nullptr) {
-				int instance{0};
-				_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
-			}
-
-			(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
-		}
-	}
+	publish_node_statuses();
 
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
@@ -1011,6 +980,101 @@ UavcanNode::Run()
 	}
 }
 
+void UavcanNode::publish_can_interface_statuses()
+{
+	constexpr hrt_abstime status_pub_interval = 100_ms;
+
+	if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
+		_last_can_status_pub = hrt_absolute_time();
+
+		for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
+			if (i > UAVCAN_NUM_IFACES) {
+				break;
+			}
+
+			auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
+
+			if (!iface) {
+				continue;
+			}
+
+			auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
+			can_interface_status_s status{
+				.timestamp = hrt_absolute_time(),
+				.io_errors = iface_perf_cnt.errors,
+				.frames_tx = iface_perf_cnt.frames_tx,
+				.frames_rx = iface_perf_cnt.frames_rx,
+				.interface = static_cast<uint8_t>(i),
+			};
+
+			if (_can_status_pub_handles[i] == nullptr) {
+				int instance{0};
+				_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
+			}
+
+			(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
+		}
+	}
+}
+
+void UavcanNode::publish_node_statuses()
+{
+	constexpr hrt_abstime status_pub_interval = 100_ms;
+
+	if (hrt_absolute_time() - _last_node_status_pub >= status_pub_interval) {
+		_last_node_status_pub = hrt_absolute_time();
+
+		_node_status_monitor.forEachNode([this](uavcan::NodeID node_id, uavcan::NodeStatusMonitor::NodeStatus node_status) {
+
+			if (node_id.get() == 0) {
+				return;
+			}
+
+			// See if we have NodeID <--> uORB_index mapped
+			int uorb_index = -1;
+
+			for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+				if (_node_status_uorb_index_map[i] == node_id.get()) {
+					uorb_index = i;
+					break;
+				}
+			}
+
+			if (uorb_index < 0) {
+				// use next available index
+				for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+					if (_node_status_uorb_index_map[i] == 0) {
+						_node_status_uorb_index_map[i] = node_id.get();
+						uorb_index = i;
+						// advertise
+						PX4_INFO("advertising node_id %u on index %u", node_id.get(), i);
+						int instance{0};
+						_node_status_pub_handles[i] = orb_advertise_multi(ORB_ID(dronecan_node_status), nullptr, &instance);
+						break;
+					}
+				}
+			}
+
+			if (uorb_index >= 0) {
+
+				dronecan_node_status_s status{
+					.timestamp = hrt_absolute_time(),
+					.uptime_sec = node_status.uptime_sec,
+					.node_id = node_id.get(),
+					.vendor_specific_status_code = node_status.vendor_specific_status_code,
+					.health = node_status.health,
+					.mode = node_status.mode,
+					.sub_mode = node_status.sub_mode,
+				};
+
+				(void)orb_publish(ORB_ID(dronecan_node_status), _node_status_pub_handles[uorb_index], &status);
+			}
+		});
+
+
+	}
+}
+
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
 bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)

src/drivers/uavcan/uavcan_main.hpp

@@ -101,6 +101,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/can_interface_status.h>
+#include <uORB/topics/dronecan_node_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -235,6 +236,9 @@ private:
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
 
+	void publish_can_interface_statuses();
+	void publish_node_statuses();
+
 	int		print_params(uavcan::protocol::param::GetSet::Response &resp);
 	int		get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
 	void 		update_params();
@@ -317,10 +321,15 @@ private:
 
 	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::PublicationMulti<can_interface_status_s> _can_status_pub{ORB_ID(can_interface_status)};
+
+	orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
+
+	// array of NodeIDs, each index maps to uORB index
+	orb_advert_t _node_status_pub_handles[ORB_MULTI_MAX_INSTANCES] = {nullptr};
+	uint8_t _node_status_uorb_index_map[ORB_MULTI_MAX_INSTANCES] = {};
 
 	hrt_abstime _last_can_status_pub{0};
-	orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
+	hrt_abstime _last_node_status_pub{0};
 
 	/*
 	 * The MAVLink parameter bridge needs to know the maximum parameter index

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_multi("dronecan_node_status", 250);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");