From 175c4454f37e9171292d154c9ad74166823f33fe Mon Sep 17 00:00:00 2001
From: Alexander Lerach <alexander@auterion.com>
Date: Tue, 21 Oct 2025 10:07:20 +0200
Subject: [PATCH] UAVCAN: Match servo status and temp/circuit via node id

---
 msg/ServoStatus.msg                    |  2 --
 src/drivers/uavcan/actuators/servo.cpp | 31 +++++++++++++++++---------
 2 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/msg/ServoStatus.msg b/msg/ServoStatus.msg
index 7e06eb5197..daf48c15b5 100644
--- a/msg/ServoStatus.msg
+++ b/msg/ServoStatus.msg
@@ -9,8 +9,6 @@ uint8 SERVO_CONNECTION_TYPE_CAN = 4			# CAN-Bus
 uint8 SERVO_CONNECTION_TYPE_DSHOT = 5			# DShot
 
 uint16 counter  					# incremented by the writing thread everytime new data is stored
-uint16 temperature_counter 				# Incremented when new temperature data is stored
-uint16 power_counter 					# Incremented when new power data is stored
 uint8 servo_count					# number of connected servo's
 
 uint8 servo_connectiontype				# how SERVOs connected to the system. Currently only CAN supports servo telemtry
diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp
index 8e42b9b217..160aa3eaaf 100644
--- a/src/drivers/uavcan/actuators/servo.cpp
+++ b/src/drivers/uavcan/actuators/servo.cpp
@@ -113,11 +113,17 @@ UavcanServoController::servo_temperature_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::device::Temperature>
 		&msg)
 {
-	if (msg.device_id < servo_status_s::CONNECTED_SERVO_MAX) {
+	for (int i = 0; i < servo_status_s::CONNECTED_SERVO_MAX; i++) {
+		auto &ref = _servo_status.servo[i];
 
-		_last_temperature[msg.device_id] = msg.temperature;
-		_last_temperature_error_flag[msg.device_id] = msg.error_flags;
-		_servo_temperature_counter += 1; // Keep counter to monitor callbacks
+		const bool is_servo_online = _servo_status.servo_online_flags & (1 << i);
+		const bool is_servo_matching = ref.servo_node_id == msg.getSrcNodeID().get();
+
+		if (is_servo_online && is_servo_matching) {
+			_last_temperature[i] = msg.temperature;
+			_last_temperature_error_flag[i] = msg.error_flags;
+			break;
+		}
 	}
 }
 
@@ -126,12 +132,17 @@ UavcanServoController::servo_circuit_status_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::power::CircuitStatus>
 		&msg)
 {
-	if (msg.circuit_id < servo_status_s::CONNECTED_SERVO_MAX) {
+	for (int i = 0; i < servo_status_s::CONNECTED_SERVO_MAX; i++) {
+		auto &ref = _servo_status.servo[i];
 
-		_last_voltage[msg.circuit_id] = msg.voltage;
-		_last_current[msg.circuit_id] = msg.current;
-		_last_power_error_flag[msg.circuit_id] = msg.error_flags;
-		_servo_power_counter += 1; // Keep counter to monitor callbacks
+		const bool is_servo_online = _servo_status.servo_online_flags & (1 << i);
+		const bool is_servo_matching = ref.servo_node_id == msg.getSrcNodeID().get();
+
+		if (is_servo_online && is_servo_matching) {
+			_last_voltage[msg.circuit_id] = msg.voltage;
+			_last_current[msg.circuit_id] = msg.current;
+			_last_power_error_flag[msg.circuit_id] = msg.error_flags;
+		}
 	}
 }
 
@@ -160,8 +171,6 @@ UavcanServoController::servo_status_sub_cb(const uavcan::ReceivedDataStructure<u
 		ref.servo_power_error_flags = _last_power_error_flag[msg.actuator_id];
 
 		_servo_status.counter += 1;
-		_servo_status.temperature_counter = _servo_temperature_counter;
-		_servo_status.power_counter = _servo_power_counter;
 		_servo_status.servo_count = _servo_count;
 		_servo_status.servo_connectiontype = servo_status_s::SERVO_CONNECTION_TYPE_CAN;
 		_servo_status.servo_online_flags = check_servos_status();