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/wg_mailbox.h"
00036 #include "ethercat_hardware/wg_util.h"
00037 #include "dll/ethercat_device_addressed_telegram.h"
00038 #include "ethercat_hardware/ethercat_device.h"
00039
00040 namespace ethercat_hardware
00041 {
00042
00043
00044 #define ERR_MODE "\033[41m"
00045 #define STD_MODE "\033[0m"
00046 #define WARN_MODE "\033[43m"
00047 #define GOOD_MODE "\033[42m"
00048 #define INFO_MODE "\033[44m"
00049
00050 #define ERROR_HDR "\033[41mERROR\033[0m"
00051 #define WARN_HDR "\033[43mERROR\033[0m"
00052
00053
00054
00055 enum MbxCmdType {LOCAL_BUS_READ=1, LOCAL_BUS_WRITE=2};
00056
00057 struct WG0XMbxHdr
00058 {
00059 uint16_t address_;
00060 union
00061 {
00062 uint16_t command_;
00063 struct
00064 {
00065 uint16_t length_:12;
00066 uint16_t seqnum_: 3;
00067 uint16_t write_nread_:1;
00068 }__attribute__ ((__packed__));
00069 };
00070 uint8_t checksum_;
00071
00072 bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum);
00073 bool verifyChecksum(void) const;
00074 }__attribute__ ((__packed__));
00075
00076 static const unsigned MBX_SIZE = 512;
00077 static const unsigned MBX_DATA_SIZE = (MBX_SIZE - sizeof(WG0XMbxHdr) - 1);
00078 struct WG0XMbxCmd
00079 {
00080 WG0XMbxHdr hdr_;
00081 uint8_t data_[MBX_DATA_SIZE];
00082 uint8_t checksum_;
00083
00084 bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data);
00085 }__attribute__ ((__packed__));
00086
00087
00088
00089 bool WG0XMbxHdr::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum)
00090 {
00091 if (type==LOCAL_BUS_WRITE)
00092 {
00093 if (length > MBX_DATA_SIZE)
00094 {
00095 fprintf(stderr, "size of %d is too large for write\n", length);
00096 return false;
00097 }
00098 }
00099 else if (type==LOCAL_BUS_READ)
00100 {
00101
00102 if (length > (MBX_SIZE-1))
00103 {
00104 fprintf(stderr, "size of %d is too large for read\n", length);
00105 return false;
00106 }
00107 }
00108 else {
00109 assert(0 && "invalid MbxCmdType");
00110 return false;
00111 }
00112
00113 address_ = address;
00114 length_ = length - 1;
00115 seqnum_ = seqnum;
00116 write_nread_ = (type==LOCAL_BUS_WRITE) ? 1 : 0;
00117 checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(this, sizeof(*this) - 1));
00118 return true;
00119 }
00120
00121 bool WG0XMbxHdr::verifyChecksum(void) const
00122 {
00123 return wg_util::computeChecksum(this, sizeof(*this)) != 0;
00124 }
00125
00126 bool WG0XMbxCmd::build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data)
00127 {
00128 if (!this->hdr_.build(address, length, type, seqnum))
00129 {
00130 return false;
00131 }
00132
00133 if (data != NULL)
00134 {
00135 memcpy(data_, data, length);
00136 }
00137 else
00138 {
00139 memset(data_, 0, length);
00140 }
00141 unsigned int checksum = wg_util::rotateRight8(wg_util::computeChecksum(data_, length));
00142 data_[length] = checksum;
00143 return true;
00144 }
00145
00146
00147 MbxDiagnostics::MbxDiagnostics() :
00148 write_errors_(0),
00149 read_errors_(0),
00150 lock_errors_(0),
00151 retries_(0),
00152 retry_errors_(0)
00153 {
00154
00155 }
00156
00157
00165 int timediff_ms(const timespec ¤t, const timespec &start)
00166 {
00167 int timediff_ms = (current.tv_sec-start.tv_sec)*1000
00168 + (current.tv_nsec-start.tv_nsec)/1000000;
00169 return timediff_ms;
00170 }
00171
00172
00180 int safe_clock_gettime(clockid_t clk_id, timespec *time)
00181 {
00182 int result = clock_gettime(clk_id, time);
00183 if (result != 0) {
00184 int error = errno;
00185 fprintf(stderr, "safe_clock_gettime : %s\n", strerror(error));
00186 return result;
00187 }
00188 return result;
00189 }
00190
00191
00199 void safe_usleep(uint32_t usec)
00200 {
00201 assert(usec<1000000);
00202 if (usec>1000000)
00203 usec=1000000;
00204 struct timespec req, rem;
00205 req.tv_sec = 0;
00206 req.tv_nsec = usec*1000;
00207 while (nanosleep(&req, &rem)!=0) {
00208 int error = errno;
00209 fprintf(stderr,"%s : Error : %s\n", __func__, strerror(error));
00210 if (error != EINTR) {
00211 break;
00212 }
00213 req = rem;
00214 }
00215 return;
00216 }
00217
00218
00219
00220
00221 void updateIndexAndWkc(EC_Telegram *tg, EC_Logic *logic)
00222 {
00223 tg->set_idx(logic->get_idx());
00224 tg->set_wkc(logic->get_wkc());
00225 }
00226
00227 WGMailbox::WGMailbox() : sh_(NULL)
00228 {
00229 int error;
00230 if ((error = pthread_mutex_init(&mailbox_lock_, NULL)) != 0)
00231 {
00232 ROS_ERROR("WG0X : init mailbox mutex :%s", strerror(error));
00233 }
00234 }
00235
00236 bool WGMailbox::initialize(EtherCAT_SlaveHandler *sh)
00237 {
00238 sh_ = sh;
00239 return true;
00240 }
00241
00242 bool WGMailbox::verifyDeviceStateForMailboxOperation()
00243 {
00244
00245 EC_State state = sh_->get_state();
00246 if ((state != EC_SAFEOP_STATE) && (state != EC_OP_STATE)) {
00247 fprintf(stderr, "%s : " ERROR_HDR
00248 "cannot do mailbox read in current device state = %d\n", __func__, state);
00249 return false;
00250 }
00251 return true;
00252 }
00253
00254
00264 void WGMailbox::diagnoseMailboxError(EthercatCom *com)
00265 {
00266
00267 }
00268
00277 bool WGMailbox::clearReadMailbox(EthercatCom *com)
00278 {
00279 if (!verifyDeviceStateForMailboxOperation()){
00280 return false;
00281 }
00282
00283 EC_Logic *logic = EC_Logic::instance();
00284 EC_UINT station_addr = sh_->get_station_address();
00285
00286
00287
00288
00289 unsigned char unused[1] = {0};
00290 NPRD_Telegram read_start(
00291 logic->get_idx(),
00292 station_addr,
00293 MBX_STATUS_PHY_ADDR,
00294 logic->get_wkc(),
00295 sizeof(unused),
00296 unused);
00297 NPRD_Telegram read_end(
00298 logic->get_idx(),
00299 station_addr,
00300 MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
00301 logic->get_wkc(),
00302 sizeof(unused),
00303 unused);
00304 read_start.attach(&read_end);
00305 EC_Ethernet_Frame frame(&read_start);
00306
00307
00308
00309 bool success=false;
00310 static const unsigned MAX_DROPS = 15;
00311 for (unsigned tries=0; tries<MAX_DROPS; ++tries) {
00312 success = com->txandrx_once(&frame);
00313 if (success) {
00314 break;
00315 }
00316 updateIndexAndWkc(&read_start, logic);
00317 updateIndexAndWkc(&read_end , logic);
00318 }
00319
00320 if (!success) {
00321 fprintf(stderr, "%s : " ERROR_HDR
00322 " too much packet loss\n", __func__);
00323 safe_usleep(100);
00324 return false;
00325 }
00326
00327
00328 if (read_start.get_wkc() != read_end.get_wkc()) {
00329 fprintf(stderr, "%s : " ERROR_HDR
00330 "read mbx working counters are inconsistant, %d, %d\n",
00331 __func__, read_start.get_wkc(), read_end.get_wkc());
00332 return false;
00333 }
00334 if (read_start.get_wkc() > 1) {
00335 fprintf(stderr, "%s : " ERROR_HDR
00336 "more than one device (%d) responded \n", __func__, read_start.get_wkc());
00337 return false;
00338 }
00339 if (read_start.get_wkc() == 1) {
00340 fprintf(stderr, "%s : " WARN_MODE "WARN" STD_MODE
00341 " read mbx contained garbage data\n", __func__);
00342
00343 }
00344
00345 return true;
00346 }
00347
00348
00349
00359 bool WGMailbox::waitForReadMailboxReady(EthercatCom *com)
00360 {
00361
00362 static const int MAX_WAIT_TIME_MS = 100;
00363 int timediff;
00364 unsigned good_results=0;
00365
00366
00367 struct timespec start_time, current_time;
00368 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
00369 return false;
00370 }
00371
00372 do {
00373
00374 uint8_t SyncManStatus=0;
00375 const unsigned SyncManAddr = 0x805+(MBX_STATUS_SYNCMAN_NUM*8);
00376 if (EthercatDevice::readData(com, sh_, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), EthercatDevice::FIXED_ADDR) == 0) {
00377 ++good_results;
00378 const uint8_t MailboxStatusMask = (1<<3);
00379 if (SyncManStatus & MailboxStatusMask) {
00380 return true;
00381 }
00382 }
00383 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
00384 return false;
00385 }
00386 timediff = timediff_ms(current_time, start_time);
00387 safe_usleep(100);
00388 } while (timediff < MAX_WAIT_TIME_MS);
00389
00390 if (good_results == 0) {
00391 fprintf(stderr, "%s : " ERROR_HDR
00392 " error reading from device\n", __func__);
00393 } else {
00394 fprintf(stderr, "%s : " ERROR_HDR
00395 " error read mbx not full after %d ms\n", __func__, timediff);
00396 }
00397
00398 return false;
00399 }
00400
00401
00411 bool WGMailbox::waitForWriteMailboxReady(EthercatCom *com)
00412 {
00413
00414 static const int MAX_WAIT_TIME_MS = 100;
00415 int timediff;
00416 unsigned good_results=0;
00417
00418
00419 struct timespec start_time, current_time;
00420 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
00421 return false;
00422 }
00423
00424 do {
00425
00426 uint8_t SyncManStatus=0;
00427 const unsigned SyncManAddr = 0x805+(MBX_COMMAND_SYNCMAN_NUM*8);
00428 if (EthercatDevice::readData(com, sh_, SyncManAddr, &SyncManStatus, sizeof(SyncManStatus), EthercatDevice::FIXED_ADDR) == 0) {
00429 ++good_results;
00430 const uint8_t MailboxStatusMask = (1<<3);
00431 if ( !(SyncManStatus & MailboxStatusMask) ) {
00432 return true;
00433 }
00434 }
00435 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
00436 return false;
00437 }
00438 timediff = timediff_ms(current_time, start_time);
00439 safe_usleep(100);
00440 } while (timediff < MAX_WAIT_TIME_MS);
00441
00442 if (good_results == 0) {
00443 fprintf(stderr, "%s : " ERROR_HDR
00444 " error reading from device\n", __func__);
00445 } else {
00446 fprintf(stderr, "%s : " ERROR_HDR
00447 " error write mbx not empty after %d ms\n", __func__, timediff);
00448 }
00449
00450 return false;
00451 }
00452
00453
00454
00466 bool WGMailbox::writeMailboxInternal(EthercatCom *com, void const *data, unsigned length)
00467 {
00468 if (length > MBX_COMMAND_SIZE) {
00469 assert(length <= MBX_COMMAND_SIZE);
00470 return false;
00471 }
00472
00473
00474 if (!verifyDeviceStateForMailboxOperation()){
00475 return false;
00476 }
00477
00478 EC_Logic *logic = EC_Logic::instance();
00479 EC_UINT station_addr = sh_->get_station_address();
00480
00481
00482
00483
00484
00485 static const unsigned TELEGRAM_OVERHEAD = 50;
00486 bool split_write = (length+TELEGRAM_OVERHEAD) < MBX_COMMAND_SIZE;
00487
00488 unsigned write_length = MBX_COMMAND_SIZE;
00489 if (split_write) {
00490 write_length = length;
00491 }
00492
00493
00494
00495
00496 {
00497
00498 unsigned char unused[1] = {0};
00499 NPWR_Telegram write_start(
00500 logic->get_idx(),
00501 station_addr,
00502 MBX_COMMAND_PHY_ADDR,
00503 logic->get_wkc(),
00504 write_length,
00505 (const unsigned char*) data);
00506 NPWR_Telegram write_end(
00507 logic->get_idx(),
00508 station_addr,
00509 MBX_COMMAND_PHY_ADDR+MBX_COMMAND_SIZE-1,
00510 logic->get_wkc(),
00511 sizeof(unused),
00512 unused);
00513
00514 if (split_write) {
00515 write_start.attach(&write_end);
00516 }
00517
00518 EC_Ethernet_Frame frame(&write_start);
00519
00520
00521 unsigned sends=0;
00522 bool success=false;
00523 for (unsigned tries=0; (tries<10) && !success; ++tries) {
00524 success = com->txandrx_once(&frame);
00525 if (!success) {
00526 updateIndexAndWkc(&write_start, logic);
00527 updateIndexAndWkc(&write_end, logic);
00528 }
00529 ++sends;
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541 }
00542 if (!success) {
00543 fprintf(stderr, "%s : " ERROR_HDR
00544 " too much packet loss\n", __func__);
00545 safe_usleep(100);
00546 return false;
00547 }
00548
00549 if (split_write && (write_start.get_wkc() != write_end.get_wkc())) {
00550 fprintf(stderr, "%s : " ERROR_HDR
00551 " write mbx working counters are inconsistant\n", __func__);
00552 return false;
00553 }
00554
00555 if (write_start.get_wkc() > 1)
00556 {
00557 fprintf(stderr, "%s : " ERROR_HDR
00558 " multiple (%d) devices responded to mailbox write\n", __func__, write_start.get_wkc());
00559 return false;
00560 }
00561 else if (write_start.get_wkc() != 1)
00562 {
00563
00564 if (sends<=1) {
00565
00566 fprintf(stderr, "%s : " ERROR_HDR
00567 " initial mailbox write refused\n", __func__);
00568 safe_usleep(100);
00569 return false;
00570 } else {
00571
00572
00573 fprintf(stderr, "%s : " WARN_HDR
00574 " repeated mailbox write refused\n", __func__);
00575 }
00576 }
00577 }
00578
00579 return true;
00580 }
00581
00582 bool WGMailbox::readMailboxRepeatRequest(EthercatCom *com)
00583 {
00584 bool success = _readMailboxRepeatRequest(com);
00585 ++mailbox_diagnostics_.retries_;
00586 if (!success) {
00587 ++mailbox_diagnostics_.retry_errors_;
00588 }
00589 return success;
00590 }
00591
00592 bool WGMailbox::_readMailboxRepeatRequest(EthercatCom *com)
00593 {
00594
00595
00596 SyncMan sm;
00597 if (!sm.readData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
00598 fprintf(stderr, "%s : " ERROR_HDR
00599 " could not read status mailbox syncman (1)\n", __func__);
00600 return false;
00601 }
00602
00603
00604 if (sm.activate.repeat_request != sm.pdi_control.repeat_ack) {
00605 fprintf(stderr, "%s : " ERROR_HDR
00606 " syncman repeat request and ack do not match\n", __func__);
00607 return false;
00608 }
00609
00610
00611 SyncManActivate orig_activate(sm.activate);
00612 sm.activate.repeat_request = ~orig_activate.repeat_request;
00613 if (!sm.activate.writeData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
00614 fprintf(stderr, "%s : " ERROR_HDR
00615 " could not write syncman repeat request\n", __func__);
00616
00617 return false;
00618 }
00619
00620
00621 static const int MAX_WAIT_TIME_MS = 100;
00622 int timediff;
00623
00624 struct timespec start_time, current_time;
00625 if (safe_clock_gettime(CLOCK_MONOTONIC, &start_time)!=0) {
00626 return false;
00627 }
00628
00629 do {
00630 if (!sm.readData(com, sh_, EthercatDevice::FIXED_ADDR, MBX_STATUS_SYNCMAN_NUM)) {
00631 fprintf(stderr, "%s : " ERROR_HDR
00632 " could not read status mailbox syncman (2)\n", __func__);
00633 return false;
00634 }
00635
00636 if (sm.activate.repeat_request == sm.pdi_control.repeat_ack) {
00637
00638 if (sm.status.mailbox_status != 1) {
00639 fprintf(stderr, "%s : " ERROR_HDR
00640 " got repeat response, but read mailbox is still empty\n", __func__);
00641
00642 return false;
00643 }
00644 return true;
00645 }
00646
00647 if ( (sm.activate.repeat_request) == (orig_activate.repeat_request) ) {
00648 fprintf(stderr, "%s : " ERROR_HDR
00649 " syncman repeat request was changed while waiting for response\n", __func__);
00650
00651
00652 return false;
00653 }
00654
00655 if (safe_clock_gettime(CLOCK_MONOTONIC, ¤t_time)!=0) {
00656 return false;
00657 }
00658
00659 timediff = timediff_ms(current_time, start_time);
00660 safe_usleep(100);
00661 } while (timediff < MAX_WAIT_TIME_MS);
00662
00663 fprintf(stderr, "%s : " ERROR_HDR
00664 " error repeat request not acknowledged after %d ms\n", __func__, timediff);
00665 return false;
00666 }
00667
00668
00669
00681 bool WGMailbox::readMailboxInternal(EthercatCom *com, void *data, unsigned length)
00682 {
00683 static const unsigned MAX_TRIES = 10;
00684 static const unsigned MAX_DROPPED = 10;
00685
00686 if (length > MBX_STATUS_SIZE) {
00687 assert(length <= MBX_STATUS_SIZE);
00688 return false;
00689 }
00690
00691
00692 if (!verifyDeviceStateForMailboxOperation()){
00693 return false;
00694 }
00695
00696 EC_Logic *logic = EC_Logic::instance();
00697 EC_UINT station_addr = sh_->get_station_address();
00698
00699
00700
00701
00702
00703 static const unsigned TELEGRAM_OVERHEAD = 50;
00704 bool split_read = (length+TELEGRAM_OVERHEAD) < MBX_STATUS_SIZE;
00705
00706 unsigned read_length = MBX_STATUS_SIZE;
00707 if (split_read) {
00708 read_length = length;
00709 }
00710
00711 unsigned char unused[1] = {0};
00712 NPRD_Telegram read_start(
00713 logic->get_idx(),
00714 station_addr,
00715 MBX_STATUS_PHY_ADDR,
00716 logic->get_wkc(),
00717 read_length,
00718 (unsigned char*) data);
00719 NPRD_Telegram read_end(
00720 logic->get_idx(),
00721 station_addr,
00722 MBX_STATUS_PHY_ADDR+MBX_STATUS_SIZE-1,
00723 logic->get_wkc(),
00724 sizeof(unused),
00725 unused);
00726
00727 if (split_read) {
00728 read_start.attach(&read_end);
00729 }
00730
00731 EC_Ethernet_Frame frame(&read_start);
00732
00733 unsigned tries = 0;
00734 unsigned total_dropped =0;
00735 for (tries=0; tries<MAX_TRIES; ++tries) {
00736
00737
00738 unsigned dropped=0;
00739 for (dropped=0; dropped<MAX_DROPPED; ++dropped) {
00740 if (com->txandrx_once(&frame)) {
00741 break;
00742 }
00743 ++total_dropped;
00744 updateIndexAndWkc(&read_start , logic);
00745 updateIndexAndWkc(&read_end , logic);
00746 }
00747
00748 if (dropped>=MAX_DROPPED) {
00749 fprintf(stderr, "%s : " ERROR_HDR
00750 " too many dropped packets : %d\n", __func__, dropped);
00751 }
00752
00753 if (split_read && (read_start.get_wkc() != read_end.get_wkc())) {
00754 fprintf(stderr, "%s : " ERROR_HDR
00755 "read mbx working counters are inconsistant\n", __func__);
00756 return false;
00757 }
00758
00759 if (read_start.get_wkc() == 0) {
00760 if (dropped == 0) {
00761 fprintf(stderr, "%s : " ERROR_HDR
00762 " inconsistancy : got wkc=%d with no dropped packets\n",
00763 __func__, read_start.get_wkc());
00764 fprintf(stderr, "total dropped = %d\n", total_dropped);
00765 return false;
00766 } else {
00767
00768
00769 fprintf(stderr, "%s : " WARN_HDR
00770 " asking for read repeat after dropping %d packets\n", __func__, dropped);
00771 if (!readMailboxRepeatRequest(com)) {
00772 return false;
00773 }
00774 continue;
00775 }
00776 } else if (read_start.get_wkc() == 1) {
00777
00778 break;
00779 } else {
00780 fprintf(stderr, "%s : " ERROR_HDR
00781 " invalid wkc for read : %d\n", __func__, read_start.get_wkc());
00782 diagnoseMailboxError(com);
00783 return false;
00784 }
00785 }
00786
00787 if (tries >= MAX_TRIES) {
00788 fprintf(stderr, "%s : " ERROR_HDR
00789 " could not get responce from device after %d retries, %d total dropped packets\n",
00790 __func__, tries, total_dropped);
00791 diagnoseMailboxError(com);
00792 return false;
00793 }
00794
00795 return true;
00796 }
00797
00798
00812 int WGMailbox::readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length)
00813 {
00814 if (!lockMailbox())
00815 return -1;
00816
00817 int result = readMailbox_(com, address, data, length);
00818 if (result != 0) {
00819 ++mailbox_diagnostics_.read_errors_;
00820 }
00821
00822 unlockMailbox();
00823 return result;
00824 }
00825
00831 int WGMailbox::readMailbox_(EthercatCom *com, unsigned address, void *data, unsigned length)
00832 {
00833
00834 if (!verifyDeviceStateForMailboxOperation()){
00835 return false;
00836 }
00837
00838
00839 if (!clearReadMailbox(com))
00840 {
00841 fprintf(stderr, "%s : " ERROR_HDR
00842 " clearing read mbx\n", __func__);
00843 return -1;
00844 }
00845
00846
00847 {
00848 WG0XMbxCmd cmd;
00849 if (!cmd.build(address, length, LOCAL_BUS_READ, sh_->get_mbx_counter(), data))
00850 {
00851 fprintf(stderr, "%s : " ERROR_HDR
00852 " builing mbx header\n", __func__);
00853 return -1;
00854 }
00855
00856 if (!writeMailboxInternal(com, &cmd.hdr_, sizeof(cmd.hdr_)))
00857 {
00858 fprintf(stderr, "%s : " ERROR_HDR " write of cmd failed\n", __func__);
00859 return -1;
00860 }
00861 }
00862
00863
00864 if (!waitForReadMailboxReady(com))
00865 {
00866 fprintf(stderr, "%s : " ERROR_HDR
00867 "waiting for read mailbox\n", __func__);
00868 return -1;
00869 }
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881 {
00882 WG0XMbxCmd stat;
00883 memset(&stat,0,sizeof(stat));
00884
00885 if (!readMailboxInternal(com, &stat, length+1))
00886 {
00887 fprintf(stderr, "%s : " ERROR_HDR " read failed\n", __func__);
00888 return -1;
00889 }
00890
00891 if (wg_util::computeChecksum(&stat, length+1) != 0)
00892 {
00893 fprintf(stderr, "%s : " ERROR_HDR
00894 "checksum error reading mailbox data\n", __func__);
00895 fprintf(stderr, "length = %d\n", length);
00896 return -1;
00897 }
00898 memcpy(data, &stat, length);
00899 }
00900
00901 return 0;
00902
00903
00904 }
00905
00906 bool WGMailbox::lockMailbox()
00907 {
00908 int error = pthread_mutex_lock(&mailbox_lock_);
00909 if (error != 0) {
00910 fprintf(stderr, "%s : " ERROR_HDR " getting mbx lock\n", __func__);
00911 ++mailbox_diagnostics_.lock_errors_;
00912 return false;
00913 }
00914 return true;
00915 }
00916
00917 void WGMailbox::unlockMailbox()
00918 {
00919 int error = pthread_mutex_unlock(&mailbox_lock_);
00920 if (error != 0) {
00921 fprintf(stderr, "%s : " ERROR_HDR " freeing mbx lock\n", __func__);
00922 ++mailbox_diagnostics_.lock_errors_;
00923 }
00924 }
00925
00926
00939 int WGMailbox::writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
00940 {
00941 if (!lockMailbox())
00942 return -1;
00943
00944 int result = writeMailbox_(com, address, data, length);
00945 if (result != 0) {
00946 ++mailbox_diagnostics_.write_errors_;
00947 }
00948
00949 unlockMailbox();
00950
00951 return result;
00952 }
00953
00959 int WGMailbox::writeMailbox_(EthercatCom *com, unsigned address, void const *data, unsigned length)
00960 {
00961
00962 if (!verifyDeviceStateForMailboxOperation()){
00963 return -1;
00964 }
00965
00966
00967 {
00968 WG0XMbxCmd cmd;
00969 if (!cmd.build(address, length, LOCAL_BUS_WRITE, sh_->get_mbx_counter(), data)) {
00970 fprintf(stderr, "%s : " ERROR_HDR " builing mbx header\n", __func__);
00971 return -1;
00972 }
00973
00974 unsigned write_length = sizeof(cmd.hdr_)+length+sizeof(cmd.checksum_);
00975 if (!writeMailboxInternal(com, &cmd, write_length)) {
00976 fprintf(stderr, "%s : " ERROR_HDR " write failed\n", __func__);
00977 diagnoseMailboxError(com);
00978 return -1;
00979 }
00980 }
00981
00982
00983
00984 if (!waitForWriteMailboxReady(com)) {
00985 fprintf(stderr, "%s : " ERROR_HDR
00986 "write mailbox\n", __func__);
00987 }
00988
00989 return 0;
00990 }
00991
00992
00993 void WGMailbox::publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00994 {
00995 if (lockMailbox()) {
00996 mailbox_publish_diagnostics_ = mailbox_diagnostics_;
00997 unlockMailbox();
00998 }
00999
01000 MbxDiagnostics const &m(mailbox_publish_diagnostics_);
01001 d.addf("Mailbox Write Errors", "%d", m.write_errors_);
01002 d.addf("Mailbox Read Errors", "%d", m.read_errors_);
01003 d.addf("Mailbox Retries", "%d", m.retries_);
01004 d.addf("Mailbox Retry Errors", "%d", m.retry_errors_);
01005 }
01006
01007
01008 };