CanPeakSys.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_drivers
00012  * ROS package name: cob_generic_can
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Feb 2009
00021  * ToDo: Remove dependency to inifiles_old -> Inifile.h
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #include <cob_generic_can/CanPeakSys.h>
00055 #include <stdlib.h>
00056 #include <cerrno>
00057 #include <cstring>
00058 #include <sys/types.h>
00059 #include <sys/stat.h>
00060 #include <fcntl.h>
00061 #include <unistd.h>
00062 //-----------------------------------------------
00063 
00064 const int CanPeakSys::c_iInterrupt = 7;
00065 const int CanPeakSys::c_iPort = 0x378;
00066 
00067 //-----------------------------------------------
00068 CanPeakSys::CanPeakSys(const char* cIniFile)
00069 {
00070         m_bInitialized = false;
00071 
00072         // read IniFile
00073         m_IniFile.SetFileName(cIniFile, "CanPeakSys.cpp");
00074         init();
00075 }
00076 
00077 //-----------------------------------------------
00078 CanPeakSys::~CanPeakSys()
00079 {
00080         if (m_bInitialized)
00081         {
00082                 CAN_Close(m_handle);
00083         }
00084 }
00085 
00086 //-----------------------------------------------
00087 void CanPeakSys::init()
00088 {
00089  std::string sCanDevice; 
00090  if( m_IniFile.GetKeyString( "TypeCan", "DevicePath", &sCanDevice, false) != 0) {
00091                 sCanDevice = "/dev/pcan32";
00092         } else std::cout << "CAN-device path read from ini-File: " << sCanDevice << std::endl;
00093         m_handle = LINUX_CAN_Open(sCanDevice.c_str(), O_RDWR);
00094         
00095 
00096         if (! m_handle)
00097         {
00098                 // Fatal error
00099                 std::cout << "Cannot open CAN-dongle on parallel port: " << strerror(errno) << std::endl;
00100                 sleep(3);
00101                 exit(0);
00102         }
00103         
00104         
00105         int ret = CAN_ERR_OK;
00106         int iBaudrateVal = 0;
00107         m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true);
00108         
00109         switch(iBaudrateVal)
00110         {
00111         case 0:
00112                 ret = CAN_Init(m_handle, CAN_BAUD_1M, CAN_INIT_TYPE_ST);
00113                 break;
00114         case 2:
00115                 ret = CAN_Init(m_handle, CAN_BAUD_500K, CAN_INIT_TYPE_ST);
00116                 break;
00117         case 4:
00118                 ret = CAN_Init(m_handle, CAN_BAUD_250K, CAN_INIT_TYPE_ST);
00119                 break;
00120         case 6:
00121                 ret = CAN_Init(m_handle, CAN_BAUD_125K, CAN_INIT_TYPE_ST);
00122                 break;
00123         case 9:
00124                 ret = CAN_Init(m_handle, CAN_BAUD_50K, CAN_INIT_TYPE_ST);
00125                 break;
00126         case 11:
00127                 ret = CAN_Init(m_handle, CAN_BAUD_20K, CAN_INIT_TYPE_ST);
00128                 break;
00129         case 13:
00130                 ret = CAN_Init(m_handle, CAN_BAUD_10K, CAN_INIT_TYPE_ST);
00131                 break;
00132         }
00133 
00134         if(ret)
00135         {
00136                 std::cout << "CanPeakSys::CanPeakSys(), error in init" << std::endl;
00137         }
00138         else
00139         {
00140                 std::cout << "CanPeakSys::CanpeakSys(), init ok" << std::endl;
00141                 m_bInitialized = true;
00142         }
00143 }
00144 
00145 //-------------------------------------------
00146 bool CanPeakSys::transmitMsg(CanMsg CMsg, bool bBlocking)
00147 {
00148         TPCANMsg TPCMsg;
00149         bool bRet = true;
00150 
00151         if (m_bInitialized == false) return false;
00152 
00153         // copy CMsg to TPCmsg
00154         TPCMsg.LEN = CMsg.m_iLen;
00155         TPCMsg.ID = CMsg.m_iID;
00156         TPCMsg.MSGTYPE = CMsg.m_iType;
00157         for(int i=0; i<8; i++)
00158                 TPCMsg.DATA[i] = CMsg.getAt(i);
00159         
00160         // write msg
00161         int iRet;
00162         iRet = CAN_Write(m_handle, &TPCMsg);
00163         iRet = CAN_Status(m_handle);
00164 
00165         if(iRet < 0)
00166         {
00167                 std::cout << "CanPeakSys::transmitMsg, errorcode= " << nGetLastError() << std::endl;
00168                 bRet = false;
00169         }
00170 
00171 
00172         return bRet;
00173 }
00174 
00175 //-------------------------------------------
00176 bool CanPeakSys::receiveMsg(CanMsg* pCMsg)
00177 {
00178         TPCANRdMsg TPCMsg;
00179         TPCMsg.Msg.LEN = 8;
00180         TPCMsg.Msg.MSGTYPE = 0;
00181         TPCMsg.Msg.ID = 0;
00182         
00183         int iRet = CAN_ERR_OK;
00184         bool bRet = false;
00185 
00186 
00187         if (m_bInitialized == false) return false;
00188 
00189         iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
00190 
00191         if (iRet == CAN_ERR_OK)
00192         {
00193                 pCMsg->m_iID = TPCMsg.Msg.ID;
00194                 pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
00195                         TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
00196                 bRet = true;
00197         }
00198         else if (CAN_Status(m_handle) != CAN_ERR_QRCVEMPTY)
00199         {
00200                 std::cout << "CanPeakSys::receiveMsg ERROR: iRet = " << iRet << std::endl;
00201                 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00202         }
00203         else
00204         {
00205                 // make sure there's never an undefined state (even when can drivers fail)
00206                 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00207         }
00208 
00209         return bRet;
00210 }
00211 
00212 //-------------------------------------------
00213 bool CanPeakSys::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
00214 {
00215         int i, iRet;
00216 
00217         TPCANRdMsg TPCMsg;
00218         TPCMsg.Msg.LEN = 8;
00219         TPCMsg.Msg.MSGTYPE = 0;
00220         TPCMsg.Msg.ID = 0;
00221 
00222         if (m_bInitialized == false) return false;
00223 
00224         // wait until msg in buffer
00225         bool bRet = true;
00226         iRet = CAN_ERR_OK;
00227         i=0;
00228         do
00229         {
00230                 iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
00231 
00232                 if(iRet == CAN_ERR_OK)
00233                         break;
00234 
00235                 i++;
00236                 usleep(100000);
00237         }
00238         while(i < iNrOfRetry);
00239 
00240         // eval return value
00241         if(iRet != CAN_ERR_OK)
00242         {
00243                 std::cout << "CanPeakSys::receiveMsgRetry: " << strerror(errno) << std::endl;
00244                 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00245                 bRet = false;
00246         }
00247         else
00248         {
00249                 pCMsg->m_iID = TPCMsg.Msg.ID;
00250                 pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
00251                         TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
00252         }
00253 
00254         return bRet;
00255 }
00256 


cob_generic_can
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:37