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
00050 PLUGINLIB_REGISTER_CLASS(6805005, WG05, EthercatDevice);
00051 PLUGINLIB_REGISTER_CLASS(6805006, WG06, EthercatDevice);
00052 PLUGINLIB_REGISTER_CLASS(6805021, WG021, EthercatDevice);
00053
00054
00055
00056 #define ERR_MODE "\033[41m"
00057 #define STD_MODE "\033[0m"
00058 #define WARN_MODE "\033[43m"
00059 #define GOOD_MODE "\033[42m"
00060 #define INFO_MODE "\033[44m"
00061
00062 #define ERROR_HDR "\033[41mERROR\033[0m"
00063 #define WARN_HDR "\033[43mERROR\033[0m"
00064
00065
00066 static unsigned int rotateRight8(unsigned in)
00067 {
00068 in &= 0xff;
00069 in = (in >> 1) | (in << 7);
00070 in &= 0xff;
00071 return in;
00072 }
00073
00074 static unsigned computeChecksum(void const *data, unsigned length)
00075 {
00076 const unsigned char *d = (const unsigned char *)data;
00077 unsigned int checksum = 0x42;
00078 for (unsigned int i = 0; i < length; ++i)
00079 {
00080 checksum = rotateRight8(checksum);
00081 checksum ^= d[i];
00082 checksum &= 0xff;
00083 }
00084 return checksum;
00085 }
00086
00087 bool WG0XMbxHdr::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum)
00088 {
00089 if (type==LOCAL_BUS_WRITE)
00090 {
00091 if (length > MBX_DATA_SIZE)
00092 {
00093 fprintf(stderr, "size of %d is too large for write\n", length);
00094 return false;
00095 }
00096 }
00097 else if (type==LOCAL_BUS_READ)
00098 {
00099
00100 if (length > (MBX_SIZE-1))
00101 {
00102 fprintf(stderr, "size of %d is too large for read\n", length);
00103 return false;
00104 }
00105 }
00106 else {
00107 assert(0 && "invalid MbxCmdType");
00108 return false;
00109 }
00110
00111 address_ = address;
00112 length_ = length - 1;
00113 seqnum_ = seqnum;
00114 write_nread_ = (type==LOCAL_BUS_WRITE) ? 1 : 0;
00115 checksum_ = rotateRight8(computeChecksum(this, sizeof(*this) - 1));
00116 return true;
00117 }
00118
00119 bool WG0XMbxHdr::verifyChecksum(void) const
00120 {
00121 return computeChecksum(this, sizeof(*this)) != 0;
00122 }
00123
00124 bool WG0XMbxCmd::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data)
00125 {
00126 if (!this->hdr_.build(address, length, type, seqnum))
00127 {
00128 return false;
00129 }
00130
00131 if (data != NULL)
00132 {
00133 memcpy(data_, data, length);
00134 }
00135 else
00136 {
00137 memset(data_, 0, length);
00138 }
00139 unsigned int checksum = rotateRight8(computeChecksum(data_, length));
00140 data_[length] = checksum;
00141 return true;
00142 }
00143
00144
00145 MbxDiagnostics::MbxDiagnostics() :
00146 write_errors_(0),
00147 read_errors_(0),
00148 lock_errors_(0),
00149 retries_(0),
00150 retry_errors_(0)
00151 {
00152
00153 }
00154
00155 WG0XDiagnostics::WG0XDiagnostics() :
00156 first_(true),
00157 valid_(false),
00158 safety_disable_total_(0),
00159 undervoltage_total_(0),
00160 over_current_total_(0),
00161 board_over_temp_total_(0),
00162 bridge_over_temp_total_(0),
00163 operate_disable_total_(0),
00164 watchdog_disable_total_(0),
00165 lock_errors_(0),
00166 checksum_errors_(0),
00167 zero_offset_(0),
00168 cached_zero_offset_(0)
00169 {
00170 memset(&safety_disable_status_, 0, sizeof(safety_disable_status_));
00171 memset(&diagnostics_info_, 0, sizeof(diagnostics_info_));
00172 }
00173
00180 void WG0XDiagnostics::update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info)
00181 {
00182 first_ = false;
00183 safety_disable_total_ += 0xFF & ((uint32_t)(new_status.safety_disable_count_ - safety_disable_status_.safety_disable_count_));
00184 {
00185 const WG0XSafetyDisableCounters &new_counters(new_diagnostics_info.safety_disable_counters_);
00186 const WG0XSafetyDisableCounters &old_counters(diagnostics_info_.safety_disable_counters_);
00187 undervoltage_total_ += 0xFF & ((uint32_t)(new_counters.undervoltage_count_ - old_counters.undervoltage_count_));
00188 over_current_total_ += 0xFF & ((uint32_t)(new_counters.over_current_count_ - old_counters.over_current_count_));
00189 board_over_temp_total_ += 0xFF & ((uint32_t)(new_counters.board_over_temp_count_ - old_counters.board_over_temp_count_));
00190 bridge_over_temp_total_ += 0xFF & ((uint32_t)(new_counters.bridge_over_temp_count_ - old_counters.bridge_over_temp_count_));
00191 operate_disable_total_ += 0xFF & ((uint32_t)(new_counters.operate_disable_count_ - old_counters.operate_disable_count_));
00192 watchdog_disable_total_ += 0xFF & ((uint32_t)(new_counters.watchdog_disable_count_ - old_counters.watchdog_disable_count_));
00193 }
00194
00195 safety_disable_status_ = new_status;
00196 diagnostics_info_ = new_diagnostics_info;
00197 }
00198
00199 WG0X::WG0X() :
00200 max_current_(0.0),
00201 too_many_dropped_packets_(false),
00202 status_checksum_error_(false),
00203 timestamp_jump_detected_(false),
00204 fpga_internal_reset_detected_(false),
00205 cached_zero_offset_(0),
00206 calibration_status_(NO_CALIBRATION),
00207 last_num_encoder_errors_(0),
00208 app_ram_status_(APP_RAM_MISSING),
00209 motor_model_(NULL),
00210 disable_motor_model_checking_(false)
00211 {
00212 int error;
00213 if ((error = pthread_mutex_init(&wg0x_diagnostics_lock_, NULL)) != 0)
00214 {
00215 ROS_ERROR("WG0X : init diagnostics mutex :%s", strerror(error));
00216 }
00217 if ((error = pthread_mutex_init(&mailbox_lock_, NULL)) != 0)
00218 {
00219 ROS_ERROR("WG0X : init mailbox mutex :%s", strerror(error));
00220 }
00221 }
00222
00223 WG0X::~WG0X()
00224 {
00225 delete sh_->get_fmmu_config();
00226 delete sh_->get_pd_config();
00227 delete motor_model_;
00228 }
00229
00230 WG06::WG06() :
00231 pressure_checksum_error_(false),
00232 accelerometer_samples_(0),
00233 accelerometer_missed_samples_(0),
00234 first_publish_(true),
00235 last_pressure_time_(0),
00236 pressure_publisher_(0),
00237 accel_publisher_(0)
00238 {
00239
00240 }
00241
00242 WG06::~WG06()
00243 {
00244 if (pressure_publisher_) delete pressure_publisher_;
00245 if (accel_publisher_) delete accel_publisher_;
00246 }
00247
00248 void WG0X::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00249 {
00250 EthercatDevice::construct(sh, start_address);
00251
00252 last_timestamp_ = 0;
00253 last_last_timestamp_ = 0;
00254 drops_ = 0;
00255 consecutive_drops_ = 0;
00256 max_consecutive_drops_ = 0;
00257 max_board_temperature_ = 0;
00258 max_bridge_temperature_ = 0;
00259 in_lockout_ = false;
00260 resetting_ = false;
00261 has_error_ = false;
00262
00263 fw_major_ = (sh->get_revision() >> 8) & 0xff;
00264 fw_minor_ = sh->get_revision() & 0xff;
00265 board_major_ = ((sh->get_revision() >> 24) & 0xff) - 1;
00266 board_minor_ = (sh->get_revision() >> 16) & 0xff;
00267
00268 bool isWG06 = sh_->get_product_code() == WG06::PRODUCT_CODE;
00269 bool isWG021 = sh_->get_product_code() == WG021::PRODUCT_CODE;
00270 unsigned int base_status = sizeof(WG0XStatus);
00271
00272 command_size_ = sizeof(WG0XCommand);
00273 status_size_ = sizeof(WG0XStatus);
00274 if (isWG06)
00275 {
00276 if (fw_major_ >= 1)
00277 status_size_ = base_status = sizeof(WG06StatusWithAccel);
00278 status_size_ += sizeof(WG06Pressure);
00279 }
00280 else if (isWG021)
00281 {
00282 status_size_ = base_status = sizeof(WG021Status);
00283 command_size_ = sizeof(WG021Command);
00284 }
00285
00286
00287 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(isWG06 ? 3 : 2);
00288
00289 (*fmmu)[0] = EC_FMMU(start_address,
00290 command_size_,
00291 0x00,
00292 0x07,
00293 COMMAND_PHY_ADDR,
00294 0x00,
00295 false,
00296 true,
00297 true);
00298
00299 start_address += command_size_;
00300
00301
00302 (*fmmu)[1] = EC_FMMU(start_address,
00303 base_status,
00304 0x00,
00305 0x07,
00306 STATUS_PHY_ADDR,
00307 0x00,
00308 true,
00309 false,
00310 true);
00311
00312 start_address += base_status;
00313
00314 if (isWG06)
00315 {
00316
00317 (*fmmu)[2] = EC_FMMU(start_address,
00318 sizeof(WG06Pressure),
00319 0x00,
00320 0x07,
00321 PRESSURE_PHY_ADDR,
00322 0x00,
00323 true,
00324 false,
00325 true);
00326
00327 start_address += sizeof(WG06Pressure);
00328 }
00329 sh->set_fmmu_config(fmmu);
00330
00331 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(isWG06 ? 5 : 4);
00332
00333
00334 (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
00335 (*pd)[0].ChannelEnable = true;
00336 (*pd)[0].ALEventEnable = true;
00337
00338 (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status);
00339 (*pd)[1].ChannelEnable = true;
00340
00341 (*pd)[2] = EC_SyncMan(MBX_COMMAND_PHY_ADDR, MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00342 (*pd)[2].ChannelEnable = true;
00343 (*pd)[2].ALEventEnable = true;
00344
00345 (*pd)[3] = EC_SyncMan(MBX_STATUS_PHY_ADDR, MBX_STATUS_SIZE, EC_QUEUED);
00346 (*pd)[3].ChannelEnable = true;
00347
00348 if (isWG06)
00349 {
00350 (*pd)[4] = EC_SyncMan(PRESSURE_PHY_ADDR, sizeof(WG06Pressure));
00351 (*pd)[4].ChannelEnable = true;
00352 }
00353
00354 sh->set_pd_config(pd);
00355 }
00356
00357 int WG05::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00358 {
00359 if ((fw_major_ == 1) && (fw_minor_ >= 21))
00360 {
00361 app_ram_status_ = APP_RAM_PRESENT;
00362 }
00363
00364 int retval = WG0X::initialize(hw, allow_unprogrammed);
00365
00366 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00367
00368
00369 if (!retval)
00370 {
00371 if (use_ros_)
00372 {
00373
00374 bool poor_measured_motor_voltage = (board_major_ <= 2);
00375 double max_pwm_ratio = double(0x3C00) / double(PWM_MAX);
00376 double board_resistance = 0.8;
00377 if (!WG0X::initializeMotorModel(hw, "WG005", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
00378 {
00379 ROS_FATAL("Initializing motor trace failed");
00380 sleep(1);
00381 return -1;
00382 }
00383 }
00384
00385 }
00386 return retval;
00387 }
00388
00389 int WG06::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00390 {
00391 if ((fw_major_ == 1) && (fw_minor_ >= 1))
00392 {
00393 app_ram_status_ = APP_RAM_PRESENT;
00394 }
00395
00396 int retval = WG0X::initialize(hw, allow_unprogrammed);
00397
00398 if (!retval && use_ros_)
00399 {
00400 bool poor_measured_motor_voltage = false;
00401 double max_pwm_ratio = double(0x2700) / double(PWM_MAX);
00402 double board_resistance = 5.0;
00403 if (!WG0X::initializeMotorModel(hw, "WG006", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
00404 {
00405 ROS_FATAL("Initializing motor trace failed");
00406 sleep(1);
00407 return -1;
00408 }
00409
00410
00411 string topic = "pressure";
00412 if (!actuator_.name_.empty())
00413 topic = topic + "/" + string(actuator_.name_);
00414 pressure_publisher_ = new realtime_tools::RealtimePublisher<pr2_msgs::PressureState>(ros::NodeHandle(), topic, 1);
00415
00416
00417 for (int i = 0; i < 2; ++i)
00418 {
00419 pressure_sensors_[i].state_.data_.resize(22);
00420 pressure_sensors_[i].name_ = string(actuator_info_.name_) + string(i ? "r_finger_tip" : "l_finger_tip");
00421 if (hw && !hw->addPressureSensor(&pressure_sensors_[i]))
00422 {
00423 ROS_FATAL("A pressure sensor of the name '%s' already exists. Device #%02d has a duplicate name", pressure_sensors_[i].name_.c_str(), sh_->get_ring_position());
00424 return -1;
00425 }
00426 }
00427
00428
00429 if (fw_major_ >= 1)
00430 {
00431 topic = "accelerometer";
00432 if (!actuator_.name_.empty())
00433 topic = topic + "/" + string(actuator_.name_);
00434 accel_publisher_ = new realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState>(ros::NodeHandle(), topic, 1);
00435
00436
00437 {
00438 accelerometer_.name_ = actuator_info_.name_;
00439 if (hw && !hw->addAccelerometer(&accelerometer_))
00440 {
00441 ROS_FATAL("An accelerometer of the name '%s' already exists. Device #%02d has a duplicate name", accelerometer_.name_.c_str(), sh_->get_ring_position());
00442 return -1;
00443 }
00444 }
00445
00446 }
00447 }
00448
00449 return retval;
00450 }
00451
00452 int WG021::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00453 {
00454
00455 app_ram_status_ = APP_RAM_NOT_APPLICABLE;
00456
00457 int retval = WG0X::initialize(hw, allow_unprogrammed);
00458
00459
00460 struct {
00461 pr2_hardware_interface::DigitalOut *d;
00462 string name;
00463 } digital_outs[] = {
00464 {&digital_out_A_, "_digital_out_A"},
00465 {&digital_out_B_, "_digital_out_B"},
00466 {&digital_out_I_, "_digital_out_I"},
00467 {&digital_out_M_, "_digital_out_M"},
00468 {&digital_out_L0_, "_digital_out_L0"},
00469 {&digital_out_L1_, "_digital_out_L1"},
00470 };
00471
00472 for (size_t i = 0; i < sizeof(digital_outs)/sizeof(digital_outs[0]); ++i)
00473 {
00474 digital_outs[i].d->name_ = string(actuator_info_.name_) + digital_outs[i].name;
00475 if (hw && !hw->addDigitalOut(digital_outs[i].d))
00476 {
00477 ROS_FATAL("A digital out of the name '%s' already exists. Device #%02d has a duplicate name", digital_outs[i].d->name_.c_str(), sh_->get_ring_position());
00478 return -1;
00479 }
00480 }
00481
00482
00483 {
00484 projector_.name_ = actuator_info_.name_;
00485 if (hw && !hw->addProjector(&projector_))
00486 {
00487 ROS_FATAL("A projector of the name '%s' already exists. Device #%02d has a duplicate name", projector_.name_.c_str(), sh_->get_ring_position());
00488 return -1;
00489 }
00490 projector_.command_.enable_ = true;
00491 projector_.command_.current_ = 0;
00492 }
00493
00494 return retval;
00495 }
00496
00499 bool WG0X::initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw,
00500 const string &device_description,
00501 double max_pwm_ratio,
00502 double board_resistance,
00503 bool poor_measured_motor_voltage)
00504 {
00505 if (!hw)
00506 return true;
00507
00508 motor_model_ = new MotorModel(1000);
00509 if (motor_model_ == NULL)
00510 return false;
00511
00512 WG0XActuatorInfo &ai_(actuator_info_);
00513 ethercat_hardware::ActuatorInfo ai;
00514 ai.id = ai_.id_;
00515 ai.name = std::string(ai_.name_);
00516 ai.robot_name = ai_.robot_name_;
00517 ai.motor_make = ai_.motor_make_;
00518 ai.motor_model = ai_.motor_model_;
00519 ai.max_current = ai_.max_current_;
00520 ai.speed_constant = ai_.speed_constant_;
00521 ai.motor_resistance = ai_.resistance_;
00522 ai.motor_torque_constant = ai_.motor_torque_constant_;
00523 ai.encoder_reduction = ai_.encoder_reduction_;
00524 ai.pulses_per_revolution = ai_.pulses_per_revolution_;
00525
00526 unsigned product_code = sh_->get_product_code();
00527 ethercat_hardware::BoardInfo bi;
00528 bi.description = device_description;
00529 bi.product_code = product_code;
00530 bi.pcb = board_major_;
00531 bi.pca = board_minor_;
00532 bi.serial = sh_->get_serial();
00533 bi.firmware_major = fw_major_;
00534 bi.firmware_minor = fw_minor_;
00535 bi.board_resistance = board_resistance;
00536 bi.max_pwm_ratio = max_pwm_ratio;
00537 bi.hw_max_current = config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_;
00538 bi.poor_measured_motor_voltage = poor_measured_motor_voltage;
00539
00540 if (!motor_model_->initialize(ai,bi))
00541 return false;
00542
00543
00544 publish_motor_trace_.name_ = string(actuator_info_.name_) + "_publish_motor_trace";
00545 publish_motor_trace_.command_.data_ = 0;
00546 publish_motor_trace_.state_.data_ = 0;
00547 if (!hw->addDigitalOut(&publish_motor_trace_)) {
00548 ROS_FATAL("A digital out of the name '%s' already exists", publish_motor_trace_.name_.c_str());
00549 return false;
00550 }
00551
00552
00553
00554 if (!ros::NodeHandle().getParam(ai.name + "/disable_motor_model_checking", disable_motor_model_checking_))
00555 {
00556 disable_motor_model_checking_ = false;
00557 }
00558
00559 return true;
00560 }
00561
00562 int WG0X::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00563 {
00564 ROS_DEBUG("Device #%02d: WG0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d, Serial #: %d",
00565 sh_->get_ring_position(),
00566 sh_->get_product_code() % 100,
00567 sh_->get_product_code(), fw_major_, fw_minor_,
00568 'A' + board_major_, board_minor_,
00569 sh_->get_serial());
00570
00571 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00572
00573 if (sh_->get_product_code() == WG05::PRODUCT_CODE)
00574 {
00575 if (fw_major_ != 1 || fw_minor_ < 7)
00576 {
00577 ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00578 return -1;
00579 }
00580 }
00581 else
00582 {
00583 if ((fw_major_ == 0 && fw_minor_ < 4) )
00584 {
00585 ROS_FATAL("Unsupported firmware revision %d.%02d", fw_major_, fw_minor_);
00586 return -1;
00587 }
00588 }
00589
00590 if (readMailbox(&com, WG0XConfigInfo::CONFIG_INFO_BASE_ADDR, &config_info_, sizeof(config_info_)) != 0)
00591 {
00592 ROS_FATAL("Unable to load configuration information");
00593 return -1;
00594 }
00595 ROS_DEBUG(" Serial #: %05d", config_info_.device_serial_number_);
00596 double board_max_current = double(config_info_.absolute_current_limit_) * config_info_.nominal_current_scale_;
00597
00598 if (readEeprom(&com) < 0)
00599 {
00600 ROS_FATAL("Unable to read actuator info from EEPROM");
00601 return -1;
00602 }
00603
00604 BOOST_STATIC_ASSERT(sizeof(actuator_info_) == 264);
00605 BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_256_) == (256-4));
00606 BOOST_STATIC_ASSERT( offsetof(WG0XActuatorInfo, crc32_264_) == (264-4));
00607 boost::crc_32_type crc32_256, crc32_264;
00608 crc32_256.process_bytes(&actuator_info_, offsetof(WG0XActuatorInfo, crc32_256_));
00609 crc32_264.process_bytes(&actuator_info_, offsetof(WG0XActuatorInfo, crc32_264_));
00610 if ((actuator_info_.crc32_264_ == crc32_264.checksum()) || (actuator_info_.crc32_256_ == crc32_256.checksum()))
00611 {
00612 if (actuator_info_.major_ != 0 || actuator_info_.minor_ != 2)
00613 {
00614 if (allow_unprogrammed)
00615 ROS_WARN("Unsupported actuator info version (%d.%d != 0.2). Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00616 else
00617 {
00618 ROS_FATAL("Unsupported actuator info version (%d.%d != 0.2). Please reprogram device #%02d", actuator_info_.major_, actuator_info_.minor_, sh_->get_ring_position());
00619 return -1;
00620 }
00621 }
00622
00623 actuator_.name_ = actuator_info_.name_;
00624 ROS_DEBUG(" Name: %s", actuator_info_.name_);
00625
00626 bool isWG021 = sh_->get_product_code() == WG021::PRODUCT_CODE;
00627 if (!isWG021)
00628 {
00629
00630 if (hw && !hw->addActuator(&actuator_))
00631 {
00632 ROS_FATAL("An actuator of the name '%s' already exists. Device #%02d has a duplicate name", actuator_.name_.c_str(), sh_->get_ring_position());
00633 return -1;
00634 }
00635
00636 }
00637
00638
00639 digital_out_.name_ = actuator_info_.name_;
00640 if (hw && !hw->addDigitalOut(&digital_out_))
00641 {
00642 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());
00643 return -1;
00644 }
00645
00646
00647 if (app_ram_status_ == APP_RAM_PRESENT)
00648 {
00649 double zero_offset;
00650 if (readAppRam(&com, zero_offset))
00651 {
00652 ROS_DEBUG("Read calibration from device %s: %f", actuator_info_.name_, zero_offset);
00653 actuator_.state_.zero_offset_ = zero_offset;
00654 cached_zero_offset_ = zero_offset;
00655 calibration_status_ = SAVED_CALIBRATION;
00656 }
00657 else
00658 {
00659 ROS_DEBUG("No calibration offset was stored on device %s", actuator_info_.name_);
00660 }
00661 }
00662 else if (app_ram_status_ == APP_RAM_MISSING)
00663 {
00664 ROS_WARN("Device %s does not support storing calibration offsets", actuator_info_.name_);
00665 }
00666 else if (app_ram_status_ == APP_RAM_NOT_APPLICABLE)
00667 {
00668
00669 }
00670
00671
00672 if (actuator_info_.max_current_ > board_max_current)
00673 {
00674 ROS_WARN("WARNING: Device #%02d : motor current limit (%f) greater than board current limit (%f)",
00675 sh_->get_ring_position(), actuator_info_.max_current_, board_max_current);
00676 }
00677 max_current_ = std::min(board_max_current, actuator_info_.max_current_);
00678 }
00679 else if (allow_unprogrammed)
00680 {
00681 ROS_WARN("WARNING: Device #%02d (%d%05d) is not programmed",
00682 sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00683 actuator_info_.crc32_264_ = 0;
00684 actuator_info_.crc32_256_ = 0;
00685
00686 max_current_ = board_max_current;
00687 }
00688 else
00689 {
00690 ROS_FATAL("Device #%02d (%d%05d) is not programmed, aborting...",
00691 sh_->get_ring_position(), sh_->get_product_code(), sh_->get_serial());
00692 return -1;
00693 }
00694
00695 return 0;
00696 }
00697
00698 #define GET_ATTR(a) \
00699 { \
00700 TiXmlElement *c; \
00701 attr = elt->Attribute((a)); \
00702 if (!attr) { \
00703 c = elt->FirstChildElement((a)); \
00704 if (!c || !(attr = c->GetText())) { \
00705 ROS_FATAL("Actuator is missing the attribute "#a); \
00706 exit(EXIT_FAILURE); \
00707 } \
00708 } \
00709 }
00710
00711 void WG0X::clearErrorFlags(void)
00712 {
00713 has_error_ = false;
00714 too_many_dropped_packets_ = false;
00715 status_checksum_error_ = false;
00716 timestamp_jump_detected_ = false;
00717 if (motor_model_) motor_model_->reset();
00718 }
00719
00720 void WG0X::packCommand(unsigned char *buffer, bool halt, bool reset)
00721 {
00722 pr2_hardware_interface::ActuatorCommand &cmd = actuator_.command_;
00723
00724 if (halt)
00725 {
00726 cmd.effort_ = 0;
00727 }
00728
00729 if (reset)
00730 {
00731 clearErrorFlags();
00732 }
00733 resetting_ = reset;
00734
00735
00736 double zero_offset = actuator_.state_.zero_offset_;
00737 if (zero_offset != cached_zero_offset_)
00738 {
00739 if (tryLockWG0XDiagnostics())
00740 {
00741 ROS_DEBUG("Calibration change of %s, new %f, old %f", actuator_info_.name_, zero_offset, cached_zero_offset_);
00742 cached_zero_offset_ = zero_offset;
00743 wg0x_collect_diagnostics_.zero_offset_ = zero_offset;
00744 calibration_status_ = CONTROLLER_CALIBRATION;
00745 unlockWG0XDiagnostics();
00746 }
00747 else
00748 {
00749
00750 }
00751 }
00752
00753
00754 double current = (cmd.effort_ / actuator_info_.encoder_reduction_) / actuator_info_.motor_torque_constant_ ;
00755 actuator_.state_.last_commanded_effort_ = cmd.effort_;
00756 actuator_.state_.last_commanded_current_ = current;
00757
00758
00759 current = max(min(current, max_current_), -max_current_);
00760
00761
00762 WG0XCommand *c = (WG0XCommand *)buffer;
00763 memset(c, 0, command_size_);
00764 c->programmed_current_ = int(current / config_info_.nominal_current_scale_);
00765 c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00766 c->mode_ |= (reset ? MODE_SAFETY_RESET : 0);
00767 c->digital_out_ = digital_out_.command_.data_;
00768 c->checksum_ = rotateRight8(computeChecksum(c, command_size_ - 1));
00769 }
00770
00771 void WG05::packCommand(unsigned char *buffer, bool halt, bool reset)
00772 {
00773 WG0X::packCommand(buffer, halt, reset);
00774
00775 if (reset)
00776 {
00777 last_num_encoder_errors_ = actuator_.state_.num_encoder_errors_;
00778 }
00779 }
00780
00781 void WG06::packCommand(unsigned char *buffer, bool halt, bool reset)
00782 {
00783 if (reset)
00784 {
00785 pressure_checksum_error_ = false;
00786 }
00787
00788 WG0X::packCommand(buffer, halt, reset);
00789
00790 if (reset)
00791 {
00792 last_num_encoder_errors_ = actuator_.state_.num_encoder_errors_;
00793 }
00794
00795 WG0XCommand *c = (WG0XCommand *)buffer;
00796
00797 if (accelerometer_.command_.range_ > 2 ||
00798 accelerometer_.command_.range_ < 0)
00799 accelerometer_.command_.range_ = 0;
00800
00801 if (accelerometer_.command_.bandwidth_ > 6 ||
00802 accelerometer_.command_.bandwidth_ < 0)
00803 accelerometer_.command_.bandwidth_ = 0;
00804
00805 c->digital_out_ = (digital_out_.command_.data_ != 0) |
00806 ((accelerometer_.command_.bandwidth_ & 0x7) << 1) |
00807 ((accelerometer_.command_.range_ & 0x3) << 4);
00808 c->checksum_ = rotateRight8(computeChecksum(c, command_size_ - 1));
00809 }
00810
00811 void WG021::packCommand(unsigned char *buffer, bool halt, bool reset)
00812 {
00813 pr2_hardware_interface::ProjectorCommand &cmd = projector_.command_;
00814
00815
00816 if (reset)
00817 {
00818 clearErrorFlags();
00819 }
00820 resetting_ = reset;
00821
00822
00823 projector_.state_.last_commanded_current_ = cmd.current_;
00824 cmd.current_ = max(min(cmd.current_, max_current_), 0.0);
00825
00826
00827 WG021Command *c = (WG021Command *)buffer;
00828 memset(c, 0, command_size_);
00829 c->digital_out_ = digital_out_.command_.data_;
00830 c->programmed_current_ = int(cmd.current_ / config_info_.nominal_current_scale_);
00831 c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
00832 c->mode_ |= reset ? MODE_SAFETY_RESET : 0;
00833 c->config0_ = ((cmd.A_ & 0xf) << 4) | ((cmd.B_ & 0xf) << 0);
00834 c->config1_ = ((cmd.I_ & 0xf) << 4) | ((cmd.M_ & 0xf) << 0);
00835 c->config2_ = ((cmd.L1_ & 0xf) << 4) | ((cmd.L0_ & 0xf) << 0);
00836 c->general_config_ = cmd.pulse_replicator_ == true;
00837 c->checksum_ = rotateRight8(computeChecksum(c, command_size_ - 1));
00838 }
00839
00840 bool WG06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00841 {
00842 bool rv = true;
00843
00844 int status_bytes = accel_publisher_ ? sizeof(WG06StatusWithAccel) : sizeof(WG0XStatus);
00845 WG06Pressure *p = (WG06Pressure *)(this_buffer + command_size_ + status_bytes);
00846
00847 unsigned char* this_status = this_buffer + command_size_;
00848 if (!verifyChecksum(this_status, status_bytes))
00849 {
00850 status_checksum_error_ = true;
00851 rv = false;
00852 goto end;
00853 }
00854
00855 if (!verifyChecksum(p, sizeof(*p)))
00856 {
00857 pressure_checksum_error_ = true;
00858 rv = false;
00859 goto end;
00860 }
00861
00862 for (int i = 0; i < 22; ++i ) {
00863 pressure_sensors_[0].state_.data_[i] =
00864 ((p->l_finger_tip_[i] >> 8) & 0xff) |
00865 ((p->l_finger_tip_[i] << 8) & 0xff00);
00866 pressure_sensors_[1].state_.data_[i] =
00867 ((p->r_finger_tip_[i] >> 8) & 0xff) |
00868 ((p->r_finger_tip_[i] << 8) & 0xff00);
00869 }
00870
00871 if (p->timestamp_ != last_pressure_time_)
00872 {
00873 if (pressure_publisher_ && pressure_publisher_->trylock())
00874 {
00875 pressure_publisher_->msg_.header.stamp = ros::Time::now();
00876 pressure_publisher_->msg_.set_l_finger_tip_size(22);
00877 pressure_publisher_->msg_.set_r_finger_tip_size(22);
00878 for (int i = 0; i < 22; ++i ) {
00879 pressure_publisher_->msg_.l_finger_tip[i] = pressure_sensors_[0].state_.data_[i];
00880 pressure_publisher_->msg_.r_finger_tip[i] = pressure_sensors_[1].state_.data_[i];
00881 }
00882 pressure_publisher_->unlockAndPublish();
00883 }
00884 }
00885 last_pressure_time_ = p->timestamp_;
00886
00887
00888 if (accel_publisher_)
00889 {
00890 WG06StatusWithAccel *status = (WG06StatusWithAccel *)(this_buffer + command_size_);
00891 WG06StatusWithAccel *last_status = (WG06StatusWithAccel *)(prev_buffer + command_size_);
00892 int count = uint8_t(status->accel_count_ - last_status->accel_count_);
00893 accelerometer_samples_ += count;
00894
00895
00896
00897 accelerometer_missed_samples_ += (count > 4) ? (count-4) : 0;
00898 count = min(4, count);
00899 accelerometer_.state_.samples_.resize(count);
00900 accelerometer_.state_.frame_id_ = string(actuator_info_.name_) + "_accelerometer_link";
00901 for (int i = 0; i < count; ++i)
00902 {
00903 int32_t acc = status->accel_[count - i - 1];
00904 int range = (acc >> 30) & 3;
00905 float d = 1 << (8 - range);
00906 accelerometer_.state_.samples_[i].x = 9.81 * ((((acc >> 0) & 0x3ff) << 22) >> 22) / d;
00907 accelerometer_.state_.samples_[i].y = 9.81 * ((((acc >> 10) & 0x3ff) << 22) >> 22) / d;
00908 accelerometer_.state_.samples_[i].z = 9.81 * ((((acc >> 20) & 0x3ff) << 22) >> 22) / d;
00909 }
00910
00911 if (accel_publisher_->trylock())
00912 {
00913 accel_publisher_->msg_.header.frame_id = accelerometer_.state_.frame_id_;
00914 accel_publisher_->msg_.header.stamp = ros::Time::now();
00915 accel_publisher_->msg_.set_samples_size(count);
00916 for (int i = 0; i < count; ++i)
00917 {
00918 accel_publisher_->msg_.samples[i].x = accelerometer_.state_.samples_[i].x;
00919 accel_publisher_->msg_.samples[i].y = accelerometer_.state_.samples_[i].y;
00920 accel_publisher_->msg_.samples[i].z = accelerometer_.state_.samples_[i].z;
00921 }
00922 accel_publisher_->unlockAndPublish();
00923 }
00924 }
00925
00926 if (!WG0X::unpackState(this_buffer, prev_buffer))
00927 {
00928 rv = false;
00929 }
00930
00931 end:
00932 return rv;
00933 }
00934
00935 bool WG0X::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00936 {
00937 pr2_hardware_interface::ActuatorState &state = actuator_.state_;
00938 WG0XStatus *this_status, *prev_status;
00939
00940 this_status = (WG0XStatus *)(this_buffer + command_size_);
00941 prev_status = (WG0XStatus *)(prev_buffer + command_size_);
00942
00943 digital_out_.state_.data_ = this_status->digital_out_;
00944
00945
00946
00947
00948 int32_t timediff = WG0X::timestampDiff(this_status->timestamp_, prev_status->timestamp_);
00949 sample_timestamp_ += WG0X::timediffToDuration(timediff);
00950 state.sample_timestamp_ = sample_timestamp_;
00951 state.timestamp_ = sample_timestamp_.toSec();
00952
00953 state.device_id_ = sh_->get_ring_position();
00954
00955 state.encoder_count_ = this_status->encoder_count_;
00956 state.position_ = double(this_status->encoder_count_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI - state.zero_offset_;
00957
00958 state.encoder_velocity_ =
00959 calcEncoderVelocity(this_status->encoder_count_, this_status->timestamp_,
00960 prev_status->encoder_count_, prev_status->timestamp_);
00961 state.velocity_ = state.encoder_velocity_ / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00962
00963 state.calibration_reading_ = this_status->calibration_reading_ & LIMIT_SENSOR_0_STATE;
00964 state.calibration_rising_edge_valid_ = this_status->calibration_reading_ & LIMIT_OFF_TO_ON;
00965 state.calibration_falling_edge_valid_ = this_status->calibration_reading_ & LIMIT_ON_TO_OFF;
00966 state.last_calibration_rising_edge_ = double(this_status->last_calibration_rising_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00967 state.last_calibration_falling_edge_ = double(this_status->last_calibration_falling_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
00968 state.is_enabled_ = bool(this_status->mode_ & MODE_ENABLE);
00969
00970 state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
00971 state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
00972
00973 state.last_executed_effort_ = this_status->programmed_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00974 state.last_measured_effort_ = this_status->measured_current_ * config_info_.nominal_current_scale_ * actuator_info_.motor_torque_constant_ * actuator_info_.encoder_reduction_;
00975
00976 state.num_encoder_errors_ = this_status->num_encoder_errors_;
00977
00978 state.motor_voltage_ = this_status->motor_voltage_ * config_info_.nominal_voltage_scale_;
00979
00980 state.max_effort_ = max_current_ * actuator_info_.encoder_reduction_ * actuator_info_.motor_torque_constant_;
00981
00982 return verifyState(this_status, prev_status);
00983 }
00984
00985
00986 bool WG0X::verifyChecksum(const void* buffer, unsigned size)
00987 {
00988 bool success = computeChecksum(buffer, size) == 0;
00989 if (!success) {
00990 if (tryLockWG0XDiagnostics()) {
00991 ++wg0x_collect_diagnostics_.checksum_errors_;
00992 unlockWG0XDiagnostics();
00993 }
00994 }
00995 return success;
00996 }
00997
00998
00999 bool WG05::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
01000 {
01001 bool rv = true;
01002
01003 unsigned char* this_status = this_buffer + command_size_;
01004 if (!verifyChecksum(this_status, status_size_))
01005 {
01006 status_checksum_error_ = true;
01007 rv = false;
01008 goto end;
01009 }
01010
01011 if (!WG0X::unpackState(this_buffer, prev_buffer))
01012 {
01013 rv = false;
01014 }
01015
01016 end:
01017 return rv;
01018 }
01019
01020
01027 int32_t WG0X::timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp)
01028 {
01029 return int32_t(new_timestamp - old_timestamp);
01030 }
01031
01038 ros::Duration WG0X::timediffToDuration(int32_t timediff_usec)
01039 {
01040 static const int USEC_PER_SEC = 1000000;
01041 int sec = timediff_usec / USEC_PER_SEC;
01042 int nsec = (timediff_usec % USEC_PER_SEC)*1000;
01043 return ros::Duration(sec,nsec);
01044 }
01045
01046
01052 int32_t WG0X::positionDiff(int32_t new_position, int32_t old_position)
01053 {
01054 return int32_t(new_position - old_position);
01055 }
01056
01063 double WG0X::calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp,
01064 int32_t old_position, uint32_t old_timestamp)
01065 {
01066 double timestamp_diff = double(timestampDiff(new_timestamp, old_timestamp)) * 1e-6;
01067 double position_diff = double(positionDiff(new_position, old_position));
01068 return (position_diff / timestamp_diff);
01069 }
01070
01071
01072
01073 bool WG0X::timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount)
01074 {
01075 uint32_t timestamp_diff = (timestamp - last_timestamp);
01076 return (timestamp_diff > amount);
01077 }
01078
01079 bool WG0X::verifyState(WG0XStatus *this_status, WG0XStatus *prev_status)
01080 {
01081 pr2_hardware_interface::ActuatorState &state = actuator_.state_;
01082 bool rv = true;
01083
01084 if (motor_model_ != NULL) {
01085
01086 ethercat_hardware::MotorTraceSample &s(motor_trace_sample_);
01087 double last_executed_current = this_status->programmed_current_ * config_info_.nominal_current_scale_;
01088 double supply_voltage = double(prev_status->supply_voltage_) * config_info_.nominal_voltage_scale_;
01089 double pwm_ratio = double(this_status->programmed_pwm_value_) / double(PWM_MAX);
01090 s.timestamp = state.timestamp_;
01091 s.enabled = state.is_enabled_;
01092 s.supply_voltage = supply_voltage;
01093 s.measured_motor_voltage = state.motor_voltage_;
01094 s.programmed_pwm = pwm_ratio;
01095 s.executed_current = last_executed_current;
01096 s.measured_current = state.last_measured_current_;
01097 s.velocity = state.velocity_;
01098 s.encoder_position = state.position_;
01099 s.encoder_error_count = state.num_encoder_errors_;
01100 motor_model_->sample(s);
01101 motor_model_->checkPublish();
01102 }
01103
01104 max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
01105 max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
01106
01107 if (this_status->timestamp_ == last_timestamp_ ||
01108 this_status->timestamp_ == last_last_timestamp_) {
01109 ++drops_;
01110 ++consecutive_drops_;
01111 max_consecutive_drops_ = max(max_consecutive_drops_, consecutive_drops_);
01112 } else {
01113 consecutive_drops_ = 0;
01114 }
01115
01116 if ( timestamp_jump(this_status->timestamp_,last_timestamp_,10000000) )
01117 {
01118 timestamp_jump_detected_ = true;
01119 }
01120 last_last_timestamp_ = last_timestamp_;
01121 last_timestamp_ = this_status->timestamp_;
01122
01123 if (consecutive_drops_ > 10)
01124 {
01125 too_many_dropped_packets_ = true;
01126 rv = false;
01127 goto end;
01128 }
01129
01130 in_lockout_ = bool(this_status->mode_ & MODE_SAFETY_LOCKOUT);
01131 if (in_lockout_ && !resetting_)
01132 {
01133 rv = false;
01134 goto end;
01135 }
01136
01137 if (fpga_internal_reset_detected_)
01138 {
01139 rv = false;
01140 goto end;
01141 }
01142
01143 if (state.is_enabled_ && motor_model_)
01144 {
01145 if (!disable_motor_model_checking_)
01146 {
01147 if(!motor_model_->verify())
01148 {
01149
01150 rv = false;
01151 goto end;
01152 }
01153 }
01154 }
01155
01156 end:
01157 if (motor_model_)
01158 {
01159
01160
01161
01162 bool new_error = in_lockout_ && !resetting_ && !has_error_;
01163 if (new_error || publish_motor_trace_.command_.data_)
01164 {
01165 const char* reason = (new_error) ? "Safety Lockout" : "Publishing manually triggered";
01166 int level = (new_error) ? 2 : 0;
01167 motor_model_->flagPublish(reason, level , 100);
01168 publish_motor_trace_.command_.data_ = 0;
01169 }
01170 }
01171 bool is_error = !rv;
01172 has_error_ = is_error || has_error_;
01173 actuator_.state_.halted_ = has_error_ || this_status->mode_ == MODE_OFF;
01174 return rv;
01175 }
01176
01177 bool WG0X::publishTrace(const string &reason, unsigned level, unsigned delay)
01178 {
01179 if (motor_model_)
01180 {
01181 motor_model_->flagPublish(reason, level, delay);
01182 return true;
01183 }
01184 return false;
01185 }
01186
01187 bool WG021::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
01188 {
01189 bool rv = true;
01190
01191 pr2_hardware_interface::ProjectorState &state = projector_.state_;
01192 WG021Status *this_status, *prev_status;
01193 this_status = (WG021Status *)(this_buffer + command_size_);
01194 prev_status = (WG021Status *)(prev_buffer + command_size_);
01195
01196 if (!verifyChecksum(this_status, status_size_))
01197 {
01198 status_checksum_error_ = true;
01199 rv = false;
01200 goto end;
01201 }
01202
01203 digital_out_.state_.data_ = this_status->digital_out_;
01204
01205 state.timestamp_us_ = this_status->timestamp_;
01206 state.falling_timestamp_us_ = this_status->output_stop_timestamp_;
01207 state.rising_timestamp_us_ = this_status->output_start_timestamp_;
01208
01209 state.output_ = (this_status->output_status_ & 0x1) == 0x1;
01210 state.falling_timestamp_valid_ = (this_status->output_status_ & 0x8) == 0x8;
01211 state.rising_timestamp_valid_ = (this_status->output_status_ & 0x4) == 0x4;
01212
01213 state.A_ = ((this_status->config0_ >> 4) & 0xf);
01214 state.B_ = ((this_status->config0_ >> 0) & 0xf);
01215 state.I_ = ((this_status->config1_ >> 4) & 0xf);
01216 state.M_ = ((this_status->config1_ >> 0) & 0xf);
01217 state.L1_ = ((this_status->config2_ >> 4) & 0xf);
01218 state.L0_ = ((this_status->config2_ >> 0) & 0xf);
01219 state.pulse_replicator_ = (this_status->general_config_ & 0x1) == 0x1;
01220
01221 state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
01222 state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
01223
01224 state.max_current_ = max_current_;
01225
01226 max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
01227 max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
01228
01229 if (!verifyState((WG0XStatus *)(this_buffer + command_size_), (WG0XStatus *)(prev_buffer + command_size_)))
01230 {
01231 rv = false;
01232 }
01233
01234 end:
01235 return rv;
01236 }
01237
01238
01239 void WG0X::collectDiagnostics(EthercatCom *com)
01240 {
01241
01242 bool success = false;
01243
01244
01245 EthercatDevice::collectDiagnostics(com);
01246
01247
01248
01249 {
01250 EC_Logic *logic = EC_Logic::instance();
01251 unsigned char buf[1];
01252 EC_UINT address = 0x0000;
01253 NPRD_Telegram nprd_telegram(logic->get_idx(),
01254 sh_->get_station_address(),
01255 address,
01256 0 ,
01257 sizeof(buf),
01258 buf);
01259 EC_Ethernet_Frame frame(&nprd_telegram);
01260 if (!com->txandrx_once(&frame)) {
01261
01262 goto end;
01263 }
01264 if (nprd_telegram.get_wkc() != 1) {
01265
01266 goto end;
01267 }
01268 }
01269
01270 WG0XSafetyDisableStatus s;
01271 if (readMailbox(com, s.BASE_ADDR, &s, sizeof(s)) != 0) {
01272 goto end;
01273 }
01274
01275 WG0XDiagnosticsInfo di;
01276 if (readMailbox(com, di.BASE_ADDR, &di, sizeof(di)) != 0) {
01277 goto end;
01278 }
01279
01280 {
01281 WG0XDiagnostics &dg(wg0x_collect_diagnostics_);
01282
01283 if ((app_ram_status_ == APP_RAM_PRESENT) && (dg.zero_offset_ != dg.cached_zero_offset_))
01284 {
01285 if (writeAppRam(com, dg.zero_offset_)){
01286 ROS_DEBUG("Writing new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
01287 dg.cached_zero_offset_ = dg.zero_offset_;
01288 }
01289 else{
01290 ROS_ERROR("Failed to write new calibration to device %s, new %f, old %f", actuator_info_.name_, dg.zero_offset_, dg.cached_zero_offset_);
01291
01292 }
01293 }
01294 }
01295
01296 success = true;
01297
01298 end:
01299 if (!lockWG0XDiagnostics()) {
01300 wg0x_collect_diagnostics_.valid_ = false;
01301 wg0x_collect_diagnostics_.first_ = false;
01302 return;
01303 }
01304
01305 wg0x_collect_diagnostics_.valid_ = success;
01306 if (success) {
01307 wg0x_collect_diagnostics_.update(s,di);
01308 }
01309
01310 unlockWG0XDiagnostics();
01311 }
01312
01313
01314 bool WG0X::writeAppRam(EthercatCom *com, double zero_offset)
01315 {
01316 WG0XUserConfigRam cfg;
01317 cfg.version_ = 1;
01318 cfg.zero_offset_ = zero_offset;
01319 boost::crc_32_type crc32;
01320 crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
01321 cfg.crc32_ = crc32.checksum();
01322 return (writeMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0);
01323 }
01324
01325 bool WG0X::readAppRam(EthercatCom *com, double &zero_offset)
01326 {
01327 WG0XUserConfigRam cfg;
01328 if (!readMailbox(com, WG0XUserConfigRam::BASE_ADDR, &cfg, sizeof(cfg)) == 0)
01329 {
01330 return false;
01331 }
01332 if (cfg.version_ != 1)
01333 {
01334 return false;
01335 }
01336 boost::crc_32_type crc32;
01337 crc32.process_bytes(&cfg, sizeof(cfg)-sizeof(cfg.crc32_));
01338 if (cfg.crc32_ != crc32.checksum()) {
01339 return false;
01340 }
01341 zero_offset = cfg.zero_offset_;
01342 return true;
01343 }
01344
01345 int WG0X::sendSpiCommand(EthercatCom *com, WG0XSpiEepromCmd const * cmd)
01346 {
01347
01348 if (writeMailbox(com, WG0XSpiEepromCmd::SPI_COMMAND_ADDR, cmd, sizeof(*cmd)))
01349 {
01350 fprintf(stderr, "ERROR WRITING EEPROM COMMAND\n");
01351 return -1;
01352 }
01353
01354 for (int tries = 0; tries < 10; ++tries)
01355 {
01356 WG0XSpiEepromCmd stat;
01357 if (readMailbox(com, WG0XSpiEepromCmd::SPI_COMMAND_ADDR, &stat, sizeof(stat)))
01358 {
01359 fprintf(stderr, "ERROR READING EEPROM BUSY STATUS\n");
01360 return -1;
01361 }
01362
01363 if (stat.operation_ != cmd->operation_)
01364 {
01365 fprintf(stderr, "READBACK OF OPERATION INVALID : got 0x%X, expected 0x%X\n", stat.operation_, cmd->operation_);
01366 return -1;
01367 }
01368
01369
01370 if (!stat.busy_)
01371 {
01372 return 0;
01373 }
01374
01375 fprintf(stderr, "eeprom busy reading again, waiting...\n");
01376 usleep(100);
01377 }
01378
01379 fprintf(stderr, "ERROR : EEPROM READING BUSY AFTER 10 TRIES\n");
01380 return -1;
01381 }
01382
01383 int WG0X::readEeprom(EthercatCom *com)
01384 {
01385 BOOST_STATIC_ASSERT(sizeof(actuator_info_) == 264);
01386
01387
01388
01389
01390 memset(&actuator_info_,0,sizeof(actuator_info_));
01391 if (writeMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, &actuator_info_, sizeof(actuator_info_)))
01392 {
01393 fprintf(stderr, "ERROR ZEROING EEPROM PAGE DATA\n");
01394 return -1;
01395 }
01396
01397 WG0XSpiEepromCmd cmd;
01398 memset(&cmd,0,sizeof(cmd));
01399 cmd.build_read(ACTUATOR_INFO_PAGE);
01400 if (sendSpiCommand(com, &cmd)) {
01401 fprintf(stderr, "ERROR SENDING SPI EEPROM READ COMMAND\n");
01402 return -1;
01403 }
01404
01405 if (readMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, &actuator_info_, sizeof(actuator_info_))) {
01406 fprintf(stderr, "ERROR READING BUFFERED EEPROM PAGE DATA\n");
01407 return -1;
01408 }
01409
01410 return 0;
01411
01412 }
01413
01414 void WG0X::program(WG0XActuatorInfo *info)
01415 {
01416 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
01417
01418 writeMailbox(&com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, info, sizeof(WG0XActuatorInfo));
01419 WG0XSpiEepromCmd cmd;
01420 cmd.build_write(ACTUATOR_INFO_PAGE);
01421 if (sendSpiCommand(&com, &cmd)) {
01422 fprintf(stderr, "ERROR SENDING SPI EEPROM WRITE COMMAND\n");
01423 }
01424
01425 char data[2];
01426 memset(data, 0, sizeof(data));
01427 data[0] = 0xD7;
01428
01429 if (writeMailbox(&com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, sizeof(data))) {
01430 fprintf(stderr, "ERROR WRITING EEPROM COMMAND BUFFER\n");
01431 }
01432
01433
01434 {
01435 WG0XSpiEepromCmd cmd;
01436 cmd.build_arbitrary(sizeof(data));
01437 if (sendSpiCommand(&com, &cmd)) {
01438 fprintf(stderr, "reading eeprom status failed");
01439 }
01440 }
01441
01442
01443 if (readMailbox(&com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, sizeof(data))) {
01444 fprintf(stderr, "ERROR READING EEPROM COMMAND BUFFER\n");
01445 }
01446 }
01447
01455 int timediff_ms(const timespec ¤t, const timespec &start)
01456 {
01457 int timediff_ms = (current.tv_sec-start.tv_sec)*1000
01458 + (current.tv_nsec-start.tv_nsec)/1000000;
01459 return timediff_ms;
01460 }
01461
01462
01470 int safe_clock_gettime(clockid_t clk_id, timespec *time)
01471 {
01472 int result = clock_gettime(clk_id, time);
01473 if (result != 0) {
01474 int error = errno;
01475 fprintf(stderr, "safe_clock_gettime : %s\n", strerror(error));
01476 return result;
01477 }
01478 return result;
01479 }
01480
01481
01489 void safe_usleep(uint32_t usec)
01490 {
01491 assert(usec<1000000);
01492 if (usec>1000000)
01493 usec=1000000;
01494 struct timespec req, rem;
01495 req.tv_sec = 0;
01496 req.tv_nsec = usec*1000;
01497 while (nanosleep(&req, &rem)!=0) {
01498 int error = errno;
01499 fprintf(stderr,"%s : Error : %s\n", __func__, strerror(error));
01500 if (error != EINTR) {
01501 break;
01502 }
01503 req = rem;
01504 }
01505 return;
01506 }
01507
01508
01509 unsigned SyncMan::baseAddress(unsigned num)
01510 {
01511 assert(num < 8);
01512 return BASE_ADDR + 8 * num;
01513 }
01514
01515
01525 bool SyncMan::readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EthercatDevice::AddrMode addrMode, unsigned num)
01526 {
01527 return ( EthercatDevice::readData(com, sh, baseAddress(num), this, sizeof(*this), addrMode) == 0);
01528 }
01529
01530
01531 unsigned SyncManActivate::baseAddress(unsigned num)
01532 {
01533 assert(num < 8);
01534 return BASE_ADDR + 8 * num;
01535 }
01536
01546 bool SyncManActivate::writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EthercatDevice::AddrMode addrMode, unsigned num) const
01547 {
01548 return ( EthercatDevice::writeData(com, sh, baseAddress(num), this, sizeof(*this), addrMode) == 0);
01549 }
01550
01551
01552 void updateIndexAndWkc(EC_Telegram *tg, EC_Logic *logic)
01553 {
01554 tg->set_idx(logic->get_idx());
01555 tg->set_wkc(logic->get_wkc());
01556 }
01557
01558
01559 bool WG0X::verifyDeviceStateForMailboxOperation()
01560 {
01561
01562 EC_State state = sh_->get_state();
01563 if ((state != EC_SAFEOP_STATE) && (state != EC_OP_STATE)) {
01564 fprintf(stderr, "%s : " ERROR_HDR
01565 "cannot do mailbox read in current device state = %d\n", __func__, state);
01566 return false;
01567 }
01568 return true;
01569 }
01570
01571
01581 void WG0X::diagnoseMailboxError(EthercatCom *com)
01582 {
01583
01584 }
01585
01594 bool WG0X::clearReadMailbox(EthercatCom *com)
01595 {
01596 if (!verifyDeviceStateForMailboxOperation()){
01597 return false;
01598 }
01599
01600 EC_Logic *logic = EC_Logic::instance();
01601 EC_UINT station_addr = sh_->get_station_address();
01602
01603
01604
01605
01606 unsigned char unused[1] = {0};
01607 NPRD_Telegram read_start(
01608 logic->get_idx(),
01609 station_addr,
01610 MBX_STATUS_PHY_ADDR,
01611 logic->get_wkc(),
01612 sizeof(unused),
01613 unused);
01614 NPRD_Telegram read_end(
01615 logic->get_idx(),
01616 station_addr,
01617 MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
01618 logic->get_wkc(),
01619 sizeof(unused),
01620 unused);
01621 read_start.attach(&read_end);
01622 EC_Ethernet_Frame frame(&read_start);
01623
01624
01625
01626 bool success=false;
01627 static const unsigned MAX_DROPS = 15;
01628 for (unsigned tries=0; tries<MAX_DROPS; ++tries) {
01629 success = com->txandrx_once(&frame);
01630 if (success) {
01631 break;
01632 }
01633 updateIndexAndWkc(&read_start, logic);
01634 updateIndexAndWkc(&read_end , logic);
01635 }
01636
01637 if (!success) {
01638 fprintf(stderr, "%s : " ERROR_HDR
01639 " too much packet loss\n", __func__);
01640 safe_usleep(100);
01641 return false;
01642 }
01643
01644
01645 if (read_start.get_wkc() != read_end.get_wkc()) {
01646 fprintf(stderr, "%s : " ERROR_HDR
01647 "read mbx working counters are inconsistant, %d, %d\n",
01648 __func__, read_start.get_wkc(), read_end.get_wkc());
01649 return false;
01650 }
01651 if (read_start.get_wkc() > 1) {
01652 fprintf(stderr, "%s : " ERROR_HDR
01653 "more than one device (%d) responded \n", __func__, read_start.get_wkc());
01654 return false;
01655 }
01656 if (read_start.get_wkc() == 1) {
01657 fprintf(stderr, "%s : " WARN_MODE "WARN" STD_MODE
01658 " read mbx contained garbage data\n", __func__);
01659
01660 }
01661
01662 return true;
01663 }
01664
01665
01666
01676 bool WG0X::waitForReadMailboxReady(EthercatCom *com)
01677 {
01678
01679 static const int MAX_WAIT_TIME_MS = 100;
01680 int timediff;
01681 unsigned good_results=0;
01682
01683
01684 struct timespec start_time, current_time;
01685 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
01686 return false;
01687 }
01688
01689 do {
01690
01691 uint8_t SyncManStatus=0;
01692 const unsigned SyncManAddr = 0x805+(MBX_STATUS_SYNCMAN_NUM*8);
01693 if (readData(com, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), FIXED_ADDR) == 0) {
01694 ++good_results;
01695 const uint8_t MailboxStatusMask = (1<<3);
01696 if (SyncManStatus & MailboxStatusMask) {
01697 return true;
01698 }
01699 }
01700 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
01701 return false;
01702 }
01703 timediff = timediff_ms(current_time, start_time);
01704 safe_usleep(100);
01705 } while (timediff < MAX_WAIT_TIME_MS);
01706
01707 if (good_results == 0) {
01708 fprintf(stderr, "%s : " ERROR_HDR
01709 " error reading from device\n", __func__);
01710 } else {
01711 fprintf(stderr, "%s : " ERROR_HDR
01712 " error read mbx not full after %d ms\n", __func__, timediff);
01713 }
01714
01715 return false;
01716 }
01717
01718
01728 bool WG0X::waitForWriteMailboxReady(EthercatCom *com)
01729 {
01730
01731 static const int MAX_WAIT_TIME_MS = 100;
01732 int timediff;
01733 unsigned good_results=0;
01734
01735
01736 struct timespec start_time, current_time;
01737 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
01738 return false;
01739 }
01740
01741 do {
01742
01743 uint8_t SyncManStatus=0;
01744 const unsigned SyncManAddr = 0x805+(MBX_COMMAND_SYNCMAN_NUM*8);
01745 if (readData(com, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), FIXED_ADDR) == 0) {
01746 ++good_results;
01747 const uint8_t MailboxStatusMask = (1<<3);
01748 if ( !(SyncManStatus & MailboxStatusMask) ) {
01749 return true;
01750 }
01751 }
01752 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
01753 return false;
01754 }
01755 timediff = timediff_ms(current_time, start_time);
01756 safe_usleep(100);
01757 } while (timediff < MAX_WAIT_TIME_MS);
01758
01759 if (good_results == 0) {
01760 fprintf(stderr, "%s : " ERROR_HDR
01761 " error reading from device\n", __func__);
01762 } else {
01763 fprintf(stderr, "%s : " ERROR_HDR
01764 " error write mbx not empty after %d ms\n", __func__, timediff);
01765 }
01766
01767 return false;
01768 }
01769
01770
01771
01783 bool WG0X::writeMailboxInternal(EthercatCom *com, void const *data, unsigned length)
01784 {
01785 if (length > MBX_COMMAND_SIZE) {
01786 assert(length <= MBX_COMMAND_SIZE);
01787 return false;
01788 }
01789
01790
01791 if (!verifyDeviceStateForMailboxOperation()){
01792 return false;
01793 }
01794
01795 EC_Logic *logic = EC_Logic::instance();
01796 EC_UINT station_addr = sh_->get_station_address();
01797
01798
01799
01800
01801
01802 static const unsigned TELEGRAM_OVERHEAD = 50;
01803 bool split_write = (length+TELEGRAM_OVERHEAD) < MBX_COMMAND_SIZE;
01804
01805 unsigned write_length = MBX_COMMAND_SIZE;
01806 if (split_write) {
01807 write_length = length;
01808 }
01809
01810
01811
01812
01813 {
01814
01815 unsigned char unused[1] = {0};
01816 NPWR_Telegram write_start(
01817 logic->get_idx(),
01818 station_addr,
01819 MBX_COMMAND_PHY_ADDR,
01820 logic->get_wkc(),
01821 write_length,
01822 (const unsigned char*) data);
01823 NPWR_Telegram write_end(
01824 logic->get_idx(),
01825 station_addr,
01826 MBX_COMMAND_PHY_ADDR+MBX_COMMAND_SIZE-1,
01827 logic->get_wkc(),
01828 sizeof(unused),
01829 unused);
01830
01831 if (split_write) {
01832 write_start.attach(&write_end);
01833 }
01834
01835 EC_Ethernet_Frame frame(&write_start);
01836
01837
01838 unsigned sends=0;
01839 bool success=false;
01840 for (unsigned tries=0; (tries<10) && !success; ++tries) {
01841 success = com->txandrx_once(&frame);
01842 if (!success) {
01843 updateIndexAndWkc(&write_start, logic);
01844 updateIndexAndWkc(&write_end, logic);
01845 }
01846 ++sends;
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858 }
01859 if (!success) {
01860 fprintf(stderr, "%s : " ERROR_HDR
01861 " too much packet loss\n", __func__);
01862 safe_usleep(100);
01863 return false;
01864 }
01865
01866 if (split_write && (write_start.get_wkc() != write_end.get_wkc())) {
01867 fprintf(stderr, "%s : " ERROR_HDR
01868 " write mbx working counters are inconsistant\n", __func__);
01869 return false;
01870 }
01871
01872 if (write_start.get_wkc() > 1)
01873 {
01874 fprintf(stderr, "%s : " ERROR_HDR
01875 " multiple (%d) devices responded to mailbox write\n", __func__, write_start.get_wkc());
01876 return false;
01877 }
01878 else if (write_start.get_wkc() != 1)
01879 {
01880
01881 if (sends<=1) {
01882
01883 fprintf(stderr, "%s : " ERROR_HDR
01884 " initial mailbox write refused\n", __func__);
01885 safe_usleep(100);
01886 return false;
01887 } else {
01888
01889
01890 fprintf(stderr, "%s : " WARN_HDR
01891 " repeated mailbox write refused\n", __func__);
01892 }
01893 }
01894 }
01895
01896 return true;
01897 }
01898
01899 bool WG0X::readMailboxRepeatRequest(EthercatCom *com)
01900 {
01901 bool success = _readMailboxRepeatRequest(com);
01902 ++mailbox_diagnostics_.retries_;
01903 if (!success) {
01904 ++mailbox_diagnostics_.retry_errors_;
01905 }
01906 return success;
01907 }
01908
01909 bool WG0X::_readMailboxRepeatRequest(EthercatCom *com)
01910 {
01911
01912
01913 SyncMan sm;
01914 if (!sm.readData(com, sh_, FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
01915 fprintf(stderr, "%s : " ERROR_HDR
01916 " could not read status mailbox syncman (1)\n", __func__);
01917 return false;
01918 }
01919
01920
01921 if (sm.activate.repeat_request != sm.pdi_control.repeat_ack) {
01922 fprintf(stderr, "%s : " ERROR_HDR
01923 " syncman repeat request and ack do not match\n", __func__);
01924 return false;
01925 }
01926
01927
01928 SyncManActivate orig_activate(sm.activate);
01929 sm.activate.repeat_request = ~orig_activate.repeat_request;
01930 if (!sm.activate.writeData(com, sh_, FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
01931 fprintf(stderr, "%s : " ERROR_HDR
01932 " could not write syncman repeat request\n", __func__);
01933
01934 return false;
01935 }
01936
01937
01938 static const int MAX_WAIT_TIME_MS = 100;
01939 int timediff;
01940
01941 struct timespec start_time, current_time;
01942 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
01943 return false;
01944 }
01945
01946 do {
01947 if (!sm.readData(com, sh_, FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
01948 fprintf(stderr, "%s : " ERROR_HDR
01949 " could not read status mailbox syncman (2)\n", __func__);
01950 return false;
01951 }
01952
01953 if (sm.activate.repeat_request == sm.pdi_control.repeat_ack) {
01954
01955 if (sm.status.mailbox_status != 1) {
01956 fprintf(stderr, "%s : " ERROR_HDR
01957 " got repeat response, but read mailbox is still empty\n", __func__);
01958
01959 return false;
01960 }
01961 return true;
01962 }
01963
01964 if ( (sm.activate.repeat_request) == (orig_activate.repeat_request) ) {
01965 fprintf(stderr, "%s : " ERROR_HDR
01966 " syncman repeat request was changed while waiting for response\n", __func__);
01967
01968
01969 return false;
01970 }
01971
01972 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
01973 return false;
01974 }
01975
01976 timediff = timediff_ms(current_time, start_time);
01977 safe_usleep(100);
01978 } while (timediff < MAX_WAIT_TIME_MS);
01979
01980 fprintf(stderr, "%s : " ERROR_HDR
01981 " error repeat request not acknowledged after %d ms\n", __func__, timediff);
01982 return false;
01983 }
01984
01985
01986
01998 bool WG0X::readMailboxInternal(EthercatCom *com, void *data, unsigned length)
01999 {
02000 static const unsigned MAX_TRIES = 10;
02001 static const unsigned MAX_DROPPED = 10;
02002
02003 if (length > MBX_STATUS_SIZE) {
02004 assert(length <= MBX_STATUS_SIZE);
02005 return false;
02006 }
02007
02008
02009 if (!verifyDeviceStateForMailboxOperation()){
02010 return false;
02011 }
02012
02013 EC_Logic *logic = EC_Logic::instance();
02014 EC_UINT station_addr = sh_->get_station_address();
02015
02016
02017
02018
02019
02020 static const unsigned TELEGRAM_OVERHEAD = 50;
02021 bool split_read = (length+TELEGRAM_OVERHEAD) < MBX_STATUS_SIZE;
02022
02023 unsigned read_length = MBX_STATUS_SIZE;
02024 if (split_read) {
02025 read_length = length;
02026 }
02027
02028 unsigned char unused[1] = {0};
02029 NPRD_Telegram read_start(
02030 logic->get_idx(),
02031 station_addr,
02032 MBX_STATUS_PHY_ADDR,
02033 logic->get_wkc(),
02034 read_length,
02035 (unsigned char*) data);
02036 NPRD_Telegram read_end(
02037 logic->get_idx(),
02038 station_addr,
02039 MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
02040 logic->get_wkc(),
02041 sizeof(unused),
02042 unused);
02043
02044 if (split_read) {
02045 read_start.attach(&read_end);
02046 }
02047
02048 EC_Ethernet_Frame frame(&read_start);
02049
02050 unsigned tries = 0;
02051 unsigned total_dropped =0;
02052 for (tries=0; tries<MAX_TRIES; ++tries) {
02053
02054
02055 unsigned dropped=0;
02056 for (dropped=0; dropped<MAX_DROPPED; ++dropped) {
02057 if (com->txandrx_once(&frame)) {
02058 break;
02059 }
02060 ++total_dropped;
02061 updateIndexAndWkc(&read_start , logic);
02062 updateIndexAndWkc(&read_end , logic);
02063 }
02064
02065 if (dropped>=MAX_DROPPED) {
02066 fprintf(stderr, "%s : " ERROR_HDR
02067 " too many dropped packets : %d\n", __func__, dropped);
02068 }
02069
02070 if (split_read && (read_start.get_wkc() != read_end.get_wkc())) {
02071 fprintf(stderr, "%s : " ERROR_HDR
02072 "read mbx working counters are inconsistant\n", __func__);
02073 return false;
02074 }
02075
02076 if (read_start.get_wkc() == 0) {
02077 if (dropped == 0) {
02078 fprintf(stderr, "%s : " ERROR_HDR
02079 " inconsistancy : got wkc=%d with no dropped packets\n",
02080 __func__, read_start.get_wkc());
02081 fprintf(stderr, "total dropped = %d\n", total_dropped);
02082 return false;
02083 } else {
02084
02085
02086 fprintf(stderr, "%s : " WARN_HDR
02087 " asking for read repeat after dropping %d packets\n", __func__, dropped);
02088 if (!readMailboxRepeatRequest(com)) {
02089 return false;
02090 }
02091 continue;
02092 }
02093 } else if (read_start.get_wkc() == 1) {
02094
02095 break;
02096 } else {
02097 fprintf(stderr, "%s : " ERROR_HDR
02098 " invalid wkc for read : %d\n", __func__, read_start.get_wkc());
02099 diagnoseMailboxError(com);
02100 return false;
02101 }
02102 }
02103
02104 if (tries >= MAX_TRIES) {
02105 fprintf(stderr, "%s : " ERROR_HDR
02106 " could not get responce from device after %d retries, %d total dropped packets\n",
02107 __func__, tries, total_dropped);
02108 diagnoseMailboxError(com);
02109 return false;
02110 }
02111
02112 return true;
02113 }
02114
02115
02129 int WG0X::readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
02130 {
02131 if (!lockMailbox())
02132 return -1;
02133
02134 int result = readMailbox_(com, address, data, length);
02135 if (result != 0) {
02136 ++mailbox_diagnostics_.read_errors_;
02137 }
02138
02139 unlockMailbox();
02140 return result;
02141 }
02142
02148 int WG0X::readMailbox_(EthercatCom *com, unsigned address, void *data, unsigned length)
02149 {
02150
02151 if (!verifyDeviceStateForMailboxOperation()){
02152 return false;
02153 }
02154
02155
02156 if (!clearReadMailbox(com))
02157 {
02158 fprintf(stderr, "%s : " ERROR_HDR
02159 " clearing read mbx\n", __func__);
02160 return -1;
02161 }
02162
02163
02164 {
02165 WG0XMbxCmd cmd;
02166 if (!cmd.build(address, length, LOCAL_BUS_READ, sh_->get_mbx_counter(), data))
02167 {
02168 fprintf(stderr, "%s : " ERROR_HDR
02169 " builing mbx header\n", __func__);
02170 return -1;
02171 }
02172
02173 if (!writeMailboxInternal(com, &cmd.hdr_, sizeof(cmd.hdr_)))
02174 {
02175 fprintf(stderr, "%s : " ERROR_HDR " write of cmd failed\n", __func__);
02176 return -1;
02177 }
02178 }
02179
02180
02181 if (!waitForReadMailboxReady(com))
02182 {
02183 fprintf(stderr, "%s : " ERROR_HDR
02184 "waiting for read mailbox\n", __func__);
02185 return -1;
02186 }
02187
02188
02189
02190
02191
02192
02193
02194
02195
02196
02197
02198 {
02199 WG0XMbxCmd stat;
02200 memset(&stat,0,sizeof(stat));
02201
02202 if (!readMailboxInternal(com, &stat, length+1))
02203 {
02204 fprintf(stderr, "%s : " ERROR_HDR " read failed\n", __func__);
02205 return -1;
02206 }
02207
02208 if (computeChecksum(&stat, length+1) != 0)
02209 {
02210 fprintf(stderr, "%s : " ERROR_HDR
02211 "checksum error reading mailbox data\n", __func__);
02212 fprintf(stderr, "length = %d\n", length);
02213 return -1;
02214 }
02215 memcpy(data, &stat, length);
02216 }
02217
02218 return 0;
02219
02220
02221 }
02222
02223 bool WG0X::lockMailbox()
02224 {
02225 int error = pthread_mutex_lock(&mailbox_lock_);
02226 if (error != 0) {
02227 fprintf(stderr, "%s : " ERROR_HDR " getting mbx lock\n", __func__);
02228 ++mailbox_diagnostics_.lock_errors_;
02229 return false;
02230 }
02231 return true;
02232 }
02233
02234 void WG0X::unlockMailbox()
02235 {
02236 int error = pthread_mutex_unlock(&mailbox_lock_);
02237 if (error != 0) {
02238 fprintf(stderr, "%s : " ERROR_HDR " freeing mbx lock\n", __func__);
02239 ++mailbox_diagnostics_.lock_errors_;
02240 }
02241 }
02242
02243 bool WG0X::lockWG0XDiagnostics()
02244 {
02245 int error = pthread_mutex_lock(&wg0x_diagnostics_lock_);
02246 if (error != 0) {
02247 fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
02248
02249 ++wg0x_collect_diagnostics_.lock_errors_;
02250 return false;
02251 }
02252 return true;
02253 }
02254
02255 bool WG0X::tryLockWG0XDiagnostics()
02256 {
02257 int error = pthread_mutex_trylock(&wg0x_diagnostics_lock_);
02258 if (error == EBUSY) {
02259 return false;
02260 }
02261 else if (error != 0) {
02262 fprintf(stderr, "%s : " ERROR_HDR " getting diagnostics lock\n", __func__);
02263
02264 ++wg0x_collect_diagnostics_.lock_errors_;
02265 return false;
02266 }
02267 return true;
02268 }
02269
02270 void WG0X::unlockWG0XDiagnostics()
02271 {
02272 int error = pthread_mutex_unlock(&wg0x_diagnostics_lock_);
02273 if (error != 0) {
02274 fprintf(stderr, "%s : " ERROR_HDR " freeing diagnostics lock\n", __func__);
02275 ++wg0x_collect_diagnostics_.lock_errors_;
02276 }
02277 }
02278
02279
02292 int WG0X::writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
02293 {
02294 if (!lockMailbox())
02295 return -1;
02296
02297 int result = writeMailbox_(com, address, data, length);
02298 if (result != 0) {
02299 ++mailbox_diagnostics_.write_errors_;
02300 }
02301
02302 unlockMailbox();
02303
02304 return result;
02305 }
02306
02312 int WG0X::writeMailbox_(EthercatCom *com, unsigned address, void const *data, unsigned length)
02313 {
02314
02315 if (!verifyDeviceStateForMailboxOperation()){
02316 return -1;
02317 }
02318
02319
02320 {
02321 WG0XMbxCmd cmd;
02322 if (!cmd.build(address, length, LOCAL_BUS_WRITE, sh_->get_mbx_counter(), data)) {
02323 fprintf(stderr, "%s : " ERROR_HDR " builing mbx header\n", __func__);
02324 return -1;
02325 }
02326
02327 unsigned write_length = sizeof(cmd.hdr_)+length+sizeof(cmd.checksum_);
02328 if (!writeMailboxInternal(com, &cmd, write_length)) {
02329 fprintf(stderr, "%s : " ERROR_HDR " write failed\n", __func__);
02330 diagnoseMailboxError(com);
02331 return -1;
02332 }
02333 }
02334
02335
02336
02337 if (!waitForWriteMailboxReady(com)) {
02338 fprintf(stderr, "%s : " ERROR_HDR
02339 "write mailbox\n", __func__);
02340 }
02341
02342 return 0;
02343 }
02344
02345
02346 #define CHECK_SAFETY_BIT(bit) \
02347 do { if (status & SAFETY_##bit) { \
02348 str += prefix + #bit; \
02349 prefix = ", "; \
02350 } } while(0)
02351
02352 string WG0X::safetyDisableString(uint8_t status)
02353 {
02354 string str, prefix;
02355
02356 if (status & SAFETY_DISABLED)
02357 {
02358 CHECK_SAFETY_BIT(DISABLED);
02359 CHECK_SAFETY_BIT(UNDERVOLTAGE);
02360 CHECK_SAFETY_BIT(OVER_CURRENT);
02361 CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
02362 CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
02363 CHECK_SAFETY_BIT(OPERATIONAL);
02364 CHECK_SAFETY_BIT(WATCHDOG);
02365 }
02366 else
02367 str = "ENABLED";
02368
02369 return str;
02370 }
02371
02372 string WG0X::modeString(uint8_t mode)
02373 {
02374 string str, prefix;
02375 if (mode) {
02376 if (mode & MODE_ENABLE) {
02377 str += prefix + "ENABLE";
02378 prefix = ", ";
02379 }
02380 if (mode & MODE_CURRENT) {
02381 str += prefix + "CURRENT";
02382 prefix = ", ";
02383 }
02384 if (mode & MODE_UNDERVOLTAGE) {
02385 str += prefix + "UNDERVOLTAGE";
02386 prefix = ", ";
02387 }
02388 if (mode & MODE_SAFETY_RESET) {
02389 str += prefix + "SAFETY_RESET";
02390 prefix = ", ";
02391 }
02392 if (mode & MODE_SAFETY_LOCKOUT) {
02393 str += prefix + "SAFETY_LOCKOUT";
02394 prefix = ", ";
02395 }
02396 if (mode & MODE_RESET) {
02397 str += prefix + "RESET";
02398 prefix = ", ";
02399 }
02400 } else {
02401 str = "OFF";
02402 }
02403 return str;
02404 }
02405
02406 void WG0X::publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
02407 {
02408 if (lockMailbox()) {
02409 mailbox_publish_diagnostics_ = mailbox_diagnostics_;
02410 unlockMailbox();
02411 }
02412
02413 MbxDiagnostics const &m(mailbox_publish_diagnostics_);
02414 d.addf("Mailbox Write Errors", "%d", m.write_errors_);
02415 d.addf("Mailbox Read Errors", "%d", m.read_errors_);
02416 d.addf("Mailbox Retries", "%d", m.retries_);
02417 d.addf("Mailbox Retry Errors", "%d", m.retry_errors_);
02418 }
02419
02420 void WG0X::publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
02421 {
02422
02423 if (tryLockWG0XDiagnostics()) {
02424 wg0x_publish_diagnostics_ = wg0x_collect_diagnostics_;
02425 unlockWG0XDiagnostics();
02426 }
02427
02428 if (too_many_dropped_packets_)
02429 d.mergeSummary(d.ERROR, "Too many dropped packets");
02430
02431 if (status_checksum_error_)
02432 {
02433 d.mergeSummary(d.ERROR, "Checksum error on status data");
02434 }
02435
02436 if (wg0x_publish_diagnostics_.first_)
02437 {
02438 d.mergeSummary(d.WARN, "Have not yet collected WG0X diagnostics");
02439 }
02440 else if (!wg0x_publish_diagnostics_.valid_)
02441 {
02442 d.mergeSummary(d.WARN, "Could not collect WG0X diagnostics");
02443 }
02444
02445 WG0XDiagnostics const &p(wg0x_publish_diagnostics_);
02446 WG0XSafetyDisableStatus const &s(p.safety_disable_status_);
02447 d.addf("Status Checksum Error Count", "%d", p.checksum_errors_);
02448 d.addf("Safety Disable Status", "%s (%02x)", safetyDisableString(s.safety_disable_status_).c_str(), s.safety_disable_status_);
02449 d.addf("Safety Disable Status Hold", "%s (%02x)", safetyDisableString(s.safety_disable_status_hold_).c_str(), s.safety_disable_status_hold_);
02450 d.addf("Safety Disable Count", "%d", p.safety_disable_total_);
02451 d.addf("Undervoltage Count", "%d", p.undervoltage_total_);
02452 d.addf("Over Current Count", "%d", p.over_current_total_);
02453 d.addf("Board Over Temp Count", "%d", p.board_over_temp_total_);
02454 d.addf("Bridge Over Temp Count", "%d", p.bridge_over_temp_total_);
02455 d.addf("Operate Disable Count", "%d", p.operate_disable_total_);
02456 d.addf("Watchdog Disable Count", "%d", p.watchdog_disable_total_);
02457
02458 if (in_lockout_)
02459 {
02460 uint8_t status = s.safety_disable_status_hold_;
02461 string prefix(": ");
02462 string str("Safety Lockout");
02463 CHECK_SAFETY_BIT(UNDERVOLTAGE);
02464 CHECK_SAFETY_BIT(OVER_CURRENT);
02465 CHECK_SAFETY_BIT(BOARD_OVER_TEMP);
02466 CHECK_SAFETY_BIT(HBRIDGE_OVER_TEMP);
02467 CHECK_SAFETY_BIT(OPERATIONAL);
02468 CHECK_SAFETY_BIT(WATCHDOG);
02469 d.mergeSummary(d.ERROR, str);
02470 }
02471
02472 if (timestamp_jump_detected_ && (s.safety_disable_status_hold_ & SAFETY_OPERATIONAL))
02473 {
02474 fpga_internal_reset_detected_ = true;
02475 }
02476
02477 if (fpga_internal_reset_detected_)
02478 {
02479 d.mergeSummaryf(d.ERROR, "FPGA internal reset detected");
02480 }
02481
02482 if (timestamp_jump_detected_)
02483 {
02484 d.mergeSummaryf(d.WARN, "Timestamp jumped");
02485 }
02486
02487 {
02488
02489 const WG0XDiagnosticsInfo &di(p.diagnostics_info_);
02490
02491 d.addf("MBX Command IRQ Count", "%d", di.mbx_command_irq_count_);
02492 d.addf("PDI Timeout Error Count", "%d", di.pdi_timeout_error_count_);
02493 d.addf("PDI Checksum Error Count", "%d", di.pdi_checksum_error_count_);
02494 unsigned product = sh_->get_product_code();
02495
02496
02497 if ((product == WG05::PRODUCT_CODE) && (board_major_ == 1))
02498 {
02499
02500 static const double WG005B_SUPPLY_CURRENT_SCALE = (1.0 / (8152.0 * 0.851)) * 4.0;
02501 double bridge_supply_current = double(di.supply_current_in_) * WG005B_SUPPLY_CURRENT_SCALE;
02502 d.addf("Bridge Supply Current", "%f", bridge_supply_current);
02503 }
02504 else if ((product == WG05::PRODUCT_CODE) || (product == WG021::PRODUCT_CODE))
02505 {
02506
02507
02508 static const double WG005_SUPPLY_CURRENT_SCALE = ((82.0 * 2.5) / (0.01 * 5100.0 * 32768.0));
02509 double supply_current = double(di.supply_current_in_) * WG005_SUPPLY_CURRENT_SCALE;
02510 d.addf("Supply Current", "%f", supply_current);
02511 }
02512 d.addf("Configured Offset A", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_A_);
02513 d.addf("Configured Offset B", "%f", config_info_.nominal_current_scale_ * di.config_offset_current_B_);
02514 }
02515 }
02516
02517
02518
02519 void WG0X::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
02520 {
02521 WG0XStatus *status = (WG0XStatus *)(buffer + command_size_);
02522
02523 stringstream str;
02524 str << "EtherCAT Device (" << actuator_info_.name_ << ")";
02525 d.name = str.str();
02526 char serial[32];
02527 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
02528 d.hardware_id = serial;
02529
02530 if (!has_error_)
02531 d.summary(d.OK, "OK");
02532
02533 d.clear();
02534 d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
02535 d.add("Name", actuator_info_.name_);
02536 d.addf("Position", "%02d", sh_->get_ring_position());
02537 d.addf("Product code",
02538 "WG0%d (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
02539 sh_->get_product_code() == WG05::PRODUCT_CODE ? 5 : 6,
02540 sh_->get_product_code(), fw_major_, fw_minor_,
02541 'A' + board_major_, board_minor_);
02542
02543 d.add("Robot", actuator_info_.robot_name_);
02544 d.addf("Motor", "%s %s", actuator_info_.motor_make_, actuator_info_.motor_model_);
02545 d.add("Serial Number", serial);
02546 d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
02547 d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
02548 d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
02549
02550 d.addf("SW Max Current", "%f", actuator_info_.max_current_);
02551 d.addf("Speed Constant", "%f", actuator_info_.speed_constant_);
02552 d.addf("Resistance", "%f", actuator_info_.resistance_);
02553 d.addf("Motor Torque Constant", "%f", actuator_info_.motor_torque_constant_);
02554 d.addf("Pulses Per Revolution", "%d", actuator_info_.pulses_per_revolution_);
02555 d.addf("Encoder Reduction", "%f", actuator_info_.encoder_reduction_);
02556
02557 publishGeneralDiagnostics(d);
02558 publishMailboxDiagnostics(d);
02559
02560 d.addf("Calibration Offset", "%f", cached_zero_offset_);
02561 d.addf("Calibration Status", "%s",
02562 (calibration_status_ == NO_CALIBRATION) ? "No calibration" :
02563 (calibration_status_ == CONTROLLER_CALIBRATION) ? "Calibrated by controller" :
02564 (calibration_status_ == SAVED_CALIBRATION) ? "Using saved calibration" : "UNKNOWN");
02565
02566 d.addf("Watchdog Limit", "%dms", config_info_.watchdog_limit_);
02567 d.add("Mode", modeString(status->mode_));
02568 d.addf("Digital out", "%d", status->digital_out_);
02569 d.addf("Programmed pwm value", "%d", status->programmed_pwm_value_);
02570 d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
02571 d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
02572 d.addf("Timestamp", "%u", status->timestamp_);
02573 d.addf("Encoder count", "%d", status->encoder_count_);
02574 d.addf("Encoder index pos", "%d", status->encoder_index_pos_);
02575 d.addf("Num encoder_errors", "%d", status->num_encoder_errors_);
02576 d.addf("Encoder status", "%d", status->encoder_status_);
02577 d.addf("Calibration reading", "%d", status->calibration_reading_);
02578 d.addf("Last calibration rising edge", "%d", status->last_calibration_rising_edge_);
02579 d.addf("Last calibration falling edge", "%d", status->last_calibration_falling_edge_);
02580 d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
02581 d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
02582 d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
02583 d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
02584 d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
02585 d.addf("Motor voltage", "%f", status->motor_voltage_ * config_info_.nominal_voltage_scale_);
02586 d.addf("Current Loop Kp", "%d", config_info_.current_loop_kp_);
02587 d.addf("Current Loop Ki", "%d", config_info_.current_loop_ki_);
02588
02589 if (motor_model_)
02590 {
02591 motor_model_->diagnostics(d);
02592 if (disable_motor_model_checking_)
02593 {
02594 d.mergeSummaryf(d.WARN, "Motor model disabled");
02595 }
02596 }
02597
02598 if (last_num_encoder_errors_ != status->num_encoder_errors_)
02599 {
02600 d.mergeSummaryf(d.WARN, "Encoder errors detected");
02601 }
02602
02603 d.addf("Packet count", "%d", status->packet_count_);
02604
02605 d.addf("Drops", "%d", drops_);
02606 d.addf("Consecutive Drops", "%d", consecutive_drops_);
02607 d.addf("Max Consecutive Drops", "%d", max_consecutive_drops_);
02608
02609 unsigned numPorts = (sh_->get_product_code()==WG06::PRODUCT_CODE) ? 1 : 2;
02610 EthercatDevice::ethercatDiagnostics(d, numPorts);
02611 }
02612
02613 void WG06::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
02614 {
02615 WG0X::diagnostics(d, buffer);
02616
02617 pr2_hardware_interface::AccelerometerCommand acmd(accelerometer_.command_);
02618
02619 const char * range_str =
02620 (acmd.range_ == 0) ? "+/-2G" :
02621 (acmd.range_ == 1) ? "+/-4G" :
02622 (acmd.range_ == 2) ? "+/-8G" :
02623 "INVALID";
02624
02625 const char * bandwidth_str =
02626 (acmd.bandwidth_ == 6) ? "1500Hz" :
02627 (acmd.bandwidth_ == 5) ? "750Hz" :
02628 (acmd.bandwidth_ == 4) ? "375Hz" :
02629 (acmd.bandwidth_ == 3) ? "190Hz" :
02630 (acmd.bandwidth_ == 2) ? "100Hz" :
02631 (acmd.bandwidth_ == 1) ? "50Hz" :
02632 (acmd.bandwidth_ == 0) ? "25Hz" :
02633 "INVALID";
02634
02635 if (pressure_checksum_error_)
02636 {
02637 d.mergeSummary(d.ERROR, "Checksum error on pressure data");
02638 }
02639
02640
02641 bool has_accelerometer = (board_major_ >= 2);
02642 double sample_frequency = 0.0;
02643 ros::Time current_time(ros::Time::now());
02644 if (!first_publish_)
02645 {
02646 sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
02647 {
02648 if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
02649 {
02650 d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
02651 }
02652 }
02653 }
02654 accelerometer_samples_ = 0;
02655 last_publish_time_ = current_time;
02656 first_publish_ = false;
02657
02658 d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
02659 d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
02660 d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
02661 d.addf("Accelerometer sample frequency", "%f", sample_frequency);
02662 d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);
02663
02664 }
02665
02666 void WG021::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
02667 {
02668 WG021Status *status = (WG021Status *)(buffer + command_size_);
02669
02670 stringstream str;
02671 str << "EtherCAT Device (" << actuator_info_.name_ << ")";
02672 d.name = str.str();
02673 char serial[32];
02674 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
02675 d.hardware_id = serial;
02676
02677 if (!has_error_)
02678 d.summary(d.OK, "OK");
02679
02680 d.clear();
02681 d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
02682 d.add("Name", actuator_info_.name_);
02683 d.addf("Position", "%02d", sh_->get_ring_position());
02684 d.addf("Product code",
02685 "WG021 (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
02686 sh_->get_product_code(), fw_major_, fw_minor_,
02687 'A' + board_major_, board_minor_);
02688
02689 d.add("Robot", actuator_info_.robot_name_);
02690 d.add("Serial Number", serial);
02691 d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
02692 d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
02693 d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
02694 d.addf("SW Max Current", "%f", actuator_info_.max_current_);
02695
02696 publishGeneralDiagnostics(d);
02697 publishMailboxDiagnostics(d);
02698
02699 d.add("Mode", modeString(status->mode_));
02700 d.addf("Digital out", "%d", status->digital_out_);
02701 d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
02702 d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
02703 d.addf("Timestamp", "%u", status->timestamp_);
02704 d.addf("Config 0", "%#02x", status->config0_);
02705 d.addf("Config 1", "%#02x", status->config1_);
02706 d.addf("Config 2", "%#02x", status->config2_);
02707 d.addf("Output Status", "%#02x", status->output_status_);
02708 d.addf("Output Start Timestamp", "%u", status->output_start_timestamp_);
02709 d.addf("Output Stop Timestamp", "%u", status->output_stop_timestamp_);
02710 d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
02711 d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
02712 d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
02713 d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
02714 d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
02715 d.addf("LED voltage", "%f", status->led_voltage_ * config_info_.nominal_voltage_scale_);
02716 d.addf("Packet count", "%d", status->packet_count_);
02717
02718 EthercatDevice::ethercatDiagnostics(d, 2);
02719 }