ethercat_device.h
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 #ifndef ETHERCAT_DEVICE_H
00036 #define ETHERCAT_DEVICE_H
00037 
00038 #include <vector>
00039 
00040 
00041 #include <ethercat/ethercat_defs.h>
00042 #include <al/ethercat_slave_handler.h>
00043 
00044 #include <pr2_hardware_interface/hardware_interface.h>
00045 
00046 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00047 
00048 #include <diagnostic_msgs/DiagnosticArray.h>
00049 
00050 #include <ethercat_hardware/ethercat_com.h>
00051 
00052 #include <pluginlib/class_list_macros.h>
00053 
00054 using namespace std;
00055 
00056 struct et1x00_error_counters
00057 {
00058   struct {
00059     uint8_t invalid_frame;
00060     uint8_t rx_error;
00061   } __attribute__((__packed__)) port[4];
00062   uint8_t forwarded_rx_error[4];
00063   uint8_t epu_error;
00064   uint8_t pdi_error;
00065   uint8_t res[2];
00066   uint8_t lost_link[4];
00067   static const EC_UINT BASE_ADDR=0x300;
00068   bool isGreaterThan(unsigned value) const;
00069   bool isGreaterThan(const et1x00_error_counters &value) const;
00070   void zero();
00071 } __attribute__((__packed__));
00072 
00073 struct et1x00_dl_status
00074 {
00075   uint16_t status;
00076   bool hasLink(unsigned port);
00077   bool isClosed(unsigned port);
00078   bool hasCommunication(unsigned port);
00079   static const EC_UINT BASE_ADDR=0x110;
00080 } __attribute__((__packed__));
00081 
00082 struct EthercatPortDiagnostics
00083 {
00084   EthercatPortDiagnostics();
00085   void zeroTotals();
00086   bool hasLink;
00087   bool isClosed;
00088   bool hasCommunication;
00089   uint64_t rxErrorTotal;
00090   uint64_t invalidFrameTotal; 
00091   uint64_t forwardedRxErrorTotal;
00092   uint64_t lostLinkTotal; 
00093 };
00094 
00095 struct EthercatDeviceDiagnostics
00096 {
00097 public:
00098   EthercatDeviceDiagnostics();
00099 
00100   // Collects diagnostic data from specific ethercat slave, and updates object state
00101   //
00102   // com  EtherCAT communication object is used send/recv packets to/from ethercat chain.
00103   // sh   slaveHandler of device to collect Diagnostics from 
00104   // prev previously collected diagnostics (can be pointer to this object)
00105   //
00106   // collectDiagnotics will send/recieve multiple packets, and may considerable amount of time complete.
00107   // 
00108   void collect(EthercatCom *com, EtherCAT_SlaveHandler *sh);
00109 
00110   // Puts reviously diagnostic collected diagnostic state to DiagnosticStatus object 
00111   // 
00112   // d         DiagnositcState to add diagnostics to.
00113   // numPorts  Number of ports device is supposed to have.  4 is max, 1 is min.
00114   void publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts=4) const;
00115 
00116 protected:
00117   void zeroTotals();
00118   void accumulate(const et1x00_error_counters &next, const et1x00_error_counters &prev);
00119   uint64_t pdiErrorTotal_;
00120   uint64_t epuErrorTotal_;
00121   EthercatPortDiagnostics portDiagnostics_[4];
00122   unsigned nodeAddress_;
00123   et1x00_error_counters errorCountersPrev_;
00124   bool errorCountersMayBeCleared_;
00125 
00126   bool diagnosticsFirst_;
00127   bool diagnosticsValid_;
00128   bool resetDetected_;
00129   int devicesRespondingToNodeAddress_;
00130 };
00131 
00132 
00133 class EthercatDevice
00134 {
00135 public:
00137   virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00138 
00140   virtual void construct(ros::NodeHandle &nh);
00141 
00142   EthercatDevice();
00143   virtual ~EthercatDevice();
00144 
00145   virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=0) = 0;
00146   
00151   virtual void packCommand(unsigned char *buffer, bool halt, bool reset) {}
00152 
00153   virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer) {return true;}
00154 
00161   virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
00162 
00169   virtual void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
00170 
00176   void ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts);
00177 
00178   virtual void collectDiagnostics(EthercatCom *com);
00179 
00187   virtual bool publishTrace(const string &reason, unsigned level, unsigned delay) {return false;}
00188 
00189   enum AddrMode {FIXED_ADDR=0, POSITIONAL_ADDR=1};
00190 
00194   static int writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode);
00195   inline int writeData(EthercatCom *com, EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode) {
00196     return writeData(com, sh_, address, buffer, length, addrMode);
00197   }
00198   
00202   static int readData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode);
00203   inline int readData(EthercatCom *com, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode) {
00204     return readData(com, sh_, address, buffer, length, addrMode);
00205   }  
00206 
00210   static int readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh,  EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode);
00211   inline int readWriteData(EthercatCom *com, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode) {
00212     return readWriteData(com, sh_, address, buffer, length, addrMode);
00213   }
00214 
00215   bool use_ros_;
00216 
00217   EtherCAT_SlaveHandler *sh_;
00218   unsigned int command_size_;
00219   unsigned int status_size_;
00220   
00221   // The device diagnostics are collected with a non-readtime thread that calls collectDiagnostics()
00222   // The device published from the realtime loop by indirectly invoking ethercatDiagnostics()
00223   // To avoid blocking of the realtime thread (for long) a double buffer is used the 
00224   // The publisher thread will lock newDiagnosticsIndex when publishing data.
00225   // The collection thread will lock deviceDiagnostics when updating deviceDiagnostics
00226   // The collection thread will also lock newDiagnosticsIndex at end of update, just before swapping buffers.
00227   unsigned newDiagnosticsIndex_;
00228   pthread_mutex_t newDiagnosticsIndexLock_;  
00229   EthercatDeviceDiagnostics deviceDiagnostics[2];
00230   pthread_mutex_t diagnosticsLock_;
00231 
00232   // Keep diagnostics status as cache.  Avoids a lot of construction/destruction of status object.
00233   diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_;
00234 };
00235 
00236 #endif /* ETHERCAT_DEVICE_H */


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Jan 2 2014 11:39:31