diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp index ecd16849..f753ee8f 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 //-------------------------------------------------------- diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp index dd825a63..bb9bda72 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 d4ba5510..c87535ca 100755 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -106,6 +106,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."); } @@ -255,6 +256,45 @@ void ClearpathDiagnosticUpdater::mcu_status_diagnostic(DiagnosticStatusWrapper & } } +/** + * @brief Report Firmware Errors to diagnostics + */ +void ClearpathDiagnosticUpdater::firmware_errors_diagnostic(DiagnosticStatusWrapper & stat) +{ + // 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 + { + stat.summary(DiagnosticStatus::ERROR, "Firmware errors reported"); + for (const auto &e : mcu_status_msg_.firmware_errors) + { + 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); + } + } + } +} + /** * @brief Save data from MCU Power messages */