wg_mailbox.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 
37 #include "dll/ethercat_device_addressed_telegram.h"
39 
40 namespace ethercat_hardware
41 {
42 
43 // Temporary,, need 'log' fuction that can switch between fprintf and ROS_LOG.
44 #define ERR_MODE "\033[41m"
45 #define STD_MODE "\033[0m"
46 #define WARN_MODE "\033[43m"
47 #define GOOD_MODE "\033[42m"
48 #define INFO_MODE "\033[44m"
49 
50 #define ERROR_HDR "\033[41mERROR\033[0m"
51 #define WARN_HDR "\033[43mERROR\033[0m"
52 
53 
54 
56 
57 struct WG0XMbxHdr
58 {
59  uint16_t address_;
60  union
61  {
62  uint16_t command_;
63  struct
64  {
65  uint16_t length_:12;
66  uint16_t seqnum_: 3; // bits[14:12] sequence number, 0=disable, 1-7 normal sequence number
67  uint16_t write_nread_:1;
68  }__attribute__ ((__packed__));
69  };
70  uint8_t checksum_;
71 
72  bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum);
73  bool verifyChecksum(void) const;
74 }__attribute__ ((__packed__));
75 
76 static const unsigned MBX_SIZE = 512;
77 static const unsigned MBX_DATA_SIZE = (MBX_SIZE - sizeof(WG0XMbxHdr) - 1);
78 struct WG0XMbxCmd
79 {
80  WG0XMbxHdr hdr_;
81  uint8_t data_[MBX_DATA_SIZE];
82  uint8_t checksum_;
83 
84  bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data);
85 }__attribute__ ((__packed__));
86 
87 
88 
89 bool WG0XMbxHdr::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum)
90 {
91  if (type==LOCAL_BUS_WRITE)
92  {
93  if (length > MBX_DATA_SIZE)
94  {
95  fprintf(stderr, "size of %d is too large for write\n", length);
96  return false;
97  }
98  }
99  else if (type==LOCAL_BUS_READ)
100  {
101  // Result of mailbox read, only stores result data + 1byte checksum
102  if (length > (MBX_SIZE-1))
103  {
104  fprintf(stderr, "size of %d is too large for read\n", length);
105  return false;
106  }
107  }
108  else {
109  assert(0 && "invalid MbxCmdType");
110  return false;
111  }
112 
113  address_ = address;
115  seqnum_ = seqnum;
116  write_nread_ = (type==LOCAL_BUS_WRITE) ? 1 : 0;
117  checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(this, sizeof(*this) - 1));
118  return true;
119 }
120 
122 {
123  return wg_util::computeChecksum(this, sizeof(*this)) != 0;
124 }
125 
126 bool WG0XMbxCmd::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data)
127 {
128  if (!this->hdr_.build(address, length, type, seqnum))
129  {
130  return false;
131  }
132 
133  if (data != NULL)
134  {
135  memcpy(data_, data, length);
136  }
137  else
138  {
139  memset(data_, 0, length);
140  }
141  unsigned int checksum = wg_util::rotateRight8(wg_util::computeChecksum(data_, length));
142  data_[length] = checksum;
143  return true;
144 }
145 
146 
148  write_errors_(0),
149  read_errors_(0),
150  lock_errors_(0),
151  retries_(0),
152  retry_errors_(0)
153 {
154  // Empty
155 }
156 
157 
165 int timediff_ms(const timespec &current, const timespec &start)
166 {
167  int timediff_ms = (current.tv_sec-start.tv_sec)*1000 // 1000 ms in a sec
168  + (current.tv_nsec-start.tv_nsec)/1000000; // 1000000 ns in a ms
169  return timediff_ms;
170 }
171 
172 
180 int safe_clock_gettime(clockid_t clk_id, timespec *time)
181 {
182  int result = clock_gettime(clk_id, time);
183  if (result != 0) {
184  int error = errno;
185  fprintf(stderr, "safe_clock_gettime : %s\n", strerror(error));
186  return result;
187  }
188  return result;
189 }
190 
191 
199 void safe_usleep(uint32_t usec)
200 {
201  assert(usec<1000000);
202  if (usec>1000000)
203  usec=1000000;
204  struct timespec req, rem;
205  req.tv_sec = 0;
206  req.tv_nsec = usec*1000;
207  while (nanosleep(&req, &rem)!=0) {
208  int error = errno;
209  fprintf(stderr,"%s : Error : %s\n", __func__, strerror(error));
210  if (error != EINTR) {
211  break;
212  }
213  req = rem;
214  }
215  return;
216 }
217 
218 
219 
220 
221 void updateIndexAndWkc(EC_Telegram *tg, EC_Logic *logic)
222 {
223  tg->set_idx(logic->get_idx());
224  tg->set_wkc(logic->get_wkc());
225 }
226 
227 WGMailbox::WGMailbox() : sh_(NULL)
228 {
229  int error;
230  if ((error = pthread_mutex_init(&mailbox_lock_, NULL)) != 0)
231  {
232  ROS_ERROR("WG0X : init mailbox mutex :%s", strerror(error));
233  }
234 }
235 
236 bool WGMailbox::initialize(EtherCAT_SlaveHandler *sh)
237 {
238  sh_ = sh;
239  return true;
240 }
241 
242 bool WGMailbox::verifyDeviceStateForMailboxOperation()
243 {
244  // Make sure slave is in correct state to do use mailbox
245  EC_State state = sh_->get_state();
246  if ((state != EC_SAFEOP_STATE) && (state != EC_OP_STATE)) {
247  fprintf(stderr, "%s : " ERROR_HDR
248  "cannot do mailbox read in current device state = %d\n", __func__, state);
249  return false;
250  }
251  return true;
252 }
253 
254 
264 void WGMailbox::diagnoseMailboxError(EthercatCom *com)
265 {
266 
267 }
268 
277 bool WGMailbox::clearReadMailbox(EthercatCom *com)
278 {
279  if (!verifyDeviceStateForMailboxOperation()){
280  return false;
281  }
282 
283  EC_Logic *logic = EC_Logic::instance();
284  EC_UINT station_addr = sh_->get_station_address();
285 
286  // Create Ethernet packet with two EtherCAT telegrams inside of it :
287  // - One telegram to read first byte of mailbox
288  // - One telegram to read last byte of mailbox
289  unsigned char unused[1] = {0};
290  NPRD_Telegram read_start(
291  logic->get_idx(),
292  station_addr,
293  MBX_STATUS_PHY_ADDR,
294  logic->get_wkc(),
295  sizeof(unused),
296  unused);
297  NPRD_Telegram read_end(
298  logic->get_idx(),
299  station_addr,
300  MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
301  logic->get_wkc(),
302  sizeof(unused),
303  unused);
304  read_start.attach(&read_end);
305  EC_Ethernet_Frame frame(&read_start);
306 
307 
308  // Retry sending packet multiple times
309  bool success=false;
310  static const unsigned MAX_DROPS = 15;
311  for (unsigned tries=0; tries<MAX_DROPS; ++tries) {
312  success = com->txandrx_once(&frame);
313  if (success) {
314  break;
315  }
316  updateIndexAndWkc(&read_start, logic);
317  updateIndexAndWkc(&read_end , logic);
318  }
319 
320  if (!success) {
321  fprintf(stderr, "%s : " ERROR_HDR
322  " too much packet loss\n", __func__);
323  safe_usleep(100);
324  return false;
325  }
326 
327  // Check result for consistancy
328  if (read_start.get_wkc() != read_end.get_wkc()) {
329  fprintf(stderr, "%s : " ERROR_HDR
330  "read mbx working counters are inconsistant, %d, %d\n",
331  __func__, read_start.get_wkc(), read_end.get_wkc());
332  return false;
333  }
334  if (read_start.get_wkc() > 1) {
335  fprintf(stderr, "%s : " ERROR_HDR
336  "more than one device (%d) responded \n", __func__, read_start.get_wkc());
337  return false;
338  }
339  if (read_start.get_wkc() == 1) {
340  fprintf(stderr, "%s : " WARN_MODE "WARN" STD_MODE
341  " read mbx contained garbage data\n", __func__);
342  // Not an error, just warning
343  }
344 
345  return true;
346 }
347 
348 
349 
359 bool WGMailbox::waitForReadMailboxReady(EthercatCom *com)
360 {
361  // Wait upto 100ms for device to toggle ack
362  static const int MAX_WAIT_TIME_MS = 100;
363  int timediff;
364  unsigned good_results=0;
365 
366 
367  struct timespec start_time, current_time;
368  if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
369  return false;
370  }
371 
372  do {
373  // Check if mailbox is full by looking at bit 3 of SyncMan status register.
374  uint8_t SyncManStatus=0;
375  const unsigned SyncManAddr = 0x805+(MBX_STATUS_SYNCMAN_NUM*8);
376  if (EthercatDevice::readData(com, sh_, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), EthercatDevice::FIXED_ADDR) == 0) {
377  ++good_results;
378  const uint8_t MailboxStatusMask = (1<<3);
379  if (SyncManStatus & MailboxStatusMask) {
380  return true;
381  }
382  }
383  if (safe_clock_gettime(CLOCK_MONOTONIC, &current_time)!=0) {
384  return false;
385  }
386  timediff = timediff_ms(current_time, start_time);
387  safe_usleep(100);
388  } while (timediff < MAX_WAIT_TIME_MS);
389 
390  if (good_results == 0) {
391  fprintf(stderr, "%s : " ERROR_HDR
392  " error reading from device\n", __func__);
393  } else {
394  fprintf(stderr, "%s : " ERROR_HDR
395  " error read mbx not full after %d ms\n", __func__, timediff);
396  }
397 
398  return false;
399 }
400 
401 
411 bool WGMailbox::waitForWriteMailboxReady(EthercatCom *com)
412 {
413  // Wait upto 100ms for device to toggle ack
414  static const int MAX_WAIT_TIME_MS = 100;
415  int timediff;
416  unsigned good_results=0;
417 
418 
419  struct timespec start_time, current_time;
420  if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
421  return false;
422  }
423 
424  do {
425  // Check if mailbox is full by looking at bit 3 of SyncMan status register.
426  uint8_t SyncManStatus=0;
427  const unsigned SyncManAddr = 0x805+(MBX_COMMAND_SYNCMAN_NUM*8);
428  if (EthercatDevice::readData(com, sh_, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), EthercatDevice::FIXED_ADDR) == 0) {
429  ++good_results;
430  const uint8_t MailboxStatusMask = (1<<3);
431  if ( !(SyncManStatus & MailboxStatusMask) ) {
432  return true;
433  }
434  }
435  if (safe_clock_gettime(CLOCK_MONOTONIC, &current_time)!=0) {
436  return false;
437  }
438  timediff = timediff_ms(current_time, start_time);
439  safe_usleep(100);
440  } while (timediff < MAX_WAIT_TIME_MS);
441 
442  if (good_results == 0) {
443  fprintf(stderr, "%s : " ERROR_HDR
444  " error reading from device\n", __func__);
445  } else {
446  fprintf(stderr, "%s : " ERROR_HDR
447  " error write mbx not empty after %d ms\n", __func__, timediff);
448  }
449 
450  return false;
451 }
452 
453 
454 
466 bool WGMailbox::writeMailboxInternal(EthercatCom *com, void const *data, unsigned length)
467 {
468  if (length > MBX_COMMAND_SIZE) {
469  assert(length <= MBX_COMMAND_SIZE);
470  return false;
471  }
472 
473  // Make sure slave is in correct state to use mailbox
474  if (!verifyDeviceStateForMailboxOperation()){
475  return false;
476  }
477 
478  EC_Logic *logic = EC_Logic::instance();
479  EC_UINT station_addr = sh_->get_station_address();
480 
481 
482  // If there enough savings, split mailbox write up into 2 parts :
483  // 1. Write of actual data to begining of mbx buffer
484  // 2. Write of last mbx buffer byte, to complete write
485  static const unsigned TELEGRAM_OVERHEAD = 50;
486  bool split_write = (length+TELEGRAM_OVERHEAD) < MBX_COMMAND_SIZE;
487 
488  unsigned write_length = MBX_COMMAND_SIZE;
489  if (split_write) {
490  write_length = length;
491  }
492 
493  // Possible do multiple things at once...
494  // 1. Clear read mailbox by reading both first and last mailbox bytes
495  // 2. Write data into write mailbox
496  {
497  // Build frame with 2-NPRD + 2 NPWR
498  unsigned char unused[1] = {0};
499  NPWR_Telegram write_start(
500  logic->get_idx(),
501  station_addr,
502  MBX_COMMAND_PHY_ADDR,
503  logic->get_wkc(),
504  write_length,
505  (const unsigned char*) data);
506  NPWR_Telegram write_end(
507  logic->get_idx(),
508  station_addr,
509  MBX_COMMAND_PHY_ADDR+MBX_COMMAND_SIZE-1,
510  logic->get_wkc(),
511  sizeof(unused),
512  unused);
513 
514  if (split_write) {
515  write_start.attach(&write_end);
516  }
517 
518  EC_Ethernet_Frame frame(&write_start);
519 
520  // Try multiple times, but remember number of of successful sends
521  unsigned sends=0;
522  bool success=false;
523  for (unsigned tries=0; (tries<10) && !success; ++tries) {
524  success = com->txandrx_once(&frame);
525  if (!success) {
526  updateIndexAndWkc(&write_start, logic);
527  updateIndexAndWkc(&write_end, logic);
528  }
529  ++sends; //EtherCAT_com d/n support split TX and RX class, assume tx part of txandrx always succeeds
530  /*
531  int handle = com->tx(&frame);
532  if (handle > 0) {
533  ++sends;
534  success = com->rx(&frame, handle);
535  }
536  if (!success) {
537  updateIndexAndWkc(&write_start, logic);
538  updateIndexAndWkc(&write_end, logic);
539  }
540  */
541  }
542  if (!success) {
543  fprintf(stderr, "%s : " ERROR_HDR
544  " too much packet loss\n", __func__);
545  safe_usleep(100);
546  return false;
547  }
548 
549  if (split_write && (write_start.get_wkc() != write_end.get_wkc())) {
550  fprintf(stderr, "%s : " ERROR_HDR
551  " write mbx working counters are inconsistant\n", __func__);
552  return false;
553  }
554 
555  if (write_start.get_wkc() > 1)
556  {
557  fprintf(stderr, "%s : " ERROR_HDR
558  " multiple (%d) devices responded to mailbox write\n", __func__, write_start.get_wkc());
559  return false;
560  }
561  else if (write_start.get_wkc() != 1)
562  {
563  // Write to cmd mbx was refused
564  if (sends<=1) {
565  // Packet was only sent once, there must be a problem with slave device
566  fprintf(stderr, "%s : " ERROR_HDR
567  " initial mailbox write refused\n", __func__);
568  safe_usleep(100);
569  return false;
570  } else {
571  // Packet was sent multiple times because a packet drop occured
572  // If packet drop occured on return path from device, a refusal is acceptable
573  fprintf(stderr, "%s : " WARN_HDR
574  " repeated mailbox write refused\n", __func__);
575  }
576  }
577  }
578 
579  return true;
580 }
581 
582 bool WGMailbox::readMailboxRepeatRequest(EthercatCom *com)
583 {
584  bool success = _readMailboxRepeatRequest(com);
585  ++mailbox_diagnostics_.retries_;
586  if (!success) {
587  ++mailbox_diagnostics_.retry_errors_;
588  }
589  return success;
590 }
591 
592 bool WGMailbox::_readMailboxRepeatRequest(EthercatCom *com)
593 {
594  // Toggle repeat request flag, wait for ack from device
595  // Returns true if ack is received, false for failure
596  SyncMan sm;
597  if (!sm.readData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
598  fprintf(stderr, "%s : " ERROR_HDR
599  " could not read status mailbox syncman (1)\n", __func__);
600  return false;
601  }
602 
603  // If device can handle repeat requests, then request and ack bit should already match
604  if (sm.activate.repeat_request != sm.pdi_control.repeat_ack) {
605  fprintf(stderr, "%s : " ERROR_HDR
606  " syncman repeat request and ack do not match\n", __func__);
607  return false;
608  }
609 
610  // Write toggled repeat request,,, wait for ack.
611  SyncManActivate orig_activate(sm.activate);
612  sm.activate.repeat_request = ~orig_activate.repeat_request;
613  if (!sm.activate.writeData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
614  fprintf(stderr, "%s : " ERROR_HDR
615  " could not write syncman repeat request\n", __func__);
616  //ec_mark(sh->getEM(), "could not write syncman repeat request", 1);
617  return false;
618  }
619 
620  // Wait upto 100ms for device to toggle ack
621  static const int MAX_WAIT_TIME_MS = 100;
622  int timediff;
623 
624  struct timespec start_time, current_time;
625  if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
626  return false;
627  }
628 
629  do {
630  if (!sm.readData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
631  fprintf(stderr, "%s : " ERROR_HDR
632  " could not read status mailbox syncman (2)\n", __func__);
633  return false;
634  }
635 
636  if (sm.activate.repeat_request == sm.pdi_control.repeat_ack) {
637  // Device responded, to some checks to make sure it seems to be telling the truth
638  if (sm.status.mailbox_status != 1) {
639  fprintf(stderr, "%s : " ERROR_HDR
640  " got repeat response, but read mailbox is still empty\n", __func__);
641  //sm.print(WG0X_MBX_Status_Syncman_Num, std::cerr);
642  return false;
643  }
644  return true;
645  }
646 
647  if ( (sm.activate.repeat_request) == (orig_activate.repeat_request) ) {
648  fprintf(stderr, "%s : " ERROR_HDR
649  " syncman repeat request was changed while waiting for response\n", __func__);
650  //sm.activate.print();
651  //orig_activate.print();
652  return false;
653  }
654 
655  if (safe_clock_gettime(CLOCK_MONOTONIC, &current_time)!=0) {
656  return false;
657  }
658 
659  timediff = timediff_ms(current_time, start_time);
660  safe_usleep(100);
661  } while (timediff < MAX_WAIT_TIME_MS);
662 
663  fprintf(stderr, "%s : " ERROR_HDR
664  " error repeat request not acknowledged after %d ms\n", __func__, timediff);
665  return false;
666 }
667 
668 
669 
681 bool WGMailbox::readMailboxInternal(EthercatCom *com, void *data, unsigned length)
682 {
683  static const unsigned MAX_TRIES = 10;
684  static const unsigned MAX_DROPPED = 10;
685 
686  if (length > MBX_STATUS_SIZE) {
687  assert(length <= MBX_STATUS_SIZE);
688  return false;
689  }
690 
691  // Make sure slave is in correct state to use mailbox
692  if (!verifyDeviceStateForMailboxOperation()){
693  return false;
694  }
695 
696  EC_Logic *logic = EC_Logic::instance();
697  EC_UINT station_addr = sh_->get_station_address();
698 
699 
700  // If read is small enough :
701  // 1. read just length bytes in one telegram
702  // 2. then read last byte to empty mailbox
703  static const unsigned TELEGRAM_OVERHEAD = 50;
704  bool split_read = (length+TELEGRAM_OVERHEAD) < MBX_STATUS_SIZE;
705 
706  unsigned read_length = MBX_STATUS_SIZE;
707  if (split_read) {
708  read_length = length;
709  }
710 
711  unsigned char unused[1] = {0};
712  NPRD_Telegram read_start(
713  logic->get_idx(),
714  station_addr,
715  MBX_STATUS_PHY_ADDR,
716  logic->get_wkc(),
717  read_length,
718  (unsigned char*) data);
719  NPRD_Telegram read_end(
720  logic->get_idx(),
721  station_addr,
722  MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
723  logic->get_wkc(),
724  sizeof(unused),
725  unused);
726 
727  if (split_read) {
728  read_start.attach(&read_end);
729  }
730 
731  EC_Ethernet_Frame frame(&read_start);
732 
733  unsigned tries = 0;
734  unsigned total_dropped =0;
735  for (tries=0; tries<MAX_TRIES; ++tries) {
736 
737  // Send read - keep track of how many packets were dropped (for later)
738  unsigned dropped=0;
739  for (dropped=0; dropped<MAX_DROPPED; ++dropped) {
740  if (com->txandrx_once(&frame)) {
741  break;
742  }
743  ++total_dropped;
744  updateIndexAndWkc(&read_start , logic);
745  updateIndexAndWkc(&read_end , logic);
746  }
747 
748  if (dropped>=MAX_DROPPED) {
749  fprintf(stderr, "%s : " ERROR_HDR
750  " too many dropped packets : %d\n", __func__, dropped);
751  }
752 
753  if (split_read && (read_start.get_wkc() != read_end.get_wkc())) {
754  fprintf(stderr, "%s : " ERROR_HDR
755  "read mbx working counters are inconsistant\n", __func__);
756  return false;
757  }
758 
759  if (read_start.get_wkc() == 0) {
760  if (dropped == 0) {
761  fprintf(stderr, "%s : " ERROR_HDR
762  " inconsistancy : got wkc=%d with no dropped packets\n",
763  __func__, read_start.get_wkc());
764  fprintf(stderr, "total dropped = %d\n", total_dropped);
765  return false;
766  } else {
767  // Packet was dropped after doing read from device,,,
768  // Ask device to repost data, so it can be read again.
769  fprintf(stderr, "%s : " WARN_HDR
770  " asking for read repeat after dropping %d packets\n", __func__, dropped);
771  if (!readMailboxRepeatRequest(com)) {
772  return false;
773  }
774  continue;
775  }
776  } else if (read_start.get_wkc() == 1) {
777  // Successfull read of status data
778  break;
779  } else {
780  fprintf(stderr, "%s : " ERROR_HDR
781  " invalid wkc for read : %d\n", __func__, read_start.get_wkc());
782  diagnoseMailboxError(com);
783  return false;
784  }
785  }
786 
787  if (tries >= MAX_TRIES) {
788  fprintf(stderr, "%s : " ERROR_HDR
789  " could not get responce from device after %d retries, %d total dropped packets\n",
790  __func__, tries, total_dropped);
791  diagnoseMailboxError(com);
792  return false;
793  }
794 
795  return true;
796 }
797 
798 
812 int WGMailbox::readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
813 {
814  if (!lockMailbox())
815  return -1;
816 
817  int result = readMailbox_(com, address, data, length);
818  if (result != 0) {
819  ++mailbox_diagnostics_.read_errors_;
820  }
821 
822  unlockMailbox();
823  return result;
824 }
825 
831 int WGMailbox::readMailbox_(EthercatCom *com, unsigned address, void *data, unsigned length)
832 {
833  // Make sure slave is in correct state to use mailbox
834  if (!verifyDeviceStateForMailboxOperation()){
835  return false;
836  }
837 
838  // 1. Clear read (status) mailbox by reading it first
839  if (!clearReadMailbox(com))
840  {
841  fprintf(stderr, "%s : " ERROR_HDR
842  " clearing read mbx\n", __func__);
843  return -1;
844  }
845 
846  // 2. Put a (read) request into command mailbox
847  {
848  WG0XMbxCmd cmd;
849  if (!cmd.build(address, length, LOCAL_BUS_READ, sh_->get_mbx_counter(), data))
850  {
851  fprintf(stderr, "%s : " ERROR_HDR
852  " builing mbx header\n", __func__);
853  return -1;
854  }
855 
856  if (!writeMailboxInternal(com, &cmd.hdr_, sizeof(cmd.hdr_)))
857  {
858  fprintf(stderr, "%s : " ERROR_HDR " write of cmd failed\n", __func__);
859  return -1;
860  }
861  }
862 
863  // Wait for result (in read mailbox) to become ready
864  if (!waitForReadMailboxReady(com))
865  {
866  fprintf(stderr, "%s : " ERROR_HDR
867  "waiting for read mailbox\n", __func__);
868  return -1;
869  }
870 
871  // Read result back from mailbox.
872  // It could take the FPGA some time to respond to a request.
873  // Since the read mailbox is initiall cleared, any read to the mailbox
874  // should be refused (WKC==0) until WG0x FPGA has written it result into it.
875  // NOTE: For this to work the mailbox syncmanagers must be set up.
876  // TODO 1: Packets may get lost on return route to device.
877  // In this case, the device will keep responding to the repeated packets with WKC=0.
878  // To work correctly, the repeat request bit needs to be toggled.
879  // TODO 2: Need a better method to determine if data read from status mailbox.
880  // is the right data, or just junk left over from last time.
881  {
882  WG0XMbxCmd stat;
883  memset(&stat,0,sizeof(stat));
884  // Read data + 1byte checksum from mailbox
885  if (!readMailboxInternal(com, &stat, length+1))
886  {
887  fprintf(stderr, "%s : " ERROR_HDR " read failed\n", __func__);
888  return -1;
889  }
890 
891  if (wg_util::computeChecksum(&stat, length+1) != 0)
892  {
893  fprintf(stderr, "%s : " ERROR_HDR
894  "checksum error reading mailbox data\n", __func__);
895  fprintf(stderr, "length = %d\n", length);
896  return -1;
897  }
898  memcpy(data, &stat, length);
899  }
900 
901  return 0;
902 
903 
904 }
905 
906 bool WGMailbox::lockMailbox()
907 {
908  int error = pthread_mutex_lock(&mailbox_lock_);
909  if (error != 0) {
910  fprintf(stderr, "%s : " ERROR_HDR " getting mbx lock\n", __func__);
911  ++mailbox_diagnostics_.lock_errors_;
912  return false;
913  }
914  return true;
915 }
916 
917 void WGMailbox::unlockMailbox()
918 {
919  int error = pthread_mutex_unlock(&mailbox_lock_);
920  if (error != 0) {
921  fprintf(stderr, "%s : " ERROR_HDR " freeing mbx lock\n", __func__);
922  ++mailbox_diagnostics_.lock_errors_;
923  }
924 }
925 
926 
939 int WGMailbox::writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
940 {
941  if (!lockMailbox())
942  return -1;
943 
944  int result = writeMailbox_(com, address, data, length);
945  if (result != 0) {
946  ++mailbox_diagnostics_.write_errors_;
947  }
948 
949  unlockMailbox();
950 
951  return result;
952 }
953 
959 int WGMailbox::writeMailbox_(EthercatCom *com, unsigned address, void const *data, unsigned length)
960 {
961  // Make sure slave is in correct state to use mailbox
962  if (!verifyDeviceStateForMailboxOperation()){
963  return -1;
964  }
965 
966  // Build message and put it into write mailbox
967  {
968  WG0XMbxCmd cmd;
969  if (!cmd.build(address, length, LOCAL_BUS_WRITE, sh_->get_mbx_counter(), data)) {
970  fprintf(stderr, "%s : " ERROR_HDR " builing mbx header\n", __func__);
971  return -1;
972  }
973 
974  unsigned write_length = sizeof(cmd.hdr_)+length+sizeof(cmd.checksum_);
975  if (!writeMailboxInternal(com, &cmd, write_length)) {
976  fprintf(stderr, "%s : " ERROR_HDR " write failed\n", __func__);
977  diagnoseMailboxError(com);
978  return -1;
979  }
980  }
981 
982  // TODO: Change slave firmware so that we can verify that localbus write was truly executed
983  // Checking that device emptied write mailbox will have to suffice for now.
984  if (!waitForWriteMailboxReady(com)) {
985  fprintf(stderr, "%s : " ERROR_HDR
986  "write mailbox\n", __func__);
987  }
988 
989  return 0;
990 }
991 
992 
993 void WGMailbox::publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
994 {
995  if (lockMailbox()) {
996  mailbox_publish_diagnostics_ = mailbox_diagnostics_;
997  unlockMailbox();
998  }
999 
1000  MbxDiagnostics const &m(mailbox_publish_diagnostics_);
1001  d.addf("Mailbox Write Errors", "%d", m.write_errors_);
1002  d.addf("Mailbox Read Errors", "%d", m.read_errors_);
1003  d.addf("Mailbox Retries", "%d", m.retries_);
1004  d.addf("Mailbox Retry Errors", "%d", m.retry_errors_);
1005 }
1006 
1007 
1008 }; //end namespace ethercat_hardware
EthercatCom
Definition: ethercat_com.h:44
ethercat_hardware::WG0XMbxHdr::__attribute__
union ethercat_hardware::WG0XMbxHdr::@63 __attribute__
EthercatDevice::FIXED_ADDR
@ FIXED_ADDR
Definition: ethercat_device.h:189
ethercat_hardware::WG0XMbxHdr::write_nread_
uint16_t write_nread_
Definition: wg_mailbox.cpp:99
ethercat_hardware::WGMailbox::WGMailbox
WGMailbox()
Definition: wg_mailbox.cpp:259
WARN_HDR
#define WARN_HDR
Definition: wg_mailbox.cpp:83
ethercat_hardware
Definition: motor_heating_model.h:53
ethercat_hardware::WG0XMbxCmd::checksum_
uint8_t checksum_
Definition: wg_mailbox.cpp:114
ethercat_hardware::MbxDiagnostics::read_errors_
uint32_t read_errors_
Definition: wg_mailbox.h:112
ethercat_hardware::LOCAL_BUS_WRITE
@ LOCAL_BUS_WRITE
Definition: wg_mailbox.cpp:87
data
data
ethercat_hardware::safe_usleep
void safe_usleep(uint32_t usec)
safe version of usleep.
Definition: wg_mailbox.cpp:231
ethercat_hardware::MBX_SIZE
static const unsigned MBX_SIZE
Definition: wg_mailbox.cpp:108
ethercat_hardware::MbxDiagnostics::MbxDiagnostics
MbxDiagnostics()
Definition: wg_mailbox.cpp:179
ethercat_hardware::wg_util::computeChecksum
unsigned computeChecksum(void const *data, unsigned length)
Definition: wg_util.cpp:81
ethercat_hardware::MbxDiagnostics::retry_errors_
uint32_t retry_errors_
Definition: wg_mailbox.h:115
ethercat_hardware::WG0XMbxHdr::build
bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum)
Definition: wg_mailbox.cpp:121
ethercat_hardware::timediff_ms
int timediff_ms(const timespec &current, const timespec &start)
Find difference between two timespec values.
Definition: wg_mailbox.cpp:197
d
d
ethercat_hardware::WG0XMbxHdr::verifyChecksum
bool verifyChecksum(void) const
Definition: wg_mailbox.cpp:153
ethercat_hardware::MBX_DATA_SIZE
static const unsigned MBX_DATA_SIZE
Definition: wg_mailbox.cpp:109
ethercat_hardware::MbxDiagnostics::write_errors_
uint32_t write_errors_
Definition: wg_mailbox.h:111
STD_MODE
#define STD_MODE
Definition: wg_mailbox.cpp:77
ethercat_hardware::WG0XMbxHdr::address_
uint16_t address_
Definition: wg_mailbox.cpp:91
ethercat_hardware::WG0XMbxHdr::length_
uint16_t length_
Definition: wg_mailbox.cpp:97
WARN_MODE
#define WARN_MODE
Definition: wg_mailbox.cpp:78
ethercat_hardware::LOCAL_BUS_READ
@ LOCAL_BUS_READ
Definition: wg_mailbox.cpp:87
wg_util.h
ethercat_hardware::safe_clock_gettime
int safe_clock_gettime(clockid_t clk_id, timespec *time)
error checking wrapper around clock_gettime
Definition: wg_mailbox.cpp:212
ethercat_hardware::WG0XMbxCmd::build
bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const *data)
Definition: wg_mailbox.cpp:158
ethercat_device.h
start
ROSCPP_DECL void start()
EthercatCom::txandrx_once
virtual bool txandrx_once(struct EtherCAT_Frame *frame)=0
ERROR_HDR
#define ERROR_HDR
Definition: wg_mailbox.cpp:82
ethercat_hardware::MbxCmdType
MbxCmdType
Definition: wg_mailbox.cpp:87
ethercat_hardware::MbxDiagnostics::retries_
uint32_t retries_
Definition: wg_mailbox.h:114
ethercat_hardware::__attribute__
ethercat_hardware::MotorHeatingModelCommon __attribute__
write_end
def write_end(s, spec)
length
uint16_t length
Definition: wg_util.h:151
ROS_ERROR
#define ROS_ERROR(...)
wg_mailbox.h
ethercat_hardware::updateIndexAndWkc
void updateIndexAndWkc(EC_Telegram *tg, EC_Logic *logic)
Definition: wg_mailbox.cpp:253
ethercat_hardware::WG0XMbxCmd::hdr_
WG0XMbxHdr hdr_
Definition: wg_mailbox.cpp:112
diagnostic_updater::DiagnosticStatusWrapper
EthercatDevice::readData
static int readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode)
Read data from device ESC.
Definition: ethercat_device.cpp:445
ethercat_hardware::WG0XMbxHdr::seqnum_
uint16_t seqnum_
Definition: wg_mailbox.cpp:98
ethercat_hardware::MbxDiagnostics
Definition: wg_mailbox.h:76
cmd
string cmd
ethercat_hardware::WG0XMbxCmd
Definition: wg_mailbox.cpp:110
ethercat_hardware::WG0XMbxHdr::checksum_
uint8_t checksum_
Definition: wg_mailbox.cpp:102
ethercat_hardware::wg_util::rotateRight8
unsigned int rotateRight8(unsigned in)
Definition: wg_util.cpp:73
ethercat_hardware::WG0XMbxHdr::command_
uint16_t command_
Definition: wg_mailbox.cpp:94
ethercat_hardware::WG0XMbxCmd::data_
uint8_t data_[MBX_DATA_SIZE]
Definition: wg_mailbox.cpp:113


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Sep 26 2024 02:44:04