From 6a18fd045f6719b2ba819128c428235b05d007e0 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Mon, 9 Feb 2026 10:25:37 +0100 Subject: [PATCH] uavcan: cleanup & node_status fix uavcan: esc fault handling review suggestions uavcan: fix node status & cleanup --- msg/DeviceInformation.msg | 3 - src/drivers/uavcan/actuators/esc.cpp | 72 +++++++++---------- src/drivers/uavcan/actuators/esc.hpp | 5 +- src/drivers/uavcan/node_info.cpp | 53 +------------- src/drivers/uavcan/node_info.hpp | 34 ++------- .../HealthAndArmingChecks/checks/escCheck.cpp | 2 +- .../failure_detector/FailureDetector.cpp | 2 +- 7 files changed, 43 insertions(+), 128 deletions(-) diff --git a/msg/DeviceInformation.msg b/msg/DeviceInformation.msg index 98c6762662..e4770486a5 100644 --- a/msg/DeviceInformation.msg +++ b/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. diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index f0d199a130..f370c1a969 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/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 &msg) { if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) { - auto &esc_report = _esc_status.esc[msg.esc_index]; - - esc_report.timestamp = hrt_absolute_time(); + esc_report_s &esc_report = _esc_status.esc[msg.esc_index]; + esc_report.timestamp = hrt_absolute_time(); esc_report.esc_address = msg.getSrcNodeID().get(); - esc_report.esc_voltage = msg.voltage; - esc_report.esc_current = msg.current; + esc_report.esc_voltage = msg.voltage; + 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_errorcount = msg.error_count; + esc_report.esc_rpm = msg.rpm; + 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 &msg) +void UavcanEscController::esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure &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_s &esc_report = _esc_status.esc[index]; + // 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 -UavcanEscController::get_failures(uint8_t esc_index) +uint32_t 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) - && 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); + if (_device_information_sub.copy(&device_information) + && device_information.device_type == device_information_s::DEVICE_TYPE_ESC + && device_information.device_id == esc_index) { + 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) - && 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; + for (auto &dronecan_node_status_sub : _dronecan_node_status_subs) { + if (dronecan_node_status_sub.copy(&node_status)) { + if (node_status.node_id == esc_report.esc_address) { + node_health = node_status.health; + vendor_specific_status_code = node_status.vendor_specific_status_code; + break; + } + } } - if (!have_node_status) { - return esc_report.failures; - } - - if (esc_report.esc_address < kMaxUavcanNodeId && (node_health == dronecan_node_status_s::HEALTH_OK || + if (esc_report.esc_address < UAVCAN_NODE_ID_MAX && (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; } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index fda70c1622..8cf2b39bfc 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -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_pub{ORB_ID(esc_status)}; - uORB::Subscription _dronecan_node_status_sub{ORB_ID(dronecan_node_status)}; + uORB::SubscriptionMultiArray _dronecan_node_status_subs{ORB_ID::dronecan_node_status}; uORB::Subscription _device_information_sub{ORB_ID(device_information)}; uint8_t _rotor_count{0}; diff --git a/src/drivers/uavcan/node_info.cpp b/src/drivers/uavcan/node_info.cpp index e8e440b244..b574080613 100644 --- a/src/drivers/uavcan/node_info.cpp +++ b/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.vendor_name) == sizeof(device_info.vendor_name), "Array size mismatch"); + static_assert(sizeof(msg.name) == sizeof(device_info.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(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; diff --git a/src/drivers/uavcan/node_info.hpp b/src/drivers/uavcan/node_info.hpp index 926d39ce49..8a1206039c 100644 --- a/src/drivers/uavcan/node_info.hpp +++ b/src/drivers/uavcan/node_info.hpp @@ -102,36 +102,10 @@ private: DeviceCapability capability{DeviceCapability::NONE}; bool has_node_info{false}; - char name[80]; - char vendor_name[32]; - char model_name[32]; - char firmware_version[24]; - 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'; - } + char name[80] = ""; + char firmware_version[24] = ""; + char hardware_version[24] = ""; + char serial_number[33] = ""; }; void handleNodeInfoRetrieved(uavcan::NodeID node_id, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 7bc47dc855..a43939cb90 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/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) { diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index cbba4c68e0..31bc3efc3e 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/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);