ethercat_device.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #include <tinyxml.h>
38 
39 #include <dll/ethercat_device_addressed_telegram.h>
40 #include <dll/ethercat_logical_addressed_telegram.h>
41 #include <dll/ethercat_frame.h>
42 
43 #include <iomanip>
44 
45 bool et1x00_error_counters::isGreaterThan(unsigned value) const
46 {
47  if ((pdi_error>value) || (epu_error>value)) {
48  return true;
49  }
50 
51  for (unsigned i=0; i<4; ++i) {
52  if ( (port[i].rx_error > value) ||
53  (forwarded_rx_error[i] > value) ||
54  (lost_link[i] > value) ||
55  (port[i].invalid_frame > value) )
56  {
57  return true;
58  }
59  }
60  return false;
61 }
62 
64 {
65  if ((pdi_error>v.pdi_error) || (epu_error>v.epu_error)) {
66  return true;
67  }
68 
69  for (unsigned i=0; i<4; ++i) {
70  if ( (port[i].rx_error > v.port[i].rx_error ) ||
72  (lost_link[i] > v.lost_link[i] ) ||
73  (port[i].invalid_frame > v.port[i].invalid_frame) )
74  {
75  return true;
76  }
77  }
78  return false;
79 }
80 
81 bool et1x00_dl_status::hasLink(unsigned port)
82 {
83  assert(port<4);
84  return status & (1<<(4+port));
85 }
86 
88 {
89  assert(port<4);
90  return status & (1<<(9+port*2));
91 }
92 
93 bool et1x00_dl_status::isClosed(unsigned port)
94 {
95  assert(port<4);
96  return status & (1<<(8+port*2));
97 }
98 
100 {
101  memset(this, 0, sizeof(et1x00_error_counters));
102 }
103 
104 
106  hasLink(false),
107  isClosed(false),
108  hasCommunication(false)
109 {
110  zeroTotals();
111 }
112 
114 {
115  rxErrorTotal=0;
118  lostLinkTotal=0;
119 }
120 
121 
123  errorCountersMayBeCleared_(false),
124  diagnosticsFirst_(true),
125  diagnosticsValid_(false),
126  resetDetected_(false),
127  devicesRespondingToNodeAddress_(-1)
128 {
129  zeroTotals();
131 }
132 
134 {
135  pdiErrorTotal_=0;
136  epuErrorTotal_=0;
141 }
142 
143 
145 {
148  for (unsigned i=0; i<4; ++i) {
150  pt.rxErrorTotal += n.port[i].rx_error - p.port[i].rx_error ;
152  pt.lostLinkTotal += n.lost_link[i] - p.lost_link[i] ;
153  pt.invalidFrameTotal += n.port[i].invalid_frame - p.port[i].invalid_frame;
154  }
155 }
156 
157 
158 void EthercatDeviceDiagnostics::collect(EthercatCom *com, EtherCAT_SlaveHandler *sh)
159 {
160  diagnosticsValid_ = false;
161  diagnosticsFirst_ = false;
162 
163  // Check if device has been reset/power cycled using its node address
164  // Node address initialize to 0 after device reset.
165  // EML library will configure each node address to non-zero when it first starts
166  // Device should respond to its node address, if it does not either:
167  // 1. communication to device is not possible (lost/broken link)
168  // 2. device was reset, and its fixed address setting is now 0
169  {
170  // Send a packet with both a Fixed address read (NPRW) and a positional read (APRD)
171  // If the NPRD has a working counter == 0, but the APRD sees the correct number of devices,
172  // then the node has likely been reset.
173  // Also, get DL status regiseter with nprd telegram
174  EC_Logic *logic = EC_Logic::instance();
175  et1x00_dl_status dl_status;
176  NPRD_Telegram nprd_telegram(logic->get_idx(),
177  sh->get_station_address(),
178  dl_status.BASE_ADDR,
179  logic->get_wkc(),
180  sizeof(dl_status),
181  (unsigned char*) &dl_status);
182  // Use positional read to re-count number of devices on chain
183  unsigned char buf[1];
184  EC_UINT address = 0x0000;
185  APRD_Telegram aprd_telegram(logic->get_idx(), // Index
186  0, // Slave position on ethercat chain (auto increment address) (
187  address, // ESC physical memory address (start address)
188  logic->get_wkc(), // Working counter
189  sizeof(buf), // Data Length,
190  buf); // Buffer to put read result into
191 
192 
193 
194  // Chain both telegrams together
195  nprd_telegram.attach(&aprd_telegram);
196 
197  EC_Ethernet_Frame frame(&nprd_telegram);
198 
199  // Send/Recv data from slave
200  if (!com->txandrx_once(&frame)) {
201  // no response - broken link to device
202  goto end;
203  }
204 
205  devicesRespondingToNodeAddress_ = nprd_telegram.get_wkc();
207  // Device has not responded to its node address.
208  if (aprd_telegram.get_adp() >= EtherCAT_AL::instance()->get_num_slaves()) {
209  resetDetected_ = true;
210  goto end;
211  }
212  }
213  else if (devicesRespondingToNodeAddress_ > 1) {
214  // Can't determine much if two (or more) devices are responding to same request.
215  goto end;
216  }
217  else {
218  resetDetected_ = false;
219  }
220 
221  // fill in port status information
222  for (unsigned i=0;i<4;++i) {
224  pt.hasLink = dl_status.hasLink(i);
225  pt.isClosed = dl_status.isClosed(i);
226  pt.hasCommunication = dl_status.hasCommunication(i);
227  }
228  }
229 
230  { // read and accumulate communication error counters
232  assert(sizeof(e) == (0x314-0x300));
233  if (0 != EthercatDevice::readData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
234  goto end;
235  }
236 
237  // If this previously tried to clear the error counters but d/n get a response
238  // then use the newly read values to guess if they got cleared or not.
242  }
244  }
246  ROS_ERROR("Device %d : previous port error counters less current values", sh->get_ring_position());
247  }
248 
249  // Accumulate
250  this->accumulate(e, errorCountersPrev_);
251  errorCountersPrev_ = e;
252 
253  // re-read and clear communication error counters
254  if(e.isGreaterThan(50)) {
255  if (0 != EthercatDevice::readWriteData(com, sh, e.BASE_ADDR, &e, sizeof(e), EthercatDevice::FIXED_ADDR)) {
256  // Packet got lost... Can't know for sure that error counters got cleared
258  goto end;
259  }
260  // We read and cleared error counters in same packet, accumulate any error counter differences
261  this->accumulate(e, errorCountersPrev_);
263  }
264  }
265 
266  // Everything was read successfully
267  diagnosticsValid_ = true;
268 
269  end:
270  return;
271 }
272 
274 {
275  if (numPorts>4) {
276  assert(numPorts<4);
277  numPorts=4;
278  }
279  assert(numPorts > 0);
280 
281  d.addf("Reset detected", "%s", (resetDetected_ ? "Yes" : "No"));
282  d.addf("Valid", "%s", (diagnosticsValid_ ? "Yes" : "No"));
283 
284  d.addf("EPU Errors", "%lld", epuErrorTotal_);
285  d.addf("PDI Errors", "%lld", pdiErrorTotal_);
286  ostringstream os, port;
287  for (unsigned i=0; i<numPorts; ++i) {
289  port.str(""); port << " Port " << i;
290  os.str(""); os << "Status" << port.str();
291  d.addf(os.str(), "%s Link, %s, %s Comm",
292  pt.hasLink ? "Has":"No",
293  pt.isClosed ? "Closed":"Open",
294  pt.hasCommunication ? "Has":"No");
295  os.str(""); os << "RX Error" << port.str();
296  d.addf(os.str(), "%lld", pt.rxErrorTotal);
297  os.str(""); os << "Forwarded RX Error" << port.str();
298  d.addf(os.str(), "%lld", pt.forwardedRxErrorTotal);
299  os.str(""); os << "Invalid Frame" << port.str();
300  d.addf(os.str(), "%lld", pt.invalidFrameTotal);
301  os.str(""); os << "Lost Link" << port.str();
302  d.addf(os.str(), "%lld", pt.lostLinkTotal);
303  }
304 
305  if (resetDetected_)
306  {
307  d.mergeSummaryf(d.ERROR, "Device reset likely");
308  }
309  else if (devicesRespondingToNodeAddress_ > 1)
310  {
311  d.mergeSummaryf(d.ERROR, "More than one device (%d) responded to node address", devicesRespondingToNodeAddress_);
312  }
313  else {
314  if (diagnosticsFirst_)
315  {
316  d.mergeSummaryf(d.WARN, "Have not yet collected diagnostics");
317  }
318  else if (!diagnosticsValid_)
319  {
320  d.mergeSummaryf(d.WARN, "Could not collect diagnostics");
321  }
322  else
323  {
324  if (!portDiagnostics_[0].hasLink) {
325  d.mergeSummaryf(d.WARN, "No link on port 0");
326  }
327  }
328  }
329 }
330 
331 void EthercatDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address)
332 {
333  sh_ = sh;
334 }
335 
337 {
338  // empty
339 }
340 
341 
343 {
344  sh_ = NULL;
345  command_size_ = 0;
346  status_size_ = 0;
348 
349  int error = pthread_mutex_init(&newDiagnosticsIndexLock_, NULL);
350  if (error != 0) {
351  ROS_FATAL("Initializing indexLock failed : %s", strerror(error));
352  sleep(1); // wait for ros to flush rosconsole output
353  exit(EXIT_FAILURE);
354  }
355 
356  error = pthread_mutex_init(&diagnosticsLock_, NULL);
357  if (error != 0) {
358  ROS_FATAL("Initializing diagnositcsLock failed : %s", strerror(error));
359  sleep(1); // wait for ros to flush rosconsole output
360  exit(EXIT_FAILURE);
361  }
362 }
363 
365 {
366  //nothing
367 }
368 
370 {
371  // Really, should not need this lock, since there should only be one thread updating diagnostics.
372  pthread_mutex_lock(&diagnosticsLock_);
373 
374  // Get references to diagnostics... code is easier to read
375  unsigned oldDiagnosticsIndex = (newDiagnosticsIndex_+1)&1;
377  EthercatDeviceDiagnostics &oldDiag = deviceDiagnostics[oldDiagnosticsIndex];
378 
379  // copy new diagnostics values in old diagnostics, because diagnostic data use accumumlators
380  oldDiag = newDiag;
381 
382  // Collect diagnostics data into "old" buffer.
383  // This way the "new" buffer is never changed while the publishing thread may be using it.
384  oldDiag.collect(com, sh_);
385 
386  // Got new diagnostics... swap buffers.
387  // Publisher thread uses "new" buffer. New to lock while swapping buffers.
388  // Note : this is just changing an integer value, so it only takes a couple of instructions.
389  pthread_mutex_lock(&newDiagnosticsIndexLock_);
390  newDiagnosticsIndex_=oldDiagnosticsIndex;
391  pthread_mutex_unlock(&newDiagnosticsIndexLock_);
392 
393  // Done, unlock
394  pthread_mutex_unlock(&diagnosticsLock_);
395 }
396 
397 
398 int EthercatDevice::readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
399 {
400  unsigned char *p = (unsigned char *)buffer;
401  EC_Logic *logic = EC_Logic::instance();
402 
403  NPRW_Telegram nprw_telegram(logic->get_idx(),
404  sh->get_station_address(),
405  address,
406  logic->get_wkc(),
407  length,
408  p);
409 
410  APRW_Telegram aprw_telegram(logic->get_idx(), // Index
411  -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
412  address, // ESC physical memory address (start address)
413  logic->get_wkc(), // Working counter
414  length, // Data Length,
415  p); // Buffer to put read result into
416 
417  // Put read telegram in ethercat/ethernet frame
418  EC_Telegram * telegram = NULL;
419  if (addrMode == FIXED_ADDR) {
420  telegram = &nprw_telegram;
421  } else if (addrMode == POSITIONAL_ADDR) {
422  telegram = &aprw_telegram;
423  } else {
424  assert(0);
425  return -1;
426  }
427 
428  // Put telegram in ethercat/ethernet frame
429  EC_Ethernet_Frame frame(telegram);
430 
431  // Send/Recv data from slave
432  if (!com->txandrx_once(&frame)) {
433  return -1;
434  }
435 
436  // In some cases (clearing status mailbox) this is expected to occur
437  if (telegram->get_wkc() != 3) {
438  return -2;
439  }
440 
441  return 0;
442 }
443 
444 
445 int EthercatDevice::readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void* buffer, EC_UINT length, AddrMode addrMode)
446 {
447  unsigned char *p = (unsigned char *)buffer;
448  EC_Logic *logic = EC_Logic::instance();
449 
450  NPRD_Telegram nprd_telegram(logic->get_idx(),
451  sh->get_station_address(),
452  address,
453  logic->get_wkc(),
454  length,
455  p);
456 
457  APRD_Telegram aprd_telegram(logic->get_idx(), // Index
458  -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
459  address, // ESC physical memory address (start address)
460  logic->get_wkc(), // Working counter
461  length, // Data Length,
462  p); // Buffer to put read result into
463 
464  // Put read telegram in ethercat/ethernet frame
465  EC_Telegram * telegram = NULL;
466  if (addrMode == FIXED_ADDR) {
467  telegram = &nprd_telegram;
468  } else if (addrMode == POSITIONAL_ADDR) {
469  telegram = &aprd_telegram;
470  } else {
471  assert(0);
472  return -1;
473  }
474 
475  // Put telegram in ethercat/ethernet frame
476  EC_Ethernet_Frame frame(telegram);
477 
478  // Send/Recv data from slave
479  if (!com->txandrx(&frame)) {
480  return -1;
481  }
482 
483  // In some cases (clearing status mailbox) this is expected to occur
484  if (telegram->get_wkc() != 1) {
485  return -2;
486  }
487 
488  return 0;
489 }
490 
491 
492 int EthercatDevice::writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode)
493 {
494  unsigned char const *p = (unsigned char const*)buffer;
495  EC_Logic *logic = EC_Logic::instance();
496 
497  NPWR_Telegram npwr_telegram(logic->get_idx(),
498  sh->get_station_address(),
499  address,
500  logic->get_wkc(),
501  length,
502  p);
503 
504  APWR_Telegram apwr_telegram(logic->get_idx(), // Index
505  -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) (
506  address, // ESC physical memory address (start address)
507  logic->get_wkc(), // Working counter
508  length, // Data Length,
509  p); // Buffer to put read result into
510 
511  // Put read telegram in ethercat/ethernet frame
512  EC_Telegram * telegram = NULL;
513  if (addrMode == FIXED_ADDR) {
514  telegram = &npwr_telegram;
515  } else if (addrMode == POSITIONAL_ADDR) {
516  telegram = &apwr_telegram;
517  } else {
518  assert(0);
519  return -1;
520  }
521 
522  // Put telegram in ethercat/ethernet frame
523  EC_Ethernet_Frame frame(telegram);
524 
525  // Send/Recv data from slave
526  if (!com->txandrx(&frame)) {
527  return -1;
528  }
529 
530  if (telegram->get_wkc() != 1) {
531  return -2;
532  }
533 
534  return 0;
535 }
536 
537 
539 {
540  if (numPorts>4) {
541  assert(numPorts<4);
542  numPorts=4;
543  }
544 
545  // By locking index, diagnostics double-buffer cannot be swapped.
546  // Update thread is only allowed to change 'old' diagnostics buffer,
547  // so new buffer data cannot be changed while index lock is held.
548  pthread_mutex_lock(&newDiagnosticsIndexLock_);
549 
551  newDiag.publish(d, numPorts);
552 
553  pthread_mutex_unlock(&newDiagnosticsIndexLock_);
554 }
555 
556 
558 {
559  stringstream str;
560  str << "EtherCAT Device (" << std::setw(2) << std::setfill('0') << sh_->get_ring_position() << ")";
561  d.name = str.str();
562  str.str("");
563  str << sh_->get_product_code() << '-' << sh_->get_serial();
564  d.hardware_id = str.str();
565 
566  d.message = "";
567  d.level = 0;
568 
569  d.clear();
570  d.addf("Position", "%02d", sh_->get_ring_position());
571  d.addf("Product code", "%08x", sh_->get_product_code());
572  d.addf("Serial", "%08x", sh_->get_serial());
573  d.addf("Revision", "%08x", sh_->get_revision());
574 
575  this->ethercatDiagnostics(d, 4); //assume unknown device has 4 ports
576 }
577 
578 void EthercatDevice::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
579 {
580  // Clean up recycled status object before reusing it.
583 
584  // If child-class does not implement multiDiagnostics(), fall back to using slave's diagnostic() function
586  vec.push_back(diagnostic_status_);
587 }
#define ROS_FATAL(...)
static const EC_UINT BASE_ADDR
unsigned int command_size_
EthercatPortDiagnostics portDiagnostics_[4]
bool isClosed(unsigned port)
uint16_t length
Definition: wg_util.h:119
bool isGreaterThan(unsigned value) const
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
For EtherCAT devices that publish more than one EtherCAT Status message. If sub-class implements mult...
void collect(EthercatCom *com, EtherCAT_SlaveHandler *sh)
EthercatDeviceDiagnostics deviceDiagnostics[2]
void ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts)
Adds diagnostic information for EtherCAT ports.
pthread_mutex_t newDiagnosticsIndexLock_
et1x00_error_counters errorCountersPrev_
unsigned newDiagnosticsIndex_
static int writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void const *buffer, EC_UINT length, AddrMode addrMode)
Write data to device ESC.
diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_
static const EC_UINT BASE_ADDR
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
void addf(const std::string &key, const char *format,...)
bool hasCommunication(unsigned port)
bool hasLink(unsigned port)
virtual void collectDiagnostics(EthercatCom *com)
pthread_mutex_t diagnosticsLock_
bool hasLink(unsigned port)
virtual ~EthercatDevice()
uint16_t status
bool isClosed(unsigned port)
static int readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode)
Read then write data to ESC.
virtual void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
For EtherCAT device that only publish one EtherCAT Status message. If sub-class implements multiDiagn...
virtual bool txandrx_once(struct EtherCAT_Frame *frame)=0
EtherCAT_SlaveHandler * sh_
void accumulate(const et1x00_error_counters &next, const et1x00_error_counters &prev)
bool hasCommunication(unsigned port)
static int readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode)
Read data from device ESC.
uint8_t forwarded_rx_error[4]
unsigned int status_size_
void mergeSummaryf(unsigned char lvl, const char *format,...)
#define ROS_ERROR(...)
virtual bool txandrx(struct EtherCAT_Frame *frame)=0
void publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts=4) const


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Tue Mar 28 2023 02:10:20