Add firmware errors to diagnostics

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<uint16_t, std::vector<std::string>> 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
   //--------------------------------------------------------

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);

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
  */