Skip to content

Added MCU alerts to diagnostics #248

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Aug 15, 2025
Merged

Added MCU alerts to diagnostics #248

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b56d214
Added MCU alerts to diagnostics
roni-kreinin Aug 15, 2025
7853c55
Linting
roni-kreinin Aug 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
54 changes: 54 additions & 0 deletions clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
#define CLEARPATH_DIAGNOSTIC_LABELS_HPP

#include <map>
#include <string>
#include <vector>

#include "sensor_msgs/msg/battery_state.hpp"
Expand Down Expand Up @@ -64,6 +65,59 @@ class DiagnosticLabels {
// Genric Robot
inline static const std::string GENERIC = "generic";

//--------------------------------------------------------
// Labels for Firmware Alert Codes
// Format is {alert_code, {"Alert Title", "Troubleshooting/Description"}}
//--------------------------------------------------------
inline static const std::map<std::string, std::vector<std::string>> FIRMWARE_ALERTS = {
{"E110", {"Main Contactor Error!", ""}},
{"E113", {"Battery Out of Range!", ""}},
{"E116", {"E-Stop Contactor Error!", ""}},
{"E117", {"Brake Contactor Error!", ""}},
{"E120", {"Motor 1 Voltage Range Error!", ""}},
{"E121", {"Motor 2 Voltage Range Error!", ""}},
{"E122", {"Motor 3 Voltage Range Error!", ""}},
{"E123", {"Motor 4 Voltage Range Error!", ""}},
{"E124",
{"Motor Voltage Range Error!", "This is a general error for a voltage issue on any motor."}},
{"E140", {"User Power Contactor Error!", ""}},
{"E150", {"24V Aux Power Fail!", ""}},
{"E151", {"12V Aux Power Fail!", ""}},
{"E152", {"12V1 Sys Power Fail!", ""}},
{"E153", {"12V2 Sys Power Fail!", ""}},
{"E154", {"12V A User Power Fail!", ""}},
{"E155", {"12V B User Power Fail!", ""}},
{"E156", {"VBAT User Power Fail!", ""}},
{"E157", {"24V User Power Fail!", ""}},
{"E160",
{"Power Supply Failure!",
"This is a general error which is set for any of the power fail errors."}},
{"E170", {"VBAT User Power Fuse Tripped!", ""}},
{"E171", {"24V User Power Fuse Tripped!", ""}},
{"E172", {"12V A User Power Fuse Tripped!", ""}},
{"E173", {"12V B User Power Fuse Tripped!", ""}},
{"E174", {"Expansion Power Fuse Tripped!", ""}},
{"E810", {"Fan 1 Below Minimum Speed!", ""}},
{"E811", {"Fan 2 Below Minimum Speed!", ""}},
{"E812", {"Fan 3 Below Minimum Speed!", ""}},
{"E813", {"Fan 4 Below Minimum Speed!", ""}},
{"E814", {"Fan 5 Below Minimum Speed!", ""}},
{"E815", {"Fan 6 Below Minimum Speed!", ""}},
{"E816", {"Fan 7 Below Minimum Speed!", ""}},
{"E817", {"Fan 8 Below Minimum Speed!", ""}},
{"E820", {"Fan Below Minimum Speed!", "This is a general error for any fan."}},
{"E830", {"Fan 1 Above Maximum Speed!", ""}},
{"E831", {"Fan 2 Above Maximum Speed!", ""}},
{"E832", {"Fan 3 Above Maximum Speed!", ""}},
{"E833", {"Fan 4 Above Maximum Speed!", ""}},
{"E834", {"Fan 5 Above Maximum Speed!", ""}},
{"E835", {"Fan 6 Above Maximum Speed!", ""}},
{"E836", {"Fan 7 Above Maximum Speed!", ""}},
{"E837", {"Fan 8 Above Maximum Speed!", ""}},
{"E840", {"Fan Above Maximum Speed!", "This is a general error for any fan."}},
};


//--------------------------------------------------------
// Labels for clearpath_platform_msgs::msg::Power
//--------------------------------------------------------
Expand Down
6 changes: 6 additions & 0 deletions clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/string.hpp"

#include "clearpath_diagnostics/clearpath_diagnostic_labels.hpp"
#include "clearpath_platform_msgs/msg/power.hpp"
Expand All @@ -63,6 +64,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node

// Callbacks
void mcu_status_callback(const clearpath_platform_msgs::msg::Status & msg);
void mcu_alerts_callback(const std_msgs::msg::String & msg);
void mcu_power_callback(const clearpath_platform_msgs::msg::Power & msg);
void bms_state_callback(const BatteryState & msg);
void stop_status_callback(const clearpath_platform_msgs::msg::StopStatus & msg);
Expand All @@ -71,6 +73,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
// Diagnostic Tasks
void firmware_diagnostic(DiagnosticStatusWrapper & stat);
void mcu_status_diagnostic(DiagnosticStatusWrapper & stat);
void firmware_alerts_diagnostic(DiagnosticStatusWrapper & stat);
void mcu_power_diagnostic(DiagnosticStatusWrapper & stat);
void bms_state_diagnostic(DiagnosticStatusWrapper & stat);
void stop_status_diagnostic(DiagnosticStatusWrapper & stat);
Expand All @@ -94,6 +97,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node

// Topic names and rates
std::string mcu_status_topic_;
std::string mcu_alerts_topic_;
std::string mcu_power_topic_;
std::string bms_state_topic_;
std::string stop_status_topic_;
Expand All @@ -112,6 +116,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
// Message Data
std::string mcu_firmware_version_;
clearpath_platform_msgs::msg::Status mcu_status_msg_;
std_msgs::msg::String mcu_alerts_msg_;
clearpath_platform_msgs::msg::Power mcu_power_msg_;
BatteryState bms_state_msg_;
clearpath_platform_msgs::msg::StopStatus stop_status_msg_;
Expand All @@ -126,6 +131,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node

// Subscriptions
rclcpp::Subscription<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_mcu_alerts_;
rclcpp::Subscription<clearpath_platform_msgs::msg::Power>::SharedPtr sub_mcu_power_;
rclcpp::Subscription<BatteryState>::SharedPtr sub_bms_state_;
rclcpp::Subscription<clearpath_platform_msgs::msg::StopStatus>::SharedPtr sub_stop_status_;
Expand Down
85 changes: 85 additions & 0 deletions clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,9 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
// Get optional parameters from the config
mcu_status_topic_ = get_string_param("mcu_status_topic");
mcu_status_topic_ = (mcu_status_topic_ == UNKNOWN) ? "platform/mcu/status" : mcu_status_topic_;
mcu_alerts_topic_ = get_string_param("mcu_alerts_topic");
mcu_alerts_topic_ = (mcu_alerts_topic_ ==
UNKNOWN) ? "platform/mcu/status/alerts" : mcu_alerts_topic_;
mcu_power_topic_ = get_string_param("mcu_power_topic");
mcu_power_topic_ = (mcu_power_topic_ == UNKNOWN) ? "platform/mcu/status/power" : mcu_power_topic_;
bms_state_topic_ = get_string_param("bms_state_topic");
Expand Down Expand Up @@ -100,12 +103,20 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
rclcpp::SensorDataQoS(),
std::bind(&ClearpathDiagnosticUpdater::mcu_status_callback, this, std::placeholders::_1));

sub_mcu_alerts_ =
this->create_subscription<std_msgs::msg::String>(
mcu_alerts_topic_,
rclcpp::SensorDataQoS(),
std::bind(&ClearpathDiagnosticUpdater::mcu_alerts_callback, this, std::placeholders::_1));

// Create MCU Frequency Status tracking objects
mcu_status_freq_status_ = std::make_shared<FrequencyStatus>(
FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, mcu_status_tolerance_, 10));

// Add diagnostic tasks for MCU data
updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic);
updater_.add("MCU Firmware Alerts", this,
&ClearpathDiagnosticUpdater::firmware_alerts_diagnostic);
updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::firmware_diagnostic);
RCLCPP_INFO(this->get_logger(), "MCU diagnostics started.");
}
Expand Down Expand Up @@ -228,6 +239,71 @@ void ClearpathDiagnosticUpdater::firmware_diagnostic(DiagnosticStatusWrapper & s
stat.add("Firmware Version on MCU", mcu_firmware_version_);
}

/**
* @brief Helper function to split a string into a vector of strings by delimiter
*
* @param s String to split
* @param delimiter Delimiter to split with
* @return std::vector<std::string> Vector of split string
*/
static std::vector<std::string> split(std::string s, std::string delimiter)
{
size_t pos_start = 0, pos_end, delim_len = delimiter.length();
std::string token;
std::vector<std::string> res;

while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) {
token = s.substr(pos_start, pos_end - pos_start);
pos_start = pos_end + delim_len;
res.push_back(token);
}

res.push_back(s.substr(pos_start));
return res;
}

/**
* @brief Report Firmware Alerts to diagnostics
*/
void ClearpathDiagnosticUpdater::firmware_alerts_diagnostic(DiagnosticStatusWrapper & stat)
{
if (mcu_alerts_msg_.data.empty() || mcu_alerts_msg_.data == "None") {
stat.summary(DiagnosticStatus::OK, "No firmware alerts reported");
} else {
unsigned char diagnostic_status = DiagnosticStatus::OK;
std::vector<std::string> alerts = split(mcu_alerts_msg_.data, ",");

for (const auto & a : alerts) {
std::string alert_title = "Firmware Alert " + a;
std::string alert_message;
unsigned char alert_status = DiagnosticStatus::OK;
try {
alert_message = DiagnosticLabels::FIRMWARE_ALERTS.at(a)[0];
// Add the troubleshooting message if it exists
if (DiagnosticLabels::FIRMWARE_ALERTS.at(a)[1].size() > 1) {
alert_message += ": " + DiagnosticLabels::FIRMWARE_ALERTS.at(a)[1];
}
} catch (const std::out_of_range &) {
alert_message = "Unknown firmware alert code";
}
stat.add(alert_title, alert_message);

// Warning for W alerts, Error for E alerts
if (a.find('W') != std::string::npos) {
alert_status = DiagnosticStatus::WARN;
} else if (a.find('E') != std::string::npos) {
alert_status = DiagnosticStatus::ERROR;
}

if (alert_status > diagnostic_status) {
diagnostic_status = alert_status;
}
}

stat.summary(diagnostic_status, "Firmware alerts reported");
}
}

/**
* @brief Save data from MCU Status messages
*/
Expand All @@ -239,6 +315,15 @@ void ClearpathDiagnosticUpdater::mcu_status_callback(
mcu_status_freq_status_->tick();
}

/**
* @brief Save data from MCU Alerts messages
*/
void ClearpathDiagnosticUpdater::mcu_alerts_callback(
const std_msgs::msg::String & msg)
{
mcu_alerts_msg_ = msg;
}

/**
* @brief Report MCU Status message information to diagnostics
*/
Expand Down
Loading