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