Go to the documentation of this file.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 "ros_ethercat_hardware/ethercat_com.h"
00036 #include <stdio.h>
00037 #include <errno.h>
00038 #include <assert.h>
00039
00040 bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
00041 {
00042 assert(frame != NULL);
00043 int handle = dll_->tx(frame);
00044 if (handle < 0)
00045 return false;
00046 return dll_->rx(frame, handle);
00047 }
00048
00049 bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
00050 {
00051 return dll_->txandrx(frame);
00052 }
00053
00054 EthercatOobCom::EthercatOobCom(struct netif *ni) :
00055 ni_(ni),
00056 state_(IDLE),
00057 frame_(NULL),
00058 handle_(-1),
00059 line_(0)
00060 {
00061 assert(ni_ != NULL);
00062
00063 pthread_mutexattr_t mutex_attr;
00064 int error = pthread_mutexattr_init(&mutex_attr);
00065 if (error != 0)
00066 {
00067 fprintf(stderr, "%s : Initializing mutex attr failed : %d\n", __func__, error);
00068 return;
00069 }
00070 error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
00071 if (error != 0)
00072 {
00073 fprintf(stderr, "%s : Setting type of mutex attr failed : %d\n", __func__, error);
00074 return;
00075 }
00076 error = pthread_mutex_init(&mutex_, &mutex_attr);
00077 if (error != 0)
00078 {
00079 fprintf(stderr, "%s : Initializing mutex failed : %d\n", __func__, error);
00080 return;
00081 }
00082 error = pthread_cond_init(&share_cond_, NULL);
00083 if (error != 0)
00084 {
00085 fprintf(stderr, "%s : Initializing share condition failed : %d\n", __func__, error);
00086 return;
00087 }
00088 error = pthread_cond_init(&busy_cond_, NULL);
00089 if (error != 0)
00090 fprintf(stderr, "%s : Initializing busy condition failed : %d\n", __func__, error);
00091 return;
00092 }
00093
00094 bool EthercatOobCom::lock(unsigned line)
00095 {
00096 int error;
00097 if (0 != (error = pthread_mutex_lock(&mutex_)))
00098 {
00099 fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00100 return false;
00101 }
00102 line_ = line;
00103 return true;
00104 }
00105
00106 bool EthercatOobCom::trylock(unsigned line)
00107 {
00108 int error;
00109 if (0 != (error = pthread_mutex_trylock(&mutex_)))
00110 {
00111 if (error != EBUSY)
00112 fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00113 return false;
00114 }
00115 line_ = line;
00116 return true;
00117 }
00118
00119 bool EthercatOobCom::unlock(unsigned line)
00120 {
00121 int error;
00122 if (0 != (error = pthread_mutex_unlock(&mutex_)))
00123 {
00124 fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
00125 return false;
00126 }
00127 line_ = 0;
00128 return true;
00129 }
00130
00131
00132
00133
00134 bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
00135 {
00136 assert(frame != NULL);
00137
00138 if (!lock(__LINE__))
00139 return false;
00140
00141
00142 while (state_ != IDLE)
00143 {
00144 pthread_cond_wait(&share_cond_, &mutex_);
00145 }
00146 frame_ = frame;
00147 state_ = READY_TO_SEND;
00148
00149
00150 do
00151 {
00152 pthread_cond_wait(&busy_cond_, &mutex_);
00153 }
00154 while (state_ != WAITING_TO_RECV);
00155
00156
00157 bool success = false;
00158 if (handle_ >= 0)
00159 success = ni_->rx(frame_, ni_, handle_);
00160 handle_ = -1;
00161
00162
00163 assert(frame_ == frame);
00164 state_ = IDLE;
00165 pthread_cond_signal(&share_cond_);
00166
00167 unlock(__LINE__);
00168
00169 return success;
00170 }
00171
00172 bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
00173 {
00174 static const unsigned MAX_TRIES = 10;
00175 for (unsigned tries = 0; tries < MAX_TRIES; ++tries)
00176 {
00177 if (this->txandrx_once(frame))
00178 return true;
00179 }
00180 return false;
00181 }
00182
00183
00184
00185 void EthercatOobCom::tx()
00186 {
00187 if (!trylock(__LINE__))
00188 return;
00189
00190 if (state_ == READY_TO_SEND)
00191 {
00192
00193 assert(frame_ != NULL);
00194 handle_ = ni_->tx(frame_, ni_);
00195 state_ = WAITING_TO_RECV;
00196 pthread_cond_signal(&busy_cond_);
00197 }
00198
00199 unlock(__LINE__);
00200 }