motor_heating_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #include <boost/crc.hpp>
38 #include <boost/static_assert.hpp>
39 #include <boost/filesystem.hpp>
40 #include <boost/bind.hpp>
41 #include <boost/foreach.hpp>
42 #include <boost/timer.hpp>
43 
44 // Use XML format when saving or loading XML file
45 #include <tinyxml.h>
46 
47 #include <stdio.h>
48 #include <errno.h>
49 #include <exception>
50 
51 namespace ethercat_hardware
52 {
53 
54 
55 static const int DEBUG_LEVEL = 0;
56 
57 
59 {
60  BOOST_STATIC_ASSERT(sizeof(ethercat_hardware::MotorHeatingModelParametersEepromConfig) == 256);
61  BOOST_STATIC_ASSERT( offsetof(ethercat_hardware::MotorHeatingModelParametersEepromConfig, crc32_) == (256-4));
62  boost::crc_32_type crc32;
63  crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
64  return (this->crc32_ == crc32.checksum());
65 }
66 
68 {
69  boost::crc_32_type crc32;
70  crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
71  this->crc32_ = crc32.checksum();
72 }
73 
74 
75 
77 {
78  // There are a couple of rosparams that can be used to control the motor heating motor
79  // * <namespace>/motor_heating_model/load_save_files :
80  // boolean : defaults to true
81  // Do not load saved motor temperature data at startup .
82  // This might be useful for things like the the burn-in test stands
83  // where different parts are constantly switched in-and-out.
84  // * <namespace>/motor_heating_model/save_directory :
85  // string, defaults to '/var/lib'
86  // Location where motor heating model save data is loaded from and saved to
87  // * <namespace>/motor_heating_model/do_not_halt :
88  // boolean, defaults to false
89  //
90  if (!nh.getParam("load_save_files", load_save_files_))
91  {
92  load_save_files_ = true;
93  }
94  if (!nh.getParam("update_save_files", update_save_files_))
95  {
96  update_save_files_ = true;
97  }
98  if (!nh.getParam("do_not_halt", disable_halt_))
99  {
100  // TODO : once there has been enough testing of motor heating model on robot in real-world conditions,
101  // enable halting by default
102  disable_halt_ = true;
103  }
104  if (!nh.getParam("save_directory", save_directory_))
105  {
106  save_directory_ = "/var/lib/motor_heating_model";
107  }
108  if (!nh.getParam("enable_model", enable_model_))
109  {
110  enable_model_ = true;
111  }
112  if (!nh.getParam("publish_temperature", publish_temperature_))
113  {
114  publish_temperature_ = false;
115  }
116 }
117 
118 
120  update_save_files_ ( true ),
121  save_directory_ ("/var/lib/motor_heating_model" ),
122  load_save_files_ ( true ),
123  disable_halt_ ( false ),
124  enable_model_( true ),
125  publish_temperature_( false )
126 {
127 
128 }
129 
130 
132 {
133  { // LOCK
134  boost::lock_guard<boost::mutex> lock(mutex_);
135  models_.push_back(model);
136  } // UNLOCK
137 }
138 
139 
141 {
142  if (update_save_files_)
143  {
144  // Save thread should not be started until all MotorHeatingModels have been attached()
145  save_thread_ = boost::thread(boost::bind(&MotorHeatingModelCommon::saveThreadFunc, this));
146  }
147  return true;
148 }
149 
150 
159 {
160  // DEB install should create directory with proper permissions.
161  //createSaveDirectory();
162 
163  while (true)
164  {
165  sleep(10);
166  { //LOCK
167  boost::lock_guard<boost::mutex> lock(mutex_);
168  BOOST_FOREACH( boost::shared_ptr<MotorHeatingModel> model, models_ )
169  {
170  model->saveTemperatureState();
171  }
172  } //UNLOCK
173  }
174 
175  { // Throw away ptrs to all motor models
176  boost::lock_guard<boost::mutex> lock(mutex_);
177  models_.clear();
178  }
179 }
180 
181 
185 {
186  // create save directory if it does not already exist
187  if (!boost::filesystem::exists(save_directory_))
188  {
189  ROS_WARN("Motor heating motor save directory '%s' does not exist, creating it", save_directory_.c_str());
190  try {
191  boost::filesystem::create_directory(save_directory_);
192  }
193  catch (const std::exception &e)
194  {
195  ROS_ERROR("Error creating save directory '%s' for motor model : %s",
196  save_directory_.c_str(), e.what());
197  return false;
198  }
199  }
200 
201  return true;
202 }
203 
204 
205 
215  const std::string &actuator_name,
216  const std::string &hwid,
217  const std::string &save_directory
218  ) :
219  overheat_(false),
220  heating_energy_sum_(0.0),
221  ambient_temperature_sum_(0.0),
222  duration_since_last_sample_(0.0),
223  publisher_(NULL),
224  motor_params_(motor_params),
225  actuator_name_(actuator_name),
226  save_filename_(save_directory + "/" + actuator_name_ + ".save"),
227  hwid_(hwid)
228 {
229 
230 
231 
232  // If the guess is too low, the motors may not halt when they get too hot.
233  // If the guess is too high the motor may halt when they should not.
234 
235  // The first time the motor heating model is run on new machine, there will be no saved motor temperature data.
236  // With the saved temperatures, we have to 'guess' an initial motor temperature.
237  // Because the motor have been used for multiple years with only a couple incidents,
238  // its probably better to error on the side of false negitives.
239  static const double default_ambient_temperature = 60.0;
240  winding_temperature_ = default_ambient_temperature;
241  housing_temperature_ = default_ambient_temperature;
242  ambient_temperature_ = default_ambient_temperature;
243 
244  // Calculate internal parameters from motor parameters
251 }
252 
253 
255 {
256  std::string topic("motor_temperature");
257  if (!actuator_name_.empty())
258  {
259  topic = topic + "/" + actuator_name_;
261  if (publisher_ == NULL)
262  {
263  ROS_ERROR("Could not allocate realtime publisher");
264  return false;
265  }
266  }
267  return true;
268 }
269 
270 
271 
272 double MotorHeatingModel::calculateMotorHeatPower(const ethercat_hardware::MotorTraceSample &s,
273  const ethercat_hardware::ActuatorInfo &ai)
274 {
275  // Normally the power being put into motor is I^2*R.
276  // However, the motor resistance changes with temperature
277  // Instead this fuction breaks motor voltage into two parts
278  // 1. back-EMF voltage
279  // 2. resistance voltage
280  // Then the power being put into motor is (resistance voltage * I)
281 
282  // Output voltage of MCB. Guesstimate by multipling pwm ratio with supply voltage
283  double output_voltage = s.programmed_pwm * s.supply_voltage;
284  // Back emf voltage of motor
285  double backemf_constant = 1.0 / (ai.speed_constant * 2.0 * M_PI * 1.0/60.0);
286  double backemf_voltage = s.velocity * ai.encoder_reduction * backemf_constant;
287  // What ever voltage is left over must be the voltage used to drive current through the motor
288  double resistance_voltage = output_voltage - backemf_voltage;
289 
290  // Power going into motor as heat (Watts)
291  double heating_power = resistance_voltage * s.measured_current;
292 
293  if (DEBUG_LEVEL) // Only use for debugging, (prints out debug info for unreasonable heating values)
294  {
295  if ((heating_power < -0.5) || (heating_power > 15.0))
296  {
297  ROS_DEBUG("heating power = %f, output_voltage=%f, backemf_voltage=%f, resistance_voltage=%f, current=%f",
298  heating_power, output_voltage, backemf_voltage, resistance_voltage, s.measured_current);
299  }
300  }
301 
302  // Heating power can never be less than 0.0
303  heating_power = std::max(heating_power, 0.0);
304 
305  return heating_power;
306 }
307 
308 
309 bool MotorHeatingModel::update(double heating_power, double ambient_temperature, double duration)
310 {
311  // motor winding gains heat power (from motor current)
312  // motor winding losses heat to motor housing
313  // motor housing gets heat from winding and looses heat to ambient
314  double heating_energy = heating_power * duration;
315  double winding_energy_loss =
317  double housing_energy_loss =
318  (housing_temperature_ - ambient_temperature) * housing_to_ambient_thermal_conductance_ * duration;
319 
320  winding_temperature_ += (heating_energy - winding_energy_loss) * winding_thermal_mass_inverse_;
321  housing_temperature_ += (winding_energy_loss - housing_energy_loss) * housing_thermal_mass_inverse_;
322 
323  { // LOCKED
324  boost::lock_guard<boost::mutex> lock(mutex_);
325  heating_energy_sum_ += heating_energy;
326  ambient_temperature_sum_ += (ambient_temperature * duration);
327  duration_since_last_sample_ += duration;
329  overheat_ = true;
330  } // LOCKED
331 
332  return !overheat_;
333 }
334 
335 
336 
338  double saved_ambient_temperature,
339  double interval,
340  unsigned cycles)
341 {
342  static const double heating_power = 0.0; // While motor was off heating power should be zero
343  for (unsigned i=0; i<cycles; ++i)
344  {
345  if (downtime > interval)
346  {
347  update(heating_power, saved_ambient_temperature, interval);
348  downtime -= interval;
349  }
350  else
351  {
352  // Done
353  update(heating_power, saved_ambient_temperature, downtime);
354  return 0.0;
355  }
356  }
357  return downtime;
358 }
359 
360 
361 void MotorHeatingModel::updateFromDowntime(double downtime, double saved_ambient_temperature)
362 {
363  ROS_DEBUG("Initial temperatures : winding = %f, housing = %f", winding_temperature_, housing_temperature_);
364  double saved_downtime = downtime;
365 
366  boost::timer timer;
367 
368  // Simulate motor heating model first with increasing step sizes :
369  // first 10 ms, then 100ms, and finally 1 seconds steps
370  downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.01, 200);
371  downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.10, 200);
372  downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 1.00, 200);
373  downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 10.0, 2000);
374 
375  // If there is any time left over, assume motor has cooled to ambient
376  if (downtime > 0.0)
377  {
378  ROS_DEBUG("Downtime too long, using ambient temperature as final motor temperature");
379  winding_temperature_ = saved_ambient_temperature;
380  housing_temperature_ = saved_ambient_temperature;
381  }
382 
383  ROS_DEBUG("Took %f milliseconds to sim %f seconds", timer.elapsed()*1000., saved_downtime);
384  ROS_DEBUG("Final temperatures : winding = %f, housing = %f", winding_temperature_, housing_temperature_);
385 }
386 
387 
389 {
390  { // LOCKED
391  boost::lock_guard<boost::mutex> lock(mutex_);
392  overheat_ = false;
393  } // LOCKED
394 }
395 
396 
398 {
399  // Sample motor temperature and heating energy every so often, this way the
400  // heating of the motor can be published when there is a problem
401  bool overheat;
402  double winding_temperature;
403  double housing_temperature;
404  double ambient_temperature_sum;
405  double duration_since_last_sample;
406  double average_ambient_temperature;
407  double average_heating_power;
408 
409  { // LOCKED
410  boost::lock_guard<boost::mutex> lock(mutex_);
411  overheat = overheat_;
412  winding_temperature = winding_temperature_;
413  housing_temperature = housing_temperature_;
414  ambient_temperature_sum = ambient_temperature_sum_;
415  duration_since_last_sample = duration_since_last_sample_;
416 
417  if (duration_since_last_sample > 0.0)
418  {
419  average_ambient_temperature = ambient_temperature_sum / duration_since_last_sample;
420  ambient_temperature_ = average_ambient_temperature;
421  average_heating_power = heating_energy_sum_ / duration_since_last_sample;
422  }
423  else
424  {
425  //ROS_WARN("Duration == 0");
426  average_ambient_temperature = ambient_temperature_;
427  average_heating_power = 0.0;
428  }
429 
431  heating_energy_sum_ = 0.0;
433  } // LOCKED
434 
435 
436 
437  const int ERROR = 2;
438  const int WARN = 1;
439  //const int GOOD = 0;
440 
441  if (overheat)
442  {
443  // Once motor overheating flag is set, keep reporting motor has overheated, until the flag is reset.
444  d.mergeSummary(ERROR, "Motor overheated");
445  }
446  else if (winding_temperature > (motor_params_.max_winding_temperature_ * 0.90))
447  {
448  // If motor winding temperature reaches 90% of limit, then make a warning
449  d.mergeSummary(WARN, "Motor hot");
450  }
451 
452  d.addf("Motor winding temp limit (C)", "%f", motor_params_.max_winding_temperature_);
453  d.addf("Motor winding temp (C)", "%f", winding_temperature);
454  d.addf("Motor housing temp (C)", "%f", housing_temperature);
455 
456  // TODO: removing this, probably not need for purposes other than debugging
457  d.addf("Heating power (Watts)", "%f", average_heating_power);
458  d.addf("Ambient temp (C)", "%f", average_ambient_temperature);
459 
460 
461  if ((publisher_ != NULL) && (publisher_->trylock()))
462  {
463  // Fill in sample values
464  ethercat_hardware::MotorTemperature &msg(publisher_->msg_);
465  msg.stamp = ros::Time::now();
466  msg.winding_temperature = winding_temperature;
467  msg.housing_temperature = housing_temperature;
468  msg.ambient_temperature = average_ambient_temperature;
469  msg.heating_power = average_heating_power;
470 
471  // publish sample
473  }
474 }
475 
476 
477 
478 static bool getStringAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, std::string &value)
479 {
480  const char *val_str = elt->Attribute(param_name);
481  if (val_str == NULL)
482  {
483  ROS_ERROR("No '%s' attribute for actuator '%s'", param_name, filename.c_str());
484  return false;
485  }
486  value = val_str;
487  return true;
488 }
489 
490 
491 static bool getDoubleAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, double &value)
492 {
493  const char *val_str = elt->Attribute(param_name);
494  if (val_str == NULL)
495  {
496  ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
497  return false;
498  }
499 
500  char *endptr=NULL;
501  value = strtod(val_str, &endptr);
502  if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
503  {
504  ROS_ERROR("Couldn't convert '%s' to double for attribute '%s' in '%s'",
505  val_str, param_name, filename.c_str());
506  return false;
507  }
508 
509  return true;
510 }
511 
512 
513 static bool getIntegerAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, int &value)
514 {
515  const char *val_str = elt->Attribute(param_name);
516  if (val_str == NULL)
517  {
518  ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
519  return false;
520  }
521 
522  char *endptr=NULL;
523  value = strtol(val_str, &endptr, 0);
524  if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
525  {
526  ROS_ERROR("Couldn't convert '%s' to integer for attribute '%s' in '%s'",
527  val_str, param_name, filename.c_str());
528  return false;
529  }
530 
531  return true;
532 }
533 
534 
535 static void saturateTemperature(double &temperature, const char *name)
536 {
537  static const double max_realistic_temperature = 200.0;
538  static const double min_realistic_temperature = -10.0;
539 
540  if (temperature > max_realistic_temperature)
541  {
542  ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
543  name, temperature, max_realistic_temperature);
544  temperature = max_realistic_temperature;
545  }
546  if (temperature < min_realistic_temperature)
547  {
548  ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
549  name, temperature, min_realistic_temperature);
550  temperature = min_realistic_temperature;
551  }
552 }
553 
554 
556 {
557  /*
558  * The tempature save file will be named after the actuator it was created for
559  * <actuactor_name>_motor_heating_model.save
560  *
561  * The temperatureState file will have a XML format:
562  * <motor_heating_model
563  * version = "1"
564  * actuator_name = "fl_caster_r_wheel_motor"
565  * winding_temperature = "100.0"
566  * housing_temperature = "100.0"
567  * save_time = "<wall_time when file was lasted saved>"
568  * checksum = "<CRC32 of all previous lines in file>, to allow user editing of files checksum can be "IGNORE-CRC"
569  * />
570  *
571  */
572 
573  if (!boost::filesystem::exists(save_filename_))
574  {
575  ROS_WARN("Motor heating model saved file '%s' does not exist. Using defaults", save_filename_.c_str());
576  return false;
577  }
578 
579  TiXmlDocument xml;
580  if (!xml.LoadFile(save_filename_))
581  {
582  ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
583  return false;
584  }
585 
586 
587  TiXmlElement *motor_temp_elt = xml.RootElement();
588  if (motor_temp_elt == NULL)
589  {
590  ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
591  return false;
592  }
593 
594  std::string version;
595  std::string actuator_name;
596  std::string hwid;
597  double winding_temperature;
598  double housing_temperature;
599  double ambient_temperature;
600  int save_time_sec, save_time_nsec;
601  if (!getStringAttribute(motor_temp_elt, save_filename_, "version", version))
602  {
603  return false;
604  }
605  const char* expected_version = "1";
606  if (version != expected_version)
607  {
608  ROS_ERROR("Unknown version '%s', expected '%s'", version.c_str(), expected_version);
609  return false;
610  }
611 
612  bool success = true;
613  success &= getStringAttribute(motor_temp_elt, save_filename_, "actuator_name", actuator_name);
614  success &= getStringAttribute(motor_temp_elt, save_filename_, "hwid", hwid);
615  success &= getDoubleAttribute(motor_temp_elt, save_filename_, "housing_temperature", housing_temperature);
616  success &= getDoubleAttribute(motor_temp_elt, save_filename_, "winding_temperature", winding_temperature);
617  success &= getDoubleAttribute(motor_temp_elt, save_filename_, "ambient_temperature", ambient_temperature);
618  success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_sec", save_time_sec);
619  success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_nsec", save_time_nsec);
620  if (!success)
621  {
622  return false;
623  }
624 
625  if (actuator_name != actuator_name_)
626  {
627  ROS_ERROR("In save file '%s' : expected actuator name '%s', got '%s'",
628  save_filename_.c_str(), actuator_name_.c_str(), actuator_name.c_str());
629  return false;
630  }
631 
632  if (hwid != hwid_)
633  {
634  ROS_WARN("In save file '%s' : expected HWID '%s', got '%s'",
635  save_filename_.c_str(), hwid_.c_str(), hwid.c_str());
636  }
637 
638  saturateTemperature(housing_temperature, "Housing");
639  saturateTemperature(winding_temperature, "Winding");
640  saturateTemperature(ambient_temperature, "Ambient");
641 
642  // Update stored temperatures
643  winding_temperature_ = winding_temperature;
644  housing_temperature_ = housing_temperature;
645  ambient_temperature_ = ambient_temperature;
646 
647  // Update motor temperature model based on saved time, update time, and saved ambient temperature
648  double downtime = (ros::Time::now() - ros::Time(save_time_sec, save_time_nsec)).toSec();
649  if (downtime < 0.0)
650  {
651  ROS_WARN("In save file '%s' : save time is %f seconds in future", save_filename_.c_str(), -downtime);
652  }
653  else
654  {
655  updateFromDowntime(downtime, ambient_temperature);
656  }
657 
658  // Make sure simulation didn't go horribly wrong
661 
662  return true;
663 }
664 
665 
666 
667 
668 
670 {
671  /*
672  * This will first generate new XML data into temp file, then rename temp file to final filename
673  *
674  */
675  std::string tmp_filename = save_filename_ + ".tmp";
676 
677  double winding_temperature;
678  double housing_temperature;
679  double ambient_temperature;
680  { // LOCKED
681  boost::lock_guard<boost::mutex> lock(mutex_);
682  winding_temperature = winding_temperature_;
683  housing_temperature = housing_temperature_;
684  ambient_temperature = ambient_temperature_;
685  } // LOCKED
686 
687  TiXmlDocument xml;
688  TiXmlDeclaration *decl = new TiXmlDeclaration( "1.0", "", "" );
689  TiXmlElement *elmt = new TiXmlElement("motor_heating_model");
690  elmt->SetAttribute("version", "1");
691  elmt->SetAttribute("actuator_name", actuator_name_);
692  elmt->SetAttribute("hwid", hwid_);
693  elmt->SetDoubleAttribute("winding_temperature", winding_temperature);
694  elmt->SetDoubleAttribute("housing_temperature", housing_temperature);
695  elmt->SetDoubleAttribute("ambient_temperature", ambient_temperature);
696  ros::Time now = ros::Time::now();
697  elmt->SetAttribute("save_time_sec", now.sec);
698  elmt->SetAttribute("save_time_nsec", now.nsec);
699 
700  xml.LinkEndChild(decl);
701  xml.LinkEndChild( elmt );
702 
703  // save xml with temporary filename
704  if (!xml.SaveFile(tmp_filename))
705  {
706  ROS_WARN("Could not save motor heating model file '%s'", tmp_filename.c_str());
707  return false;
708  }
709 
710  // now rename file
711  if (rename(tmp_filename.c_str(), save_filename_.c_str()) != 0)
712  {
713  int error = errno;
714  char errbuf[100];
715  strerror_r(error, errbuf, sizeof(errbuf));
716  errbuf[sizeof(errbuf)-1] = '\0';
717  ROS_WARN("Problem renaming '%s' to '%s' : (%d) '%s'",
718  tmp_filename.c_str(), save_filename_.c_str(), error, errbuf);
719  return false;
720  }
721 
722  return true;
723 }
724 
725 
726 
727 }; // end namespace ethercat_hardware
msg
double winding_thermal_time_constant_
Thermal time constant of motor winding : in seconds.
static bool getDoubleAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, double &value)
static const int DEBUG_LEVEL
static void saturateTemperature(double &temperature, const char *name)
std::string save_filename_
path to file where temperature data will be saved
WARN
double updateFromDowntimeWithInterval(double downtime, double saved_ambient_temperature, double interval, unsigned cycles)
Updates estimated motor temperature for certain amount of downtime.
bool overheat_
True if most has overheat, once set, will only clear when reset() is called.
MotorHeatingModelCommon()
Constructor will use default settings for all parameters.
double calculateMotorHeatPower(const ethercat_hardware::MotorTraceSample &sample, const ethercat_hardware::ActuatorInfo &actuator_info)
static bool getStringAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, std::string &value)
double heating_energy_sum_
Sum of heat energy for last sample interval.
#define ROS_WARN(...)
void addf(const std::string &key, const char *format,...)
MotorHeatingModel(const MotorHeatingModelParameters &motor_params, const std::string &actuator_name, const std::string &hwid, const std::string &save_directory)
Constructor.
std::string save_directory_
Directory where temperature save files should be put.
void saveThreadFunc()
Continuously saves motor heating model state.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
Appends heating diagnostic data to status wrapper.
boost::mutex mutex_
Lock around models list.
double winding_to_housing_thermal_resistance_
Thermal resistance between motor winding and motor housing : in C/Watt.
void updateFromDowntime(double downtime, double saved_ambient_temperature)
Updates estimated motor temperature for long period of off-time.
realtime_tools::RealtimePublisher< ethercat_hardware::MotorTemperature > * publisher_
Sample interval for trace (in seconds)
std::vector< boost::shared_ptr< MotorHeatingModel > > models_
List of MotorHeatingModels that need to have file data saved.
double winding_thermal_mass_inverse_
Inverse of thermal mass for motor winding : in Joules/C.
double duration_since_last_sample_
Time (in seconds) since late sample interval occurred.
MotorHeatingModelParameters motor_params_
std::string actuator_name_
name of actuator (ex. fl_caster_rotation_motor)
bool createSaveDirectory()
Creates directory for saved motor heating information.
double housing_thermal_time_constant_
Thermal time constant of motor housing : in seconds.
double winding_temperature_
Temperature estimate of motor winding : in Celcius.
static bool getIntegerAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, int &value)
void attach(boost::shared_ptr< MotorHeatingModel > model)
Append model to list of models that need to have temperature data saved.
double max_winding_temperature_
temperature limit of motor windings : in Celcius
bool loadTemperatureState()
Load saved temperature estimate from directory.
void mergeSummary(unsigned char lvl, const std::string s)
double housing_to_ambient_thermal_conductance_
Thermal conductance between motor housing and ambient : in Watt/C.
double housing_thermal_mass_inverse_
Inverse of thermal mass for motor housing : in Joules/C.
double ambient_temperature_
Last recorded ambient temperature : in Celcius.
void reset()
Resets motor overheat flag.
double winding_to_housing_thermal_conductance_
Thermal conductance between motor winding and housing : in Watt/C.
bool getParam(const std::string &key, std::string &s) const
static Time now()
double housing_to_ambient_thermal_resistance_
Thermal resistance between motor housing and ambient : in C/Watt.
boost::thread save_thread_
thread that will periodically save temperature data
double ambient_temperature_sum_
Sum of (abient heat * time) over last sample interval.
boost::mutex mutex_
mutex protects values updates by realtime thread and used by diagnostics thread
double housing_temperature_
Temperature estimate of motor housing : in Celcius.
#define ROS_ERROR(...)
uint32_t crc32_
CRC32 of first 256-4 bytes of structure.
ERROR
bool update(double heating_power, double ambient_temperature, double duration)
Updates motor temperature estimate.
std::string hwid_
Hardware ID of device (ex. 680500501000)
#define ROS_DEBUG(...)


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Fri Mar 15 2019 02:53:29