diff --git a/msg/EscReport.msg b/msg/EscReport.msg index ca80753f14..25331d951f 100644 --- a/msg/EscReport.msg +++ b/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 diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index af560293dc..f0d199a130 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -117,15 +117,16 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &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(); - ref.esc_address = msg.getSrcNodeID().get(); - ref.esc_voltage = msg.voltage; - ref.esc_current = msg.current; - ref.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius - ref.esc_rpm = msg.rpm; - ref.esc_errorcount = msg.error_count; + 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_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_status.esc_count = _rotor_count; _esc_status.counter += 1; @@ -133,6 +134,8 @@ UavcanEscController::esc_status_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.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; +} diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 0c513b13e7..fda70c1622 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/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 */ @@ -58,12 +59,14 @@ #include #include #include +#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&)> StatusCbBinder; @@ -113,11 +121,13 @@ private: typedef uavcan::MethodBinder TimerCbBinder; - bool _initialized = false; + bool _initialized{}; 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::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 78ada3beb5..e8e440b244 100644 --- a/src/drivers/uavcan/node_info.cpp +++ b/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)); diff --git a/src/drivers/uavcan/node_info.hpp b/src/drivers/uavcan/node_info.hpp index 025df2ab94..926d39ce49 100644 --- a/src/drivers/uavcan/node_info.hpp +++ b/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;