add vendor specific error handling

Vendor specific failure handling: IQ_Motion

msg/EscReport.msg

@@ -4,6 +4,7 @@ int32 esc_rpm						# Motor RPM, negative for reverse rotation [RPM] - if support
 float32 esc_voltage					# Voltage measured from current ESC [V] - if supported
 float32 esc_current					# Current measured from current ESC [A] - if supported
 float32 esc_temperature					# Temperature measured from current ESC [degC] - if supported
+int16 motor_temperature 				# Temperature measured from current motor [degC] - if supported
 uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set by driver)
 uint8 esc_cmdcount					# Counter of number of commands
 

src/drivers/uavcan/actuators/esc.cpp

@@ -117,15 +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 &ref = _esc_status.esc[msg.esc_index];
+		auto &esc_report = _esc_status.esc[msg.esc_index];
 
-		ref.timestamp       = hrt_absolute_time();
+		esc_report.timestamp       = hrt_absolute_time();
-		ref.esc_address = msg.getSrcNodeID().get();
+		esc_report.esc_address = msg.getSrcNodeID().get();
-		ref.esc_voltage     = msg.voltage;
+		esc_report.esc_voltage     = msg.voltage;
-		ref.esc_current     = msg.current;
+		esc_report.esc_current     = msg.current;
-		ref.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
+		esc_report.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
-		ref.esc_rpm         = msg.rpm;
+		// esc_report.motor_temperature is filled in the extended status callback
-		ref.esc_errorcount  = msg.error_count;
+		esc_report.esc_rpm         = msg.rpm;
+		esc_report.esc_errorcount  = msg.error_count;
 
 		_esc_status.esc_count = _rotor_count;
 		_esc_status.counter += 1;
@@ -133,6 +134,8 @@ 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);
 	}
 
@@ -144,6 +147,20 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	}
 }
 
+void
+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];
+		// This will be published along with the regular ESC status
+		esc_report.motor_temperature = msg.motor_temperature_degC;
+		esc_report.esc_power = msg.input_pct;
+	}
+}
+
 uint8_t
 UavcanEscController::check_escs_status()
 {
@@ -160,3 +177,76 @@ UavcanEscController::check_escs_status()
 
 	return esc_status_flags;
 }
+
+uint32_t
+UavcanEscController::get_failures(uint8_t esc_index)
+{
+	auto &esc_report = _esc_status.esc[esc_index];
+
+
+	// Update device vendor/model information from device_information topic
+	device_information_s device_info;
+	char esc_name[80] {};
+
+	if (_device_information_sub.copy(&device_info)
+	    && device_info.device_type == device_information_s::DEVICE_TYPE_ESC
+	    && device_info.device_id == esc_index) {
+		strncpy(esc_name, device_info.name, sizeof(esc_name) - 1);
+		esc_name[sizeof(esc_name) - 1] = '\0';
+	}
+
+	// Update node health from all available dronecan_node_status messages
+	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)
+	    && esc_report.esc_address < kMaxUavcanNodeId
+	    && node_status.node_id == esc_report.esc_address) {
+		node_health = node_status.health;
+		vendor_specific_status_code = node_status.vendor_specific_status_code;
+		have_node_status = true;
+	}
+
+	if (!have_node_status) {
+		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;
+
+	} else if (strstr(esc_name, "iq_motion") != nullptr) {
+		// Parse iq_motion ESC errors
+		static const struct {
+			uint8_t bit;
+			uint8_t failure_type;
+		} bit_to_failure_map[] = {
+			{0,  esc_report_s::FAILURE_OVER_VOLTAGE},
+			{1,  esc_report_s::FAILURE_OVER_VOLTAGE},
+			{2,  esc_report_s::FAILURE_OVER_VOLTAGE},
+			{3,  esc_report_s::FAILURE_OVER_CURRENT},
+			{4,  esc_report_s::FAILURE_OVER_CURRENT},
+			{5,  esc_report_s::FAILURE_OVER_ESC_TEMPERATURE},
+			{6,  esc_report_s::FAILURE_MOTOR_OVER_TEMPERATURE},
+			{7,  esc_report_s::FAILURE_GENERIC},
+			{8,  esc_report_s::FAILURE_OVER_RPM},
+			{9,  esc_report_s::FAILURE_WARN_ESC_TEMPERATURE},
+			{10, esc_report_s::FAILURE_MOTOR_WARN_TEMPERATURE},
+			{11, esc_report_s::FAILURE_OVER_VOLTAGE},
+		};
+
+		for (const auto &mapping : bit_to_failure_map) {
+			if (vendor_specific_status_code & (1 << mapping.bit)) {
+				esc_report.failures |= (1 << mapping.failure_type);
+			}
+		}
+
+	} else {
+		// Generic parsing
+		esc_report.failures |= (1 << esc_report_s::FAILURE_GENERIC);
+	}
+
+	return esc_report.failures;
+}

src/drivers/uavcan/actuators/esc.hpp

@@ -38,6 +38,7 @@
  *     uavcan.equipment.esc.RawCommand
  *     uavcan.equipment.esc.RPMCommand
  *     uavcan.equipment.esc.Status
+ *     uavcan.equipment.esc.StatusExtended
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
@@ -58,12 +59,14 @@
 #include <uORB/topics/device_information.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/uavcan/node_info.hpp>
+#include "../node_info.hpp"
 
 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_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
 
@@ -75,8 +78,7 @@ public:
 
 	bool initialized() { return _initialized; };
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
+	void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
-
 	/**
 	 * Sets the number of rotors and enable timer
 	 */
@@ -103,6 +105,12 @@ private:
 	 */
 	uint8_t check_escs_status();
 
+
+	/**
+	 * Gets failure flags for a specific ESC
+	 */
+	uint32_t get_failures(uint8_t esc_index);
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
@@ -113,11 +121,13 @@ private:
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
-	bool _initialized = false;
+	bool _initialized{};
 
 	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::Subscription _device_information_sub{ORB_ID(device_information)};
 
 	uint8_t		_rotor_count{0};
 

src/drivers/uavcan/node_info.cpp

@@ -206,7 +206,6 @@ void NodeInfoPublisher::publishSingleDeviceInformation(const DeviceInformation &
 	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");
-	// Copy strings using memcpy and ensure null termination
 	static_assert(sizeof(msg.name) == sizeof(device_info.name), "Array size mismatch");
 
 	memcpy(msg.model_name, device_info.model_name, sizeof(msg.model_name));

src/drivers/uavcan/node_info.hpp

@@ -148,10 +148,10 @@ private:
 	// Publishing methods
 	void publishDeviceInformationPeriodic();
 	void publishSingleDeviceInformation(const DeviceInformation &device_info);
-	void parseNodeName(const char *name, DeviceInformation &device_info);
 
 	// Helper functions
 	void populateDeviceInfoFields(DeviceInformation &device_info, const NodeInfo &info);
+	void parseNodeName(const char *name, DeviceInformation &device_info);
 	bool extendDeviceInformationsArray();
 
 	uavcan::NodeInfoRetriever &_node_info_retriever;