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 "ethercat_hardware/ethercat_device.h"
00036
00037 #include <tinyxml.h>
00038
00039 #include <dll/ethercat_device_addressed_telegram.h>
00040 #include <dll/ethercat_logical_addressed_telegram.h>
00041 #include <dll/ethercat_frame.h>
00042
00043 #include <iomanip>
00044
00045 bool et1x00_error_counters::isGreaterThan(unsigned value) const
00046 {
00047 if ((pdi_error>value) || (epu_error>value)) {
00048 return true;
00049 }
00050
00051 for (unsigned i=0; i<4; ++i) {
00052 if ( (port[i].rx_error > value) ||
00053 (forwarded_rx_error[i] > value) ||
00054 (lost_link[i] > value) ||
00055 (port[i].invalid_frame > value) )
00056 {
00057 return true;
00058 }
00059 }
00060 return false;
00061 }
00062
00063 bool et1x00_error_counters::isGreaterThan(const et1x00_error_counters &v) const
00064 {
00065 if ((pdi_error>v.pdi_error) || (epu_error>v.epu_error)) {
00066 return true;
00067 }
00068
00069 for (unsigned i=0; i<4; ++i) {
00070 if ( (port[i].rx_error > v.port[i].rx_error ) ||
00071 (forwarded_rx_error[i] > v.forwarded_rx_error[i]) ||
00072 (lost_link[i] > v.lost_link[i] ) ||
00073 (port[i].invalid_frame > v.port[i].invalid_frame) )
00074 {
00075 return true;
00076 }
00077 }
00078 return false;
00079 }
00080
00081 bool et1x00_dl_status::hasLink(unsigned port)
00082 {
00083 assert(port<4);
00084 return status & (1<<(4+port));
00085 }
00086
00087 bool et1x00_dl_status::hasCommunication(unsigned port)
00088 {
00089 assert(port<4);
00090 return status & (1<<(9+port*2));
00091 }
00092
00093 bool et1x00_dl_status::isClosed(unsigned port)
00094 {
00095 assert(port<4);
00096 return status & (1<<(8+port*2));
00097 }
00098
00099 void et1x00_error_counters::zero()
00100 {
00101 memset(this, 0, sizeof(et1x00_error_counters));
00102 }
00103
00104
00105 EthercatPortDiagnostics::EthercatPortDiagnostics() :
00106 hasLink(false),
00107 isClosed(false),
00108 hasCommunication(false)
00109 {
00110 zeroTotals();
00111 }
00112
00113 void EthercatPortDiagnostics::zeroTotals()
00114 {
00115 rxErrorTotal=0;
00116 invalidFrameTotal=0;
00117 forwardedRxErrorTotal=0;
00118 lostLinkTotal=0;
00119 }
00120
00121
00122 EthercatDeviceDiagnostics::EthercatDeviceDiagnostics() :
00123 errorCountersMayBeCleared_(false),
00124 diagnosticsFirst_(true),
00125 diagnosticsValid_(false),
00126 resetDetected_(false),
00127 devicesRespondingToNodeAddress_(-1)
00128 {
00129 zeroTotals();
00130 errorCountersPrev_.zero();
00131 }
00132
00133 void EthercatDeviceDiagnostics::zeroTotals()
00134 {
00135 pdiErrorTotal_=0;
00136 epuErrorTotal_=0;
00137 portDiagnostics_[0].zeroTotals();
00138 portDiagnostics_[1].zeroTotals();
00139 portDiagnostics_[2].zeroTotals();
00140 portDiagnostics_[3].zeroTotals();
00141 }
00142
00143
00144 void EthercatDeviceDiagnostics::accumulate(const et1x00_error_counters &n, const et1x00_error_counters &p)
00145 {
00146 pdiErrorTotal_ += n.pdi_error - p.pdi_error;
00147 epuErrorTotal_ += n.epu_error - p.epu_error;
00148 for (unsigned i=0; i<4; ++i) {
00149 EthercatPortDiagnostics &pt(portDiagnostics_[i]);
00150 pt.rxErrorTotal += n.port[i].rx_error - p.port[i].rx_error ;
00151 pt.forwardedRxErrorTotal += n.forwarded_rx_error[i] - p.forwarded_rx_error[i];
00152 pt.lostLinkTotal += n.lost_link[i] - p.lost_link[i] ;
00153 pt.invalidFrameTotal += n.port[i].invalid_frame - p.port[i].invalid_frame;
00154 }
00155 }
00156
00157
00158 void EthercatDeviceDiagnostics::collect(EthercatCom *com, EtherCAT_SlaveHandler *sh)
00159 {
00160 diagnosticsValid_ = false;
00161 diagnosticsFirst_ = false;
00162
00163
00164
00165
00166
00167
00168
00169 {
00170
00171
00172
00173
00174 EC_Logic *logic = EC_Logic::instance();
00175 et1x00_dl_status dl_status;
00176 NPRD_Telegram nprd_telegram(logic->get_idx(),
00177 sh->get_station_address(),
00178 dl_status.BASE_ADDR,
00179 logic->get_wkc(),
00180 sizeof(dl_status),
00181 (unsigned char*) &dl_status);
00182
00183 unsigned char buf[1];
00184 EC_UINT address = 0x0000;
00185 APRD_Telegram aprd_telegram(logic->get_idx(),
00186 0,
00187 address,
00188 logic->get_wkc(),
00189 sizeof(buf),
00190 buf);
00191
00192
00193
00194
00195 nprd_telegram.attach(&aprd_telegram);
00196
00197 EC_Ethernet_Frame frame(&nprd_telegram);
00198
00199
00200 if (!com->txandrx_once(&frame)) {
00201
00202 goto end;
00203 }
00204
00205 devicesRespondingToNodeAddress_ = nprd_telegram.get_wkc();
00206 if (devicesRespondingToNodeAddress_ == 0) {
00207
00208 if (aprd_telegram.get_adp() >= EtherCAT_AL::instance()->get_num_slaves()) {
00209 resetDetected_ = true;
00210 goto end;
00211 }
00212 }
00213 else if (devicesRespondingToNodeAddress_ > 1) {
00214
00215 goto end;
00216 }
00217 else {
00218 resetDetected_ = false;
00219 }
00220
00221
00222 for (unsigned i=0;i<4;++i) {
00223 EthercatPortDiagnostics &pt(portDiagnostics_[i]);
00224 pt.hasLink = dl_status.hasLink(i);
00225 pt.isClosed = dl_status.isClosed(i);
00226 pt.hasCommunication = dl_status.hasCommunication(i);
00227 }
00228 }
00229
00230 {
00231 et1x00_error_counters e;
00232 assert(sizeof(e) == (0x314-0x300));
00233 if (0 != EthercatDevice::readData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
00234 goto end;
00235 }
00236
00237
00238
00239 if (errorCountersMayBeCleared_) {
00240 if (!e.isGreaterThan(errorCountersPrev_)) {
00241 errorCountersPrev_.zero();
00242 }
00243 errorCountersMayBeCleared_ = false;
00244 }
00245 if (errorCountersPrev_.isGreaterThan(e)) {
00246 ROS_ERROR("Device %d : previous port error counters less current values", sh->get_ring_position());
00247 }
00248
00249
00250 this->accumulate(e, errorCountersPrev_);
00251 errorCountersPrev_ = e;
00252
00253
00254 if(e.isGreaterThan(50)) {
00255 if (0 != EthercatDevice::readWriteData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
00256
00257 errorCountersMayBeCleared_ = true;
00258 goto end;
00259 }
00260
00261 this->accumulate(e, errorCountersPrev_);
00262 errorCountersPrev_.zero();
00263 }
00264 }
00265
00266
00267 diagnosticsValid_ = true;
00268
00269 end:
00270 return;
00271 }
00272
00273 void EthercatDeviceDiagnostics::publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts) const
00274 {
00275 if (numPorts>4) {
00276 assert(numPorts<4);
00277 numPorts=4;
00278 }
00279 assert(numPorts > 0);
00280
00281 d.addf("Reset detected", "%s", (resetDetected_ ? "Yes" : "No"));
00282 d.addf("Valid", "%s", (diagnosticsValid_ ? "Yes" : "No"));
00283
00284 d.addf("EPU Errors", "%lld", epuErrorTotal_);
00285 d.addf("PDI Errors", "%lld", pdiErrorTotal_);
00286 ostringstream os, port;
00287 for (unsigned i=0; i<numPorts; ++i) {
00288 const EthercatPortDiagnostics &pt(portDiagnostics_[i]);
00289 port.str(""); port << " Port " << i;
00290 os.str(""); os << "Status" << port.str();
00291 d.addf(os.str(), "%s Link, %s, %s Comm",
00292 pt.hasLink ? "Has":"No",
00293 pt.isClosed ? "Closed":"Open",
00294 pt.hasCommunication ? "Has":"No");
00295 os.str(""); os << "RX Error" << port.str();
00296 d.addf(os.str(), "%lld", pt.rxErrorTotal);
00297 os.str(""); os << "Forwarded RX Error" << port.str();
00298 d.addf(os.str(), "%lld", pt.forwardedRxErrorTotal);
00299 os.str(""); os << "Invalid Frame" << port.str();
00300 d.addf(os.str(), "%lld", pt.invalidFrameTotal);
00301 os.str(""); os << "Lost Link" << port.str();
00302 d.addf(os.str(), "%lld", pt.lostLinkTotal);
00303 }
00304
00305 if (resetDetected_)
00306 {
00307 d.mergeSummaryf(d.ERROR, "Device reset likely");
00308 }
00309 else if (devicesRespondingToNodeAddress_ > 1)
00310 {
00311 d.mergeSummaryf(d.ERROR, "More than one device (%d) responded to node address", devicesRespondingToNodeAddress_);
00312 }
00313 else {
00314 if (diagnosticsFirst_)
00315 {
00316 d.mergeSummaryf(d.WARN, "Have not yet collected diagnostics");
00317 }
00318 else if (!diagnosticsValid_)
00319 {
00320 d.mergeSummaryf(d.WARN, "Could not collect diagnostics");
00321 }
00322 else
00323 {
00324 if (!portDiagnostics_[0].hasLink) {
00325 d.mergeSummaryf(d.WARN, "No link on port 0");
00326 }
00327 }
00328 }
00329 }
00330
00331 void EthercatDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00332 {
00333 sh_ = sh;
00334 }
00335
00336 void EthercatDevice::construct(ros::NodeHandle &nh)
00337 {
00338
00339 }
00340
00341
00342 EthercatDevice::EthercatDevice() : use_ros_(true)
00343 {
00344 sh_ = NULL;
00345 command_size_ = 0;
00346 status_size_ = 0;
00347 newDiagnosticsIndex_ = 0;
00348
00349 int error = pthread_mutex_init(&newDiagnosticsIndexLock_, NULL);
00350 if (error != 0) {
00351 ROS_FATAL("Initializing indexLock failed : %s", strerror(error));
00352 sleep(1);
00353 exit(EXIT_FAILURE);
00354 }
00355
00356 error = pthread_mutex_init(&diagnosticsLock_, NULL);
00357 if (error != 0) {
00358 ROS_FATAL("Initializing diagnositcsLock failed : %s", strerror(error));
00359 sleep(1);
00360 exit(EXIT_FAILURE);
00361 }
00362 }
00363
00364 EthercatDevice::~EthercatDevice()
00365 {
00366
00367 }
00368
00369 void EthercatDevice::collectDiagnostics(EthercatCom *com)
00370 {
00371
00372 pthread_mutex_lock(&diagnosticsLock_);
00373
00374
00375 unsigned oldDiagnosticsIndex = (newDiagnosticsIndex_+1)&1;
00376 const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_];
00377 EthercatDeviceDiagnostics &oldDiag = deviceDiagnostics[oldDiagnosticsIndex];
00378
00379
00380 oldDiag = newDiag;
00381
00382
00383
00384 oldDiag.collect(com, sh_);
00385
00386
00387
00388
00389 pthread_mutex_lock(&newDiagnosticsIndexLock_);
00390 newDiagnosticsIndex_=oldDiagnosticsIndex;
00391 pthread_mutex_unlock(&newDiagnosticsIndexLock_);
00392
00393
00394 pthread_mutex_unlock(&diagnosticsLock_);
00395 }
00396
00397
00398 int EthercatDevice::readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
00399 {
00400 unsigned char *p = (unsigned char *)buffer;
00401 EC_Logic *logic = EC_Logic::instance();
00402
00403 NPRW_Telegram nprw_telegram(logic->get_idx(),
00404 sh->get_station_address(),
00405 address,
00406 logic->get_wkc(),
00407 length,
00408 p);
00409
00410 APRW_Telegram aprw_telegram(logic->get_idx(),
00411 -sh->get_ring_position(),
00412 address,
00413 logic->get_wkc(),
00414 length,
00415 p);
00416
00417
00418 EC_Telegram * telegram = NULL;
00419 if (addrMode == FIXED_ADDR) {
00420 telegram = &nprw_telegram;
00421 } else if (addrMode == POSITIONAL_ADDR) {
00422 telegram = &aprw_telegram;
00423 } else {
00424 assert(0);
00425 return -1;
00426 }
00427
00428
00429 EC_Ethernet_Frame frame(telegram);
00430
00431
00432 if (!com->txandrx_once(&frame)) {
00433 return -1;
00434 }
00435
00436
00437 if (telegram->get_wkc() != 3) {
00438 return -2;
00439 }
00440
00441 return 0;
00442 }
00443
00444
00445 int EthercatDevice::readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
00446 {
00447 unsigned char *p = (unsigned char *)buffer;
00448 EC_Logic *logic = EC_Logic::instance();
00449
00450 NPRD_Telegram nprd_telegram(logic->get_idx(),
00451 sh->get_station_address(),
00452 address,
00453 logic->get_wkc(),
00454 length,
00455 p);
00456
00457 APRD_Telegram aprd_telegram(logic->get_idx(),
00458 -sh->get_ring_position(),
00459 address,
00460 logic->get_wkc(),
00461 length,
00462 p);
00463
00464
00465 EC_Telegram * telegram = NULL;
00466 if (addrMode == FIXED_ADDR) {
00467 telegram = &nprd_telegram;
00468 } else if (addrMode == POSITIONAL_ADDR) {
00469 telegram = &aprd_telegram;
00470 } else {
00471 assert(0);
00472 return -1;
00473 }
00474
00475
00476 EC_Ethernet_Frame frame(telegram);
00477
00478
00479 if (!com->txandrx(&frame)) {
00480 return -1;
00481 }
00482
00483
00484 if (telegram->get_wkc() != 1) {
00485 return -2;
00486 }
00487
00488 return 0;
00489 }
00490
00491
00492 int EthercatDevice::writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode)
00493 {
00494 unsigned char const *p = (unsigned char const*)buffer;
00495 EC_Logic *logic = EC_Logic::instance();
00496
00497 NPWR_Telegram npwr_telegram(logic->get_idx(),
00498 sh->get_station_address(),
00499 address,
00500 logic->get_wkc(),
00501 length,
00502 p);
00503
00504 APWR_Telegram apwr_telegram(logic->get_idx(),
00505 -sh->get_ring_position(),
00506 address,
00507 logic->get_wkc(),
00508 length,
00509 p);
00510
00511
00512 EC_Telegram * telegram = NULL;
00513 if (addrMode == FIXED_ADDR) {
00514 telegram = &npwr_telegram;
00515 } else if (addrMode == POSITIONAL_ADDR) {
00516 telegram = &apwr_telegram;
00517 } else {
00518 assert(0);
00519 return -1;
00520 }
00521
00522
00523 EC_Ethernet_Frame frame(telegram);
00524
00525
00526 if (!com->txandrx(&frame)) {
00527 return -1;
00528 }
00529
00530 if (telegram->get_wkc() != 1) {
00531 return -2;
00532 }
00533
00534 return 0;
00535 }
00536
00537
00538 void EthercatDevice::ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts)
00539 {
00540 if (numPorts>4) {
00541 assert(numPorts<4);
00542 numPorts=4;
00543 }
00544
00545
00546
00547
00548 pthread_mutex_lock(&newDiagnosticsIndexLock_);
00549
00550 const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_];
00551 newDiag.publish(d, numPorts);
00552
00553 pthread_mutex_unlock(&newDiagnosticsIndexLock_);
00554 }
00555
00556
00557 void EthercatDevice::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00558 {
00559 stringstream str;
00560 str << "EtherCAT Device (" << std::setw(2) << std::setfill('0') << sh_->get_ring_position() << ")";
00561 d.name = str.str();
00562 str.str("");
00563 str << sh_->get_product_code() << '-' << sh_->get_serial();
00564 d.hardware_id = str.str();
00565
00566 d.message = "";
00567 d.level = 0;
00568
00569 d.clear();
00570 d.addf("Position", "%02d", sh_->get_ring_position());
00571 d.addf("Product code", "%08x", sh_->get_product_code());
00572 d.addf("Serial", "%08x", sh_->get_serial());
00573 d.addf("Revision", "%08x", sh_->get_revision());
00574
00575 this->ethercatDiagnostics(d, 4);
00576 }
00577
00578 void EthercatDevice::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00579 {
00580
00581 diagnostic_status_.clearSummary();
00582 diagnostic_status_.clear();
00583
00584
00585 diagnostics(diagnostic_status_, buffer);
00586 vec.push_back(diagnostic_status_);
00587 }