From bdf91586dbe604a122b67e35ff2c6655779e62bb Mon Sep 17 00:00:00 2001 From: Mike Hosmar Date: Fri, 4 Jul 2025 11:00:54 -0400 Subject: [PATCH 1/4] Add firmware error titles/Troubleshooting steps --- .../clearpath_diagnostic_labels.hpp | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp index ecd16849..1be95bab 100644 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp @@ -64,6 +64,56 @@ class DiagnosticLabels { // Genric Robot inline static const std::string GENERIC = "generic"; + //-------------------------------------------------------- + // Labels for Firmware Error Codes + // Generated with a script in clearpath_documentation featrure/error-code-script + // Format is {error_code, {"Error Title", "Troubleshooting/Description"}} + //-------------------------------------------------------- + inline static const std::map> FIRMWARE_ERRORS = { + {110, {"Main Contactor Error!", ""}}, + {113, {"Battery Out of Range!", ""}}, + {116, {"E-Stop Contactor Error!", ""}}, + {117, {"Brake Contactor Error!", ""}}, + {120, {"Motor 1 Voltage Range Error!", ""}}, + {121, {"Motor 2 Voltage Range Error!", ""}}, + {122, {"Motor 3 Voltage Range Error!", ""}}, + {123, {"Motor 4 Voltage Range Error!", ""}}, + {124, {"Motor Voltage Range Error!", "This is a general error for a voltage issue on any motor."}}, + {140, {"User Power Contactor Error!", ""}}, + {150, {"24V Aux Power Fail!", ""}}, + {151, {"12V Aux Power Fail!", ""}}, + {152, {"12V1 Sys Power Fail!", ""}}, + {153, {"12V2 Sys Power Fail!", ""}}, + {154, {"12V A User Power Fail!", ""}}, + {155, {"12V B User Power Fail!", ""}}, + {156, {"VBAT User Power Fail!", ""}}, + {157, {"24V User Power Fail!", ""}}, + {160, {"Power Supply Failure!", "This is a general error which is set for any of the power fail errors."}}, + {170, {"VBAT User Power Fuse Tripped!", ""}}, + {171, {"24V User Power Fuse Tripped!", ""}}, + {172, {"12V A User Power Fuse Tripped!", ""}}, + {173, {"12V B User Power Fuse Tripped!", ""}}, + {174, {"Expansion Power Fuse Tripped!", ""}}, + {810, {"Fan 1 Below Minimum Speed!", ""}}, + {811, {"Fan 2 Below Minimum Speed!", ""}}, + {812, {"Fan 3 Below Minimum Speed!", ""}}, + {813, {"Fan 4 Below Minimum Speed!", ""}}, + {814, {"Fan 5 Below Minimum Speed!", ""}}, + {815, {"Fan 6 Below Minimum Speed!", ""}}, + {816, {"Fan 7 Below Minimum Speed!", ""}}, + {817, {"Fan 8 Below Minimum Speed!", ""}}, + {820, {"Fan Below Minimum Speed!", "This is a general error for any fan."}}, + {830, {"Fan 1 Above Maximum Speed!", ""}}, + {831, {"Fan 2 Above Maximum Speed!", ""}}, + {832, {"Fan 3 Above Maximum Speed!", ""}}, + {833, {"Fan 4 Above Maximum Speed!", ""}}, + {834, {"Fan 5 Above Maximum Speed!", ""}}, + {835, {"Fan 6 Above Maximum Speed!", ""}}, + {836, {"Fan 7 Above Maximum Speed!", ""}}, + {837, {"Fan 8 Above Maximum Speed!", ""}}, + {840, {"Fan Above Maximum Speed!", "This is a general error for any fan."}}, + }; + //-------------------------------------------------------- // Labels for clearpath_platform_msgs::msg::Power //-------------------------------------------------------- From 787784b94a16640edb122dc71031c1eac0145fa0 Mon Sep 17 00:00:00 2001 From: Mike Hosmar Date: Fri, 4 Jul 2025 18:02:25 +0000 Subject: [PATCH 2/4] add firmware error dignostics --- .../clearpath_diagnostic_labels.hpp | 2 +- .../clearpath_diagnostic_updater.hpp | 1 + .../src/clearpath_diagnostic_updater.cpp | 23 +++++++++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp index 1be95bab..f753ee8f 100644 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp @@ -69,7 +69,7 @@ class DiagnosticLabels { // Generated with a script in clearpath_documentation featrure/error-code-script // Format is {error_code, {"Error Title", "Troubleshooting/Description"}} //-------------------------------------------------------- - inline static const std::map> FIRMWARE_ERRORS = { + inline static const std::map> FIRMWARE_ERRORS = { {110, {"Main Contactor Error!", ""}}, {113, {"Battery Out of Range!", ""}}, {116, {"E-Stop Contactor Error!", ""}}, diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp index 3912c084..07ef4606 100644 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp @@ -71,6 +71,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Diagnostic Tasks void firmware_diagnostic(DiagnosticStatusWrapper & stat); void mcu_status_diagnostic(DiagnosticStatusWrapper & stat); + void firmware_errors_diagnostic(DiagnosticStatusWrapper & stat); void mcu_power_diagnostic(DiagnosticStatusWrapper & stat); void bms_state_diagnostic(DiagnosticStatusWrapper & stat); void stop_status_diagnostic(DiagnosticStatusWrapper & stat); diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp index 3b5f657d..72e01081 100755 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -94,6 +94,7 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater() // Add diagnostic tasks for MCU data updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic); + updater_.add("MCU Firmware Errors", this, &ClearpathDiagnosticUpdater::firmware_errors_diagnostic); updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::firmware_diagnostic); RCLCPP_INFO(this->get_logger(), "MCU diagnostics started."); } @@ -237,6 +238,28 @@ void ClearpathDiagnosticUpdater::mcu_status_diagnostic(DiagnosticStatusWrapper & } } +/** + * @brief Report Firmware Errors to diagnostics + */ +void ClearpathDiagnosticUpdater::firmware_errors_diagnostic(DiagnosticStatusWrapper & stat) +{ + if (mcu_status_msg_.firmware_errors.empty()) + { + stat.summary(DiagnosticStatus::OK, "No firmware errors reported"); + } + else + { + stat.summary(DiagnosticStatus::ERROR, "Firmware errors reported"); + for (auto e : mcu_status_msg_.firmware_errors) + { + std::string error_title = "Firmware Error " + e; + std::string error_message = + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[0] + ": " + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1]; + stat.add(error_title, error_message); + } + } +} + /** * @brief Save data from MCU Power messages */ From 6f5ffa159b72fe3e05f7d5617c816d2e48ae8f71 Mon Sep 17 00:00:00 2001 From: Mike Hosmar Date: Fri, 4 Jul 2025 18:28:23 +0000 Subject: [PATCH 3/4] Handle unknown errors --- .../src/clearpath_diagnostic_updater.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp index 72e01081..a240d635 100755 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -250,11 +250,23 @@ void ClearpathDiagnosticUpdater::firmware_errors_diagnostic(DiagnosticStatusWrap else { stat.summary(DiagnosticStatus::ERROR, "Firmware errors reported"); - for (auto e : mcu_status_msg_.firmware_errors) + for (const auto &e : mcu_status_msg_.firmware_errors) { - std::string error_title = "Firmware Error " + e; - std::string error_message = - DiagnosticLabels::FIRMWARE_ERRORS.at(e)[0] + ": " + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1]; + std::string error_title = "Firmware Error " + std::to_string(e); + std::string error_message; + try + { + error_message = DiagnosticLabels::FIRMWARE_ERRORS.at(e)[0]; + // Add the troubleshooting message if it exists + if( DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1].size() > 1) + { + error_message += ": " + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1]; + } + } + catch (const std::out_of_range &) + { + error_message = "Unknown firmware error code"; + } stat.add(error_title, error_message); } } From bb9e090ffb7d0ade5cb4ed8fcc43f0c359c1bfd7 Mon Sep 17 00:00:00 2001 From: Mike Hosmar Date: Fri, 4 Jul 2025 18:37:30 +0000 Subject: [PATCH 4/4] Check frequency of status message --- .../src/clearpath_diagnostic_updater.cpp | 45 ++++++++++--------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp index a240d635..8730a065 100755 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -243,31 +243,36 @@ void ClearpathDiagnosticUpdater::mcu_status_diagnostic(DiagnosticStatusWrapper & */ void ClearpathDiagnosticUpdater::firmware_errors_diagnostic(DiagnosticStatusWrapper & stat) { - if (mcu_status_msg_.firmware_errors.empty()) - { - stat.summary(DiagnosticStatus::OK, "No firmware errors reported"); - } - else - { - stat.summary(DiagnosticStatus::ERROR, "Firmware errors reported"); - for (const auto &e : mcu_status_msg_.firmware_errors) + // Get the frequency status from the MCU status message + mcu_status_freq_status_->run(stat); + + if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) { + if (mcu_status_msg_.firmware_errors.empty()) + { + stat.summary(DiagnosticStatus::OK, "No firmware errors reported"); + } + else { - std::string error_title = "Firmware Error " + std::to_string(e); - std::string error_message; - try + stat.summary(DiagnosticStatus::ERROR, "Firmware errors reported"); + for (const auto &e : mcu_status_msg_.firmware_errors) { - error_message = DiagnosticLabels::FIRMWARE_ERRORS.at(e)[0]; - // Add the troubleshooting message if it exists - if( DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1].size() > 1) + std::string error_title = "Firmware Error " + std::to_string(e); + std::string error_message; + try { - error_message += ": " + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1]; + error_message = DiagnosticLabels::FIRMWARE_ERRORS.at(e)[0]; + // Add the troubleshooting message if it exists + if( DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1].size() > 1) + { + error_message += ": " + DiagnosticLabels::FIRMWARE_ERRORS.at(e)[1]; + } } + catch (const std::out_of_range &) + { + error_message = "Unknown firmware error code"; + } + stat.add(error_title, error_message); } - catch (const std::out_of_range &) - { - error_message = "Unknown firmware error code"; - } - stat.add(error_title, error_message); } } }