Merge pull request #113 from nasa-jpl/feat-thermal-model-await-seed-temp

Feat thermal model await seed temp

src/fastcat_devices/three_node_thermal_model.cc

@@ -53,17 +53,25 @@ bool ThreeNodeThermalModel::ConfigFromYaml(YAML::Node node)
     return false;
   }
 
-  if (!ParseVal(node, "persistence_limit", persistence_limit_)) {
-    return false;
+  if (!ParseOptVal(node, "ref_temp", ref_temp_)) {
+    awaiting_seed_temp_ = true;
   }
 
-  if (!ParseVal(node, "ref_temp", ref_temp_)) {
+  if (!ParseOptVal(node, "exp_smoothing_alpha", exp_smoothing_alpha_)) {
+    exp_smoothing_alpha_ = 1.0; // If we set this to 1, it is the same as having no exp. smoothing
+  }
+  else if (exp_smoothing_alpha_ < 0.0 || exp_smoothing_alpha_ > 1.0) {
+    ERROR("Invalid choice for exp_smoothing_alpha! This must be between 0.0 and 1.0 for stability!");
     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_;
+
+  else {
+    awaiting_seed_temp_ = 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;
@@ -103,10 +111,17 @@ bool ThreeNodeThermalModel::Read()
     }
   }
 
-  // store them
-  node_temps_[2] =
-      signals_[NODE_3_TEMP_IDX].value;  // node 3 temperature is directly taken
-                                        // from the signal measurement
+  double node_3_temp_sample = signals_[NODE_3_TEMP_IDX].value;
+
+  if (awaiting_seed_temp_) { 
+    for (size_t idx = 0; idx < node_temps_.size(); ++idx) {
+      node_temps_[idx] = node_3_temp_sample;
+    }
+    awaiting_seed_temp_ = false;
+  }
+
+  node_temps_[2] = exp_smoothing_alpha_ * node_3_temp_sample + (1.0 - exp_smoothing_alpha_) * node_temps_[2];
+
   motor_current_ = signals_[MOTOR_CURRENT_IDX].value;
   return true;
 }

src/fastcat_devices/three_node_thermal_model.h

@@ -77,6 +77,13 @@ class ThreeNodeThermalModel : public DeviceBase
   double ref_temp_{0.0};  ///< the reference temperature for the winding
                           ///< resistance parameter, along with being used for
                           ///< calculating the dynamically varying resistance
+  double exp_smoothing_alpha_{1.0}; ///< this parameter specifies the extent
+                                    ///< to which smoothing is applied to the
+                                    ///< temperature sensor value      
+  bool awaiting_seed_temp_{false};  ///< this variable is used to delay setting 
+                                    ///< initial temperatures until we read the
+                                    ///< first temperature for node 3 and sets
+                                    ///< all nodes to that starting temperature
 
   // declare variables for storing signal data and estimates
   double motor_current_{