From 08372cb97f37d33407128476fa5f311aa9ff6a7e Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Tue, 28 Jan 2025 14:08:13 -0500 Subject: [PATCH] Split into header and source. Also, added to launch. --- .../launch/generator.py | 9 + clearpath_hardware_interfaces/CMakeLists.txt | 24 +- .../a300/fan_control.hpp | 172 +++++++++ .../src/a300/fan_control.cpp | 355 +++++++----------- 4 files changed, 342 insertions(+), 218 deletions(-) create mode 100644 clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/fan_control.hpp diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 580fc44..5f0f79a 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -293,6 +293,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ] )) + # A300 Fan Control Node + self.a300_fan_control = LaunchFile.Node( + package='clearpath_hardware_interfaces', + executable='fan_control_node', + name='a300_fan_control', + namespace=self.namespace, + ) + # Components required for each platform common_platform_components = [ self.wireless_watcher_node, @@ -321,6 +329,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.configure_mcu, self.lighting_node, self.lynx_node, + self.a300_fan_control, ], Platform.W200: common_platform_components + [ self.imu_0_filter_node, diff --git a/clearpath_hardware_interfaces/CMakeLists.txt b/clearpath_hardware_interfaces/CMakeLists.txt index 2916b11..8cbfe6c 100644 --- a/clearpath_hardware_interfaces/CMakeLists.txt +++ b/clearpath_hardware_interfaces/CMakeLists.txt @@ -37,10 +37,10 @@ find_package(tf2_ros REQUIRED) set(LIGHTING_EXECUTABLE lighting_node) set(LIGHTING_LIB clearpath_platform_lighting) set(LIGHTING_DEPENDENCIES - clearpath_motor_msgs - clearpath_platform_msgs - sensor_msgs - rclcpp + clearpath_motor_msgs + clearpath_platform_msgs + sensor_msgs + rclcpp ) add_library( @@ -72,13 +72,21 @@ target_include_directories( # Cooling set(FAN_CONTROL_EXECUTABLE fan_control_node) set(FAN_CONTROL_DEPENDENCIES - clearpath_motor_msgs - clearpath_platform_msgs - sensor_msgs - rclcpp + clearpath_motor_msgs + clearpath_platform_msgs + sensor_msgs + rclcpp ) add_executable(${FAN_CONTROL_EXECUTABLE} src/a300/fan_control.cpp) + +target_include_directories( + ${FAN_CONTROL_EXECUTABLE} + PRIVATE + include +) + +target_link_libraries(${FAN_CONTROL_EXECUTABLE}) ament_target_dependencies(${FAN_CONTROL_EXECUTABLE} ${FAN_CONTROL_DEPENDENCIES}) diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/fan_control.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/fan_control.hpp new file mode 100644 index 0000000..6e0ff1e --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a300/fan_control.hpp @@ -0,0 +1,172 @@ +#ifndef CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP +#define CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace a300_cooling +{ +/// Fan control topic name +const std::string FAN_CONTROL_TOPIC_NAME = "platform/mcu/cmd_fans"; + +/// MCU temperature topic name +const std::string MCU_TEMPERATURE_TOPIC = "platform/mcu/status/temperature"; + +/// Motor temperature topic name +const std::string MOTOR_TEMPERATURE_TOPIC = "platform/motors/status"; + +/// Battery temperature topic name +const std::string BATTERY_TEMPERATURE_TOPIC = "platform/bms/battery_state"; + +/// Fan command for high error state, percent of maximum speed +constexpr float FAN_CMD_HIGH_ERROR = 1.0f; + +/// Fan command for high warning state, percent of maximum speed +constexpr float FAN_CMD_HIGH_WARNING = 0.75f; + +/// Fan command for low error state, percent of maximum speed +constexpr float FAN_CMD_LOW_ERROR = 0.0f; + +/// Fan command for low warning state, percent of maximum speed +constexpr float FAN_CMD_LOW_WARNING = 0.25f; + +/// Fan command for normal state, percent of maximum speed +constexpr float FAN_CMD_NORMAL = 0.25f; + +/// Initial temperature reading when no data is available, degrees Celsius +constexpr float INITIAL_READING = 30.0f; + +/// Enumeration to represent thermal status +enum class ThermalStatus +{ + Normal, ///< Thermal state is within normal range + LowError, ///< Low error thermal state + LowWarning, ///< Low warning thermal state + HighWarning, ///< High warning thermal state + HighError ///< High error thermal state +}; + +/** + * @brief Convert thermal status to string + * + * @param status The thermal status + * @return A string representing the thermal status + */ +std::string thermalStatusToString(ThermalStatus status); + +/** + * @brief Represents a thermal sensor with associated status thresholds + */ +class ThermalSensor +{ +public: + /** + * @brief Construct a new ThermalSensor object with specific threshold values + * + * @param low_error Temperature below this value is an error + * @param low_warning Temperature below this value is a warning + * @param high_warning Temperature above this value is a warning + * @param high_error Temperature above this value is an error + */ + ThermalSensor(float low_error, float low_warning, float high_warning, float high_error); + + /** + * @brief Construct a new ThermalSensor object with default values + */ + ThermalSensor(); + + /** + * @brief Set the temperature reading of the sensor + * + * @param value Temperature value to be set + */ + void setValue(float value); + + /** + * @brief Get the current thermal status of the sensor + * + * @return The current thermal status + */ + ThermalStatus getStatus() const; + +private: + float reading_; ///< Current temperature reading + float low_error_; ///< Threshold for low error + float low_warning_; ///< Threshold for low warning + float high_warning_;///< Threshold for high warning + float high_error_; ///< Threshold for high error + ThermalStatus thermal_status_; ///< Current thermal status +}; + +/** + * @brief Collection of thermal sensors in the system + */ +class ThermalSensors +{ +public: + /** + * @brief Default constructor that initializes the sensor list with default values + */ + ThermalSensors(); + + /** + * @brief Set the value of a specific sensor + * + * @param name The sensor's name + * @param value The temperature value to set + */ + void setSensorValue(const std::string &name, float value); + + /** + * @brief Get the highest thermal status from all sensors + * + * @return The highest thermal status + */ + ThermalStatus getHighestStatus() const; + +private: + std::map sensors; ///< Map of sensor names to sensor objects +}; + +/** + * @brief Main class for managing fan control based on thermal sensor readings + */ +class FanController : public rclcpp::Node +{ +public: + /** + * @brief Construct a new FanController object + */ + FanController(); + +private: + /** + * @brief Timer callback to periodically check and control the fans + */ + void timerCallback(); + + /** + * @brief Compute the fan value based on the thermal status + * + * @param status The thermal status of the highest priority sensor + * @return Computed fan value as a uint8_t (0 to 255) + */ + uint8_t computeFanValue(ThermalStatus status); + + std::mutex update_mutex_; ///< Mutex for updating thermal sensors + ThermalSensors thermal_sensors_; ///< Thermal sensor data + rclcpp::Publisher::SharedPtr fan_publisher_; ///< Fan control publisher + rclcpp::Subscription::SharedPtr temp_subscription_; ///< Temperature subscription + rclcpp::Subscription::SharedPtr battery_subscription_; ///< Battery state subscription + rclcpp::Subscription::SharedPtr motor_subscription_; ///< Motor status subscription + rclcpp::TimerBase::SharedPtr control_timer_; ///< Timer for controlling the fan +}; + +} // namespace a300_cooling + +#endif // CLEARPATH_HARDWARE_INTERFACES_A300_FAN_CONTROL_HPP diff --git a/clearpath_hardware_interfaces/src/a300/fan_control.cpp b/clearpath_hardware_interfaces/src/a300/fan_control.cpp index 3ac069e..7c21a62 100644 --- a/clearpath_hardware_interfaces/src/a300/fan_control.cpp +++ b/clearpath_hardware_interfaces/src/a300/fan_control.cpp @@ -1,247 +1,182 @@ -#include -#include -#include -#include -#include -#include -#include +#include "clearpath_hardware_interfaces/a300/fan_control.hpp" -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 +std::string a300_cooling::thermalStatusToString(a300_cooling::ThermalStatus status) { - 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"; - } + switch (status) + { + case a300_cooling::ThermalStatus::Normal: + return "Normal"; + case a300_cooling::ThermalStatus::LowError: + return "LowError"; + case a300_cooling::ThermalStatus::LowWarning: + return "LowWarning"; + case a300_cooling::ThermalStatus::HighWarning: + return "HighWarning"; + case a300_cooling::ThermalStatus::HighError: + return "HighError"; + default: + return "Unknown"; + } } -class ThermalSensor -{ -public: +a300_cooling::ThermalSensor::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) {} - 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) {} +a300_cooling::ThermalSensor::ThermalSensor() + : reading_(INITIAL_READING), low_error_(0.0), low_warning_(0.0), high_warning_(0.0), high_error_(0.0) {} - // Default constructor - ThermalSensor() - : reading_(INITIAL_READING), low_error_(0.0), low_warning_(0.0), high_warning_(0.0), high_error_(0.0) {} +void a300_cooling::ThermalSensor::setValue(float value) { this->reading_ = value; } - void setValue(float value) { this->reading_ = value; } +a300_cooling::ThermalStatus a300_cooling::ThermalSensor::getStatus() const +{ + if (this->reading_ < this->low_error_) return a300_cooling::ThermalStatus::LowError; + if (this->reading_ < this->low_warning_) return a300_cooling::ThermalStatus::LowWarning; + if (this->reading_ > this->high_error_) return a300_cooling::ThermalStatus::HighError; + if (this->reading_ > this->high_warning_) return a300_cooling::ThermalStatus::HighWarning; + return a300_cooling::ThermalStatus::Normal; +} - ThermalStatus getStatus() const +a300_cooling::ThermalSensors::ThermalSensors() +{ + sensors = { - 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_; -}; + {"mcu", ThermalSensor{-20, 0, 60, 80}}, + {"fan1", ThermalSensor{-20, 0, 60, 80}}, + {"fan2", ThermalSensor{-20, 0, 60, 80}}, + {"fan3", ThermalSensor{-20, 0, 60, 80}}, + {"fan4", ThermalSensor{-20, 0, 60, 80}}, + {"5v_inductor", ThermalSensor{-20, 0, 60, 80}}, + {"main_gnd_lug", ThermalSensor{-20, 0, 60, 80}}, + {"dcdc_24v", ThermalSensor{-20, 0, 60, 80}}, + {"dcdc_12v", ThermalSensor{-20, 0, 60, 80}}, + {"battery", ThermalSensor{-20, 0, 60, 80}}, + {"pcb_motor1", ThermalSensor{-20, 0, 60, 80}}, + {"mcu_motor1", ThermalSensor{-20, 0, 60, 80}}, + {"pcb_motor2", ThermalSensor{-20, 0, 60, 80}}, + {"mcu_motor2", ThermalSensor{-20, 0, 60, 80}}, + {"pcb_motor3", ThermalSensor{-20, 0, 60, 80}}, + {"mcu_motor3", ThermalSensor{-20, 0, 60, 80}}, + {"pcb_motor4", ThermalSensor{-20, 0, 60, 80}}, + {"mcu_motor4", ThermalSensor{-20, 0, 60, 80}}, + }; +} -class ThermalSensors +void a300_cooling::ThermalSensors::setSensorValue(const std::string &name, float value) { -public: - std::map sensors; - - ThermalSensors() + if (sensors.find(name) != sensors.end()) { - sensors = - { - {"mcu", ThermalSensor{-20, 0, 60, 80}}, - {"fan1", ThermalSensor{-20, 0, 60, 80}}, - {"fan2", ThermalSensor{-20, 0, 60, 80}}, - {"fan3", ThermalSensor{-20, 0, 60, 80}}, - {"fan4", ThermalSensor{-20, 0, 60, 80}}, - {"5v_inductor", ThermalSensor{-20, 0, 60, 80}}, - {"main_gnd_lug", ThermalSensor{-20, 0, 60, 80}}, - {"dcdc_24v", ThermalSensor{-20, 0, 60, 80}}, - {"dcdc_12v", ThermalSensor{-20, 0, 60, 80}}, - {"battery", ThermalSensor{-20, 0, 60, 80}}, - {"pcb_motor1", ThermalSensor{-20, 0, 60, 80}}, - {"mcu_motor1", ThermalSensor{-20, 0, 60, 80}}, - {"pcb_motor2", ThermalSensor{-20, 0, 60, 80}}, - {"mcu_motor2", ThermalSensor{-20, 0, 60, 80}}, - {"pcb_motor3", ThermalSensor{-20, 0, 60, 80}}, - {"mcu_motor3", ThermalSensor{-20, 0, 60, 80}}, - {"pcb_motor4", ThermalSensor{-20, 0, 60, 80}}, - {"mcu_motor4", ThermalSensor{-20, 0, 60, 80}}, - }; + sensors[name].setValue(value); } - - void setSensorValue(const std::string &name, float value) + else { - if (sensors.find(name) != sensors.end()) - { - sensors[name].setValue(value); - } - else - { - RCLCPP_WARN(rclcpp::get_logger("a300_fan_controller"), "Could not find thermal sensor %s", name.c_str()); - } + RCLCPP_WARN(rclcpp::get_logger("a300_fan_controller"), "Could not find thermal sensor %s", name.c_str()); } +} - ThermalStatus getHighestStatus() const - { - ThermalStatus highest_status = ThermalStatus::Normal; - for (const auto &sensor : sensors) { - if (sensor.second.getStatus() > highest_status) +a300_cooling::ThermalStatus a300_cooling::ThermalSensors::getHighestStatus() const +{ + a300_cooling::ThermalStatus highest_status = a300_cooling::ThermalStatus::Normal; + for (const auto &sensor : sensors) { + if (sensor.second.getStatus() > highest_status) + { + if (sensor.second.getStatus() >= a300_cooling::ThermalStatus::Normal) { - if (sensor.second.getStatus() >= ThermalStatus::Normal) - { - RCLCPP_WARN(rclcpp::get_logger("a300_fan_controller"), "Thermal sensor %s has %s", - sensor.first.c_str(), thermalStatusToString(sensor.second.getStatus()).c_str()); - } - - highest_status = sensor.second.getStatus(); + RCLCPP_WARN(rclcpp::get_logger("a300_fan_controller"), "Thermal sensor %s has %s", + sensor.first.c_str(), thermalStatusToString(sensor.second.getStatus()).c_str()); } + highest_status = sensor.second.getStatus(); } - return highest_status; } -}; + return highest_status; +} -class FanController : public rclcpp::Node +a300_cooling::FanController::FanController() : Node("a300_fan_controller") { -private: - std::mutex update_mutex_; - ThermalSensors thermal_sensors_; - rclcpp::Publisher::SharedPtr fan_publisher_; + fan_publisher_ = create_publisher(FAN_CONTROL_TOPIC_NAME, rclcpp::SensorDataQoS()); - 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) + temp_subscription_ = create_subscription( + MCU_TEMPERATURE_TOPIC, + rclcpp::SensorDataQoS(), + [this](const clearpath_platform_msgs::msg::Temperature::SharedPtr msg) { - 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_); + 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]); + }); + + motor_subscription_ = create_subscription( + MOTOR_TEMPERATURE_TOPIC, + rclcpp::SensorDataQoS(), + [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); + }); + + battery_subscription_ = create_subscription( + BATTERY_TEMPERATURE_TOPIC, + rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::BatteryState::SharedPtr msg) { std::lock_guard lock(update_mutex_); + this->thermal_sensors_.setSensorValue("battery", msg->temperature); + }); - 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_DEBUG(get_logger(), "Set all fans to: %d", fan_value); - } + control_timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&FanController::timerCallback, this)); +} -public: - FanController() : Node("a300_fan_controller") - { - fan_publisher_ = create_publisher(FAN_CONTROL_TOPIC_NAME, - rclcpp::SensorDataQoS()); +void a300_cooling::FanController::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_DEBUG(get_logger(), "Set all fans to: %d", fan_value); +} - temp_subscription_ = create_subscription( - MCU_TEMPERATURE_TOPIC, - rclcpp::SensorDataQoS(), - [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, - rclcpp::SensorDataQoS(), - [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); - } - ); +uint8_t a300_cooling::FanController::computeFanValue(a300_cooling::ThermalStatus status) +{ + float fan_fraction; - control_timer_ = create_wall_timer(1s, std::bind(&FanController::timerCallback, this)); + switch (status) + { + case a300_cooling::ThermalStatus::HighError: + fan_fraction = a300_cooling::FAN_CMD_HIGH_ERROR; + break; + case a300_cooling::ThermalStatus::HighWarning: + fan_fraction = a300_cooling::FAN_CMD_HIGH_WARNING; + break; + case a300_cooling::ThermalStatus::LowError: + fan_fraction = a300_cooling::FAN_CMD_LOW_ERROR; + break; + case a300_cooling::ThermalStatus::LowWarning: + fan_fraction = a300_cooling::FAN_CMD_LOW_WARNING; + break; + default: + fan_fraction = a300_cooling::FAN_CMD_NORMAL; + break; } -}; + return static_cast(fan_fraction * 255.0f); +} int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } -