From 710286da722beae72a488af52b6c1e603f68eea7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C3=98yvind=20Taksdal=20Stubhaug?= <o_github@oystub.com>
Date: Wed, 20 Mar 2024 17:38:47 +0100
Subject: [PATCH] uavcan: publish new can interface status as uorb topic
 (#22873)

---
 msg/CMakeLists.txt                   |  1 +
 msg/CanInterfaceStatus.msg           |  6 +++++
 src/drivers/uavcan/CMakeLists.txt    |  1 +
 src/drivers/uavcan/uavcan_main.cpp   | 35 ++++++++++++++++++++++++++++
 src/drivers/uavcan/uavcan_main.hpp   |  5 ++++
 src/modules/logger/logged_topics.cpp |  1 +
 6 files changed, 49 insertions(+)
 create mode 100644 msg/CanInterfaceStatus.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 3b1e2647e3..c53402bd0f 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -58,6 +58,7 @@ set(msg_files
 	CameraCapture.msg
 	CameraStatus.msg
 	CameraTrigger.msg
+	CanInterfaceStatus.msg
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
diff --git a/msg/CanInterfaceStatus.msg b/msg/CanInterfaceStatus.msg
new file mode 100644
index 0000000000..4129c8d563
--- /dev/null
+++ b/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
diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt
index 4146edab05..39a9bb3ff2 100644
--- a/src/drivers/uavcan/CMakeLists.txt
+++ b/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
diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp
index 277b2526dd..fbdc534a9c 100644
--- a/src/drivers/uavcan/uavcan_main.cpp
+++ b/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
diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp
index 54119259ef..5afa04ef67 100644
--- a/src/drivers/uavcan/uavcan_main.hpp
+++ b/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
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 68b3d27b42..cd72d324a0 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/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");