00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
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
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
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
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
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
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
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
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