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
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
00101
00102
00103
00104
00105
00106
00107
00108 void collect(EthercatCom *com, EtherCAT_SlaveHandler *sh);
00109
00110
00111
00112
00113
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
00222
00223
00224
00225
00226
00227 unsigned newDiagnosticsIndex_;
00228 pthread_mutex_t newDiagnosticsIndexLock_;
00229 EthercatDeviceDiagnostics deviceDiagnostics[2];
00230 pthread_mutex_t diagnosticsLock_;
00231
00232
00233 diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_;
00234 };
00235
00236 #endif