schunk_libm5api: schunk_libm5api: ESDDevice.cpp Source File

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