Skip to content

Commit

Permalink
Added initial fan control.
Browse files Browse the repository at this point in the history
  • Loading branch information
@tonybaltovski
tonybaltovski committed Jan 24, 2025
1 parent 037ac37 commit 2b5a7ff
Show file tree
Hide file tree
Showing 2 changed files with 255 additions and 0 deletions.
13 changes: 13 additions & 0 deletions clearpath_hardware_interfaces/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
242 changes: 242 additions & 0 deletions clearpath_hardware_interfaces/src/a300/fan_control.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,242 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <clearpath_platform_msgs/msg/temperature.hpp>
#include <clearpath_platform_msgs/msg/fans.hpp>
#include <clearpath_motor_msgs/msg/lynx_multi_status.hpp>
#include <mutex>
#include <map>

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<std::string, ThermalSensor> 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<clearpath_platform_msgs::msg::Fans>::SharedPtr fan_publisher_;

rclcpp::Subscription<clearpath_platform_msgs::msg::Temperature>::SharedPtr temp_subscription_;
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_subscription_;
rclcpp::Subscription<clearpath_motor_msgs::msg::LynxMultiStatus>::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<uint8_t>(fan_fraction * 255.0f);
}

void timerCallback()
{
std::lock_guard<std::mutex> 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<clearpath_platform_msgs::msg::Fans>(FAN_CONTROL_TOPIC_NAME, 10);

temp_subscription_ = create_subscription<clearpath_platform_msgs::msg::Temperature>(
MCU_TEMPERATURE_TOPIC, 1, [this](const clearpath_platform_msgs::msg::Temperature::SharedPtr msg)
{
std::lock_guard<std::mutex> 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<sensor_msgs::msg::BatteryState>(
// BATTERY_TEMPERATURE_TOPIC, 1, [this](const sensor_msgs::msg::BatteryState::SharedPtr msg)
// {
// std::lock_guard<std::mutex> lock(update_mutex_);
// this->thermal_sensors_.setSensorValue("battery", msg->temperature);
// }
// );

motor_subscription_ = create_subscription<clearpath_motor_msgs::msg::LynxMultiStatus>(
MOTOR_TEMPERATURE_TOPIC, 1, [this](const clearpath_motor_msgs::msg::LynxMultiStatus::SharedPtr msg)
{
std::lock_guard<std::mutex> 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<FanController>());
rclcpp::shutdown();
return 0;
}

0 comments on commit 2b5a7ff

Please sign in to comment.