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 #ifndef ETHERCAT_DEVICE_H
00036 #define ETHERCAT_DEVICE_H
00037
00038 #include <vector>
00039
00040 #include <tinyxml/tinyxml.h>
00041
00042 #include <ethercat/ethercat_defs.h>
00043 #include <al/ethercat_slave_handler.h>
00044
00045 #include <pr2_hardware_interface/hardware_interface.h>
00046
00047 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00048
00049 #include <diagnostic_msgs/DiagnosticArray.h>
00050
00051 #include <ethercat_hardware/ethercat_com.h>
00052
00053 #include <pluginlib/class_list_macros.h>
00054
00055 using namespace std;
00056
00057 struct et1x00_error_counters
00058 {
00059 struct {
00060 uint8_t invalid_frame;
00061 uint8_t rx_error;
00062 } __attribute__((__packed__)) port[4];
00063 uint8_t forwarded_rx_error[4];
00064 uint8_t epu_error;
00065 uint8_t pdi_error;
00066 uint8_t res[2];
00067 uint8_t lost_link[4];
00068 static const EC_UINT BASE_ADDR=0x300;
00069 bool isGreaterThan(unsigned value) const;
00070 bool isGreaterThan(const et1x00_error_counters &value) const;
00071 void zero();
00072 } __attribute__((__packed__));
00073
00074 struct et1x00_dl_status
00075 {
00076 uint16_t status;
00077 bool hasLink(unsigned port);
00078 bool isClosed(unsigned port);
00079 bool hasCommunication(unsigned port);
00080 static const EC_UINT BASE_ADDR=0x110;
00081 } __attribute__((__packed__));
00082
00083 struct EthercatPortDiagnostics
00084 {
00085 EthercatPortDiagnostics();
00086 void zeroTotals();
00087 bool hasLink;
00088 bool isClosed;
00089 bool hasCommunication;
00090 uint64_t rxErrorTotal;
00091 uint64_t invalidFrameTotal;
00092 uint64_t forwardedRxErrorTotal;
00093 uint64_t lostLinkTotal;
00094 };
00095
00096 struct EthercatDeviceDiagnostics
00097 {
00098 public:
00099 EthercatDeviceDiagnostics();
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 void collect(EthercatCom *com, EtherCAT_SlaveHandler *sh);
00110
00111
00112
00113
00114
00115 void publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts=4) const;
00116
00117 protected:
00118 void zeroTotals();
00119 void accumulate(const et1x00_error_counters &next, const et1x00_error_counters &prev);
00120 uint64_t pdiErrorTotal_;
00121 uint64_t epuErrorTotal_;
00122 EthercatPortDiagnostics portDiagnostics_[4];
00123 unsigned nodeAddress_;
00124 et1x00_error_counters errorCountersPrev_;
00125 bool errorCountersMayBeCleared_;
00126
00127 bool diagnosticsFirst_;
00128 bool diagnosticsValid_;
00129 bool resetDetected_;
00130 int devicesRespondingToNodeAddress_;
00131 };
00132
00133
00134 class EthercatDevice
00135 {
00136 public:
00137 virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00138
00139 EthercatDevice();
00140 virtual ~EthercatDevice();
00141
00142 virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=0) = 0;
00143
00148 virtual void packCommand(unsigned char *buffer, bool halt, bool reset) {}
00149
00150 virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer) {return true;}
00151
00158 virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
00159
00166 virtual void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
00167
00173 void ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts);
00174
00175 virtual void collectDiagnostics(EthercatCom *com);
00176
00184 virtual bool publishTrace(const string &reason, unsigned level, unsigned delay) {return false;}
00185
00186 enum AddrMode {FIXED_ADDR=0, POSITIONAL_ADDR=1};
00187
00191 static int writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode);
00192 inline int writeData(EthercatCom *com, EC_UINT address, void const* buffer, EC_UINT length, AddrMode addrMode) {
00193 return writeData(com, sh_, address, buffer, length, addrMode);
00194 }
00195
00199 static int readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode);
00200 inline int readData(EthercatCom *com, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode) {
00201 return readData(com, sh_, address, buffer, length, addrMode);
00202 }
00203
00207 static int readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode);
00208 inline int readWriteData(EthercatCom *com, EC_UINT address, void *buffer, EC_UINT length, AddrMode addrMode) {
00209 return readWriteData(com, sh_, address, buffer, length, addrMode);
00210 }
00211
00212 bool use_ros_;
00213
00214 EtherCAT_SlaveHandler *sh_;
00215 unsigned int command_size_;
00216 unsigned int status_size_;
00217
00218
00219
00220
00221
00222
00223
00224 unsigned newDiagnosticsIndex_;
00225 pthread_mutex_t newDiagnosticsIndexLock_;
00226 EthercatDeviceDiagnostics deviceDiagnostics[2];
00227 pthread_mutex_t diagnosticsLock_;
00228
00229
00230 diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_;
00231 };
00232
00233 #endif