00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <iomanip>
00036
00037 #include <math.h>
00038 #include <stddef.h>
00039
00040 #include <ethercat_hardware/wg0x.h>
00041
00042 #include <dll/ethercat_dll.h>
00043 #include <al/ethercat_AL.h>
00044 #include <dll/ethercat_device_addressed_telegram.h>
00045 #include <dll/ethercat_frame.h>
00046
00047 #include <boost/crc.hpp>
00048 #include <boost/static_assert.hpp>
00049 #include <boost/make_shared.hpp>
00050
00051
00052 #define ERR_MODE "\033[41m"
00053 #define STD_MODE "\033[0m"
00054 #define WARN_MODE "\033[43m"
00055 #define GOOD_MODE "\033[42m"
00056 #define INFO_MODE "\033[44m"
00057
00058 #define ERROR_HDR "\033[41mERROR\033[0m"
00059 #define WARN_HDR "\033[43mERROR\033[0m"
00060
00061 #include "ethercat_hardware/wg_util.h"
00062
00063
00064 WG0XDiagnostics::WG0XDiagnostics() :
00065 first_(true),
00066 valid_(false),
00067 safety_disable_total_(0),
00068 undervoltage_total_(0),
00069 over_current_total_(0),
00070 board_over_temp_total_(0),
00071 bridge_over_temp_total_(0),
00072 operate_disable_total_(0),
00073 watchdog_disable_total_(0),
00074 lock_errors_(0),
00075 checksum_errors_(0),
00076 zero_offset_(0),
00077 cached_zero_offset_(0)
00078 {
00079 memset(&safety_disable_status_, 0, sizeof(safety_disable_status_));
00080 memset(&diagnostics_info_, 0, sizeof(diagnostics_info_));
00081 }
00082
00089 void WG0XDiagnostics::update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info)
00090 {
00091 first_ = false;
00092 safety_disable_total_ += 0xFF & ((uint32_t)(new_status.safety_disable_count_ - safety_disable_status_.safety_disable_count_));
00093 {
00094 const WG0XSafetyDisableCounters &new_counters(new_diagnostics_info.safety_disable_counters_);
00095 const WG0XSafetyDisableCounters &old_counters(diagnostics_info_.safety_disable_counters_);
00096 undervoltage_total_ += 0xFF & ((uint32_t)(new_counters.undervoltage_count_ - old_counters.undervoltage_count_));
00097 over_current_total_ += 0xFF & ((uint32_t)(new_counters.over_current_count_ - old_counters.over_current_count_));
00098 board_over_temp_total_ += 0xFF & ((uint32_t)(new_counters.board_over_temp_count_ - old_counters.board_over_temp_count_));
00099 bridge_over_temp_total_ += 0xFF & ((uint32_t)(new_counters.bridge_over_temp_count_ - old_counters.bridge_over_temp_count_));
00100 operate_disable_total_ += 0xFF & ((uint32_t)(new_counters.operate_disable_count_ - old_counters.operate_disable_count_));
00101 watchdog_disable_total_ += 0xFF & ((uint32_t)(new_counters.watchdog_disable_count_ - old_counters.watchdog_disable_count_));
00102 }
00103
00104 safety_disable_status_ = new_status;
00105 diagnostics_info_ = new_diagnostics_info;
00106 }
00107
00108
00130 bool WG0XActuatorInfo::verifyCRC() const
00131 {
00132
00133 BOOST_STATIC_ASSERT(sizeof(WG0XActuatorInfo) == 264);
00134 BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_256_) == (256-4));
00135 BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_264_) == (264-4));
00136 boost::crc_32_type crc32_256, crc32_264;
00137 crc32_256.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_256_));
00138 crc32_264.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_264_));
00139 return ((this->crc32_264_ == crc32_264.checksum()) || (this->crc32_256_ == crc32_256.checksum()));
00140 }
00141
00145 void WG0XActuatorInfo::generateCRC(void)
00146 {
00147 boost::crc_32_type crc32;
00148 crc32.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_256_));
00149 this->crc32_256_ = crc32.checksum();
00150 crc32.reset();
00151 crc32.process_bytes(this, offsetof(WG0XActuatorInfo, crc32_264_));
00152 this->crc32_264_ = crc32.checksum();
00153 }
00154
00155
00156 WG0X::WG0X() :
00157 max_current_(0.0),
00158 too_many_dropped_packets_(false),
00159 status_checksum_error_(false),
00160 timestamp_jump_detected_(false),
00161 fpga_internal_reset_detected_(false),
00162 encoder_errors_detected_(false),
00163 cached_zero_offset_(0),
00164 calibration_status_(NO_CALIBRATION),
00165 app_ram_status_(APP_RAM_MISSING),
00166 motor_model_(NULL),
00167 disable_motor_model_checking_(false)
00168 {
00169
00170 last_timestamp_ = 0;
00171 last_last_timestamp_ = 0;
00172 drops_ = 0;
00173 consecutive_drops_ = 0;
00174 max_consecutive_drops_ = 0;
00175 max_board_temperature_ = 0;
00176 max_bridge_temperature_ = 0;
00177 in_lockout_ = false;
00178 resetting_ = false;
00179 has_error_ = false;
00180
00181 int error;
00182 if ((error = pthread_mutex_init(&wg0x_diagnostics_lock_, NULL)) != 0)
00183 {
00184 ROS_ERROR("WG0X : init diagnostics mutex :%s", strerror(error));
00185 }
00186
00187 }
00188
00189 WG0X::~WG0X()
00190 {
00191 delete sh_->get_fmmu_config();
00192 delete sh_->get_pd_config();
00193 delete motor_model_;
00194 }
00195
00196
00197 void WG0X::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00198 {
00199 EthercatDevice::construct(sh, start_address);
00200
00201
00202 fw_major_ = (sh->get_revision() >> 8) & 0xff;
00203 fw_minor_ = sh->get_revision() & 0xff;
00204 board_major_ = ((sh->get_revision() >> 24) & 0xff) - 1;
00205 board_minor_ = (sh->get_revision() >> 16) & 0xff;
00206
00207
00208
00209
00210 }
00211
00212
00219 void WG0X::copyActuatorInfo(ethercat_hardware::ActuatorInfo &out, const WG0XActuatorInfo &in)
00220 {
00221 out.id = in.id_;
00222 out.name = std::string(in.name_);
00223 out.robot_name = in.robot_name_;
00224 out.motor_make = in.motor_make_;
00225 out.motor_model = in.motor_model_;
00226 out.max_current = in.max_current_;
00227 out.speed_constant = in.speed_constant_;
00228 out.motor_resistance = in.resistance_;
00229 out.motor_torque_constant = in.motor_torque_constant_;
00230 out.encoder_reduction = in.encoder_reduction_;
00231 out.pulses_per_revolution = in.pulses_per_revolution_;
00232 }
00233
00234
00237 bool WG0X::initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw,
00238 const string &device_description,
00239 double max_pwm_ratio,
00240 double board_resistance,
00241 bool poor_measured_motor_voltage)
00242 {
00243 if (!hw)
00244 return true;
00245
00246 motor_model_ = new MotorModel(1000);
00247 if (motor_model_ == NULL)
00248 return false;
00249
00250 const ethercat_hardware::ActuatorInfo &ai(actuator_info_msg_);
00251
00252 unsigned product_code = sh_->get_product_code();
00253 ethercat_hardware::BoardInfo bi;
00254 bi.description = device_description;
00255 bi.product_code = product_code;
00256 bi.pcb = board_major_;
00257 bi.pca = board_minor_;
00258 bi.serial = sh_->get_serial();
00259 bi.firmware_major = fw_major_;
00260 bi.firmware_minor = fw_minor_;
00261 bi.board_resistance = board_resistance;
00262 bi.max_pwm_ratio = max_pwm_ratio;
00263 bi.hw_max_current = config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_;
00264 bi.poor_measured_motor_voltage = poor_measured_motor_voltage;
00265
00266 if (!motor_model_->initialize(ai,bi))
00267 return false;
00268
00269
00270 publish_motor_trace_.name_ = string(actuator_info_.name_) + "_publish_motor_trace";
00271 publish_motor_trace_.command_.data_ = 0;
00272 publish_motor_trace_.state_.data_ = 0;
00273 if (!hw->addDigitalOut(&publish_motor_trace_)) {
00274 ROS_FATAL("A digital out of the name '%s' already exists", publish_motor_trace_.name_.c_str());
00275 return false;
00276 }
00277
00278
00279
00280 if (!ros::param::get("~/" + ai.name + "/disable_motor_model_checking", disable_motor_model_checking_))
00281 {
00282 disable_motor_model_checking_ = false;
00283 }
00284 if (disable_motor_model_checking_)
00285 {
00286 ROS_WARN("Disabling motor model on %s", ai.name.c_str());
00287 }
00288
00289 return true;
00290 }
00291
00292
00293 boost::shared_ptr<ethercat_hardware::MotorHeatingModelCommon> WG0X::motor_heating_model_common_;
00294
00295 bool WG0X::initializeMotorHeatingModel(bool allow_unprogrammed)
00296 {
00297
00298 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00299 ethercat_hardware::MotorHeatingModelParametersEepromConfig config;
00300 if (!readMotorHeatingModelParametersFromEeprom(&com, config))
00301 {
00302 ROS_FATAL("Unable to read motor heating model config parameters from EEPROM");
00303 return false;
00304 }
00305
00306
00307
00308 if (!config.verifyCRC())
00309 {
00310 if (allow_unprogrammed)
00311 {
00312 ROS_WARN("%s EEPROM does not contain motor heating model parameters",
00313 actuator_info_.name_);
00314 return true;
00315 }
00316 else
00317 {
00318 ROS_WARN("%s EEPROM does not contain motor heating model parameters",
00319 actuator_info_.name_);
00320 return true;
00321
00322 ROS_FATAL("%s EEPROM does not contain motor heating model parameters",
00323 actuator_info_.name_);
00324 return false;
00325 }
00326 }
00327
00328
00329
00330 if (config.enforce_ == 0)
00331 {
00332 return true;
00333 }
00334
00335
00336 if (!use_ros_)
00337 {
00338 return true;
00339 }
00340
00341
00342 std::ostringstream hwid;
00343 hwid << unsigned(sh_->get_product_code()) << std::setw(5) << std::setfill('0') << unsigned(sh_->get_serial());
00344
00345
00346 if (motor_heating_model_common_.get() == NULL)
00347 {
00348 ros::NodeHandle nh("~motor_heating_model");
00349 motor_heating_model_common_ = boost::make_shared<ethercat_hardware::MotorHeatingModelCommon>(nh);
00350 motor_heating_model_common_->initialize();
00351
00352 if (!motor_heating_model_common_->enable_model_)
00353 {
00354 ROS_WARN("Motor heating model disabled for all devices");
00355 }
00356 if (!motor_heating_model_common_->load_save_files_)
00357 {
00358 ROS_WARN("Not loading motor heating model files");
00359 }
00360 if (!motor_heating_model_common_->update_save_files_)
00361 {
00362 ROS_WARN("Not saving motor heating model files");
00363 }
00364 }
00365
00366 if (!motor_heating_model_common_->enable_model_)
00367 {
00368 return true;
00369 }
00370
00371 motor_heating_model_ =
00372 boost::make_shared<ethercat_hardware::MotorHeatingModel>(config.params_,
00373 actuator_info_.name_,
00374 hwid.str(),
00375 motor_heating_model_common_->save_directory_);
00376
00377 if (motor_heating_model_common_->load_save_files_)
00378 {
00379 if (!motor_heating_model_->loadTemperatureState())
00380 {
00381 ROS_WARN("Could not load motor temperature state for %s", actuator_info_.name_);
00382 }
00383 }
00384 if (motor_heating_model_common_->publish_temperature_)
00385 {
00386 motor_heating_model_->startTemperaturePublisher();
00387 }
00388 motor_heating_model_common_->attach(motor_heating_model_);
00389
00390 return true;
00391 }
00392
00393
00394 int WG0X::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00395 {
00396 ROS_DEBUG("Device #%02d: WG0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d, Serial #: %d",
00397 sh_->get_ring_position(),
00398 sh_->get_product_code() % 100,
00399 sh_->get_product_code(), fw_major_, fw_minor_,
00400 'A' + board_major_, board_minor_,
00401 sh_->get_serial());
00402
00403 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00404
00405 mailbox_.initialize(sh_);
00406
00407 if (sh_->get_product_code() == WG05_PRODUCT_CODE)
00408 {
00409 if (fw_major_ != 1 || fw_minor_ < 7)
00410 {
00411 ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00412 return -1;
00413 }
00414 }
00415 else
00416 {
00417 if ((fw_major_ == 0 && fw_minor_ < 4) )
00418 {
00419 ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00420 return -1;
00421 }
00422 }
00423
00424 if (readMailbox(&com, WG0XConfigInfo::CONFIG_INFO_BASE_ADDR, &config_info_, sizeof(config_info_)) != 0)
00425 {
00426 ROS_FATAL("Unable to load configuration information");
00427 return -1;
00428 }
00429 ROS_DEBUG(" Serial #: %05d", config_info_.device_serial_number_);
00430 double board_max_current = double(config_info_.absolute_current_limit_) * config_info_.nominal_current_scale_;
00431
00432 if (!readActuatorInfoFromEeprom(&com, actuator_info_))
00433 {
00434 ROS_FATAL("Unable to read actuator info from EEPROM");
00435 return -1;
00436 }
00437
00438 if (actuator_info_.verifyCRC())
00439 {
00440 if (actuator_info_.major_ != 0 || actuator_info_.minor_ != 2)
00441 {
00442 if (allow_unprogrammed)
00443 ROS_WARN("Unsupported actuator info version (%d.%d != 0.2). Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00444 else
00445 {
00446 ROS_FATAL("Unsupported actuator info version (%d.%d != 0.2). Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00447 return -1;
00448 }
00449 }
00450
00451 actuator_.name_ = actuator_info_.name_;
00452 ROS_DEBUG(" Name: %s", actuator_info_.name_);
00453
00454
00455 copyActuatorInfo(actuator_info_msg_, actuator_info_);
00456
00457 if (!initializeMotorHeatingModel(allow_unprogrammed))
00458 {
00459 return -1;
00460 }
00461
00462
00463 bool isWG021 = sh_->get_product_code() == WG021_PRODUCT_CODE;
00464 if (!isWG021)
00465 {
00466
00467 if (hw && !hw->addActuator(&actuator_))
00468 {
00469 ROS_FATAL("An actuator of the name '%s' already exists. Device #%02d has a duplicate name", actuator_.name_.c_str(), sh_->get_ring_position());
00470 return -1;
00471 }
00472
00473 }
00474
00475
00476 digital_out_.name_ = actuator_info_.name_;
00477 if (hw && !hw->addDigitalOut(&digital_out_))
00478 {
00479 ROS_FATAL("A digital out of the name '%s' already exists. Device #%02d has a duplicate name", digital_out_.name_.c_str(), sh_->get_ring_position());
00480 return -1;
00481 }
00482
00483
00484 if (app_ram_status_ == APP_RAM_PRESENT)
00485 {
00486 double zero_offset;
00487 if (readAppRam(&com, zero_offset))
00488 {
00489 ROS_DEBUG("Read calibration from device %s: %f", actuator_info_.name_, zero_offset);
00490 actuator_.state_.zero_offset_ = zero_offset;
00491 cached_zero_offset_ = zero_offset;
00492 calibration_status_ = SAVED_CALIBRATION;
00493 }
00494 else
00495 {
00496 ROS_DEBUG("No calibration offset was stored on device %s", actuator_info_.name_);
00497 }
00498 }
00499 else if (app_ram_status_ == APP_RAM_MISSING)
00500 {
00501 ROS_WARN("Device %s does not support storing calibration offsets", actuator_info_.name_);
00502 }
00503 else if (app_ram_status_ == APP_RAM_NOT_APPLICABLE)
00504 {
00505
00506 }
00507
00508
00509 if (actuator_info_.max_current_ > board_max_current)
00510 {
00511 ROS_WARN("WARNING: Device #%02d : motor current limit (%f) greater than board current limit (%f)",
00512 sh_->get_ring_position(), actuator_info_.max_current_, board_max_current);
00513 }
00514 max_current_ = std::min(board_max_current, actuator_info_.max_current_);
00515 }
00516 else if (allow_unprogrammed)
00517 {
00518 ROS_WARN("WARNING: Device #%02d (%d%05d) is not programmed",
00519 sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00520
00521
00522
00523 max_current_ = board_max_current;
00524 }
00525 else
00526 {
00527 ROS_FATAL("Device #%02d (%d%05d) is not programmed, aborting...",
00528 sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00529 return -1;
00530 }
00531
00532 return 0;
00533 }
00534
00535 #define GET_ATTR(a) \
00536 { \
00537 TiXmlElement *c; \
00538 attr = elt->Attribute((a)); \
00539 if (!attr) { \
00540 c = elt->FirstChildElement((a)); \
00541 if (!c || !(attr = c->GetText())) { \
00542 ROS_FATAL("Actuator is missing the attribute "#a); \
00543 exit(EXIT_FAILURE); \
00544 } \
00545 } \
00546 }
00547
00548 void WG0X::clearErrorFlags(void)
00549 {
00550 has_error_ = false;
00551 too_many_dropped_packets_ = false;
00552 status_checksum_error_ = false;
00553 timestamp_jump_detected_ = false;
00554 encoder_errors_detected_ = false;
00555 if (motor_model_) motor_model_->reset();
00556 if (motor_heating_model_.get() != NULL)
00557 {
00558 motor_heating_model_->reset();
00559 }
00560 }
00561
00562 void WG0X::packCommand(unsigned char *buffer, bool halt, bool reset)
00563 {
00564 pr2_hardware_interface::ActuatorCommand &cmd = actuator_.command_;
00565
00566 if (halt)
00567 {
00568 cmd.effort_ = 0;
00569 }
00570
00571 if (reset)
00572 {
00573 clearErrorFlags();
00574 }
00575 resetting_ = reset;
00576
00577
00578 double zero_offset = actuator_.state_.zero_offset_;
00579 if (zero_offset != cached_zero_offset_)
00580 {
00581 if (tryLockWG0XDiagnostics())
00582 {
00583 ROS_DEBUG("Calibration change of %s, new %f, old %f", actuator_info_.name_, zero_offset, cached_zero_offset_);
00584 cached_zero_offset_ = zero_offset;
00585 wg0x_collect_diagnostics_.zero_offset_ = zero_offset;
00586 calibration_status_ = CONTROLLER_CALIBRATION;
00587 unlockWG0XDiagnostics();
00588 }
00589 else
00590 {
00591
00592 }
00593 }
00594
00595
00596 double current = (cmd.effort_ / actuator_info_.encoder_reduction_) / actuator_info_.motor_torque_constant_ ;
00597 actuator_.state_.last_commanded_effort_ = cmd.effort_;
00598 actuator_.state_.last_commanded_current_ = current;
00599
00600
00601 current = max(min(current, max_current_), -max_current_);
00602
00603
00604 WG0XCommand *c = (WG0XCommand *)buffer;
00605 memset(c, 0, command_size_);
00606 c->programmed_current_ = int(current / config_info_.nominal_current_scale_);
00607 c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00608 c->mode_ |= (reset ? MODE_SAFETY_RESET : 0);
00609 c->digital_out_ = digital_out_.command_.data_;
00610 c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00611 }
00612
00613 bool WG0X::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00614 {
00615 pr2_hardware_interface::ActuatorState &state = actuator_.state_;
00616 WG0XStatus *this_status, *prev_status;
00617
00618 this_status = (WG0XStatus *)(this_buffer + command_size_);
00619 prev_status = (WG0XStatus *)(prev_buffer + command_size_);
00620
00621 digital_out_.state_.data_ = this_status->digital_out_;
00622
00623
00624
00625
00626 int32_t timediff = WG0X::timestampDiff(this_status->timestamp_, prev_status->timestamp_);
00627 sample_timestamp_ += WG0X::timediffToDuration(timediff);
00628 state.sample_timestamp_ = sample_timestamp_;
00629 state.timestamp_ = sample_timestamp_.toSec();
00630
00631 state.device_id_ = sh_->get_ring_position();
00632
00633 state.encoder_count_ = this_status->encoder_count_;
00634 state.position_ = double(this_status->encoder_count_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI - state.zero_offset_;
00635
00636 state.encoder_velocity_ =
00637 calcEncoderVelocity(this_status->encoder_count_, this_status->timestamp_,
00638 prev_status->encoder_count_, prev_status->timestamp_);
00639 state.velocity_ = state.encoder_velocity_ / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00640
00641 state.calibration_reading_ = this_status->calibration_reading_ & LIMIT_SENSOR_0_STATE;
00642 state.calibration_rising_edge_valid_ = this_status->calibration_reading_ & LIMIT_OFF_TO_ON;
00643 state.calibration_falling_edge_valid_ = this_status->calibration_reading_ & LIMIT_ON_TO_OFF;
00644 state.last_calibration_rising_edge_ = double(this_status->last_calibration_rising_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00645 state.last_calibration_falling_edge_ = double(this_status->last_calibration_falling_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00646 state.is_enabled_ = bool(this_status->mode_ & MODE_ENABLE);
00647
00648 state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00649 state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
00650
00651 state.last_executed_effort_ = this_status->programmed_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00652 state.last_measured_effort_ = this_status->measured_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00653
00654 state.num_encoder_errors_ = this_status->num_encoder_errors_;
00655
00656 state.motor_voltage_ = this_status->motor_voltage_ * config_info_.nominal_voltage_scale_;
00657
00658 state.max_effort_ = max_current_ * actuator_info_.encoder_reduction_ * actuator_info_.motor_torque_constant_;
00659
00660 return verifyState(this_status, prev_status);
00661 }
00662
00663
00664 bool WG0X::verifyChecksum(const void* buffer, unsigned size)
00665 {
00666 bool success = wg_util::computeChecksum(buffer, size) == 0;
00667 if (!success) {
00668 if (tryLockWG0XDiagnostics()) {
00669 ++wg0x_collect_diagnostics_.checksum_errors_;
00670 unlockWG0XDiagnostics();
00671 }
00672 }
00673 return success;
00674 }
00675
00676
00677
00684 int32_t WG0X::timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp)
00685 {
00686 return int32_t(new_timestamp - old_timestamp);
00687 }
00688
00695 ros::Duration WG0X::timediffToDuration(int32_t timediff_usec)
00696 {
00697 static const int USEC_PER_SEC = 1000000;
00698 int sec = timediff_usec / USEC_PER_SEC;
00699 int nsec = (timediff_usec % USEC_PER_SEC)*1000;
00700 return ros::Duration(sec,nsec);
00701 }
00702
00703
00709 int32_t WG0X::positionDiff(int32_t new_position, int32_t old_position)
00710 {
00711 return int32_t(new_position - old_position);
00712 }
00713
00720 double WG0X::calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp,
00721 int32_t old_position, uint32_t old_timestamp)
00722 {
00723 double timestamp_diff = double(timestampDiff(new_timestamp, old_timestamp)) * 1e-6;
00724 double position_diff = double(positionDiff(new_position, old_position));
00725 return (position_diff / timestamp_diff);
00726 }
00727
00728
00735 double WG0X::convertRawTemperature(int16_t raw_temp)
00736 {
00737 return 0.0078125 * double(raw_temp);
00738 }
00739
00740
00741
00742 bool WG0X::timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount)
00743 {
00744 uint32_t timestamp_diff = (timestamp - last_timestamp);
00745 return (timestamp_diff > amount);
00746 }
00747
00748 bool WG0X::verifyState(WG0XStatus *this_status, WG0XStatus *prev_status)
00749 {
00750 pr2_hardware_interface::ActuatorState &state = actuator_.state_;
00751 bool rv = true;
00752
00753 if ((motor_model_ != NULL) || (motor_heating_model_ != NULL))
00754 {
00755
00756 ethercat_hardware::MotorTraceSample &s(motor_trace_sample_);
00757 double last_executed_current = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00758 double supply_voltage = double(prev_status->supply_voltage_) * config_info_.nominal_voltage_scale_;
00759 double pwm_ratio = double(this_status->programmed_pwm_value_) / double(PWM_MAX);
00760 s.timestamp = state.timestamp_;
00761 s.enabled = state.is_enabled_;
00762 s.supply_voltage = supply_voltage;
00763 s.measured_motor_voltage = state.motor_voltage_;
00764 s.programmed_pwm = pwm_ratio;
00765 s.executed_current = last_executed_current;
00766 s.measured_current = state.last_measured_current_;
00767 s.velocity = state.velocity_;
00768 s.encoder_position = state.position_;
00769 s.encoder_error_count = state.num_encoder_errors_;
00770
00771 if (motor_model_ != NULL)
00772 {
00773
00774 motor_model_->sample(s);
00775 motor_model_->checkPublish();
00776 }
00777 if (motor_heating_model_ != NULL)
00778 {
00779 double ambient_temperature = convertRawTemperature(this_status->board_temperature_);
00780 double duration = double(timestampDiff(this_status->timestamp_, prev_status->timestamp_)) * 1e-6;
00781 motor_heating_model_->update(s, actuator_info_msg_, ambient_temperature, duration);
00782
00783 if ((!motor_heating_model_common_->disable_halt_) && (motor_heating_model_->hasOverheated()))
00784 {
00785 rv = false;
00786 }
00787 }
00788 }
00789
00790 max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
00791 max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
00792
00793 if (this_status->timestamp_ == last_timestamp_ ||
00794 this_status->timestamp_ == last_last_timestamp_) {
00795 ++drops_;
00796 ++consecutive_drops_;
00797 max_consecutive_drops_ = max(max_consecutive_drops_, consecutive_drops_);
00798 } else {
00799 consecutive_drops_ = 0;
00800 }
00801
00802 if ( timestamp_jump(this_status->timestamp_,last_timestamp_,10000000) )
00803 {
00804 timestamp_jump_detected_ = true;
00805 }
00806 last_last_timestamp_ = last_timestamp_;
00807 last_timestamp_ = this_status->timestamp_;
00808
00809 if (consecutive_drops_ > 10)
00810 {
00811 too_many_dropped_packets_ = true;
00812 rv = false;
00813 goto end;
00814 }
00815
00816 in_lockout_ = bool(this_status->mode_ & MODE_SAFETY_LOCKOUT);
00817 if (in_lockout_ && !resetting_)
00818 {
00819 rv = false;
00820 goto end;
00821 }
00822
00823 if (fpga_internal_reset_detected_)
00824 {
00825 rv = false;
00826 goto end;
00827 }
00828
00829 if (this_status->num_encoder_errors_ != prev_status->num_encoder_errors_)
00830 {
00831 encoder_errors_detected_ = true;
00832 }
00833
00834 if (state.is_enabled_ && motor_model_)
00835 {
00836 if (!disable_motor_model_checking_)
00837 {
00838 if(!motor_model_->verify())
00839 {
00840
00841 rv = false;
00842 goto end;
00843 }
00844 }
00845 }
00846
00847 end:
00848 if (motor_model_)
00849 {
00850
00851
00852
00853 bool new_error = in_lockout_ && !resetting_ && !has_error_;
00854 if (new_error || publish_motor_trace_.command_.data_)
00855 {
00856 const char* reason = "Publishing manually triggered";
00857 if (new_error)
00858 {
00859 bool undervoltage = (this_status->mode_ & MODE_UNDERVOLTAGE);
00860 reason = (undervoltage) ? "Undervoltage Lockout" : "Safety Lockout";
00861 }
00862 int level = (new_error) ? 2 : 0;
00863 motor_model_->flagPublish(reason, level , 100);
00864 publish_motor_trace_.command_.data_ = 0;
00865 }
00866 }
00867 bool is_error = !rv;
00868 has_error_ = is_error || has_error_;
00869 actuator_.state_.halted_ = has_error_ || this_status->mode_ == MODE_OFF;
00870 return rv;
00871 }
00872
00873 bool WG0X::publishTrace(const string &reason, unsigned level, unsigned delay)
00874 {
00875 if (motor_model_)
00876 {
00877 motor_model_->flagPublish(reason, level, delay);
00878 return true;
00879 }
00880 return false;
00881 }
00882
00883
00884 void WG0X::collectDiagnostics(EthercatCom *com)
00885 {
00886
00887 bool success = false;
00888
00889
00890 EthercatDevice::collectDiagnostics(com);
00891
00892
00893
00894 {
00895 EC_Logic *logic = EC_Logic::instance();
00896 unsigned char buf[1];
00897 EC_UINT address = 0x0000;
00898 NPRD_Telegram nprd_telegram(logic->get_idx(),
00899 sh_->get_station_address(),
00900 address,
00901 0 ,
00902 sizeof(buf),
00903 buf);
00904 EC_Ethernet_Frame frame(&nprd_telegram);
00905 if (!com->txandrx_once(&frame)) {
00906
00907 goto end;
00908 }
00909 if (nprd_telegram.get_wkc() != 1) {
00910
00911 goto end;
00912 }
00913 }
00914
00915 WG0XSafetyDisableStatus s;
00916 if (readMailbox(com, s.BASE_ADDR, &s, sizeof(s)) != 0) {
00917 goto end;
00918 }
00919
00920 WG0XDiagnosticsInfo di;
00921 if (readMailbox(com, di.BASE_ADDR, &di, sizeof(di)) != 0) {
00922 goto end;
00923 }
00924
00925 {
00926 WG0XDiagnostics &dg(wg0x_collect_diagnostics_);
00927
00928 if ((app_ram_status_ == APP_RAM_PRESENT) && (dg.zero_offset_ != dg.cached_zero_offset_))
00929 {
00930 if (writeAppRam(com, dg.zero_offset_)){
00931 ROS_DEBUG("Writing new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
00932 dg.cached_zero_offset_ = dg.zero_offset_;
00933 }
00934 else{
00935 ROS_ERROR("Failed to write new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
00936
00937 }
00938 }
00939 }
00940
00941 success = true;
00942
00943 end:
00944 if (!lockWG0XDiagnostics()) {
00945 wg0x_collect_diagnostics_.valid_ = false;
00946 wg0x_collect_diagnostics_.first_ = false;
00947 return;
00948 }
00949
00950 wg0x_collect_diagnostics_.valid_ = success;
00951 if (success) {
00952 wg0x_collect_diagnostics_.update(s,di);
00953 }
00954
00955 unlockWG0XDiagnostics();
00956 }
00957
00958
00959 bool WG0X::writeAppRam(EthercatCom *com, double zero_offset)
00960 {
00961 WG0XUserConfigRam cfg;
00962 cfg.version_ = 1;
00963 cfg.zero_offset_ = zero_offset;
00964 boost::crc_32_type crc32;
00965 crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
00966 cfg.crc32_ = crc32.checksum();
00967 return (writeMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0);
00968 }
00969
00970 bool WG0X::readAppRam(EthercatCom *com, double &zero_offset)
00971 {
00972 WG0XUserConfigRam cfg;
00973 if (!readMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0)
00974 {
00975 return false;
00976 }
00977 if (cfg.version_ != 1)
00978 {
00979 return false;
00980 }
00981 boost::crc_32_type crc32;
00982 crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
00983 if (cfg.crc32_ != crc32.checksum()) {
00984 return false;
00985 }
00986 zero_offset = cfg.zero_offset_;
00987 return true;
00988 }
00989
00990
00998 bool WG0X::readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info)
00999 {
01000 BOOST_STATIC_ASSERT(sizeof(actuator_info) == 264);
01001
01002 if (!eeprom_.readEepromPage(com, &mailbox_, ACTUATOR_INFO_PAGE, &actuator_info, sizeof(actuator_info)))
01003 {
01004 ROS_ERROR("Reading acutuator info from eeprom");
01005 return false;
01006 }
01007 return true;
01008 }
01009
01017 bool WG0X::readMotorHeatingModelParametersFromEeprom(EthercatCom *com, MotorHeatingModelParametersEepromConfig &config)
01018 {
01019 BOOST_STATIC_ASSERT(sizeof(config) == 256);
01020
01021 if (!eeprom_.readEepromPage(com, &mailbox_, config.EEPROM_PAGE, &config, sizeof(config)))
01022 {
01023 ROS_ERROR("Reading motor heating model config from eeprom");
01024 return false;
01025 }
01026 return true;
01027 }
01028
01029
01030
01031
01050 bool WG0X::program(EthercatCom *com, const WG0XActuatorInfo &actutor_info)
01051 {
01052 if (!eeprom_.writeEepromPage(com, &mailbox_, ACTUATOR_INFO_PAGE, &actutor_info, sizeof(actutor_info)))
01053 {
01054 ROS_ERROR("Writing actuator infomation to EEPROM");
01055 return false;
01056 }
01057
01058 return true;
01059 }
01060
01061
01076 bool WG0X::program(EthercatCom *com, const ethercat_hardware::MotorHeatingModelParametersEepromConfig &heating_config)
01077 {
01078 if (!eeprom_.writeEepromPage(com, &mailbox_, heating_config.EEPROM_PAGE, &heating_config, sizeof(heating_config)))
01079 {
01080 ROS_ERROR("Writing motor heating model configuration to EEPROM");
01081 return false;
01082 }
01083
01084 return true;
01085 }
01086
01087
01101 int WG0X::readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
01102 {
01103 return mailbox_.readMailbox(com, address, data, length);
01104 }
01105
01106 bool WG0X::lockWG0XDiagnostics()
01107 {
01108 int error = pthread_mutex_lock(&wg0x_diagnostics_lock_);
01109 if (error != 0) {
01110 fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
01111
01112 ++wg0x_collect_diagnostics_.lock_errors_;
01113 return false;
01114 }
01115 return true;
01116 }
01117
01118 bool WG0X::tryLockWG0XDiagnostics()
01119 {
01120 int error = pthread_mutex_trylock(&wg0x_diagnostics_lock_);
01121 if (error == EBUSY) {
01122 return false;
01123 }
01124 else if (error != 0) {
01125 fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
01126
01127 ++wg0x_collect_diagnostics_.lock_errors_;
01128 return false;
01129 }
01130 return true;
01131 }
01132
01133 void WG0X::unlockWG0XDiagnostics()
01134 {
01135 int error = pthread_mutex_unlock(&wg0x_diagnostics_lock_);
01136 if (error != 0) {
01137 fprintf(stderr, "%s : " ERROR_HDR " freeing diagnostics lock\n", __func__);
01138 ++wg0x_collect_diagnostics_.lock_errors_;
01139 }
01140 }
01141
01142
01155 int WG0X::writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
01156 {
01157 return mailbox_.writeMailbox(com,address,data,length);
01158 }
01159
01160
01161
01162 #define CHECK_SAFETY_BIT(bit) \
01163 do { if (status & SAFETY_##bit) { \
01164 str += prefix + #bit; \
01165 prefix = ", "; \
01166 } } while(0)
01167
01168 string WG0X::safetyDisableString(uint8_t status)
01169 {
01170 string str, prefix;
01171
01172 if (status & SAFETY_DISABLED)
01173 {
01174 CHECK_SAFETY_BIT(DISABLED);
01175 CHECK_SAFETY_BIT(UNDERVOLTAGE);
01176 CHECK_SAFETY_BIT(OVER_CURRENT);
01177 CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
01178 CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
01179 CHECK_SAFETY_BIT(OPERATIONAL);
01180 CHECK_SAFETY_BIT(WATCHDOG);
01181 }
01182 else
01183 str = "ENABLED";
01184
01185 return str;
01186 }
01187
01188 string WG0X::modeString(uint8_t mode)
01189 {
01190 string str, prefix;
01191 if (mode) {
01192 if (mode & MODE_ENABLE) {
01193 str += prefix + "ENABLE";
01194 prefix = ", ";
01195 }
01196 if (mode & MODE_CURRENT) {
01197 str += prefix + "CURRENT";
01198 prefix = ", ";
01199 }
01200 if (mode & MODE_UNDERVOLTAGE) {
01201 str += prefix + "UNDERVOLTAGE";
01202 prefix = ", ";
01203 }
01204 if (mode & MODE_SAFETY_RESET) {
01205 str += prefix + "SAFETY_RESET";
01206 prefix = ", ";
01207 }
01208 if (mode & MODE_SAFETY_LOCKOUT) {
01209 str += prefix + "SAFETY_LOCKOUT";
01210 prefix = ", ";
01211 }
01212 if (mode & MODE_RESET) {
01213 str += prefix + "RESET";
01214 prefix = ", ";
01215 }
01216 } else {
01217 str = "OFF";
01218 }
01219 return str;
01220 }
01221
01222 void WG0X::publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
01223 {
01224
01225 if (tryLockWG0XDiagnostics()) {
01226 wg0x_publish_diagnostics_ = wg0x_collect_diagnostics_;
01227 unlockWG0XDiagnostics();
01228 }
01229
01230 if (too_many_dropped_packets_)
01231 {
01232 d.mergeSummary(d.ERROR, "Too many dropped packets");
01233 }
01234
01235 if (status_checksum_error_)
01236 {
01237 d.mergeSummary(d.ERROR, "Checksum error on status data");
01238 }
01239
01240 if (wg0x_publish_diagnostics_.first_)
01241 {
01242 d.mergeSummary(d.WARN, "Have not yet collected WG0X diagnostics");
01243 }
01244 else if (!wg0x_publish_diagnostics_.valid_)
01245 {
01246 d.mergeSummary(d.WARN, "Could not collect WG0X diagnostics");
01247 }
01248
01249 WG0XDiagnostics const &p(wg0x_publish_diagnostics_);
01250 WG0XSafetyDisableStatus const &s(p.safety_disable_status_);
01251 d.addf("Status Checksum Error Count", "%d", p.checksum_errors_);
01252 d.addf("Safety Disable Status", "%s (%02x)", safetyDisableString(s.safety_disable_status_).c_str(), s.safety_disable_status_);
01253 d.addf("Safety Disable Status Hold", "%s (%02x)", safetyDisableString(s.safety_disable_status_hold_).c_str(), s.safety_disable_status_hold_);
01254 d.addf("Safety Disable Count", "%d", p.safety_disable_total_);
01255 d.addf("Undervoltage Count", "%d", p.undervoltage_total_);
01256 d.addf("Over Current Count", "%d", p.over_current_total_);
01257 d.addf("Board Over Temp Count", "%d", p.board_over_temp_total_);
01258 d.addf("Bridge Over Temp Count", "%d", p.bridge_over_temp_total_);
01259 d.addf("Operate Disable Count", "%d", p.operate_disable_total_);
01260 d.addf("Watchdog Disable Count", "%d", p.watchdog_disable_total_);
01261
01262 if (in_lockout_)
01263 {
01264 uint8_t status = s.safety_disable_status_hold_;
01265 string prefix(": ");
01266 string str("Safety Lockout");
01267 CHECK_SAFETY_BIT(UNDERVOLTAGE);
01268 CHECK_SAFETY_BIT(OVER_CURRENT);
01269 CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
01270 CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
01271 CHECK_SAFETY_BIT(OPERATIONAL);
01272 CHECK_SAFETY_BIT(WATCHDOG);
01273 d.mergeSummary(d.ERROR, str);
01274 }
01275
01276 if (timestamp_jump_detected_ && (s.safety_disable_status_hold_ & SAFETY_OPERATIONAL))
01277 {
01278 fpga_internal_reset_detected_ = true;
01279 }
01280
01281 if (fpga_internal_reset_detected_)
01282 {
01283 d.mergeSummaryf(d.ERROR, "FPGA internal reset detected");
01284 }
01285
01286 if (timestamp_jump_detected_)
01287 {
01288 d.mergeSummaryf(d.WARN, "Timestamp jumped");
01289 }
01290
01291 {
01292
01293 const WG0XDiagnosticsInfo &di(p.diagnostics_info_);
01294
01295 d.addf("MBX Command IRQ Count", "%d", di.mbx_command_irq_count_);
01296 d.addf("PDI Timeout Error Count", "%d", di.pdi_timeout_error_count_);
01297 d.addf("PDI Checksum Error Count", "%d", di.pdi_checksum_error_count_);
01298 unsigned product = sh_->get_product_code();
01299
01300
01301 if ((product == WG05_PRODUCT_CODE) && (board_major_ == 1))
01302 {
01303
01304 static const double WG005B_SUPPLY_CURRENT_SCALE = (1.0 / (8152.0 * 0.851)) * 4.0;
01305 double bridge_supply_current = double(di.supply_current_in_) * WG005B_SUPPLY_CURRENT_SCALE;
01306 d.addf("Bridge Supply Current", "%f", bridge_supply_current);
01307 }
01308 else if ((product == WG05_PRODUCT_CODE) || (product == WG021_PRODUCT_CODE))
01309 {
01310
01311
01312 static const double WG005_SUPPLY_CURRENT_SCALE = ((82.0 * 2.5) / (0.01 * 5100.0 * 32768.0));
01313 double supply_current = double(di.supply_current_in_) * WG005_SUPPLY_CURRENT_SCALE;
01314 d.addf("Supply Current", "%f", supply_current);
01315 }
01316 d.addf("Configured Offset A", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_A_);
01317 d.addf("Configured Offset B", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_B_);
01318 }
01319 }
01320
01321
01322
01323 void WG0X::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
01324 {
01325 WG0XStatus *status = (WG0XStatus *)(buffer + command_size_);
01326
01327 stringstream str;
01328 str << "EtherCAT Device (" << actuator_info_.name_ << ")";
01329 d.name = str.str();
01330 char serial[32];
01331 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
01332 d.hardware_id = serial;
01333
01334 d.summary(d.OK, "OK");
01335
01336 d.clear();
01337 d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
01338 d.add("Name", actuator_info_.name_);
01339 d.addf("Position", "%02d", sh_->get_ring_position());
01340 d.addf("Product code",
01341 "WG0%d (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
01342 sh_->get_product_code() == WG05_PRODUCT_CODE ? 5 : 6,
01343 sh_->get_product_code(), fw_major_, fw_minor_,
01344 'A' + board_major_, board_minor_);
01345
01346 d.add("Robot", actuator_info_.robot_name_);
01347 d.addf("Motor", "%s %s", actuator_info_.motor_make_, actuator_info_.motor_model_);
01348 d.add("Serial Number", serial);
01349 d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
01350 d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
01351 d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
01352
01353 d.addf("SW Max Current", "%f", actuator_info_.max_current_);
01354 d.addf("Speed Constant", "%f", actuator_info_.speed_constant_);
01355 d.addf("Resistance", "%f", actuator_info_.resistance_);
01356 d.addf("Motor Torque Constant", "%f", actuator_info_.motor_torque_constant_);
01357 d.addf("Pulses Per Revolution", "%d", actuator_info_.pulses_per_revolution_);
01358 d.addf("Encoder Reduction", "%f", actuator_info_.encoder_reduction_);
01359
01360 publishGeneralDiagnostics(d);
01361 mailbox_.publishMailboxDiagnostics(d);
01362
01363 d.addf("Calibration Offset", "%f", cached_zero_offset_);
01364 d.addf("Calibration Status", "%s",
01365 (calibration_status_ == NO_CALIBRATION) ? "No calibration" :
01366 (calibration_status_ == CONTROLLER_CALIBRATION) ? "Calibrated by controller" :
01367 (calibration_status_ == SAVED_CALIBRATION) ? "Using saved calibration" : "UNKNOWN");
01368
01369 d.addf("Watchdog Limit", "%dms", config_info_.watchdog_limit_);
01370 d.add("Mode", modeString(status->mode_));
01371 d.addf("Digital out", "%d", status->digital_out_);
01372 d.addf("Programmed pwm value", "%d", status->programmed_pwm_value_);
01373 d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
01374 d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
01375 d.addf("Timestamp", "%u", status->timestamp_);
01376 d.addf("Encoder count", "%d", status->encoder_count_);
01377 d.addf("Encoder index pos", "%d", status->encoder_index_pos_);
01378 d.addf("Num encoder_errors", "%d", status->num_encoder_errors_);
01379 d.addf("Encoder status", "%d", status->encoder_status_);
01380 d.addf("Calibration reading", "%d", status->calibration_reading_);
01381 d.addf("Last calibration rising edge", "%d", status->last_calibration_rising_edge_);
01382 d.addf("Last calibration falling edge", "%d", status->last_calibration_falling_edge_);
01383 d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
01384 d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
01385 d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
01386 d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
01387 d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
01388 d.addf("Motor voltage", "%f", status->motor_voltage_ * config_info_.nominal_voltage_scale_);
01389 d.addf("Current Loop Kp", "%d", config_info_.current_loop_kp_);
01390 d.addf("Current Loop Ki", "%d", config_info_.current_loop_ki_);
01391
01392 if (motor_model_)
01393 {
01394 motor_model_->diagnostics(d);
01395 if (disable_motor_model_checking_)
01396 {
01397 d.mergeSummaryf(d.WARN, "Motor model disabled");
01398 }
01399 }
01400
01401 if (motor_heating_model_.get() != NULL)
01402 {
01403 motor_heating_model_->diagnostics(d);
01404 }
01405
01406 if (encoder_errors_detected_)
01407 {
01408 d.mergeSummaryf(d.WARN, "Encoder errors detected");
01409 }
01410
01411 d.addf("Packet count", "%d", status->packet_count_);
01412
01413 d.addf("Drops", "%d", drops_);
01414 d.addf("Consecutive Drops", "%d", consecutive_drops_);
01415 d.addf("Max Consecutive Drops", "%d", max_consecutive_drops_);
01416
01417 unsigned numPorts = (sh_->get_product_code()==WG06_PRODUCT_CODE) ? 1 : 2;
01418 EthercatDevice::ethercatDiagnostics(d, numPorts);
01419 }
01420