uavcan: cleanup & node_status fix

uavcan: esc fault handling review suggestions

uavcan: fix node status & cleanup

msg/DeviceInformation.msg

@@ -6,7 +6,6 @@
 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
@@ -25,8 +24,6 @@ uint8 DEVICE_TYPE_BATTERY = 14                # Battery
 uint8 DEVICE_TYPE_HYGROMETER = 15             # Hygrometer
 
 char[80] name  # Name of node
-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.

src/drivers/uavcan/actuators/esc.cpp

@@ -65,7 +65,7 @@ UavcanEscController::init()
 		return res;
 	}
 
-	//ESC Status Extended subscription
+	// ESC Status Extended subscription
 	res = _uavcan_sub_status_extended.start(StatusExtendedCbBinder(this, &UavcanEscController::esc_status_extended_sub_cb));
 
 	if (res < 0) {
@@ -117,16 +117,16 @@ void
 UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
 	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
-		auto &esc_report = _esc_status.esc[msg.esc_index];
+		esc_report_s &esc_report = _esc_status.esc[msg.esc_index];
-
+		esc_report.timestamp = hrt_absolute_time();
-		esc_report.timestamp       = hrt_absolute_time();
 		esc_report.esc_address = msg.getSrcNodeID().get();
-		esc_report.esc_voltage     = msg.voltage;
+		esc_report.esc_voltage = msg.voltage;
-		esc_report.esc_current     = msg.current;
+		esc_report.esc_current = msg.current;
 		esc_report.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
 		// esc_report.motor_temperature is filled in the extended status callback
-		esc_report.esc_rpm         = msg.rpm;
+		esc_report.esc_rpm = msg.rpm;
-		esc_report.esc_errorcount  = msg.error_count;
+		esc_report.esc_errorcount = msg.error_count;
+		esc_report.failures = get_failures(msg.esc_index);
 
 		_esc_status.esc_count = _rotor_count;
 		_esc_status.counter += 1;
@@ -134,7 +134,6 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 		_esc_status.esc_online_flags = check_escs_status();
 		_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
 		_esc_status.timestamp = hrt_absolute_time();
-		esc_report.failures = get_failures(msg.esc_index);
 
 		_esc_status_pub.publish(_esc_status);
 	}
@@ -147,15 +146,13 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	}
 }
 
-void
+void UavcanEscController::esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &msg)
-UavcanEscController::esc_status_extended_sub_cb(const
-		uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &msg)
 {
 	uint8_t index = msg.esc_index;
 
 	if (index < esc_status_s::CONNECTED_ESC_MAX) {
-		auto &esc_report = _esc_status.esc[index];
+		esc_report_s &esc_report = _esc_status.esc[index];
-		// This will be published along with the regular ESC status
+		// published with the non-extended esc::Status
 		esc_report.motor_temperature = msg.motor_temperature_degC;
 		esc_report.esc_power = msg.input_pct;
 	}
@@ -178,20 +175,20 @@ UavcanEscController::check_escs_status()
 	return esc_status_flags;
 }
 
-uint32_t
+uint32_t UavcanEscController::get_failures(uint8_t esc_index)
-UavcanEscController::get_failures(uint8_t esc_index)
 {
-	auto &esc_report = _esc_status.esc[esc_index];
+	esc_report_s &esc_report = _esc_status.esc[esc_index];
-
+	uint32_t failures = 0;
 
 	// Update device vendor/model information from device_information topic
-	device_information_s device_info;
+	device_information_s device_information;
+
 	char esc_name[80] {};
 
-	if (_device_information_sub.copy(&device_info)
+	if (_device_information_sub.copy(&device_information)
-	    && device_info.device_type == device_information_s::DEVICE_TYPE_ESC
+	    && device_information.device_type == device_information_s::DEVICE_TYPE_ESC
-	    && device_info.device_id == esc_index) {
+	    && device_information.device_id == esc_index) {
-		strncpy(esc_name, device_info.name, sizeof(esc_name) - 1);
+		strncpy(esc_name, device_information.name, sizeof(esc_name) - 1);
 		esc_name[sizeof(esc_name) - 1] = '\0';
 	}
 
@@ -199,23 +196,20 @@ UavcanEscController::get_failures(uint8_t esc_index)
 	dronecan_node_status_s node_status {};
 	uint8_t node_health{dronecan_node_status_s::HEALTH_OK};
 	uint16_t vendor_specific_status_code{0};
-	bool have_node_status{false};
 
-	if (_dronecan_node_status_sub.copy(&node_status)
+	for (auto &dronecan_node_status_sub : _dronecan_node_status_subs) {
-	    && esc_report.esc_address < kMaxUavcanNodeId
+		if (dronecan_node_status_sub.copy(&node_status)) {
-	    && node_status.node_id == esc_report.esc_address) {
+			if (node_status.node_id == esc_report.esc_address) {
-		node_health = node_status.health;
+				node_health = node_status.health;
-		vendor_specific_status_code = node_status.vendor_specific_status_code;
+				vendor_specific_status_code = node_status.vendor_specific_status_code;
-		have_node_status = true;
+				break;
+			}
+		}
 	}
 
-	if (!have_node_status) {
+	if (esc_report.esc_address < UAVCAN_NODE_ID_MAX && (node_health == dronecan_node_status_s::HEALTH_OK ||
-		return esc_report.failures;
-	}
-
-	if (esc_report.esc_address < kMaxUavcanNodeId && (node_health == dronecan_node_status_s::HEALTH_OK ||
 			node_health == dronecan_node_status_s::HEALTH_WARNING)) {
-		esc_report.failures = 0;
+		failures = 0;
 
 	} else if (strstr(esc_name, "iq_motion") != nullptr) {
 		// Parse iq_motion ESC errors
@@ -239,14 +233,14 @@ UavcanEscController::get_failures(uint8_t esc_index)
 
 		for (const auto &mapping : bit_to_failure_map) {
 			if (vendor_specific_status_code & (1 << mapping.bit)) {
-				esc_report.failures |= (1 << mapping.failure_type);
+				failures |= (1 << mapping.failure_type);
 			}
 		}
 
 	} else {
 		// Generic parsing
-		esc_report.failures |= (1 << esc_report_s::FAILURE_GENERIC);
+		failures |= (1 << esc_report_s::FAILURE_GENERIC);
 	}
 
-	return esc_report.failures;
+	return failures;
 }

src/drivers/uavcan/actuators/esc.hpp

@@ -52,6 +52,7 @@
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/esc_report.h>
@@ -66,7 +67,7 @@ class UavcanEscController
 public:
 	static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX;
 	static constexpr unsigned MAX_RATE_HZ = 400;
-	static constexpr int16_t kMaxUavcanNodeId = 128; // UAVCAN supports up to 128 nodes (0-127)
+	static constexpr int16_t UAVCAN_NODE_ID_MAX = 128; // UAVCAN supports up to 128 nodes (0-127)
 
 	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
 
@@ -126,7 +127,7 @@ private:
 	esc_status_s	_esc_status{};
 
 	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
-	uORB::Subscription _dronecan_node_status_sub{ORB_ID(dronecan_node_status)};
+	uORB::SubscriptionMultiArray<dronecan_node_status_s, ORB_MULTI_MAX_INSTANCES> _dronecan_node_status_subs{ORB_ID::dronecan_node_status};
 	uORB::Subscription _device_information_sub{ORB_ID(device_information)};
 
 	uint8_t		_rotor_count{0};

src/drivers/uavcan/node_info.cpp

@@ -201,18 +201,10 @@ void NodeInfoPublisher::publishSingleDeviceInformation(const DeviceInformation &
 	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.name) == sizeof(device_info.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");
-	static_assert(sizeof(msg.name) == sizeof(device_info.name), "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.name, device_info.name, sizeof(msg.name));
 	msg.name[sizeof(msg.name) - 1] = '\0';
@@ -237,9 +229,6 @@ void NodeInfoPublisher::populateDeviceInfoFields(DeviceInformation &device_info,
 {
 	device_info.has_node_info = true;
 
-	// Remain backward compatible - for now.
-	parseNodeName(info.name, device_info);
-
 	snprintf(device_info.name, sizeof(device_info.name), "%s", info.name);
 	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));
@@ -253,46 +242,6 @@ void NodeInfoPublisher::populateDeviceInfoFields(DeviceInformation &device_info,
 		 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;

src/drivers/uavcan/node_info.hpp

@@ -102,36 +102,10 @@ private:
 		DeviceCapability capability{DeviceCapability::NONE};
 		bool has_node_info{false};
 
-		char name[80];
+		char name[80] = "";
-		char vendor_name[32];
+		char firmware_version[24] = "";
-		char model_name[32];
+		char hardware_version[24] = "";
-		char firmware_version[24];
+		char serial_number[33] = "";
-		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
-			name[0] = '\0';
-			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
-			name[0] = '\0';
-			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,

src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp

@@ -132,7 +132,7 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
 			mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
 		}
 
-		for (int index = 0; index < math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); ++index) {
+		for (int index = 0; index <  esc_status_s::CONNECTED_ESC_MAX; ++index) {
 
 			if (esc_status.esc[index].failures != 0) {
 

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -184,7 +184,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
 		bool is_esc_failure = !is_all_escs_armed;
 
 		for (int i = 0; i < limited_esc_count; i++) {
-			is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
+			is_esc_failure = is_esc_failure;
 		}
 
 		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);