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
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 #include <cob_generic_can/CanESD.h>
00058
00059
00060
00061
00062
00063 CanESD::CanESD(const char* cIniFile, bool bObjectMode)
00064 {
00065 m_bObjectMode = bObjectMode;
00066 m_bIsTXError = false;
00067 m_IniFile.SetFileName(cIniFile, "CanESD.cpp");
00068 initIntern();
00069 }
00070
00071
00076 CanESD::~CanESD()
00077 {
00078 std::cout << "Closing CAN handle" << std::endl;
00079 canClose(m_Handle);
00080 }
00081
00082
00083
00084 void CanESD::initIntern()
00085 {
00086 int ret=0;
00087 ret = 0;
00088 int iCanNet = 1;
00089 m_IniFile.GetKeyInt( "CanCtrl", "NetESD", &iCanNet, true);
00090
00091 int iBaudrateVal = 2;
00092 m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true);
00093
00094 std::cout << "Initializing CAN network with id =" << iCanNet << ", baudrate=" << iBaudrateVal << std::endl;
00095
00096 int iRet;
00097 if( m_bObjectMode )
00098 iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 1000, 0, &m_Handle);
00099 else
00100 iRet = canOpen(iCanNet, 0, 10000, 10000, 1000, 0, &m_Handle);
00101 Sleep(300);
00102
00103 if(iRet == NTCAN_SUCCESS)
00104 std::cout << "CanESD::CanESD(), init ok" << std::endl;
00105 else
00106 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00107
00108 iRet = canSetBaudrate(m_Handle, iBaudrateVal);
00109 if(iRet == NTCAN_SUCCESS)
00110 std::cout << "CanESD::CanESD(), canSetBaudrate ok" << std::endl;
00111 else
00112 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00113 Sleep(300);
00114
00115
00116 iRet = canIoctl(m_Handle, NTCAN_IOCTL_FLUSH_RX_FIFO, NULL);
00117
00118
00119 for( int i=0; i<=0x7FF; ++i ) {
00120 iRet = canIdAdd( m_Handle, i );
00121 if(iRet != NTCAN_SUCCESS)
00122 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00123 }
00124
00125
00126 Sleep(300);
00127
00128 m_LastID = -1;
00129 }
00130
00131
00138 bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
00139 {
00140 CMSG NTCANMsg;
00141 NTCANMsg.id = CMsg.m_iID;
00142 NTCANMsg.len = CMsg.m_iLen;
00143
00144 for(int i=0; i<8; i++)
00145 NTCANMsg.data[i] = CMsg.getAt(i);
00146
00147 int ret;
00148 int32_t len;
00149 bool bRet = true;
00150
00151 len = 1;
00152
00153 if (bBlocking)
00154 ret = canWrite(m_Handle, &NTCANMsg, &len, NULL);
00155 else
00156 ret = canSend(m_Handle, &NTCANMsg, &len);
00157
00158 if( ret != NTCAN_SUCCESS)
00159 {
00160 std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
00161 bRet = false;
00162 }
00163
00164 m_LastID = (int)NTCANMsg.data[0];
00165
00166 m_bIsTXError = !bRet;
00167 return bRet;
00168 }
00169
00170
00171 bool CanESD::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
00172 {
00173
00174 CMSG NTCANMsg;
00175 NTCANMsg.len = 8;
00176
00177 int32_t len;
00178 int i, ret;
00179 bool bRet = true;
00180
00181 i=0;
00182
00183 len = 1;
00184
00185 do
00186 {
00187 len = 1;
00188 ret = canTake(m_Handle, &NTCANMsg, &len);
00189 i++;
00190 Sleep(10);
00191 }
00192
00193 while((len == 0) && (i < iNrOfRetry));
00194
00195 if(i == iNrOfRetry)
00196 {
00197 if( ret != NTCAN_SUCCESS )
00198 std::cout << "error in CANESD::receiveMsgRetry: " << GetErrorStr(ret) << std::endl;
00199
00200 bRet = false;
00201 }
00202 else
00203 {
00204 pCMsg->m_iID = NTCANMsg.id;
00205 pCMsg->m_iLen = NTCANMsg.len;
00206 pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00207 NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00208 }
00209
00210 return bRet;
00211 }
00212
00213
00214 bool CanESD::receiveMsg(CanMsg* pCMsg)
00215 {
00216 CMSG NTCANMsg;
00217 NTCANMsg.len = 8;
00218
00219 int ret;
00220 int32_t len;
00221 bool bRet = true;
00222
00223 len = 1;
00224
00225
00226 NTCANMsg.data[0] = 0;
00227 NTCANMsg.data[1] = 0;
00228 NTCANMsg.data[2] = 0;
00229 NTCANMsg.data[3] = 0;
00230 NTCANMsg.data[4] = 0;
00231 NTCANMsg.data[5] = 0;
00232 NTCANMsg.data[6] = 0;
00233 NTCANMsg.data[7] = 0;
00234 NTCANMsg.msg_lost = 0;
00235 NTCANMsg.id = 0;
00236 NTCANMsg.len = 0;
00237
00238 pCMsg->set(0,0,0,0,0,0,0,0);
00239
00240
00241 if( !isObjectMode() ) {
00242 pCMsg->m_iID = 0;
00243 } else {
00244 NTCANMsg.id = pCMsg->m_iID;
00245 }
00246
00247 ret = canTake(m_Handle, &NTCANMsg, &len);
00248
00249 if( !isObjectMode() ) {
00250 if( (len == 1) && (ret == NTCAN_SUCCESS) )
00251 {
00252
00253 pCMsg->m_iID = NTCANMsg.id;
00254 pCMsg->m_iLen = NTCANMsg.len;
00255 pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00256 NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00257 bRet = true;
00258 }
00259 else
00260 {
00261
00262 if( ret != NTCAN_SUCCESS)
00263 {
00264
00265 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(ret) << std::endl;
00266 }
00267
00268 pCMsg->m_iID = NTCANMsg.id;
00269 pCMsg->set(0,0,0,0,0,0,0,0);
00270
00271 bRet = false;
00272 }
00273 } else {
00274 if( len == 16 ) {
00275
00276 pCMsg->m_iID = NTCANMsg.id;
00277 pCMsg->set(0,0,0,0,0,0,0,0);
00278 bRet = false;
00279 } else {
00280 pCMsg->m_iID = NTCANMsg.id;
00281 pCMsg->m_iLen = NTCANMsg.len;
00282 pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00283 NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00284 bRet = true;
00285 }
00286 }
00287
00288 if( NTCANMsg.msg_lost != 0 )
00289 std::cout << (int)(NTCANMsg.msg_lost) << " messages lost!" << std::endl;
00290
00291 return bRet;
00292 }
00293
00294
00303 int CanESD::canIdAddGroup(NTCAN_HANDLE handle, int id)
00304 {
00305 int result = NTCAN_SUCCESS;
00306 int i = 0;
00307 int iRes = 0;
00308 int cmd_id = invert(id);
00309
00310 for( i=0; i<8; ++i) {
00311 iRes = canIdAdd(m_Handle, cmd_id+i);
00312
00313 if( iRes != NTCAN_SUCCESS ) {
00314 std::cout << "Adding CAN ID " << cmd_id+i << " failed with errorcode: " << iRes << " " << GetErrorStr(iRes) << std::endl;
00315 result = iRes;
00316 }
00317 }
00318
00319 return result;
00320 }
00321
00322
00323 std::string CanESD::GetErrorStr(int ntstatus) const
00324 {
00325 switch (ntstatus)
00326 {
00327 case NTCAN_SUCCESS: return "NTCAN_SUCCESS";
00328 case NTCAN_RX_TIMEOUT: return "NTCAN_RX_TIMEOUT";
00329 case NTCAN_TX_TIMEOUT: return "NTCAN_TX_TIMEOUT";
00330 case NTCAN_TX_ERROR: return "NTCAN_TX_ERROR";
00331 case NTCAN_CONTR_OFF_BUS: return "NTCAN_CONTR_OFF_BUS";
00332 case NTCAN_CONTR_BUSY: return "NTCAN_CONTR_BUSY";
00333 case NTCAN_CONTR_WARN: return "NTCAN_CONTR_WARN";
00334 case NTCAN_NO_ID_ENABLED: return "NTCAN_NO_ID_ENABLED";
00335 case NTCAN_ID_ALREADY_ENABLED: return "NTCAN_ID_ALREADY_ENABLED";
00336 case NTCAN_ID_NOT_ENABLED: return "NTCAN_ID_NOT_ENABLED";
00337
00338 case NTCAN_INVALID_FIRMWARE: return "NTCAN_INVALID_FIRMWARE";
00339 case NTCAN_MESSAGE_LOST: return "NTCAN_MESSAGE_LOST";
00340 case NTCAN_INVALID_HARDWARE: return "NTCAN_INVALID_HARDWARE";
00341
00342 case NTCAN_PENDING_WRITE: return "NTCAN_PENDING_WRITE";
00343 case NTCAN_PENDING_READ: return "NTCAN_PENDING_READ";
00344 case NTCAN_INVALID_DRIVER: return "NTCAN_INVALID_DRIVER";
00345
00346 case NTCAN_INVALID_PARAMETER: return "NTCAN_INVALID_PARAMETER";
00347 case NTCAN_INVALID_HANDLE: return "NTCAN_INVALID_HANDLE";
00348 case NTCAN_NET_NOT_FOUND: return "NTCAN_NET_NOT_FOUND";
00349 case NTCAN_INSUFFICIENT_RESOURCES: return "NTCAN_INSUFFICIENT_RESOURCES";
00350
00351 case NTCAN_OPERATION_ABORTED: return "NTCAN_OPERATION_ABORTED";
00352 }
00353 char msg[100];
00354 sprintf(msg, "unknown error code %d", ntstatus);
00355 return msg;
00356 }
00357
00367 int CanESD::readEvent()
00368 {
00369 EVMSG evmsg;
00370 int iRet = 0;
00371 int ret;
00372
00373 ret = canReadEvent(m_Handle, &evmsg, NULL);
00374
00375 if(ret == NTCAN_SUCCESS)
00376 {
00377 if( (int)evmsg.evid == NTCAN_EV_CAN_ERROR ) {
00378 switch( evmsg.evdata.s[0] ) {
00379 case 0x00:
00380 iRet = 0;
00381 break;
00382 case 0xC0:
00383 iRet = -6;
00384 std::cout << "BUS OFF" << std::endl;
00385 break;
00386 case 0x40:
00387 iRet = -7;
00388 std::cout << "ERROR PASSIVE" << std::endl;
00389 break;
00390 }
00391 if( evmsg.evdata.s[3] != 0 ) {
00392 iRet = -3;
00393 std::cout << "Lost " << (int)evmsg.evdata.s[3] << " messages" << std::endl;
00394 } else if( evmsg.evdata.s[5] != 0 ) {
00395 iRet = -5;
00396 std::cout << "Lost " << (int)evmsg.evdata.s[5] << " messages from fifo" << std::endl;
00397 }
00398 }
00399 }
00400 return iRet;
00401 }
00402