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> 62 boost::crc_32_type crc32;
64 return (this->
crc32_ == crc32.checksum());
69 boost::crc_32_type crc32;
71 this->
crc32_ = crc32.checksum();
90 if (!nh.
getParam(
"load_save_files", load_save_files_))
92 load_save_files_ =
true;
94 if (!nh.
getParam(
"update_save_files", update_save_files_))
96 update_save_files_ =
true;
98 if (!nh.
getParam(
"do_not_halt", disable_halt_))
102 disable_halt_ =
true;
104 if (!nh.
getParam(
"save_directory", save_directory_))
106 save_directory_ =
"/var/lib/motor_heating_model";
108 if (!nh.
getParam(
"enable_model", enable_model_))
110 enable_model_ =
true;
112 if (!nh.
getParam(
"publish_temperature", publish_temperature_))
114 publish_temperature_ =
false;
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 )
134 boost::lock_guard<boost::mutex> lock(
mutex_);
167 boost::lock_guard<boost::mutex> lock(
mutex_);
170 model->saveTemperatureState();
176 boost::lock_guard<boost::mutex> lock(
mutex_);
193 catch (
const std::exception &e)
195 ROS_ERROR(
"Error creating save directory '%s' for motor model : %s",
215 const std::string &actuator_name,
216 const std::string &hwid,
217 const std::string &save_directory
220 heating_energy_sum_(0.0),
221 ambient_temperature_sum_(0.0),
222 duration_since_last_sample_(0.0),
224 motor_params_(motor_params),
225 actuator_name_(actuator_name),
226 save_filename_(save_directory +
"/" + actuator_name_ +
".save"),
239 static const double default_ambient_temperature = 60.0;
256 std::string topic(
"motor_temperature");
263 ROS_ERROR(
"Could not allocate realtime publisher");
273 const ethercat_hardware::ActuatorInfo &ai)
283 double output_voltage = s.programmed_pwm * s.supply_voltage;
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;
288 double resistance_voltage = output_voltage - backemf_voltage;
291 double heating_power = resistance_voltage * s.measured_current;
295 if ((heating_power < -0.5) || (heating_power > 15.0))
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);
303 heating_power = std::max(heating_power, 0.0);
305 return heating_power;
314 double heating_energy = heating_power * duration;
315 double winding_energy_loss =
317 double housing_energy_loss =
324 boost::lock_guard<boost::mutex> lock(
mutex_);
338 double saved_ambient_temperature,
342 static const double heating_power = 0.0;
343 for (
unsigned i=0; i<cycles; ++i)
345 if (downtime > interval)
347 update(heating_power, saved_ambient_temperature, interval);
348 downtime -= interval;
353 update(heating_power, saved_ambient_temperature, downtime);
364 double saved_downtime = downtime;
378 ROS_DEBUG(
"Downtime too long, using ambient temperature as final motor temperature");
383 ROS_DEBUG(
"Took %f milliseconds to sim %f seconds", timer.elapsed()*1000., saved_downtime);
391 boost::lock_guard<boost::mutex> lock(
mutex_);
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;
410 boost::lock_guard<boost::mutex> lock(
mutex_);
417 if (duration_since_last_sample > 0.0)
419 average_ambient_temperature = ambient_temperature_sum / duration_since_last_sample;
427 average_heating_power = 0.0;
453 d.
addf(
"Motor winding temp (C)",
"%f", winding_temperature);
454 d.
addf(
"Motor housing temp (C)",
"%f", housing_temperature);
457 d.
addf(
"Heating power (Watts)",
"%f", average_heating_power);
458 d.
addf(
"Ambient temp (C)",
"%f", average_ambient_temperature);
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;
478 static bool getStringAttribute(TiXmlElement *elt,
const std::string& filename,
const char* param_name, std::string &value)
480 const char *val_str = elt->Attribute(param_name);
483 ROS_ERROR(
"No '%s' attribute for actuator '%s'", param_name, filename.c_str());
491 static bool getDoubleAttribute(TiXmlElement *elt,
const std::string& filename,
const char* param_name,
double &value)
493 const char *val_str = elt->Attribute(param_name);
496 ROS_ERROR(
"No '%s' attribute in '%s'", param_name, filename.c_str());
501 value = strtod(val_str, &endptr);
502 if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
504 ROS_ERROR(
"Couldn't convert '%s' to double for attribute '%s' in '%s'",
505 val_str, param_name, filename.c_str());
513 static bool getIntegerAttribute(TiXmlElement *elt,
const std::string& filename,
const char* param_name,
int &value)
515 const char *val_str = elt->Attribute(param_name);
518 ROS_ERROR(
"No '%s' attribute in '%s'", param_name, filename.c_str());
523 value = strtol(val_str, &endptr, 0);
524 if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
526 ROS_ERROR(
"Couldn't convert '%s' to integer for attribute '%s' in '%s'",
527 val_str, param_name, filename.c_str());
537 static const double max_realistic_temperature = 200.0;
538 static const double min_realistic_temperature = -10.0;
540 if (temperature > max_realistic_temperature)
542 ROS_WARN(
"%s temperature of %f Celcius is unrealisic. Using %f instead",
543 name, temperature, max_realistic_temperature);
544 temperature = max_realistic_temperature;
546 if (temperature < min_realistic_temperature)
548 ROS_WARN(
"%s temperature of %f Celcius is unrealisic. Using %f instead",
549 name, temperature, min_realistic_temperature);
550 temperature = min_realistic_temperature;
587 TiXmlElement *motor_temp_elt = xml.RootElement();
588 if (motor_temp_elt == NULL)
595 std::string actuator_name;
597 double winding_temperature;
598 double housing_temperature;
599 double ambient_temperature;
600 int save_time_sec, save_time_nsec;
605 const char* expected_version =
"1";
606 if (version != expected_version)
608 ROS_ERROR(
"Unknown version '%s', expected '%s'", version.c_str(), expected_version);
627 ROS_ERROR(
"In save file '%s' : expected actuator name '%s', got '%s'",
634 ROS_WARN(
"In save file '%s' : expected HWID '%s', got '%s'",
677 double winding_temperature;
678 double housing_temperature;
679 double ambient_temperature;
681 boost::lock_guard<boost::mutex> lock(
mutex_);
688 TiXmlDeclaration *decl =
new TiXmlDeclaration(
"1.0",
"",
"" );
689 TiXmlElement *elmt =
new TiXmlElement(
"motor_heating_model");
690 elmt->SetAttribute(
"version",
"1");
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);
697 elmt->SetAttribute(
"save_time_sec", now.
sec);
698 elmt->SetAttribute(
"save_time_nsec", now.
nsec);
700 xml.LinkEndChild(decl);
701 xml.LinkEndChild( elmt );
704 if (!xml.SaveFile(tmp_filename))
706 ROS_WARN(
"Could not save motor heating model file '%s'", tmp_filename.c_str());
715 strerror_r(error, errbuf,
sizeof(errbuf));
716 errbuf[
sizeof(errbuf)-1] =
'\0';
717 ROS_WARN(
"Problem renaming '%s' to '%s' : (%d) '%s'",
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
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.
bool verifyCRC(void) const
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.
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.
bool startTemperaturePublisher()
std::string save_directory_
Directory where temperature save files should be put.
void saveThreadFunc()
Continuously saves motor heating model state.
bool saveTemperatureState()
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
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.
uint32_t crc32_
CRC32 of first 256-4 bytes of structure.
bool update(double heating_power, double ambient_temperature, double duration)
Updates motor temperature estimate.
std::string hwid_
Hardware ID of device (ex. 680500501000)