diff --git a/clearpath_hardware_interfaces/CMakeLists.txt b/clearpath_hardware_interfaces/CMakeLists.txt index d016cf7..ad56c0a 100644 --- a/clearpath_hardware_interfaces/CMakeLists.txt +++ b/clearpath_hardware_interfaces/CMakeLists.txt @@ -69,6 +69,19 @@ target_include_directories( include ) +# Cooling +set(FAN_CONTROL_EXECUTABLE fan_control_node) +set(FAN_CONTROL_DEPENDENCIES + clearpath_motor_msgs + clearpath_platform_msgs + sensor_msgs + rclcpp +) + +add_executable(${FAN_CONTROL_EXECUTABLE} src/a300/fan_control.cpp) +ament_target_dependencies(${FAN_CONTROL_EXECUTABLE} ${FAN_CONTROL_DEPENDENCIES}) + + # A200 Hardware add_library( a200_hardware diff --git a/clearpath_hardware_interfaces/src/a300/fan_control.cpp b/clearpath_hardware_interfaces/src/a300/fan_control.cpp new file mode 100644 index 0000000..b48ede5 --- /dev/null +++ b/clearpath_hardware_interfaces/src/a300/fan_control.cpp @@ -0,0 +1,242 @@ +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +const std::string FAN_CONTROL_TOPIC_NAME = "platform/mcu/cmd_fans"; +const std::string MCU_TEMPERATURE_TOPIC = "platform/mcu/status/temperature"; +const std::string MOTOR_TEMPERATURE_TOPIC = "platform/motors/status"; +const std::string BATTERY_TEMPERATURE_TOPIC = "platform/bms/battery_state"; + +constexpr float INITIAL_READING = 30.0f; + +enum class ThermalStatus +{ + Normal, + LowError, + LowWarning, + HighWarning, + HighError +}; + +// Helper function to convert enum to string +std::string thermalStatusToString(ThermalStatus status) +{ + switch (status) + { + case ThermalStatus::Normal: + return "Normal"; + case ThermalStatus::LowError: + return "LowError"; + case ThermalStatus::LowWarning: + return "LowWarning"; + case ThermalStatus::HighWarning: + return "HighWarning"; + case ThermalStatus::HighError: + return "HighError"; + default: + return "Unknown"; + } +} + +class ThermalSensor +{ +public: + + ThermalSensor(float low_error, float low_warning, float high_warning, float high_error) + : reading_(INITIAL_READING), low_error_(low_error), low_warning_(low_warning), high_warning_(high_warning), high_error_(high_error) {} + + // Default constructor + ThermalSensor() + : reading_(INITIAL_READING), low_error_(0.0), low_warning_(0.0), high_warning_(0.0), high_error_(0.0) {} + + void setValue(float value) { this->reading_ = value; } + + ThermalStatus getStatus() const + { + if (this->reading_ < this->low_error_) return ThermalStatus::LowError; + if (this->reading_ < this->low_warning_) return ThermalStatus::LowWarning; + if (this->reading_ > this->high_error_) return ThermalStatus::HighError; + if (this->reading_ > this->high_warning_) return ThermalStatus::HighWarning; + return ThermalStatus::Normal; + } + +private: + float low_error_; + float low_warning_; + float high_warning_; + float high_error_; + float reading_; + ThermalStatus thermal_status_; +}; + +class ThermalSensors +{ +public: + std::map sensors; + + ThermalSensors() + { + sensors = + { + {"mcu", ThermalSensor{0, 30, 45, 60}}, + {"fan1", ThermalSensor{5, 30, 45, 60}}, + {"fan2", ThermalSensor{5, 30, 45, 60}}, + {"fan3", ThermalSensor{5, 30, 45, 60}}, + {"fan4", ThermalSensor{5, 30, 45, 60}}, + {"5v_inductor", ThermalSensor{0, 30, 45, 60}}, + {"main_gnd_lug", ThermalSensor{0, 30, 45, 60}}, + {"dcdc_24v", ThermalSensor{0, 30, 45, 60}}, + {"dcdc_12v", ThermalSensor{0, 30, 45, 60}}, + {"battery", ThermalSensor{5, 30, 45, 60}}, + {"pcb_motor1", ThermalSensor{0, 30, 50, 70}}, + {"mcu_motor1", ThermalSensor{0, 30, 50, 70}}, + {"pcb_motor2", ThermalSensor{0, 30, 50, 70}}, + {"mcu_motor2", ThermalSensor{0, 30, 50, 70}}, + {"pcb_motor3", ThermalSensor{0, 30, 50, 70}}, + {"mcu_motor3", ThermalSensor{0, 30, 50, 70}}, + {"pcb_motor4", ThermalSensor{0, 30, 50, 70}}, + {"mcu_motor4", ThermalSensor{0, 30, 50, 70}}, + }; + } + + void setSensorValue(const std::string &name, float value) + { + if (sensors.find(name) != sensors.end()) + { + sensors[name].setValue(value); + } + else + { + RCLCPP_WARN(rclcpp::get_logger("ThermalSensors"), "Could not find sensor %s", name.c_str()); + } + } + + ThermalStatus getHighestStatus() const + { + ThermalStatus highest_status = ThermalStatus::Normal; + for (const auto &sensor : sensors) { + if (sensor.second.getStatus() > highest_status) + { + if (sensor.second.getStatus() >= ThermalStatus::Normal) + { + RCLCPP_WARN(rclcpp::get_logger("ThermalSensors"), "Thermal sensor %s has %s", + sensor.first.c_str(), thermalStatusToString(sensor.second.getStatus()).c_str()); + } + + highest_status = sensor.second.getStatus(); + } + } + return highest_status; + } +}; + +class FanController : public rclcpp::Node +{ +private: + std::mutex update_mutex_; + ThermalSensors thermal_sensors_; + rclcpp::Publisher::SharedPtr fan_publisher_; + + rclcpp::Subscription::SharedPtr temp_subscription_; + rclcpp::Subscription::SharedPtr battery_subscription_; + rclcpp::Subscription::SharedPtr motor_subscription_; + + rclcpp::TimerBase::SharedPtr control_timer_; + + uint8_t computeFanValue(ThermalStatus status) + { + float fan_fraction; + + switch (status) + { + case ThermalStatus::HighError: + fan_fraction = 1.0; + break; + case ThermalStatus::HighWarning: + fan_fraction = 0.75; + break; + case ThermalStatus::LowError: + fan_fraction = 0.0; + break; + case ThermalStatus::LowWarning: + fan_fraction = 0.25; + break; + default: + fan_fraction = 0.5; + break; + } + return static_cast(fan_fraction * 255.0f); + } + + void timerCallback() + { + std::lock_guard lock(update_mutex_); + + uint8_t fan_value = computeFanValue(thermal_sensors_.getHighestStatus()); + clearpath_platform_msgs::msg::Fans fans_msg; + fans_msg.fans = {fan_value, fan_value, fan_value, fan_value, 0, 0, 0, 0}; + fan_publisher_->publish(fans_msg); + RCLCPP_INFO(get_logger(), "Set all fans to: %d", fan_value); + } + +public: + FanController() : Node("fan_controller") + { + fan_publisher_ = create_publisher(FAN_CONTROL_TOPIC_NAME, 10); + + temp_subscription_ = create_subscription( + MCU_TEMPERATURE_TOPIC, 1, [this](const clearpath_platform_msgs::msg::Temperature::SharedPtr msg) + { + std::lock_guard lock(update_mutex_); + this->thermal_sensors_.setSensorValue("mcu", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_MCU]); + this->thermal_sensors_.setSensorValue("fan1", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_FAN1]); + this->thermal_sensors_.setSensorValue("fan2", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_FAN2]); + this->thermal_sensors_.setSensorValue("fan3", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_FAN3]); + this->thermal_sensors_.setSensorValue("fan4", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_FAN4]); + this->thermal_sensors_.setSensorValue("5v_inductor", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_5V_INDUCTOR]); + this->thermal_sensors_.setSensorValue("main_gnd_lug", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_MAIN_GND_LUG]); + this->thermal_sensors_.setSensorValue("dcdc_24v", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_24V_DCDC]); + this->thermal_sensors_.setSensorValue("dcdc_12v", msg->temperatures[clearpath_platform_msgs::msg::Temperature::CC01_12V_DCDC]); + } + ); + + // battery_subscription_ = create_subscription( + // BATTERY_TEMPERATURE_TOPIC, 1, [this](const sensor_msgs::msg::BatteryState::SharedPtr msg) + // { + // std::lock_guard lock(update_mutex_); + // this->thermal_sensors_.setSensorValue("battery", msg->temperature); + // } + // ); + + motor_subscription_ = create_subscription( + MOTOR_TEMPERATURE_TOPIC, 1, [this](const clearpath_motor_msgs::msg::LynxMultiStatus::SharedPtr msg) + { + std::lock_guard lock(update_mutex_); + this->thermal_sensors_.setSensorValue("pcb_motor1", msg->drivers[0].pcb_temperature); + this->thermal_sensors_.setSensorValue("mcu_motor1", msg->drivers[0].mcu_temperature); + this->thermal_sensors_.setSensorValue("pcb_motor2", msg->drivers[1].pcb_temperature); + this->thermal_sensors_.setSensorValue("mcu_motor2", msg->drivers[1].mcu_temperature); + this->thermal_sensors_.setSensorValue("pcb_motor3", msg->drivers[2].pcb_temperature); + this->thermal_sensors_.setSensorValue("mcu_motor3", msg->drivers[2].mcu_temperature); + this->thermal_sensors_.setSensorValue("pcb_motor4", msg->drivers[3].pcb_temperature); + this->thermal_sensors_.setSensorValue("mcu_motor4", msg->drivers[3].mcu_temperature); + } + ); + + control_timer_ = create_wall_timer(1s, std::bind(&FanController::timerCallback, this)); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +