uavcan: publish new can interface status as uorb topic (#22873)

msg/CMakeLists.txt

@@ -58,6 +58,7 @@ set(msg_files
 	CameraCapture.msg
 	CameraStatus.msg
 	CameraTrigger.msg
+	CanInterfaceStatus.msg
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg

msg/CanInterfaceStatus.msg

@@ -0,0 +1,6 @@
+uint64 timestamp		# time since system start (microseconds)
+uint8 interface
+
+uint64 io_errors
+uint64 frames_tx
+uint64 frames_rx

src/drivers/uavcan/CMakeLists.txt

@@ -75,6 +75,7 @@ add_definitions(
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
 	-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
+	-DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces}
 	-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
 	-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
 	-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1

src/drivers/uavcan/uavcan_main.cpp

@@ -744,6 +744,41 @@ UavcanNode::Run()
 
 	_node.spinOnce(); // expected to be non-blocking
 
+	// Publish status
+	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);
+		}
+	}
+
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update

src/drivers/uavcan/uavcan_main.hpp

@@ -96,6 +96,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/can_interface_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -309,6 +310,10 @@ 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)};
+
+	hrt_abstime _last_can_status_pub{0};
+	orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
 
 	/*
 	 * The MAVLink parameter bridge needs to know the maximum parameter index

src/modules/logger/logged_topics.cpp

@@ -53,6 +53,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("autotune_attitude_control_status", 100);
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
+	add_optional_topic("can_interface_status", 10);
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
 	add_topic("config_overrides");