PCanDevice.cpp
Go to the documentation of this 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 "PCanDevice.h"
00050 #include <errno.h>
00051 // ========================================================================== ;
00052 //                                                                            ;
00053 // ---- private auxiliary functions ----------------------------------------- ;
00054 //                                                                            ;
00055 // ========================================================================== ;
00056 
00057 // ========================================================================== ;
00058 //                                                                            ;
00059 // ---- protected auxiliary functions --------------------------------------- ;
00060 //                                                                            ;
00061 // ========================================================================== ;
00062 
00063 //possible incomplete
00064 //
00065 
00066 int CPCanDevice::getDeviceError(int iErrorState)
00067 {
00068         int error = ERRID_DEV_WRITEERROR;
00069 
00070         if(((unsigned int)iErrorState & 0xFFFF) & CAN_ERR_QRCVEMPTY)
00071         {
00072                 warning("receive queue is empty");
00073                 error= ERRID_DEV_WRITEERROR;
00074         }
00075         if(iErrorState & CAN_ERR_OVERRUN)
00076         {
00077                 warning("receive buffer overrun");
00078                 error= ERRID_DEV_READERROR;
00079         }
00080         if(iErrorState & CAN_ERR_XMTFULL)
00081         {
00082                 warning("transmit buffer full");
00083                 error =  ERRID_DEV_WRITEERROR;
00084         }
00085         if(iErrorState & CAN_ERR_BUSOFF)
00086         {
00087                 warning("CAN_ERR_OFF_BUS");
00088                 error =  ERRID_DEV_READERROR;
00089         }
00090         if(iErrorState & CAN_ERR_ILLPARAMTYPE )
00091         {
00092                 warning("CAN_ERR_ILLPARAMTYPE");
00093                 error =  ERRID_DEV_READERROR;
00094         }
00095         /*
00096          * no corresponding errors in pcan.h found
00097          *
00098         else if(iErrorState == NTCAN_MESSAGE_LOST)
00099         {
00100                 warning("NTCAN_MESSAGE_LOST");
00101                 return ERRID_DEV_READERROR;
00102         }
00103         else if(iErrorState == NTCAN_RX_TIMEOUT)
00104         {
00105                 warning("NTCAN_RX_TIMEOUT");
00106                 return ERRID_DEV_READTIMEOUT;
00107         }
00108         else if(iErrorState == NTCAN_TX_TIMEOUT)
00109         {
00110                 warning("NTCAN_TX_TIMEOUT");
00111                 return ERRID_DEV_WRITETIMEOUT;
00112         }
00113         */
00114         if(((unsigned int)iErrorState & 0xFFFF) & CAN_ERR_QXMTFULL)
00115         {
00116                 warning("transmit queue full");
00117                 error =  ERRID_DEV_WRITEERROR;
00118         }
00119         if(iErrorState & CAN_ERR_BUSLIGHT)
00120         {
00121                 warning("bus error");
00122                 error =  ERRID_DEV_WRITEERROR;
00123         }
00124         if(((unsigned int)iErrorState & 0xFFFF) & CAN_ERR_BUSHEAVY)
00125         {
00126                 warning("bus error");
00127                 error =  ERRID_DEV_WRITEERROR;
00128         }
00129         if(((unsigned int)iErrorState & 0xFFFF) & CAN_ERR_RESOURCE)
00130         {
00131                 warning("can't create resource");
00132                 error =  ERRID_DEV_WRITEERROR;
00133         }
00134 
00135         return error;
00136 }
00137 
00138 //ok
00139 int CPCanDevice::setBaudRate(unsigned char iBaudRate)
00140 {
00141         m_uiBaudRate = (unsigned long) iBaudRate;
00142         return setBaudRate();
00143 }
00144 
00145 //ok
00146 int CPCanDevice::setBaudRate()
00147 {
00148         /*
00149          * baud rate must be set when initializing can device!
00150          *
00151          * */
00152 
00153         debug(0,"entering CPCanDevice::setBaudRate()...\n");
00154         warning("PCan Device must be reset to set the new baud rate!\n");
00155         
00156         int iRetVal = 0;
00157         m_iErrorState = 0;
00158 
00159         switch( m_iBaudRate )
00160         {
00161                 case 125:
00162                         m_uiBaudRate=CAN_BAUD_125K;     // 125k
00163                         break;
00164                 case 250:
00165                         m_uiBaudRate=CAN_BAUD_250K;     // 250k
00166                         break;
00167                 case 500:
00168                         m_uiBaudRate=CAN_BAUD_500K;     // 500k
00169                         break;
00170                 case 1000:
00171                         m_uiBaudRate=CAN_BAUD_1M;       // 1000k
00172                         break;
00173                 default:
00174                         m_uiBaudRate=CAN_BAUD_250K;     // 250k
00175                         break;
00176         }
00177 
00178         
00179         if (m_bInitFlag)
00180         {
00181                 CAN_Close(m_handle);
00182         }
00183         iRetVal = init(m_uiBaudRate);
00184         debug(0,"InitFlag set to %d\n",m_bInitFlag);
00185         if(iRetVal != CAN_ERR_OK)
00186         {
00187                 warning("can set baudrate 0x%x failed Errorcode: %d", m_uiBaudRate, iRetVal);
00188                 getDeviceError(iRetVal);
00189                 m_iErrorState = ERRID_DEV_INITERROR;
00190                 return m_iErrorState;
00191         }       
00192         else
00193         {
00194           debug(0,"PCanDevice: setting baud rate to %d\n",m_iBaudRate);
00195         }
00196         return m_iErrorState;
00197 }
00198 
00199 //ok
00200 int CPCanDevice::setMessageId(unsigned long uiMessageId)
00201 {
00202         int iRetVal = 0;
00203         m_iErrorState = 0;
00204         //iRetVal = canIdAdd(m_hDevice, uiMessageId);
00205         iRetVal = CAN_MsgFilter(m_handle, uiMessageId ,uiMessageId,MSGTYPE_STANDARD);
00206         if(iRetVal != CAN_ERR_OK)
00207         {
00208                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00209                 getDeviceError(iRetVal);
00210                 m_iErrorState = ERRID_DEV_INITERROR;
00211                 return m_iErrorState;
00212         }
00213         return m_iErrorState;
00214 }
00215 
00216 //ok
00217 int CPCanDevice::clearReadQueue()
00218 {
00219         int iRetVal = 0;
00220         TPCANRdMsg TPCMsg;
00221         
00222         debug(0,"entering CPCanDevice::clearReadQueue()...\n");
00223         TPCMsg.Msg.LEN = 8;
00224         TPCMsg.Msg.MSGTYPE = 0;
00225         TPCMsg.Msg.ID = 0;
00226 
00227         m_iErrorState = 0;
00228         do
00229         {       
00230                 //iRetVal = canRead(m_hDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);                        
00231                 debug(0,"Trying to read messages ...");
00232                 iRetVal = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, m_uiTimeOut);
00233                 debug(0," 0x%04x\n",iRetVal);
00234 
00235         }while( iRetVal != CAN_ERR_QRCVEMPTY ) ;
00236         
00237         return m_iErrorState;
00238 }
00239 
00240 
00241 //ok
00242 int CPCanDevice::reinit(unsigned char ucBaudRateId)
00243 {
00244 
00245         int  iRetVal = 0;
00246         
00247         m_iErrorState = 0;
00248         if(!m_bInitFlag)
00249         {
00250                 warning("reinit:device not initialized");
00251                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
00252                 return m_iErrorState;
00253         }
00254         iRetVal = setBaudRate(ucBaudRateId);
00255 
00256         /* done in setBaudRate
00257         switch(ucBaudRateId)
00258         {
00259                 case BAUDRATEID_MOD_CAN_125K:
00260                         m_iBaudRate = 125;
00261                         break;
00262                 case BAUDRATEID_MOD_CAN_250K:
00263                         m_iBaudRate = 250;
00264                         break;
00265                 case BAUDRATEID_MOD_CAN_500K:
00266                         m_iBaudRate = 500;
00267                         break;
00268                 case BAUDRATEID_MOD_CAN_1000K:
00269                         m_iBaudRate = 1000;
00270                         break;
00271         }
00272         iRetVal = canClose(m_hDevice);
00273         if(iRetVal != CAN_ERR_OK)
00274         {
00275                 warning("can close failed Errorcode: %d", iRetVal);
00276                 getDeviceError(iRetVal);
00277                 m_iErrorState = ERRID_DEV_EXITERROR;
00278         }
00279         iRetVal = canClose(m_hSyncDevice);
00280         if(iRetVal != CAN_ERR_OK)
00281         {
00282                 warning("can close failed Errorcode: %d", iRetVal);
00283                 getDeviceError(iRetVal);
00284                 m_iErrorState = ERRID_DEV_EXITERROR;
00285         }
00286         m_bInitFlag = false;
00287         iRetVal = canOpen(      
00288                                 m_iDeviceId,                    // Net
00289                                 0,                                              // Mode
00290                                 m_uiQueueSize,                  // TX Queue
00291                                 m_uiQueueSize,                  // RX Queue
00292                                 20*m_uiTimeOut,                 // Tx Timeout
00293                                 m_uiTimeOut,                    // Rx Timeout
00294                                 &m_hDevice);
00295         if(iRetVal != CAN_ERR_OK)
00296         {
00297                 warning("can open failed Errorcode: %d", iRetVal);
00298                 getDeviceError(iRetVal);
00299                 m_iErrorState = ERRID_DEV_INITERROR;
00300                 return m_iErrorState;
00301         }
00302         */
00303 
00304         //iRetVal = canIdAdd(m_hDevice, (MSGID_ACK + i));
00305         iRetVal = CAN_MsgFilter(m_handle, MSGID_ACK ,MSGID_ACK+m_iModuleCountMax,MSGTYPE_STANDARD);
00306         if(iRetVal != CAN_ERR_OK)
00307         {
00308                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00309                 getDeviceError(iRetVal);
00310                 m_iErrorState = ERRID_DEV_INITERROR;
00311                 return m_iErrorState;
00312         }
00313         //iRetVal = canIdAdd(m_hDevice, (MSGID_STATE + i));
00314         iRetVal = CAN_MsgFilter(m_handle, MSGID_STATE ,MSGID_STATE+m_iModuleCountMax,MSGTYPE_STANDARD);
00315         if(iRetVal != CAN_ERR_OK)
00316         {
00317                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00318                 getDeviceError(iRetVal);
00319                 m_iErrorState = ERRID_DEV_INITERROR;
00320                 return m_iErrorState;
00321         }
00322 
00323         //iRetVal = canIdAdd(m_hDevice, (MSGID_MP55_RECV + i));
00324         iRetVal = CAN_MsgFilter(m_handle, MSGID_MP55_RECV ,MSGID_MP55_RECV+MAX_MP55,MSGTYPE_STANDARD);
00325         if(iRetVal != CAN_ERR_OK)
00326         {
00327                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00328                 getDeviceError(iRetVal);
00329                 m_iErrorState = ERRID_DEV_INITERROR;
00330                 return m_iErrorState;
00331         }
00332 
00333         //iRetVal = canIdAdd(m_hDevice, (0x180 + i));
00334         iRetVal = CAN_MsgFilter(m_handle, 0x180 ,0x180+MAX_MP55,MSGTYPE_STANDARD);
00335         if(iRetVal != CAN_ERR_OK)
00336         {
00337                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00338                 getDeviceError(iRetVal);
00339                 m_iErrorState = ERRID_DEV_INITERROR;
00340                 return m_iErrorState;
00341         }
00342 
00343         //iRetVal = canIdAdd(m_hDevice, (MSGID_SCHUNK_RECV + i));
00344         iRetVal = CAN_MsgFilter(m_handle, MSGID_SCHUNK_RECV ,MSGID_SCHUNK_RECV+MAX_SCHUNK,MSGTYPE_STANDARD);
00345         if(iRetVal != CAN_ERR_OK)
00346         {
00347                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00348                 getDeviceError(iRetVal);
00349                 m_iErrorState = ERRID_DEV_INITERROR;
00350                 return m_iErrorState;
00351         }
00352 
00353         //iRetVal = canIdAdd(m_hSyncDevice, MSGID_ALL);
00354         iRetVal = CAN_MsgFilter(m_handle, MSGID_ALL ,MSGID_ALL,MSGTYPE_STANDARD);
00355         if(iRetVal != CAN_ERR_OK)
00356         {
00357                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00358                 getDeviceError(iRetVal);
00359                 m_iErrorState = ERRID_DEV_INITERROR;
00360                 return m_iErrorState;
00361         }
00362 
00363         m_iErrorState = clearReadQueue();
00364         if(m_iErrorState != 0)
00365                 return m_iErrorState;
00366 
00367         if(m_iErrorState == 0)
00368         {
00369                 m_bInitFlag = true;
00370         }
00371 
00372         
00373         updateModuleIdMap();
00374         return m_iErrorState;
00375 }
00376 
00377 
00378 
00379 int CPCanDevice::readDevice(CProtocolMessage& rclProtocolMessage)
00380 {
00381         int iRetVal = 0;
00382         
00383         TPCANRdMsg TPCMsg;
00384         TPCMsg.Msg.LEN = 8;
00385         TPCMsg.Msg.MSGTYPE = 0;
00386         TPCMsg.Msg.ID = 0;
00387 
00388         m_iErrorState = 0;
00389         int no = 0;
00390         do
00391         {
00392                 m_iErrorState = 0;
00393                 iRetVal = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, m_uiTimeOut);
00394                 if (iRetVal == CAN_ERR_OK)
00395                   break;
00396                 else
00397                   CAN_Status(m_handle);
00398                 no++;
00399 
00400                 m_iErrorState = getDeviceError(iRetVal);
00401                 debug(2,"Read error (%s), attempt %d of %d",strerror(nGetLastError()),no,m_iNoOfRetries+1);
00402                 //sleep(100);
00403         }while (no <= m_iNoOfRetries);
00404         if (iRetVal == CAN_ERR_OK)
00405         {
00406                 rclProtocolMessage.m_uiMessageId = TPCMsg.Msg.ID;
00407                 rclProtocolMessage.m_ucMessageLength = TPCMsg.Msg.LEN;
00408                 memcpy(rclProtocolMessage.m_aucMessageData, TPCMsg.Msg.DATA, rclProtocolMessage.m_ucMessageLength);
00409                 printMessage(rclProtocolMessage,READ);
00410         }
00411         else
00412         {
00413                 //warning("Last Error reported: %s",strerror(nGetLastError()));
00414                 //m_iErrorState = getDeviceError(iRetVal);
00415                 warning("CAN read failed Errorcode: 0x%04x", iRetVal);
00416 //              return m_iErrorState;
00417         }
00418         return m_iErrorState;
00419 }
00420 
00421 
00422 //ok
00423 int CPCanDevice::writeDevice(CProtocolMessage& rclProtocolMessage)
00424 {
00425         int iRetVal = 0;
00426         TPCANMsg TPCMsg;
00427         
00428         TPCMsg.MSGTYPE = MSGTYPE_STANDARD;
00429         m_iErrorState = 0;
00430         TPCMsg.ID = rclProtocolMessage.m_uiMessageId;
00431         printMessage(rclProtocolMessage,WRITE);
00432                                                                               
00433         TPCMsg.LEN =  rclProtocolMessage.m_ucMessageLength;
00434         if(rclProtocolMessage.m_bRTRFlag)
00435                 TPCMsg.MSGTYPE = MSGTYPE_RTR;
00436         memcpy(TPCMsg.DATA, rclProtocolMessage.m_aucMessageData, rclProtocolMessage.m_ucMessageLength);
00437         iRetVal = CAN_Write(m_handle, &TPCMsg);
00438         if(iRetVal != CAN_ERR_OK)
00439         {
00440                 warning("can send failed Errorcode:0x%04x", iRetVal);
00441                 m_iErrorState = getDeviceError(iRetVal);
00442                 //return m_iErrorState;
00443         }
00444         iRetVal = CAN_Status(m_handle);
00445         if (iRetVal < 0)
00446         {
00447                 warning("Last Error reported: %s",strerror(nGetLastError()));
00448                 m_iErrorState = ERRID_DEV_WRITEERROR;
00449         }
00450 
00451         return m_iErrorState;
00452 }
00453 
00454 // ========================================================================== ;
00455 //                                                                            ;
00456 // ---- constructors / destructor ------------------------------------------- ;
00457 //                                                                            ;
00458 // ========================================================================== ;
00459 
00460 CPCanDevice::CPCanDevice() : m_hDevice(0), m_hSyncDevice(0), m_iDeviceId(-1), m_uiBaudRate(0), m_uiQueueSize(128), m_uiTimeOut(3)               // ESD C331
00461 {
00462         initMessage("CPCanDevice", g_iDebugLevel, g_bDebug, g_bDebugFile);
00463         m_DeviceName = (char*) malloc(200 * sizeof(char));
00464         memset(m_DeviceName,0,sizeof(m_DeviceName));
00465 }
00466 
00467 CPCanDevice::CPCanDevice(const CPCanDevice& rclPCanDevice)
00468 {
00469         error(-1, "Sorry constructor is not implemented");
00470 }
00471 
00472 CPCanDevice::~CPCanDevice()
00473 {
00474         free(m_DeviceName);
00475         if (m_bInitFlag)
00476           this->exit();
00477 }
00478 
00479 // ========================================================================== ;
00480 //                                                                            ;
00481 // ---- operators ----------------------------------------------------------- ;
00482 //                                                                            ;
00483 // ========================================================================== ;
00484 
00485 CPCanDevice& CPCanDevice::operator=(const CPCanDevice& rclPCanDevice)
00486 {
00487         error(-1, "Sorry operator= is not implemented");
00488         return *this;
00489 }
00490 
00491 // ========================================================================== ;
00492 //                                                                            ;
00493 // ---- query functions ----------------------------------------------------- ;
00494 //                                                                            ;
00495 // ========================================================================== ;
00496 
00497 // ========================================================================== ;
00498 //                                                                            ;
00499 // ---- modify functions ---------------------------------------------------- ;
00500 //                                                                            ;
00501 // ========================================================================== ;
00502 
00503 void CPCanDevice::setQueueSize(unsigned short uiQueueSize)
00504 {
00505         m_uiQueueSize = uiQueueSize;
00506 }
00507 
00508 void CPCanDevice::setTimeOut(unsigned long uiTimeOut)
00509 {
00510         m_uiTimeOut= uiTimeOut;
00511 }
00512 
00513 // ========================================================================== ;
00514 //                                                                            ;
00515 // ---- I/O functions ------------------------------------------------------- ;
00516 //                                                                            ;
00517 // ========================================================================== ;
00518 
00519 // ========================================================================== ;
00520 //                                                                            ;
00521 // ---- exec functions ------------------------------------------------------ ;
00522 //                                                                            ;
00523 // ========================================================================== ;
00524 
00525 //ok
00526 int CPCanDevice::init()
00527 {
00528         return init(CAN_BAUD_250K);
00529 }
00530 int CPCanDevice::init(unsigned long baudRate)
00531 {
00532         int iRetVal = CAN_ERR_OK;
00533         printf("Initializing pcan device ...\n");
00534         //m_handle = LINUX_CAN_Open("/dev/pcan32", O_RDWR);
00535         m_handle = LINUX_CAN_Open(m_DeviceName,0);
00536 
00537         if (! m_handle)
00538         {
00539                 // Fatal error
00540                 printf("Error: Cannot open CAN on USB (%s): %s\n", m_DeviceName, strerror(errno));
00541                 iRetVal = -1;
00542         }
00543         else
00544         {
00545                 iRetVal = CAN_Init(m_handle, baudRate, CAN_INIT_TYPE_ST);
00546         }
00547         if(iRetVal != CAN_ERR_OK)
00548         {
00549                 printf("PcanDevice: error in init" );
00550         }
00551         else
00552         {
00553                 printf("PcanDevice, init ok\n" );
00554                 m_bInitFlag = true;
00555         }
00556         return iRetVal;
00557 }
00558 
00559 /*
00560  * Not needed for pcan
00561  */
00562 int CPCanDevice::init(const char* acInitString)
00563 {
00564         InitializeCriticalSection(&m_csDevice);
00565         int iRetVal = 0;
00566         m_uiTimeOut =6;
00567         m_iNoOfRetries = 10;
00568         char* pcToken;
00569         char acString[128];
00570         int deb = getDebugLevel();
00571         if (deb > 0)
00572         {
00573                 printf("CPCanDevice::init: DebugLevel: %d\n",deb);
00574                 printf("writing debug output to file debug.txt!\n");
00575         }
00576         debug(0,"entering CPCanDevice::init(const char* acInitString) ...\n");
00577         if(m_bInitFlag)
00578         {
00579                 warning("device already initialized");
00580                 m_iErrorState = ERRID_DEV_ISINITIALIZED;
00581                 return m_iErrorState;
00582         }
00583         m_iDeviceId = -1;
00584         m_iErrorState = 0;
00585         strncpy(m_acInitString,acInitString,128);
00586         strncpy(acString,acInitString,128);
00587         pcToken = strtok( acString, ":" );
00588         if( !pcToken )
00589         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00590                 return m_iErrorState;
00591         }
00592         if( strcmp( pcToken, "PCAN" ) != 0 )
00593         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00594                 return m_iErrorState;
00595         }
00596         pcToken = strtok( NULL, "," );
00597         if( !pcToken )
00598         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00599                 return m_iErrorState;
00600         }
00601 
00602 
00603         
00604         
00605         m_iDeviceId = atoi(pcToken);
00606         //std::cout << m_iDeviceId << std::endl;
00607 
00608 
00609         strncpy(m_DeviceName,pcToken,12);
00610         
00611         /*
00612         
00613         if (m_iDeviceId == 0)
00614         {
00615                 strcpy(m_DeviceName,"/dev/pcan32");
00616         }
00617         else if (m_iDeviceId == 1)
00618         {
00619                 strcpy(m_DeviceName,"/dev/pcan33");
00620         }
00621         else if (m_iDeviceId == 14)
00622         {
00623                 strcpy(m_DeviceName,"/dev/pcan0");
00624         }
00625         else if (m_iDeviceId == 15)
00626         {
00627                 strcpy(m_DeviceName,"/dev/pcan1");
00628         }
00629         else if (m_iDeviceId == 16)
00630         {
00631                 strcpy(m_DeviceName,"/dev/pcan2");
00632         }
00633         else if (m_iDeviceId == 17)
00634         {
00635                 strcpy(m_DeviceName,"/dev/pcan3");
00636         }
00637         else
00638         {
00639                 printf("Warning: currently only support for 2 devices!\n");
00640         }
00641         */
00642         
00643         //printf("Device %s: %s\n",pcToken,m_DeviceName);
00644 
00645 
00646         pcToken = strtok( NULL, "," );
00647         if( !pcToken )
00648         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00649                 return m_iErrorState;
00650         }
00651         m_iBaudRate = atoi(pcToken);
00652 
00653 
00654         try
00655         {
00656                 m_handle = LINUX_CAN_Open(m_DeviceName,0);
00657                 if (! m_handle)
00658                 {
00659                         // Fatal error
00660                         printf("Error: Cannot open CAN on USB (%s): %s\n", m_DeviceName, strerror(errno));
00661                         iRetVal = -1;
00662                 }
00663                 else
00664                 {
00665                         printf("PCanDevice successfully opened on %s\n",m_DeviceName);
00666                 }
00667         }
00668         catch(...)
00669         {
00670                         warning("open PCAN device failed no library found");
00671                         m_iErrorState = ERRID_DEV_NOLIBRARY;
00672                         return m_iErrorState;
00673         }
00674 
00675 
00676         iRetVal = CAN_MsgFilter(m_handle, MSGID_ACK ,MSGID_ACK+m_iModuleCountMax,MSGTYPE_STANDARD);
00677         if(iRetVal != CAN_ERR_OK)
00678         {
00679                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00680                 getDeviceError(iRetVal);
00681                 m_iErrorState = ERRID_DEV_INITERROR;
00682                 return m_iErrorState;
00683         }
00684         //iRetVal = canIdAdd(m_hDevice, (MSGID_STATE + i));
00685         iRetVal = CAN_MsgFilter(m_handle, MSGID_STATE ,MSGID_STATE+m_iModuleCountMax,MSGTYPE_STANDARD);
00686         if(iRetVal != CAN_ERR_OK)
00687         {
00688                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00689                 getDeviceError(iRetVal);
00690                 m_iErrorState = ERRID_DEV_INITERROR;
00691                 return m_iErrorState;
00692         }
00693 
00694         //iRetVal = canIdAdd(m_hDevice, (MSGID_MP55_RECV + i));
00695         iRetVal = CAN_MsgFilter(m_handle, MSGID_MP55_RECV ,MSGID_MP55_RECV+MAX_MP55,MSGTYPE_STANDARD);
00696         if(iRetVal != CAN_ERR_OK)
00697         {
00698                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00699                 getDeviceError(iRetVal);
00700                 m_iErrorState = ERRID_DEV_INITERROR;
00701                 return m_iErrorState;
00702         }
00703 
00704         //iRetVal = canIdAdd(m_hDevice, (0x180 + i));
00705         iRetVal = CAN_MsgFilter(m_handle, 0x180 ,0x180+MAX_MP55,MSGTYPE_STANDARD);
00706         if(iRetVal != CAN_ERR_OK)
00707         {
00708                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00709                 getDeviceError(iRetVal);
00710                 m_iErrorState = ERRID_DEV_INITERROR;
00711                 return m_iErrorState;
00712         }
00713 
00714         //iRetVal = canIdAdd(m_hDevice, (MSGID_SCHUNK_RECV + i));
00715         iRetVal = CAN_MsgFilter(m_handle, MSGID_SCHUNK_RECV ,MSGID_SCHUNK_RECV+MAX_SCHUNK,MSGTYPE_STANDARD);
00716         if(iRetVal != CAN_ERR_OK)
00717         {
00718                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00719                 getDeviceError(iRetVal);
00720                 m_iErrorState = ERRID_DEV_INITERROR;
00721                 return m_iErrorState;
00722         }
00723 
00724         //iRetVal = canIdAdd(m_hSyncDevice, MSGID_ALL);
00725         iRetVal = CAN_MsgFilter(m_handle, MSGID_ALL ,MSGID_ALL,MSGTYPE_STANDARD);
00726         if(iRetVal != CAN_ERR_OK)
00727         {
00728                 warning("Can_MsgFilter failed Errorcode: %d", iRetVal);
00729                 getDeviceError(iRetVal);
00730                 m_iErrorState = ERRID_DEV_INITERROR;
00731                 return m_iErrorState;
00732         }
00733         
00734 
00735         m_iErrorState = setBaudRate();
00736         if(m_iErrorState != 0)
00737                 return m_iErrorState;
00738 
00739         m_iErrorState = clearReadQueue();
00740         if(m_iErrorState != 0)
00741                 return m_iErrorState;
00742 
00743         if(m_iErrorState == 0)
00744         {
00745                 m_bInitFlag = true;
00746                 debug(0,"PCanDevice:init successfull!\n");
00747         }
00748         updateModuleIdMap();
00749 
00750         return m_iErrorState;
00751 }
00752 
00753 
00754 
00755 //ok
00756 int CPCanDevice::exit()
00757 {
00758         int iRetVal = 0;
00759         m_iErrorState = 0;
00760 
00761         //printf diagnosis
00762         //i
00763         TPDIAG Diag;
00764         iRetVal = LINUX_CAN_Statistics(m_handle,&Diag);
00765         debug(0,"PCanDevice: exit():");
00766         debug(0,"--------------STATISTICS-------------------");
00767         debug(0,"Total number of reads: %d",Diag.dwReadCounter);
00768         debug(0,"Total number of writes: %d",Diag.dwWriteCounter);
00769         debug(0,"Total number of interrupts: %d",Diag.dwIRQcounter);
00770         debug(0,"Total number of errors: %d",Diag.dwErrorCounter);
00771         debug(0,"Error flag: 0x%04x",Diag.wErrorFlag);
00772         
00773         if(!m_bInitFlag)
00774         {
00775                 warning("exit:device not initialized");
00776                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
00777                 return m_iErrorState;
00778         }
00779         EnterCriticalSection(&m_csDevice);
00780         iRetVal = CAN_ERR_OK;
00781         iRetVal = CAN_Close(m_handle);
00782         if(iRetVal != CAN_ERR_OK)
00783         {
00784                 warning("can close failed Errorcode: %d", iRetVal);
00785                 getDeviceError(iRetVal);
00786                 m_iErrorState = ERRID_DEV_EXITERROR;
00787         }
00788         m_bInitFlag = false;
00789         LeaveCriticalSection(&m_csDevice);
00790         DeleteCriticalSection(&m_csDevice);
00791         return m_iErrorState;
00792 }
00793 
00794 int CPCanDevice::waitForStartMotionAll()
00795 {
00796         int iRetVal = 0;
00797         bool bRecieved = false;
00798         TPCANRdMsg TPCMsg;
00799         TPCMsg.Msg.LEN = 8;
00800         TPCMsg.Msg.MSGTYPE = 0;
00801         TPCMsg.Msg.ID = 0;
00802 
00803         m_iErrorState = 0;
00804         iRetVal = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, m_uiTimeOut);
00805 
00806         m_iErrorState = 0;
00807 
00808         do
00809         {       
00810                 //iRetVal = canRead(m_hSyncDevice, &clESDProtocolMessage, &iNumberOfMessages, NULL);                    
00811                 iRetVal = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, m_uiTimeOut);
00812                 if(iRetVal != CAN_ERR_OK)
00813                 {
00814                         warning("can read failed Errorcode: 0x%04x", iRetVal);
00815                         m_iErrorState = getDeviceError(iRetVal);
00816                         return m_iErrorState;
00817                 }
00818                 bRecieved = true;
00819                 if(TPCMsg.Msg.ID != MSGID_ALL)
00820                 {
00821                         debug(1, "received CAN-ID %x, expected %x", TPCMsg.Msg.ID, MSGID_ALL);
00822                         bRecieved = false;
00823                 }
00824                 if(TPCMsg.Msg.DATA[0] != CMDID_STARTMOVE)
00825                 {
00826                         debug(1, "wrong command ID");
00827                         bRecieved = false;
00828                 }
00829         }
00830         while(!bRecieved);
00831         return m_iErrorState;
00832 }
00833 


schunk_libm5api
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 15:06:52