ethercat_device.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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   // Check if device has been reset/power cycled using its node address
00164   // Node address initialize to 0 after device reset.
00165   // EML library will configure each node address to non-zero when it first starts
00166   // Device should respond to its node address, if it does not either:
00167   //  1. communication to device is not possible (lost/broken link)
00168   //  2. device was reset, and its fixed address setting is now 0
00169   { 
00170     // Send a packet with both a Fixed address read (NPRW) and a positional read (APRD)
00171     // If the NPRD has a working counter == 0, but the APRD sees the correct number of devices,
00172     // then the node has likely been reset.
00173     // Also, get DL status regiseter with nprd telegram
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     // Use positional read to re-count number of devices on chain
00183     unsigned char buf[1];    
00184     EC_UINT address = 0x0000;
00185     APRD_Telegram aprd_telegram(logic->get_idx(),  // Index
00186                                 0,                 // Slave position on ethercat chain (auto increment address) (
00187                                 address,           // ESC physical memory address (start address) 
00188                                 logic->get_wkc(),  // Working counter
00189                                 sizeof(buf),       // Data Length,
00190                                 buf);              // Buffer to put read result into
00191     
00192 
00193 
00194     // Chain both telegrams together
00195     nprd_telegram.attach(&aprd_telegram);
00196 
00197     EC_Ethernet_Frame frame(&nprd_telegram);
00198     
00199     // Send/Recv data from slave
00200     if (!com->txandrx_once(&frame)) {
00201       // no response - broken link to device
00202       goto end;
00203     }
00204 
00205     devicesRespondingToNodeAddress_ = nprd_telegram.get_wkc();
00206     if (devicesRespondingToNodeAddress_ == 0) {
00207       // Device has not responded to its node address.
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       // Can't determine much if two (or more) devices are responding to same request.
00215       goto end;
00216     }
00217     else {
00218       resetDetected_ = false;
00219     }
00220     
00221     // fill in port status information
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   { // read and accumulate communication error counters
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     // If this previously tried to clear the error counters but d/n get a response
00238     // then use the newly read values to guess if they got cleared or not.
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     // Accumulate
00250     this->accumulate(e, errorCountersPrev_);
00251     errorCountersPrev_ = e;
00252 
00253     // re-read and clear communication error counters    
00254     if(e.isGreaterThan(50)) {
00255       if (0 != EthercatDevice::readWriteData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
00256         // Packet got lost... Can't know for sure that error counters got cleared
00257         errorCountersMayBeCleared_ = true;
00258         goto end;
00259       }
00260       // We read and cleared error counters in same packet, accumulate any error counter differences
00261       this->accumulate(e, errorCountersPrev_);
00262       errorCountersPrev_.zero();
00263     }
00264   }
00265   
00266   // Everything was read successfully
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   // empty
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); // wait for ros to flush rosconsole output
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); // wait for ros to flush rosconsole output
00360     exit(EXIT_FAILURE);
00361   }    
00362 }
00363 
00364 EthercatDevice::~EthercatDevice()
00365 {
00366   //nothing
00367 }
00368 
00369 void EthercatDevice::collectDiagnostics(EthercatCom *com)
00370 {
00371   // Really, should not need this lock, since there should only be one thread updating diagnostics.
00372   pthread_mutex_lock(&diagnosticsLock_);  
00373   
00374   // Get references to diagnostics... code is easier to read
00375   unsigned oldDiagnosticsIndex = (newDiagnosticsIndex_+1)&1;
00376   const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_];
00377   EthercatDeviceDiagnostics &oldDiag = deviceDiagnostics[oldDiagnosticsIndex];
00378 
00379   // copy new diagnostics values in old diagnostics, because diagnostic data use accumumlators
00380   oldDiag = newDiag; 
00381 
00382   // Collect diagnostics data into "old" buffer.  
00383   // This way the "new" buffer is never changed while the publishing thread may be using it.
00384   oldDiag.collect(com, sh_);
00385 
00386   // Got new diagnostics... swap buffers.  
00387   // Publisher thread uses "new" buffer.  New to lock while swapping buffers.  
00388   // Note : this is just changing an integer value, so it only takes a couple of instructions.  
00389   pthread_mutex_lock(&newDiagnosticsIndexLock_);    
00390   newDiagnosticsIndex_=oldDiagnosticsIndex;
00391   pthread_mutex_unlock(&newDiagnosticsIndexLock_);
00392   
00393   // Done, unlock
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(),  // Index
00411                               -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
00412                               address,               // ESC physical memory address (start address) 
00413                               logic->get_wkc(),  // Working counter
00414                               length,                   // Data Length,
00415                               p);             // Buffer to put read result into
00416 
00417   // Put read telegram in ethercat/ethernet frame  
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   // Put telegram in ethercat/ethernet frame
00429   EC_Ethernet_Frame frame(telegram);
00430   
00431   // Send/Recv data from slave
00432   if (!com->txandrx_once(&frame)) {
00433     return -1;
00434   }
00435 
00436   // In some cases (clearing status mailbox) this is expected to occur
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(),  // Index
00458                               -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
00459                               address,               // ESC physical memory address (start address) 
00460                               logic->get_wkc(),  // Working counter
00461                               length,                   // Data Length,
00462                               p);             // Buffer to put read result into
00463 
00464   // Put read telegram in ethercat/ethernet frame  
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   // Put telegram in ethercat/ethernet frame
00476   EC_Ethernet_Frame frame(telegram);
00477 
00478   // Send/Recv data from slave
00479   if (!com->txandrx(&frame)) {
00480     return -1;
00481   }
00482 
00483   // In some cases (clearing status mailbox) this is expected to occur
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(),  // Index
00505                               -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
00506                               address,               // ESC physical memory address (start address) 
00507                               logic->get_wkc(),  // Working counter
00508                               length,                   // Data Length,
00509                               p);             // Buffer to put read result into
00510 
00511   // Put read telegram in ethercat/ethernet frame  
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   // Put telegram in ethercat/ethernet frame
00523   EC_Ethernet_Frame frame(telegram);
00524 
00525   // Send/Recv data from slave
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   // By locking index, diagnostics double-buffer cannot be swapped.
00546   // Update thread is only allowed to change 'old' diagnostics buffer, 
00547   //  so new buffer data cannot be changed while index lock is held.
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); //assume unknown device has 4 ports
00576 }
00577 
00578 void EthercatDevice::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00579 {
00580   // Clean up recycled status object before reusing it.
00581   diagnostic_status_.clearSummary();
00582   diagnostic_status_.clear();
00583 
00584   // If child-class does not implement multiDiagnostics(), fall back to using slave's diagnostic() function
00585   diagnostics(diagnostic_status_, buffer);
00586   vec.push_back(diagnostic_status_);
00587 }


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:44