-
Notifications
You must be signed in to change notification settings - Fork 11
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feature/add three node motor thermal model device (#98)
feature/add three node motor thermal model device
- Loading branch information
1 parent
dfba432
commit 37080b7
Showing
11 changed files
with
630 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,168 @@ | ||
| // Include related header (for cc files) | ||
| #include "fastcat/fastcat_devices/three_node_thermal_model.h" | ||
|
|
||
| namespace fastcat | ||
| { | ||
| ThreeNodeThermalModel::ThreeNodeThermalModel() | ||
| { | ||
| state_ = std::make_shared<DeviceState>(); | ||
| state_->type = THREE_NODE_THERMAL_MODEL_STATE; | ||
| last_time_ = state_->time; // init time | ||
| } | ||
|
|
||
| bool ThreeNodeThermalModel::ConfigFromYaml(YAML::Node node) | ||
| { | ||
| if (!ParseVal(node, "name", name_)) { | ||
| return false; | ||
| } | ||
| state_->name = name_; | ||
|
|
||
| if (!ParseVal(node, "thermal_mass_node_1", thermal_mass_node_1_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "thermal_mass_node_2", thermal_mass_node_2_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "thermal_res_nodes_1_to_2", thermal_res_nodes_1_to_2_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "thermal_res_nodes_2_to_3", thermal_res_nodes_2_to_3_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "winding_res", winding_res_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "winding_thermal_cor", winding_thermal_cor_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "k1", k1_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "k2", k2_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "k3", k3_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "persistence_limit", persistence_limit_)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (!ParseVal(node, "ref_temp", ref_temp_)) { | ||
| return false; | ||
| } | ||
| // initialize all temps to ref_temp | ||
| // Note: this can be manually seeded later | ||
| for (size_t idx = 0; idx < node_temps_.size(); ++idx) { | ||
| node_temps_[idx] = ref_temp_; | ||
| } | ||
|
|
||
| YAML::Node max_allowable_temp_node; | ||
| if (!ParseList(node, "max_allowable_temps", max_allowable_temp_node)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (max_allowable_temp_node.size() != max_allowable_temps_.size()) { | ||
| ERROR("There must be exactly 4 temperature limit values provided"); | ||
| return false; | ||
| } | ||
|
|
||
| for (size_t idx = 0; idx < max_allowable_temp_node.size(); ++idx) { | ||
| max_allowable_temps_[idx] = max_allowable_temp_node[idx].as<double>(); | ||
| } | ||
|
|
||
| if (!ConfigSignalsFromYaml(node, signals_, false)) { | ||
| return false; | ||
| } | ||
|
|
||
| if (signals_.size() != FC_TNTM_NUM_SIGNALS) { | ||
| ERROR("Expecting exactly %ld signals for Three Node Thermal Model device", | ||
| FC_TNTM_NUM_SIGNALS); | ||
| return false; | ||
| } | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| bool ThreeNodeThermalModel::Read() | ||
| { | ||
| // update signals | ||
| for (size_t idx = 0; idx < FC_TNTM_NUM_SIGNALS; idx++) { | ||
| if (!UpdateSignal(signals_[idx])) { | ||
| ERROR("Could not extract signal"); | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| // store them | ||
| node_temps_[2] = | ||
| signals_[NODE_3_TEMP_IDX].value; // node 3 temperature is directly taken | ||
| // from the signal measurement | ||
| motor_current_ = signals_[MOTOR_CURRENT_IDX].value; | ||
| return true; | ||
| } | ||
|
|
||
| FaultType ThreeNodeThermalModel::Process() | ||
| { | ||
| // TODO should there be a check/flag that's monitored for if the model has | ||
| // been initialized/seeded update motor resistance | ||
| motor_res_ = | ||
| winding_res_ * (1 + winding_thermal_cor_ * (node_temps_[0] - ref_temp_)); | ||
| // calculate heat transfer rates | ||
| double q_in = motor_current_ * motor_current_ * motor_res_; // I^2 * R | ||
| double q_node_1_to_2 = | ||
| (node_temps_[0] - node_temps_[1]) / thermal_res_nodes_1_to_2_; | ||
| double q_node_2_to_3 = | ||
| (node_temps_[1] - node_temps_[2]) / thermal_res_nodes_2_to_3_; | ||
| // calculate temperatures | ||
| node_temps_[0] += (q_in - q_node_1_to_2) * | ||
| ((state_->time - last_time_) / thermal_mass_node_1_); | ||
| node_temps_[1] += (q_node_1_to_2 - q_node_2_to_3) * | ||
| ((state_->time - last_time_) / thermal_mass_node_2_); | ||
| node_temps_[3] = | ||
| (k1_ * node_temps_[0] + k2_ * node_temps_[1] + k3_ * node_temps_[2]) / | ||
| (k1_ + k2_ + k3_); | ||
| // update persistence counter for each node | ||
| for (size_t idx = 0; idx < node_temps_.size(); ++idx) { | ||
| if (node_temps_[idx] > max_allowable_temps_[idx]) { | ||
| node_overtemp_persistences_[idx]++; | ||
| } else { | ||
| node_overtemp_persistences_[idx] = 0; | ||
| } | ||
| // throw fault if limit exceeded | ||
| if (node_overtemp_persistences_[idx] > persistence_limit_) { | ||
| ERROR("Node %ld temperature exceeded safety limit -- faulting", idx + 1u); | ||
| return ALL_DEVICE_FAULT; | ||
| } | ||
| } | ||
| state_->three_node_thermal_model_state.node_1_temp = node_temps_[0]; | ||
| state_->three_node_thermal_model_state.node_2_temp = node_temps_[1]; | ||
| state_->three_node_thermal_model_state.node_3_temp = node_temps_[2]; | ||
| state_->three_node_thermal_model_state.node_4_temp = node_temps_[3]; | ||
| last_time_ = state_->time; // update loop time | ||
|
|
||
| return NO_FAULT; | ||
| } | ||
|
|
||
| bool ThreeNodeThermalModel::Write(DeviceCmd& cmd) | ||
| { | ||
| if (cmd.type == SEED_THERMAL_MODEL_TEMPERATURE_CMD) { | ||
| for (size_t idx = 0; idx < node_temps_.size(); ++idx) { | ||
| node_temps_[idx] = | ||
| cmd.seed_thermal_model_temperature_cmd.seed_temperature; | ||
| } | ||
| } else { | ||
| return false; // if command is not present, return false | ||
| } | ||
| return true; | ||
| } | ||
| } // namespace fastcat |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,106 @@ | ||
| #ifndef FASTCAT_THREE_NODE_THERMAL_MODEL_H_ | ||
| #define FASTCAT_THREE_NODE_THERMAL_MODEL_H_ | ||
|
|
||
| // Include external then project includes | ||
| #include "fastcat/device_base.h" | ||
|
|
||
| // Include c then c++ libraries | ||
| #include <cmath> | ||
|
|
||
| // Include external then project includes | ||
| #include "fastcat/signal_handling.h" | ||
| #include "fastcat/yaml_parser.h" | ||
| #include "jsd/jsd_print.h" | ||
|
|
||
| namespace fastcat | ||
| { | ||
| /** | ||
| * @brief Class implementing a Three-Node Thermal Model for estimating | ||
| * internal motor temperatures, through fastcat. | ||
| */ | ||
| class ThreeNodeThermalModel : public DeviceBase | ||
| { | ||
| public: | ||
| ThreeNodeThermalModel(); ///< ThreeNodeThermalModel constructor. | ||
|
|
||
| /** | ||
| * @brief Parses input yaml file to set model constants and temperature | ||
| * limits. | ||
| * @param node The portion of the yaml file corresponding to this device. | ||
| * @return True if configuration completes without error; false otherwise. | ||
| */ | ||
| bool ConfigFromYaml(YAML::Node node) override; | ||
|
|
||
| /** | ||
| * @brief Reads in most recent temperature and current signal values, and | ||
| * stores them for further calculations. | ||
| * @return True if device state is read without error; false otherwise. | ||
| */ | ||
| bool Read() override; | ||
|
|
||
| /** | ||
| * @brief Performs one update step of the thermal prediction model, tracking | ||
| * the most recently predicted temperature values at each node, and | ||
| * reporting a fault if any of the limits are exceeded | ||
| * @return FaultType enum value corresponding to appropriate fault state. | ||
| */ | ||
| FaultType Process() override; | ||
|
|
||
| /** | ||
| * @brief Commands device | ||
| * Currently, only the SEED_THERMAL_MODEL_TEMPERATURE_CMD, used to | ||
| * reseed the model is accepted | ||
| * @param cmd Command provided to the device, of a type that is a subclass of | ||
| * DeviceCmd | ||
| * @return boolean for if the command was accepted and successful or not | ||
| */ | ||
| bool Write(DeviceCmd& cmd) override; | ||
|
|
||
| protected: | ||
| // declare motor parameters | ||
| double thermal_mass_node_1_{0.0}; | ||
| double thermal_mass_node_2_{0.0}; | ||
| double thermal_res_nodes_1_to_2_{ | ||
| 0.0}; ///< thermal resistance from node 1 to 2 (deg C / W) | ||
| double thermal_res_nodes_2_to_3_{ | ||
| 0.0}; ///< thermal resistance from node 2 to 3 (deg C / W) | ||
| double winding_res_{0.0}; ///< motor winding electrical resistance (ohms) | ||
| double winding_thermal_cor_{0.0}; ///< coefficient of resistance | ||
| double k1_{0.0}, k2_{0.0}, k3_{0.0}; ///< weights for T4 estimate | ||
|
|
||
| // declare fault protection parameters | ||
| std::vector<double> max_allowable_temps_{0.0, 0.0, 0.0, 0.0}; | ||
| uint32_t persistence_limit_{ | ||
| 0}; ///< represents how many time cycles a temperature limit is able to | ||
| ///< be exceeded before throwing a fault | ||
| double ref_temp_{0.0}; ///< the reference temperature for the winding | ||
| ///< resistance parameter, along with being used for | ||
| ///< calculating the dynamically varying resistance | ||
|
|
||
| // declare variables for storing signal data and estimates | ||
| double motor_current_{ | ||
| 0.0}; ///< this value is retrieved from a motor controller measurement | ||
| double motor_res_{0.0}; ///< this value is estimated based on the temp 1 | ||
| ///< estimate and represents the resistance of the | ||
| ///< motor, which is used for calculated power | ||
| std::vector<double> node_temps_{ | ||
| 0.0, 0.0, 0.0, 0.0}; ///< this value is estimated from the model and | ||
| ///< represents winding temperature | ||
| std::vector<size_t> node_overtemp_persistences_{ | ||
| 0, 0, 0, 0}; ///< this is used as a counter for how many cycles node 1 | ||
| ///< has been over the temperature limit | ||
|
|
||
| // tracked variables | ||
| double last_time_{0.0}; | ||
|
|
||
| // constants | ||
| // the required number of signals for this device | ||
| static constexpr size_t FC_TNTM_NUM_SIGNALS = 2; | ||
| // signal index for node 3 temperature | ||
| static constexpr size_t NODE_3_TEMP_IDX = 0; | ||
| // signal index for motor current | ||
| static constexpr size_t MOTOR_CURRENT_IDX = 1; | ||
| }; | ||
| } // namespace fastcat | ||
|
|
||
| #endif |
Oops, something went wrong.