ESDDevice.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012 SCHUNK GmbH & Co. KG
00003  * Copyright (c) 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *   http://www.apache.org/licenses/LICENSE-2.0
00010 
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 #include "ESDDevice.h"
00019 #if defined (_WIN32)
00020 #include "../include/ntcan.h"
00021 #endif
00022 #if defined(__LINUX__)
00023 #include <libntcan/ntcan.h>
00024 #endif
00025 #if defined (__QNX__)
00026 #include "../include/ntcan_qnx.h"
00027 #endif
00028 
00029 // ========================================================================== ;
00030 //                                                                            ;
00031 // ---- private auxiliary functions ----------------------------------------- ;
00032 //                                                                            ;
00033 // ========================================================================== ;
00034 
00035 // ========================================================================== ;
00036 //                                                                            ;
00037 // ---- protected auxiliary functions --------------------------------------- ;
00038 //                                                                            ;
00039 // ========================================================================== ;
00040 
00041 int CESDDevice::getDeviceError(int iErrorState)
00042 {
00043         if(iErrorState == NTCAN_CONTR_BUSY)
00044         {
00045                 warning("NTCAN_CONTR_BUSY");
00046                 return ERRID_DEV_READERROR;
00047         }
00048         else if(iErrorState == NTCAN_CONTR_OFF_BUS)
00049         {
00050                 warning("NTCAN_CONTR_OFF_BUS");
00051                 return ERRID_DEV_READERROR;
00052         }
00053         else if(iErrorState == NTCAN_CONTR_WARN)
00054         {
00055                 warning("NTCAN_CONTR_WARN");
00056                 return ERRID_DEV_READERROR;
00057         }
00058         else if(iErrorState == NTCAN_ID_NOT_ENABLED)
00059         {
00060                 warning("NTCAN_ID_NOT_ENABLED");
00061                 return ERRID_DEV_READERROR;
00062         }
00063 #if defined(_WIN32)     
00064         else if(iErrorState == NTCAN_INVALID_HANDLE)
00065         {
00066                 warning("NTCAN_INVALID_HANDLE");
00067                 return ERRID_DEV_READERROR;
00068         }
00069         else if(iErrorState == NTCAN_INVALID_HARDWARE)
00070         {
00071                 warning("NTCAN_INVALID_HARDWARE");
00072                 return ERRID_DEV_READERROR;
00073         }
00074         else if(iErrorState == NTCAN_INVALID_PARAMETER)
00075         {
00076                 warning("NTCAN_INVALID_PARAMETER");
00077                 return ERRID_DEV_READERROR;
00078         }
00079         else if(iErrorState == NTCAN_NET_NOT_FOUND)
00080         {
00081                 warning("NTCAN_NET_NOT_FOUND");
00082                 return ERRID_DEV_READERROR;
00083         }
00084 #endif
00085         else if(iErrorState == NTCAN_MESSAGE_LOST)
00086         {
00087                 warning("NTCAN_MESSAGE_LOST");
00088                 return ERRID_DEV_READERROR;
00089         }
00090         else if(iErrorState == NTCAN_NO_ID_ENABLED)
00091         {
00092                 warning("NTCAN_NO_ID_ENABLED");
00093                 return ERRID_DEV_READERROR;
00094         }
00095         else if(iErrorState == NTCAN_RX_TIMEOUT)
00096         {
00097                 warning("NTCAN_RX_TIMEOUT");
00098                 return ERRID_DEV_READTIMEOUT;
00099         }
00100         else if(iErrorState == NTCAN_TX_TIMEOUT)
00101         {
00102                 warning("NTCAN_TX_TIMEOUT");
00103                 return ERRID_DEV_WRITETIMEOUT;
00104         }
00105         else if(iErrorState == NTCAN_TX_ERROR)
00106         {
00107                 warning("NTCAN_TX_ERROR");
00108                 return ERRID_DEV_WRITEERROR;
00109         }
00110         return ERRID_DEV_WRITEERROR;
00111 }
00112 
00113 int CESDDevice::setBaudRate()
00114 {
00115         int iRetVal = 0;
00116         m_iErrorState = 0;
00117         switch( m_iBaudRate )
00118         {
00119                 case 125:
00120                         m_uiBaudRate=0x06;      // 125k
00121                         break;
00122                 case 250:
00123                         m_uiBaudRate=0x04;      // 250k
00124                         break;
00125                 case 500:
00126                         m_uiBaudRate=0x02;      // 500k
00127                         break;
00128                 case 1000:
00129                         m_uiBaudRate=0x00;      // 1000k
00130                         break;
00131                 default:
00132                         m_uiBaudRate=0x04;      // 250k
00133                         break;
00134         }
00135 
00136         iRetVal = canSetBaudrate(m_hDevice, m_uiBaudRate);
00137         if(iRetVal != NTCAN_SUCCESS)
00138         {
00139                 warning("can set baudrate 0x%x failed Errorcode: %d", m_uiBaudRate, iRetVal);
00140                 getDeviceError(iRetVal);
00141                 m_iErrorState = ERRID_DEV_INITERROR;
00142                 return m_iErrorState;
00143         }       
00144         iRetVal = canSetBaudrate(m_hSyncDevice, m_uiBaudRate);
00145         if(iRetVal != NTCAN_SUCCESS)
00146         {
00147                 warning("can set baudrate 0x%x failed Errorcode: %d", m_uiBaudRate, iRetVal);
00148                 getDeviceError(iRetVal);
00149                 m_iErrorState = ERRID_DEV_INITERROR;
00150                 return m_iErrorState;
00151         }       
00152         return m_iErrorState;
00153 }
00154 
00155 int CESDDevice::setMessageId(unsigned long uiMessageId)
00156 {
00157         int iRetVal = 0;
00158         m_iErrorState = 0;
00159         iRetVal = canIdAdd(m_hDevice, uiMessageId);
00160         if(iRetVal != NTCAN_SUCCESS)
00161         {
00162                 warning("can add ID failed Errorcode: %d", iRetVal);
00163                 getDeviceError(iRetVal);
00164                 m_iErrorState = ERRID_DEV_INITERROR;
00165                 return m_iErrorState;
00166         }
00167         return m_iErrorState;
00168 }
00169 
00170 int CESDDevice::clearReadQueue()
00171 {
00172         int iRetVal = 0;
00173         int32_t iNumberOfMessages = 1;
00174         static CMSG clESDProtocolMessage;
00175         m_iErrorState = 0;
00176         do
00177         {       
00178                 iRetVal = canRead(m_hDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);                  
00179         }while( ! ( ( iNumberOfMessages == 0 ) && ( iRetVal == NTCAN_SUCCESS ) ) );
00180         
00181         return m_iErrorState;
00182 }
00183 
00184 int CESDDevice::reinit(unsigned char ucBaudRateId)
00185 {
00186         int i, iRetVal = 0;
00187         m_iErrorState = 0;
00188         if(!m_bInitFlag)
00189         {
00190                 warning("device not initialized");
00191                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
00192                 return m_iErrorState;
00193         }
00194         switch(ucBaudRateId)
00195         {
00196                 case BAUDRATEID_MOD_CAN_125K:
00197                         m_iBaudRate = 125;
00198                         break;
00199                 case BAUDRATEID_MOD_CAN_250K:
00200                         m_iBaudRate = 250;
00201                         break;
00202                 case BAUDRATEID_MOD_CAN_500K:
00203                         m_iBaudRate = 500;
00204                         break;
00205                 case BAUDRATEID_MOD_CAN_1000K:
00206                         m_iBaudRate = 1000;
00207                         break;
00208         }
00209         iRetVal = canClose(m_hDevice);
00210         if(iRetVal != NTCAN_SUCCESS)
00211         {
00212                 warning("can close failed Errorcode: %d", iRetVal);
00213                 getDeviceError(iRetVal);
00214                 m_iErrorState = ERRID_DEV_EXITERROR;
00215         }
00216         iRetVal = canClose(m_hSyncDevice);
00217         if(iRetVal != NTCAN_SUCCESS)
00218         {
00219                 warning("can close failed Errorcode: %d", iRetVal);
00220                 getDeviceError(iRetVal);
00221                 m_iErrorState = ERRID_DEV_EXITERROR;
00222         }
00223         m_bInitFlag = false;
00224         iRetVal = canOpen(      
00225                                 m_iDeviceId,                    // Net
00226                                 0,                                              // Mode
00227                                 m_uiQueueSize,                  // TX Queue
00228                                 m_uiQueueSize,                  // RX Queue
00229                                 20*m_uiTimeOut,                 // Tx Timeout
00230                                 m_uiTimeOut,                    // Rx Timeout
00231                                 &m_hDevice);
00232         if(iRetVal != NTCAN_SUCCESS)
00233         {
00234                 warning("can open failed Errorcode: %d", iRetVal);
00235                 getDeviceError(iRetVal);
00236                 m_iErrorState = ERRID_DEV_INITERROR;
00237                 return m_iErrorState;
00238         }
00239         /*iRetVal = canOpen(    
00240                                 m_iDeviceId,                    // Net
00241                                 0,                                              // Mode
00242                                 1,                                              // TX Queue
00243                                 1,                                              // RX Queue
00244                                 600,                                    // Tx Timeout
00245                                 100,                                    // Rx Timeout
00246                                 &m_hSyncDevice);
00247         if(iRetVal != NTCAN_SUCCESS)
00248         {
00249                 warning("can open failed Errorcode: %d", iRetVal);
00250                 getDeviceError(iRetVal);
00251                 m_iErrorState = ERRID_DEV_INITERROR;
00252                 return m_iErrorState;
00253         }*/
00254 
00255         m_iErrorState = setBaudRate();
00256         if(m_iErrorState != 0)
00257                 return m_iErrorState;
00258 
00259         for(i = 0; i <= m_iModuleCountMax; i++)
00260         {
00261                 iRetVal = canIdAdd(m_hDevice, (MSGID_ACK + i));
00262                 if(iRetVal != NTCAN_SUCCESS)
00263                 {
00264                         warning("can add ID failed Errorcode: %d", iRetVal);
00265                         getDeviceError(iRetVal);
00266                         m_iErrorState = ERRID_DEV_INITERROR;
00267                         return m_iErrorState;
00268                 }
00269                 iRetVal = canIdAdd(m_hDevice, (MSGID_STATE + i));
00270                 if(iRetVal != NTCAN_SUCCESS)
00271                 {
00272                         warning("can add ID failed Errorcode: %d", iRetVal);
00273                         getDeviceError(iRetVal);
00274                         m_iErrorState = ERRID_DEV_INITERROR;
00275                         return m_iErrorState;
00276                 }
00277         }
00278 
00279         for(i = 0; i < MAX_MP55; i++ )
00280         {
00281                 iRetVal = canIdAdd(m_hDevice, (MSGID_MP55_RECV + i));
00282                 if(iRetVal != NTCAN_SUCCESS)
00283                 {
00284                         warning("can add ID failed Errorcode: %d", iRetVal);
00285                         getDeviceError(iRetVal);
00286                         m_iErrorState = ERRID_DEV_INITERROR;
00287                         return m_iErrorState;
00288                 }
00289 
00290                 iRetVal = canIdAdd(m_hDevice, (0x180 + i));
00291                 if(iRetVal != NTCAN_SUCCESS)
00292                 {
00293                         warning("can add ID failed Errorcode: %d", iRetVal);
00294                         getDeviceError(iRetVal);
00295                         m_iErrorState = ERRID_DEV_INITERROR;
00296                         return m_iErrorState;
00297                 }
00298         }
00299 
00300         for(i = 0; i < MAX_SCHUNK; i++ )
00301         {
00302                 iRetVal = canIdAdd(m_hDevice, (MSGID_SCHUNK_RECV + i));
00303                 if(iRetVal != NTCAN_SUCCESS)
00304                 {
00305                         warning("can add ID failed Errorcode: %d", iRetVal);
00306                         getDeviceError(iRetVal);
00307                         m_iErrorState = ERRID_DEV_INITERROR;
00308                         return m_iErrorState;
00309                 }
00310         }
00311 
00312         iRetVal = canIdAdd(m_hSyncDevice, MSGID_ALL);
00313         if(iRetVal != NTCAN_SUCCESS)
00314         {
00315                 warning("can add ID failed Errorcode: %d", iRetVal);
00316                 getDeviceError(iRetVal);
00317                 m_iErrorState = ERRID_DEV_INITERROR;
00318                 return m_iErrorState;
00319         }
00320 
00321         m_iErrorState = clearReadQueue();
00322         if(m_iErrorState != 0)
00323                 return m_iErrorState;
00324 
00325         if(m_iErrorState == 0)
00326                 m_bInitFlag = true;
00327 
00328         updateModuleIdMap();
00329         return m_iErrorState;
00330 }
00331 
00332 int CESDDevice::readDevice(CProtocolMessage& rclProtocolMessage)
00333 {
00334         int iRetVal = 0;
00335         int32_t iNumberOfMessages = 1;
00336         CMSG clESDProtocolMessage;
00337         m_iErrorState = 0;
00338 
00339 #if defined ( _WIN32 )
00340         float fTimeDiff = 0;
00341         double dFrequency = 0;
00342         LARGE_INTEGER liTime, liTimeStart, liTimeEnd;
00343 
00344         QueryPerformanceFrequency( &liTime );
00345         dFrequency = liTime.LowPart;
00346 
00347         QueryPerformanceCounter( &liTimeStart );
00348         do
00349         {
00350                 iNumberOfMessages = 1;
00351                 iRetVal = canTake( m_hDevice, &clESDProtocolMessage, &iNumberOfMessages );      
00352                 if(iRetVal != NTCAN_SUCCESS)
00353                 {
00354                         warning("can read failed Errorcode: %d", iRetVal);
00355                         m_iErrorState = getDeviceError(iRetVal);
00356                         return m_iErrorState;
00357                 }
00358                 QueryPerformanceCounter( &liTimeEnd );
00359                 fTimeDiff = (float)(liTimeEnd.LowPart - liTimeStart.LowPart) / (float)dFrequency * 1000;
00360 
00361                 if( fTimeDiff >= m_uiTimeOut )
00362                 {
00363                         m_iErrorState = ERRID_DEV_READTIMEOUT;
00364                         return m_iErrorState;
00365                 }
00366 
00367         } while( iNumberOfMessages == 0 );
00368 #else
00369         iRetVal = canRead(m_hDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);                  
00370         if(iRetVal != NTCAN_SUCCESS)
00371         {
00372                 warning("can read failed Errorcode: %d", iRetVal);
00373                 m_iErrorState = getDeviceError(iRetVal);
00374                 return m_iErrorState;
00375         }
00376 #endif
00377 
00378         rclProtocolMessage.m_uiMessageId = clESDProtocolMessage.id;
00379         rclProtocolMessage.m_ucMessageLength = clESDProtocolMessage.len;
00380         memcpy(rclProtocolMessage.m_aucMessageData, clESDProtocolMessage.data, rclProtocolMessage.m_ucMessageLength);
00381         printMessage(rclProtocolMessage,READ);
00382 
00383         return m_iErrorState;
00384 }
00385 
00386 int CESDDevice::writeDevice(CProtocolMessage& rclProtocolMessage)
00387 {
00388         int iRetVal = 0;
00389         int32_t iNumberOfMessages = 1;
00390         CMSG clESDProtocolMessage;
00391         m_iErrorState = 0;
00392 
00393         //debug output
00394 
00395         printMessage(rclProtocolMessage,WRITE);
00396         clESDProtocolMessage.id = rclProtocolMessage.m_uiMessageId;
00397         clESDProtocolMessage.len = rclProtocolMessage.m_ucMessageLength;
00398         if(rclProtocolMessage.m_bRTRFlag)
00399                 clESDProtocolMessage.len |= 0x10;
00400         memcpy(clESDProtocolMessage.data, rclProtocolMessage.m_aucMessageData, rclProtocolMessage.m_ucMessageLength);
00401         iRetVal = canWrite(m_hDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);
00402 //      iRetVal = canSend(m_hDevice, &clESDProtocolMessage, &iNumberOfMessages);
00403         if(iRetVal != NTCAN_SUCCESS)
00404         {
00405                 warning("can send failed Errorcode: %d", iRetVal);
00406                 m_iErrorState = getDeviceError(iRetVal);
00407                 return m_iErrorState;
00408         }
00409 
00410         return m_iErrorState;
00411 }
00412 
00413 // ========================================================================== ;
00414 //                                                                            ;
00415 // ---- constructors / destructor ------------------------------------------- ;
00416 //                                                                            ;
00417 // ========================================================================== ;
00418 
00419 CESDDevice::CESDDevice() : m_hDevice(0), m_hSyncDevice(0), m_iDeviceId(-1), m_uiBaudRate(0), m_uiQueueSize(128), m_uiTimeOut(3)         // ESD C331
00420 {
00421         initMessage("CESDDevice", g_iDebugLevel, g_bDebug, g_bDebugFile);
00422 }
00423 
00424 CESDDevice::CESDDevice(const CESDDevice& rclESDDevice)
00425 {
00426         error(-1, "Sorry constructor is not implemented");
00427 }
00428 
00429 CESDDevice::~CESDDevice()
00430 {
00431         exit();
00432 }
00433 
00434 // ========================================================================== ;
00435 //                                                                            ;
00436 // ---- operators ----------------------------------------------------------- ;
00437 //                                                                            ;
00438 // ========================================================================== ;
00439 
00440 CESDDevice& CESDDevice::operator=(const CESDDevice& rclESDDevice)
00441 {
00442         error(-1, "Sorry operator= is not implemented");
00443         return *this;
00444 }
00445 
00446 // ========================================================================== ;
00447 //                                                                            ;
00448 // ---- query functions ----------------------------------------------------- ;
00449 //                                                                            ;
00450 // ========================================================================== ;
00451 
00452 // ========================================================================== ;
00453 //                                                                            ;
00454 // ---- modify functions ---------------------------------------------------- ;
00455 //                                                                            ;
00456 // ========================================================================== ;
00457 
00458 void CESDDevice::setQueueSize(unsigned short uiQueueSize)
00459 {
00460         m_uiQueueSize = uiQueueSize;
00461 }
00462 
00463 void CESDDevice::setTimeOut(unsigned long uiTimeOut)
00464 {
00465         m_uiTimeOut= uiTimeOut;
00466 }
00467 
00468 // ========================================================================== ;
00469 //                                                                            ;
00470 // ---- I/O functions ------------------------------------------------------- ;
00471 //                                                                            ;
00472 // ========================================================================== ;
00473 
00474 // ========================================================================== ;
00475 //                                                                            ;
00476 // ---- exec functions ------------------------------------------------------ ;
00477 //                                                                            ;
00478 // ========================================================================== ;
00479 
00480 int CESDDevice::init()
00481 {
00482         return init(m_acInitString);
00483 }
00484 
00485 int CESDDevice::init(const char* acInitString)
00486 {
00487         InitializeCriticalSection(&m_csDevice);
00488         int i, iRetVal = 0;
00489         int txTimeOut = 0;
00490         char* pcToken;
00491         char acString[128];
00492         if(m_bInitFlag)
00493         {
00494                 warning("device already initialized");
00495                 m_iErrorState = ERRID_DEV_ISINITIALIZED;
00496                 return m_iErrorState;
00497         }
00498         m_iDeviceId = -1;
00499         m_iErrorState = 0;
00500         strncpy(m_acInitString,acInitString,128);
00501         strncpy(acString,acInitString,128);
00502         pcToken = strtok( acString, ":" );
00503         if( !pcToken )
00504         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00505                 return m_iErrorState;
00506         }
00507         if( strcmp( pcToken, "ESD" ) != 0 )
00508         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00509                 return m_iErrorState;
00510         }
00511         pcToken = strtok( NULL, "," );
00512         if( !pcToken )
00513         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00514                 return m_iErrorState;
00515         }
00516         m_iDeviceId = atoi(pcToken);
00517 
00518         pcToken = strtok( NULL, "," );
00519         if( !pcToken )
00520         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00521                 return m_iErrorState;
00522         }
00523         m_iBaudRate = atoi(pcToken);
00524 
00525 #if defined(__LINUX__)
00526         m_uiTimeOut = 6;
00527 #endif
00528 #if defined (_WIN32)
00529         switch( m_iBaudRate )
00530         {
00531         case 125:
00532         case 250:
00533                 m_uiTimeOut = 4;
00534                 break;
00535         case 500:
00536                 m_uiTimeOut = 3;
00537                 break;
00538         case 1000:
00539                 m_uiTimeOut = 2;
00540                 break;
00541         default:
00542                 m_uiTimeOut = 10;
00543                 break;
00544         }
00545 #endif
00546 
00547         try
00548         {
00549                 iRetVal = canOpen(      
00550                                         m_iDeviceId,                    // Net
00551                                         0,                                              // Mode
00552                                         m_uiQueueSize,                  // TX Queue
00553                                         m_uiQueueSize,                  // RX Queue
00554                                         m_uiTimeOut,                    // Tx Timeout
00555                                         m_uiTimeOut,                    // Rx Timeout
00556                                         &m_hDevice);
00557                 if(iRetVal != NTCAN_SUCCESS)
00558                 {
00559                         warning("can open failed Errorcode: %d", iRetVal);
00560                         getDeviceError(iRetVal);
00561                         m_iErrorState = ERRID_DEV_INITERROR;
00562                         return m_iErrorState;
00563                 }
00564                 iRetVal = canOpen(      
00565                                         m_iDeviceId,                    // Net
00566                                         0,                                              // Mode
00567                                         1,                                              // TX Queue
00568                                         1,                                              // RX Queue
00569                                         600,                                    // Tx Timeout
00570                                         100,                                    // Rx Timeout
00571                                         &m_hSyncDevice);
00572                 if(iRetVal != NTCAN_SUCCESS)
00573                 {
00574                         warning("can open failed Errorcode: %d", iRetVal);
00575                         getDeviceError(iRetVal);
00576                         m_iErrorState = ERRID_DEV_INITERROR;
00577                         return m_iErrorState;
00578                 }
00579         }
00580         catch(...)
00581         {
00582                         warning("init ESD device failed no library found");
00583                         m_iErrorState = ERRID_DEV_NOLIBRARY;
00584                         return m_iErrorState;
00585         }
00586 
00587         for(i = 0; i <= m_iModuleCountMax; i++)
00588         {
00589                 iRetVal = canIdAdd(m_hDevice, (MSGID_ACK + i));
00590                 if(iRetVal != NTCAN_SUCCESS)
00591                 {
00592                         warning("can add ID failed Errorcode: %d", iRetVal);
00593                         getDeviceError(iRetVal);
00594                         m_iErrorState = ERRID_DEV_INITERROR;
00595                         return m_iErrorState;
00596                 }
00597                 iRetVal = canIdAdd(m_hDevice, (MSGID_STATE + i));
00598                 if(iRetVal != NTCAN_SUCCESS)
00599                 {
00600                         warning("can add ID failed Errorcode: %d", iRetVal);
00601                         getDeviceError(iRetVal);
00602                         m_iErrorState = ERRID_DEV_INITERROR;
00603                         return m_iErrorState;
00604                 }
00605         }
00606 
00607         for(i = 0; i <= MAX_MP55; i++ )
00608         {
00609                 iRetVal = canIdAdd(m_hDevice, (MSGID_MP55_RECV + i));
00610                 if(iRetVal != NTCAN_SUCCESS)
00611                 {
00612                         warning("can add ID failed Errorcode: %d", iRetVal);
00613                         getDeviceError(iRetVal);
00614                         m_iErrorState = ERRID_DEV_INITERROR;
00615                         return m_iErrorState;
00616                 }
00617 
00618                 iRetVal = canIdAdd(m_hDevice, (0x180 + i));
00619                 if(iRetVal != NTCAN_SUCCESS)
00620                 {
00621                         warning("can add ID failed Errorcode: %d", iRetVal);
00622                         getDeviceError(iRetVal);
00623                         m_iErrorState = ERRID_DEV_INITERROR;
00624                         return m_iErrorState;
00625                 }
00626                 iRetVal = canIdAdd(m_hDevice, (0x600 + i));
00627                 if(iRetVal != NTCAN_SUCCESS)
00628                 {
00629                         warning("can add ID failed Errorcode: %d", iRetVal);
00630                         getDeviceError(iRetVal);
00631                         m_iErrorState = ERRID_DEV_INITERROR;
00632                         return m_iErrorState;
00633                 }
00634         }
00635 
00636         for(i = 0; i < MAX_SCHUNK; i++ )
00637         {
00638                 iRetVal = canIdAdd(m_hDevice, (MSGID_SCHUNK_RECV + i));
00639                 if(iRetVal != NTCAN_SUCCESS)
00640                 {
00641                         warning("can add ID failed Errorcode: %d", iRetVal);
00642                         getDeviceError(iRetVal);
00643                         m_iErrorState = ERRID_DEV_INITERROR;
00644                         return m_iErrorState;
00645                 }
00646         }
00647         
00648         iRetVal = canIdAdd(m_hSyncDevice, MSGID_ALL);
00649         if(iRetVal != NTCAN_SUCCESS)
00650         {
00651                 warning("can add ID failed Errorcode: %d", iRetVal);
00652                 getDeviceError(iRetVal);
00653                 m_iErrorState = ERRID_DEV_INITERROR;
00654                 return m_iErrorState;
00655         }
00656 
00657         m_iErrorState = setBaudRate();
00658         if(m_iErrorState != 0)
00659                 return m_iErrorState;
00660 
00661         m_iErrorState = clearReadQueue();
00662         if(m_iErrorState != 0)
00663                 return m_iErrorState;
00664 
00665         if(m_iErrorState == 0)
00666                 m_bInitFlag = true;
00667 
00668         updateModuleIdMap();
00669         return m_iErrorState;
00670 }
00671 
00672 int CESDDevice::exit()
00673 {
00674         int iRetVal = 0;
00675         m_iErrorState = 0;
00676         if(!m_bInitFlag)
00677         {
00678                 warning("device not initialized");
00679                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
00680                 return m_iErrorState;
00681         }
00682         EnterCriticalSection(&m_csDevice);
00683         iRetVal = canClose(m_hDevice);
00684         if(iRetVal != NTCAN_SUCCESS)
00685         {
00686                 warning("can close failed Errorcode: %d", iRetVal);
00687                 getDeviceError(iRetVal);
00688                 m_iErrorState = ERRID_DEV_EXITERROR;
00689         }
00690         iRetVal = canClose(m_hSyncDevice);
00691         if(iRetVal != NTCAN_SUCCESS)
00692         {
00693                 warning("can close failed Errorcode: %d", iRetVal);
00694                 getDeviceError(iRetVal);
00695                 m_iErrorState = ERRID_DEV_EXITERROR;
00696         }
00697         m_bInitFlag = false;
00698         LeaveCriticalSection(&m_csDevice);
00699         DeleteCriticalSection(&m_csDevice);
00700         return m_iErrorState;
00701 }
00702 
00703 int CESDDevice::waitForStartMotionAll()
00704 {
00705         int iRetVal = 0;
00706         bool bRecieved = false;
00707         int32_t iNumberOfMessages = 1;
00708         CMSG clESDProtocolMessage;
00709         m_iErrorState = 0;
00710 
00711         do
00712         {       
00713                 iRetVal = canRead(m_hSyncDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);                      
00714                 if(iRetVal != NTCAN_SUCCESS)
00715                 {
00716                         warning("can read failed Errorcode: %d", iRetVal);
00717                         m_iErrorState = getDeviceError(iRetVal);
00718                         return m_iErrorState;
00719                 }
00720                 bRecieved = true;
00721                 if(clESDProtocolMessage.id != MSGID_ALL)
00722                 {
00723                         debug(1, "received CAN-ID %x, expected %x", clESDProtocolMessage.id, MSGID_ALL);
00724                         bRecieved = false;
00725                 }
00726                 if(clESDProtocolMessage.data[0] != CMDID_STARTMOVE)
00727                 {
00728                         debug(1, "wrong command ID");
00729                         bRecieved = false;
00730                 }
00731         }
00732         while(!bRecieved);
00733         return m_iErrorState;
00734 }
00735

schunk_libm5api
Author(s): Florian Weisshardt
autogenerated on Sat Jun 8 2019 20:25:13