wg_mailbox.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/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 // Temporary,, need 'log' fuction that can switch between fprintf and ROS_LOG.
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;  // bits[14:12] sequence number, 0=disable, 1-7 normal sequence number
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     // Result of mailbox read, only stores result data + 1byte checksum
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   // Empty
00155 }
00156 
00157 
00165 int timediff_ms(const timespec &current, const timespec &start)
00166 {
00167   int timediff_ms = (current.tv_sec-start.tv_sec)*1000 // 1000 ms in a sec
00168     + (current.tv_nsec-start.tv_nsec)/1000000; // 1000000 ns in a ms
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   // Make sure slave is in correct state to do use mailbox
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   // Create Ethernet packet with two EtherCAT telegrams inside of it : 
00287   //  - One telegram to read first byte of mailbox
00288   //  - One telegram to read last byte of mailbox
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   // Retry sending packet multiple times 
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   // Check result for consistancy
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     // Not an error, just warning
00343   } 
00344   
00345   return true;  
00346 }
00347 
00348 
00349 
00359 bool WGMailbox::waitForReadMailboxReady(EthercatCom *com)
00360 {
00361   // Wait upto 100ms for device to toggle ack
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     // Check if mailbox is full by looking at bit 3 of SyncMan status register.
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, &current_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   // Wait upto 100ms for device to toggle ack
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     // Check if mailbox is full by looking at bit 3 of SyncMan status register.
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, &current_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   // Make sure slave is in correct state to use mailbox
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   // If there enough savings, split mailbox write up into 2 parts : 
00483   //  1. Write of actual data to begining of mbx buffer
00484   //  2. Write of last mbx buffer byte, to complete write
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   // Possible do multiple things at once...
00494   //  1. Clear read mailbox by reading both first and last mailbox bytes
00495   //  2. Write data into write mailbox
00496   {
00497     // Build frame with 2-NPRD + 2 NPWR
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     // Try multiple times, but remember number of of successful sends
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; //EtherCAT_com d/n support split TX and RX class, assume tx part of txandrx always succeeds
00530       /* 
00531       int handle = com->tx(&frame);
00532       if (handle > 0) {
00533         ++sends;
00534         success = com->rx(&frame, handle);
00535       }
00536       if (!success) {
00537         updateIndexAndWkc(&write_start, logic);
00538         updateIndexAndWkc(&write_end, logic);
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       // Write to cmd mbx was refused 
00564       if (sends<=1) {
00565         // Packet was only sent once, there must be a problem with slave device
00566         fprintf(stderr, "%s : " ERROR_HDR 
00567                 " initial mailbox write refused\n", __func__);
00568         safe_usleep(100);
00569         return false;
00570       } else {
00571         // Packet was sent multiple times because a packet drop occured  
00572         // If packet drop occured on return path from device, a refusal is acceptable
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   // Toggle repeat request flag, wait for ack from device
00595   // Returns true if ack is received, false for failure
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   // If device can handle repeat requests, then request and ack bit should already match
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   // Write toggled repeat request,,, wait for ack.
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     //ec_mark(sh->getEM(), "could not write syncman repeat request", 1);
00617     return false;
00618   }
00619   
00620   // Wait upto 100ms for device to toggle ack
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       // Device responded, to some checks to make sure it seems to be telling the truth
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         //sm.print(WG0X_MBX_Status_Syncman_Num, std::cerr);
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       //sm.activate.print();
00651       //orig_activate.print();
00652       return false;
00653     }
00654 
00655     if (safe_clock_gettime(CLOCK_MONOTONIC, &current_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   // Make sure slave is in correct state to use mailbox
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   // If read is small enough :
00701   //  1. read just length bytes in one telegram
00702   //  2. then read last byte to empty mailbox
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     // Send read - keep track of how many packets were dropped (for later)
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         // Packet was dropped after doing read from device,,,
00768         // Ask device to repost data, so it can be read again.
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       // Successfull read of status data
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   // Make sure slave is in correct state to use mailbox
00834   if (!verifyDeviceStateForMailboxOperation()){
00835     return false;
00836   }
00837 
00838   //  1. Clear read (status) mailbox by reading it first
00839   if (!clearReadMailbox(com)) 
00840   {
00841     fprintf(stderr, "%s : " ERROR_HDR 
00842             " clearing read mbx\n", __func__);
00843     return -1;
00844   }
00845 
00846   //  2. Put a (read) request into command mailbox
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   // Wait for result (in read mailbox) to become ready
00864   if (!waitForReadMailboxReady(com)) 
00865   {
00866     fprintf(stderr, "%s : " ERROR_HDR 
00867             "waiting for read mailbox\n", __func__);
00868     return -1;
00869   }
00870 
00871   // Read result back from mailbox.
00872   // It could take the FPGA some time to respond to a request.  
00873   // Since the read mailbox is initiall cleared, any read to the mailbox
00874   // should be refused (WKC==0) until WG0x FPGA has written it result into it.     
00875   // NOTE: For this to work the mailbox syncmanagers must be set up.
00876   // TODO 1: Packets may get lost on return route to device.
00877   //   In this case, the device will keep responding to the repeated packets with WKC=0.
00878   //   To work correctly, the repeat request bit needs to be toggled.
00879   // TODO 2: Need a better method to determine if data read from status mailbox.
00880   //   is the right data, or just junk left over from last time.
00881   { 
00882     WG0XMbxCmd stat;
00883     memset(&stat,0,sizeof(stat));
00884     // Read data + 1byte checksum from mailbox
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   // Make sure slave is in correct state to use mailbox
00962   if (!verifyDeviceStateForMailboxOperation()){
00963     return -1;
00964   }
00965     
00966   // Build message and put it into write mailbox
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   // TODO: Change slave firmware so that we can verify that localbus write was truly executed
00983   //  Checking that device emptied write mailbox will have to suffice for now.
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 }; //end namespace ethercat_hardware


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:45