diff --git a/msg/DeviceInformation.msg b/msg/DeviceInformation.msg index e4770486a5..5962ac02d3 100644 --- a/msg/DeviceInformation.msg +++ b/msg/DeviceInformation.msg @@ -23,7 +23,7 @@ uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13 # Differential pressure uint8 DEVICE_TYPE_BATTERY = 14 # Battery uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer -char[80] name # Name of node +char[80] name # Name of device e.g. DroneCAN node name 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 0077e1df59..d44b565131 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -129,8 +129,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; _esc_status.esc_online_flags = check_escs_status(); _esc_status.esc_armed_flags = (1 << _rotor_count) - 1; - _esc_status.timestamp = hrt_absolute_time(); - + _esc_status.timestamp = esc_report.timestamp; _esc_status_pub.publish(_esc_status); } @@ -172,29 +171,15 @@ uint8_t UavcanEscController::check_escs_status() uint32_t UavcanEscController::get_failures(uint8_t 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_information{}; - - char esc_name[80] {}; - - 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'; - } - - // 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}; + // Check DoneCAN node health of the ESC + dronecan_node_status_s node_status{}; + uint8_t esc_node_id = _esc_status.esc[esc_index].esc_address; + uint8_t node_health = dronecan_node_status_s::HEALTH_OK; + uint16_t vendor_specific_status_code = 0; 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) { + if (node_status.node_id == esc_node_id) { node_health = node_status.health; vendor_specific_status_code = node_status.vendor_specific_status_code; break; @@ -202,12 +187,20 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index) } } - if (esc_report.esc_address <= uavcan::NodeID::Max && (node_health == dronecan_node_status_s::HEALTH_OK || - node_health == dronecan_node_status_s::HEALTH_WARNING)) { - failures = 0; + const bool failure = (node_health == dronecan_node_status_s::HEALTH_ERROR) + || (node_health == dronecan_node_status_s::HEALTH_CRITICAL); - } else if (strstr(esc_name, "iq_motion") != nullptr) { - // Parse iq_motion ESC errors + if (!failure) { + return 0; + } + + // Parse VertiQ = iq_motion ESC error flags + device_information_s device_information{}; + + if (_device_information_sub.copy(&device_information) + && device_information.device_type == device_information_s::DEVICE_TYPE_ESC + && device_information.device_id == esc_index + && strstr(device_information.name, "iq_motion") != nullptr) { static const struct { uint8_t bit; uint8_t failure_type; @@ -226,16 +219,17 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index) {11, esc_report_s::FAILURE_OVER_VOLTAGE}, }; + uint32_t failures = 0; + for (const auto &mapping : bit_to_failure_map) { if (vendor_specific_status_code & (1 << mapping.bit)) { failures |= (1 << mapping.failure_type); } } - } else { - // Generic parsing - failures |= (1 << esc_report_s::FAILURE_GENERIC); + return failures; } - return failures; + // Generic parsing + return (1 << esc_report_s::FAILURE_GENERIC); } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 71eb2e6fe3..9a53820bff 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -74,6 +74,7 @@ public: bool initialized() { return _initialized; }; void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size); + /** * Sets the number of rotors and enable timer */ @@ -100,7 +101,6 @@ private: */ uint8_t check_escs_status(); - /** * Gets failure flags for a specific ESC */ @@ -110,13 +110,12 @@ private: void (UavcanEscController::*)(const uavcan::ReceivedDataStructure&)> StatusCbBinder; typedef uavcan::MethodBinder&)> - StatusExtendedCbBinder; + void (UavcanEscController::*)(const uavcan::ReceivedDataStructure&)> StatusExtendedCbBinder; typedef uavcan::MethodBinder TimerCbBinder; - bool _initialized{}; + bool _initialized = false; esc_status_s _esc_status{}; @@ -133,7 +132,7 @@ private: uavcan::INode &_node; uavcan::Publisher _uavcan_pub_raw_cmd; uavcan::Subscriber _uavcan_sub_status; - uavcan::Subscriber _uavcan_sub_status_extended; + uavcan::Subscriber _uavcan_sub_status_extended; NodeInfoPublisher *_node_info_publisher{nullptr}; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index a43939cb90..dc8afc8bcf 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 < 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 fa0cfedbf8..cbba4c68e0 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -183,6 +183,10 @@ 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); + } + _esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms); _esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);