cob_forcetorque: CanESD.cpp Source File

00001 //-----------------------------------------------
00002 // Neobotix: Linux-Version
00003 // www.neobotix.de
00004 // Copyright (c) 2003. All rights reserved.
00005 
00006 // author: Oliver Barth, Winfried Baum, Jens Kubacki
00007 //-----------------------------------------------
00008 //#include "stdafx.h"
00009 #include <cob_forcetorque/CanESD.h>
00010 
00011 
00012 //-----------------------------------------------
00013 CanESD::CanESD(const char* cIniFile, bool bObjectMode)
00014 {
00015         m_bObjectMode = bObjectMode;
00016         m_bIsTXError = false;
00017         //m_IniFile.SetFileName(cIniFile, "CanESD.cpp");
00018         initIntern();
00019 }
00020 
00021 //-----------------------------------------------
00026 CanESD::~CanESD()
00027 {
00028         std::cout << "Closing CAN handle" << std::endl;
00029         canClose(m_Handle);
00030         //~CanItf();
00031 }
00032 
00033 
00034 //-----------------------------------------------
00035 void CanESD::initIntern()
00036 {       
00037         int ret=0;
00038 /* Does this make any sense as the handle is opened somewhere later?!
00039         //m_Handle = 9;
00040         
00041         //m_Mutex.lock();
00042 
00043         //ret=canClose(m_Handle);
00044 
00045         if(ret == NTCAN_SUCCESS)
00046         {
00047                 std::cout << "Can close ok ret = " << ret std::endl;
00048         }
00049 
00050         CAN_IF_STATUS *status;
00051 
00052         ret=canStatus(m_Handle,status);
00053 
00054         if(ret == NTCAN_SUCCESS)
00055         {
00056                 std::cout << "Get status from can card ok ret = " << ret << std::endl;
00057         }
00058 */      
00059         ret = 0;
00060         int iCanNet = 0;
00061         //m_IniFile.GetKeyInt( "CanCtrl", "NetESD", &iCanNet, true);
00062         
00063         int iBaudrateVal = NTCAN_BAUD_250;
00064         //m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true);
00065 
00066         std::cout << "Initializing CAN network with id =" << iCanNet << ", baudrate=" << iBaudrateVal << std::endl;
00067 
00068         int iRet;
00069         if( m_bObjectMode )
00070                 //iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 0, 0, &m_Handle);
00071                 iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 1000, 0, &m_Handle);
00072         else
00073                 //iRet = canOpen(iCanNet, 0, 10000, 10000, 0, 0, &m_Handle);
00074                 iRet = canOpen(iCanNet, 0, 10000, 10000, 1000, 0, &m_Handle);
00075         sleep(0.3);
00076 
00077         if(iRet == NTCAN_SUCCESS)
00078                 std::cout << "CanESD::CanESD(), init ok" << std::endl;
00079         else
00080                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00081 
00082 /*      ret=canClose(m_Handle);
00083         if(ret == NTCAN_SUCCESS)
00084         {
00085                 std::cout << "Can close ok ret = " << ret << std::endl;
00086                 if( m_bObjectMode )
00087                         iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 0, 0, &m_Handle);
00088                 else
00089                         iRet = canOpen(iCanNet, 0, 10000, 10000, 0, 0, &m_Handle);
00090                         Sleep(300);
00091 
00092                 if(iRet == NTCAN_SUCCESS)
00093                         std::cout << "CanESD::CanESD(), init ok" << ret << std::endl;
00094                 else
00095                         std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00096         }
00097 */
00098         iRet = canSetBaudrate(m_Handle, iBaudrateVal);
00099         if(iRet == NTCAN_SUCCESS)
00100                 std::cout << "CanESD::CanESD(), canSetBaudrate ok" << std::endl;
00101         else
00102                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00103         sleep(0.3);
00104 
00105         long lArg;
00106         iRet = canIoctl(m_Handle, NTCAN_IOCTL_FLUSH_RX_FIFO, NULL);
00107         //if( iRet == NTCAN_SUCCESS )
00108         //      std::cout << (int)(lArg) << " messages in CAN receive queue" << std::endl;
00109 
00110         // MMB/24.02.2006: Add all 11-bit identifiers as there is no loss in performance.
00111         for( int i=0; i<=0x7FF; ++i ) {
00112                 iRet = canIdAdd( m_Handle, i );
00113                 if(iRet != NTCAN_SUCCESS)
00114                         std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00115         }
00116 
00117 
00118         /*
00119         iRet = canIdAddGroup(m_Handle, 0x98);
00120         
00121         // for COB Arm
00122         iRet = canIdAdd(m_Handle, 0x100);
00123         iRet = canIdAdd(m_Handle, 0x101);
00124         iRet = canIdAdd(m_Handle, 0x200);
00125         iRet = canIdAdd(m_Handle, 0x201);
00126         iRet = canIdAdd(m_Handle, 0x300);
00127         iRet = canIdAdd(m_Handle, 0x301);
00128         iRet = canIdAdd(m_Handle, 0x400);
00129         iRet = canIdAdd(m_Handle, 0x401);
00130         iRet = canIdAdd(m_Handle, 0x500);
00131         iRet = canIdAdd(m_Handle, 0x501);
00132         iRet = canIdAdd(m_Handle, 0x600);
00133         iRet = canIdAdd(m_Handle, 0x601);
00134         
00135         // for CAN-Open Harmonica
00136         iRet = canIdAdd(m_Handle, 0x283);
00137         iRet = canIdAdd(m_Handle, 0x282);
00138         iRet = canIdAdd(m_Handle, 0x583);
00139         iRet = canIdAdd(m_Handle, 0x582);
00140         iRet = canIdAdd(m_Handle, 0x603);
00141         iRet = canIdAdd(m_Handle, 0x602);
00142 
00143         // for CAN-Open Harmonica and Cello Mimro
00144         iRet = canIdAdd(m_Handle, 0x18a);
00145         iRet = canIdAdd(m_Handle, 0x28a);
00146         iRet = canIdAdd(m_Handle, 0x194);
00147         iRet = canIdAdd(m_Handle, 0x294);
00148         iRet = canIdAdd(m_Handle, 0x19e);
00149         iRet = canIdAdd(m_Handle, 0x29e);
00150         
00151         // for RCS5000 arm
00152         iRet = canIdAdd(m_Handle, 0x111);
00153         iRet = canIdAdd(m_Handle, 0x112);
00154         iRet = canIdAdd(m_Handle, 0x113);
00155         iRet = canIdAdd(m_Handle, 0x114);
00156         iRet = canIdAdd(m_Handle, 0x115);
00157         iRet = canIdAdd(m_Handle, 0x116);
00158         iRet = canIdAdd(m_Handle, 0x788);
00159         iRet = canIdAdd(m_Handle, 0x789);
00160         iRet = canIdAdd(m_Handle, 0x78A);
00161         iRet = canIdAdd(m_Handle, 0x78B);
00162         iRet = canIdAdd(m_Handle, 0x78C);
00163         iRet = canIdAdd(m_Handle, 0x78D);
00164         iRet = canIdAdd(m_Handle, 0x78E);
00165         iRet = canIdAdd(m_Handle, 0x78F);
00166 
00167         iRet = canIdAddGroup(m_Handle, 0x00);
00168         iRet = canIdAddGroup(m_Handle, 0x08);
00169         iRet = canIdAddGroup(m_Handle, 0x10);
00170         iRet = canIdAddGroup(m_Handle, 0x18);
00171         iRet = canIdAddGroup(m_Handle, 0x20);
00172         iRet = canIdAddGroup(m_Handle, 0x28);
00173         iRet = canIdAddGroup(m_Handle, 0x30);
00174         iRet = canIdAddGroup(m_Handle, 0x38);
00175         iRet = canIdAddGroup(m_Handle, 0x40);
00176         iRet = canIdAddGroup(m_Handle, 0x48);
00177         iRet = canIdAddGroup(m_Handle, 0x50);
00178         iRet = canIdAddGroup(m_Handle, 0x58);
00179         iRet = canIdAddGroup(m_Handle, 0x60);
00180         iRet = canIdAddGroup(m_Handle, 0x68);
00181         iRet = canIdAddGroup(m_Handle, 0x98);
00182 
00183         // for Mimro drive ctrl
00184         iRet = canIdAdd(m_Handle, 0x123);
00185         */
00186 
00187 /*
00188         for(int i=0; i<2047; i++)
00189         {
00190                 if (iRet != NTCAN_SUCCESS)
00191                 {
00192                         TRACE("canIdAdd: %x Error \n", i);
00193                 }
00194         }
00195 */
00196         sleep(0.3);
00197 
00198         m_LastID = -1;
00199 
00200         //m_Mutex.unlock();
00201 }
00202 
00203 //-----------------------------------------------
00210 bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
00211 {
00212         CMSG NTCANMsg;
00213         NTCANMsg.id = CMsg.m_iID;
00214         NTCANMsg.len = CMsg.m_iLen;
00215 
00216         for(int i=0; i<8; i++)
00217         {
00218                 NTCANMsg.data[i] = CMsg.getAt(i);
00219                 //std::cout << "Msg: "std::cout << CMsg.getAt(i) << std::endl;
00220         }
00221         
00222         int ret;
00223         //long len;
00224         int32_t len;
00225         bool bRet = true;
00226         
00227         len = 1;
00228 
00229         //m_Mutex.lock();
00230         if (bBlocking)
00231                 ret = canWrite(m_Handle, &NTCANMsg, &len,+ NULL);
00232         else
00233                 ret = canSend(m_Handle, &NTCANMsg, &len);
00234 
00235         //m_Mutex.unlock();
00236 
00237         if( ret != NTCAN_SUCCESS)
00238         {
00239                 std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
00240                 bRet = false;
00241         }
00242 
00243         m_LastID = (int)NTCANMsg.data[0];
00244 
00245         //readEvent();
00246 
00247         //ret auswerten
00248         m_bIsTXError = !bRet;
00249         return bRet;
00250 }
00251 
00252 //-----------------------------------------------
00253 bool CanESD::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
00254 {
00255         int id = pCMsg->m_iID;
00256         CMSG NTCANMsg;
00257         NTCANMsg.len = 8;
00258         
00259         //long len;
00260         int32_t len;
00261         int i, ret;
00262         bool bRet = true;
00263         
00264         i=0;
00265         
00266         len = 1;
00267         //NTCANMsg.id = pCMsg->m_iID;
00268         //ret = canTake(m_Handle, &NTCANMsg, &len);
00269         
00270         do
00271         {
00272                 len = 1;
00273                 //m_Mutex.lock();
00274                 ret = canTake(m_Handle, &NTCANMsg, &len);
00275                 //m_Mutex.unlock();
00276                 //if( NTCANMsg.id == id ) break;
00277                 i++;
00278                 sleep(0.01);
00279         }
00280         //while((len != 1) && (i < iNrOfRetry));
00281         //while((len != 1) && (i < iNrOfRetry));
00282         while((len == 0) && (i < iNrOfRetry));
00283         
00284         
00285         //if((i == iNrOfRetry) || (ret != NTCAN_SUCCESS ))
00286         if(i == iNrOfRetry)
00287         //if( (ret != NTCAN_SUCCESS) || (len & 0x10) || (len == 0) )
00288         {
00289                 if( ret != NTCAN_SUCCESS )
00290                         std::cout << "error in CANESD::receiveMsgRetry: " << GetErrorStr(ret) << std::endl;
00291                 //std::cout << "PCan:ReceiveMessage error, msg id= " << m_LastID << std::endl;
00292                 //std::cout.unsetf(std::ios::dec);
00293                 //std::cout.setf(std::ios::hex);
00294                 //std::cout << "PCan:ReceiveMessage error, msg id= " << pCMsg->m_iID << std::endl;
00295                 //std::cout.unsetf(std::ios::hex);
00296                 //std::cout.setf(std::ios::dec);
00297                 bRet = false;
00298         }
00299         else
00300         {
00301                 pCMsg->m_iID = NTCANMsg.id;
00302                 pCMsg->m_iLen = NTCANMsg.len;
00303                 pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00304                         NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00305         }
00306                 
00307         //readEvent();
00308 
00309         return bRet;
00310 }
00311 
00312 //-----------------------------------------------
00313 bool CanESD::receiveMsg(CanMsg* pCMsg)
00314 {
00315         CMSG NTCANMsg;
00316         NTCANMsg.len = 8;
00317 
00318         int ret;
00319         //long len;
00320         int32_t len;
00321         bool bRet = true;
00322         
00323         len = 1;
00324 
00325         // Debug valgrind
00326         NTCANMsg.data[0] = 0;
00327         NTCANMsg.data[1] = 0;
00328         NTCANMsg.data[2] = 0;
00329         NTCANMsg.data[3] = 0;
00330         NTCANMsg.data[4] = 0;
00331         NTCANMsg.data[5] = 0;
00332         NTCANMsg.data[6] = 0;
00333         NTCANMsg.data[7] = 0;
00334         NTCANMsg.msg_lost = 0;
00335         NTCANMsg.id = 0;
00336         NTCANMsg.len = 0;
00337 
00338         pCMsg->set(0,0,0,0,0,0,0,0);
00339 
00340         
00341         if( !isObjectMode() ) {
00342                 pCMsg->m_iID = 0;
00343         } else {
00344                 NTCANMsg.id = pCMsg->m_iID;
00345         }
00346 
00347         //m_Mutex.lock();
00348         ret = canRead(m_Handle, &NTCANMsg, &len, NULL);
00349         //m_Mutex.unlock();
00350 
00351         if( !isObjectMode() ) {
00352                 if( (len == 1) && (ret == NTCAN_SUCCESS) )
00353                 {
00354                         // message received
00355                         pCMsg->m_iID = NTCANMsg.id;
00356                         pCMsg->m_iLen = NTCANMsg.len;
00357                         pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00358                                 NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00359                         bRet = true;
00360                 }
00361                 else
00362                 {
00363                         // no message
00364                         if( ret != NTCAN_SUCCESS)
00365                         {
00366                                 // error
00367                                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(ret) << std::endl;
00368                         }
00369         
00370                         pCMsg->m_iID = NTCANMsg.id;
00371                         pCMsg->set(0,0,0,0,0,0,0,0);
00372         
00373 //                      std::cout << "len=" << len << "; ret = " << GetErrorStr(ret) << std::endl;
00374                         bRet = false;
00375                 }
00376         } else {
00377                 //std::cout << "ID=" << NTCANMsg.id << "; len=" << (int)len << std::endl;
00378                 if( len == 16 ) {
00379                         // No message was received yet.
00380                         pCMsg->m_iID = NTCANMsg.id;
00381                         pCMsg->set(0,0,0,0,0,0,0,0);
00382                         bRet = false;
00383                 } else {
00384                         pCMsg->m_iID = NTCANMsg.id;
00385                         pCMsg->m_iLen = NTCANMsg.len;
00386                         pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00387                                    NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00388                         bRet = true;
00389                 }
00390         }
00391         
00392         if( NTCANMsg.msg_lost != 0 )
00393                 std::cout << (int)(NTCANMsg.msg_lost) << " messages lost!" << std::endl;
00394 
00395         //readEvent();
00396         /*
00397         DWORD status;
00398         long lArg;
00399         status = canIoctl(m_Handle, NTCAN_IOCTL_GET_RX_MSG_COUNT, &lArg);
00400         if( status == NTCAN_SUCCESS )
00401                 std::cout << (int)(lArg) << " messages in CAN receive queue" << std::endl;
00402         */
00403         return bRet;
00404 }
00405 
00406 
00418 int CanESD::canIdAddGroup(HANDLE handle, int id)
00419 {
00420         int result = NTCAN_SUCCESS;
00421         int i = 0;
00422         int iRes = 0;
00423         int cmd_id = invert(id);
00424         
00425         //m_Mutex.lock();
00426         for( i=0; i<8; ++i) {
00427                 iRes = canIdAdd(m_Handle, cmd_id+i);
00428                 //std::cout << "Adding CAN ID " << cmd_id+i << std::endl;
00429                 if( iRes != NTCAN_SUCCESS ) {
00430                         std::cout << "Adding CAN ID " << cmd_id+i << " failed with errorcode: " << iRes << " " << GetErrorStr(iRes) << std::endl;
00431                         result = iRes;
00432                 }
00433         }
00434         //m_Mutex.unlock();
00435 
00436         return result;
00437 }
00438 
00439 //-----------------------------------------------
00440 std::string CanESD::GetErrorStr(int ntstatus) const
00441 {
00442         switch (ntstatus)
00443         {
00444         case NTCAN_SUCCESS:                     return "NTCAN_SUCCESS";
00445         case NTCAN_RX_TIMEOUT:                  return "NTCAN_RX_TIMEOUT";
00446         case NTCAN_TX_TIMEOUT:                  return "NTCAN_TX_TIMEOUT";
00447         case NTCAN_TX_ERROR:                    return "NTCAN_TX_ERROR";
00448         case NTCAN_CONTR_OFF_BUS:               return "NTCAN_CONTR_OFF_BUS";
00449         case NTCAN_CONTR_BUSY:                  return "NTCAN_CONTR_BUSY";
00450         case NTCAN_CONTR_WARN:                  return "NTCAN_CONTR_WARN";
00451         case NTCAN_NO_ID_ENABLED:               return "NTCAN_NO_ID_ENABLED";
00452         case NTCAN_ID_ALREADY_ENABLED:          return "NTCAN_ID_ALREADY_ENABLED";
00453         case NTCAN_ID_NOT_ENABLED:              return "NTCAN_ID_NOT_ENABLED";
00454 
00455         case NTCAN_INVALID_FIRMWARE:            return "NTCAN_INVALID_FIRMWARE";
00456         case NTCAN_MESSAGE_LOST:                return "NTCAN_MESSAGE_LOST";
00457         case NTCAN_INVALID_HARDWARE:            return "NTCAN_INVALID_HARDWARE";
00458 
00459         case NTCAN_PENDING_WRITE:               return "NTCAN_PENDING_WRITE";
00460         case NTCAN_PENDING_READ:                return "NTCAN_PENDING_READ";
00461         case NTCAN_INVALID_DRIVER:              return "NTCAN_INVALID_DRIVER";
00462 
00463         case NTCAN_INVALID_PARAMETER:           return "NTCAN_INVALID_PARAMETER";
00464         case NTCAN_INVALID_HANDLE:              return "NTCAN_INVALID_HANDLE";
00465         case NTCAN_NET_NOT_FOUND:               return "NTCAN_NET_NOT_FOUND";
00466         case NTCAN_INSUFFICIENT_RESOURCES:      return "NTCAN_INSUFFICIENT_RESOURCES";
00467         
00468         case NTCAN_OPERATION_ABORTED:           return "NTCAN_OPERATION_ABORTED";
00469         }
00470         char msg[100];
00471         sprintf(msg, "unknown error code %d", ntstatus);
00472         return msg;
00473 }
00474 
00484 int CanESD::readEvent()
00485 {
00486         EVMSG evmsg;
00487         int iRet = 0;
00488         int ret;
00489 
00490         //m_Mutex.lock();
00491         ret = canReadEvent(m_Handle, &evmsg, NULL);
00492         //m_Mutex.unlock();
00493         
00494         if(ret == NTCAN_SUCCESS)
00495         {
00496                 if( (int)evmsg.evid == NTCAN_EV_CAN_ERROR ) {
00497                         switch( evmsg.evdata.s[0] ) {
00498                                 case 0x00:
00499                                         iRet = 0;
00500                                         break;
00501                                 case 0xC0:
00502                                         iRet = -6;
00503                                         std::cout << "BUS OFF" << std::endl;
00504                                         break;
00505                                 case 0x40:
00506                                         iRet = -7;
00507                                         std::cout << "ERROR PASSIVE" << std::endl;
00508                                         break;
00509                         }
00510                         if( evmsg.evdata.s[3] != 0 ) {
00511                                 iRet = -3;
00512                                 std::cout << "Lost " << (int)evmsg.evdata.s[3] << " messages" << std::endl;
00513                         } else if( evmsg.evdata.s[5] != 0 ) {
00514                                 iRet = -5;
00515                                 std::cout << "Lost " << (int)evmsg.evdata.s[5] << " messages from fifo" << std::endl;
00516                         }
00517                 }
00518         }
00519         return iRet;
00520 }
00521