00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <cob_generic_can/CanPeakSysUSB.h>
00021 #include <stdlib.h>
00022 #include <cerrno>
00023 #include <cstring>
00024 #include <stdlib.h>
00025 #include <sys/types.h>
00026 #include <sys/stat.h>
00027 #include <fcntl.h>
00028 #include <unistd.h>
00029
00030 CANPeakSysUSB::CANPeakSysUSB(const char* device, int baudrate)
00031 {
00032 m_bInitialized = false;
00033
00034 p_cDevice = device;
00035 m_iBaudrateVal = baudrate;
00036 }
00037
00038 CANPeakSysUSB::CANPeakSysUSB(const char* cIniFile)
00039 {
00040 m_bInitialized = false;
00041
00042
00043 m_IniFile.SetFileName(cIniFile, "CanPeakSysUSB.cpp");
00044
00045 init();
00046 }
00047
00048
00049 CANPeakSysUSB::~CANPeakSysUSB()
00050 {
00051 if (m_bInitialized)
00052 {
00053 CAN_Close(m_handle);
00054 }
00055 }
00056
00057
00058 bool CANPeakSysUSB::init_ret()
00059 {
00060 bool ret = true;
00061
00062
00063 m_handle = LINUX_CAN_Open(p_cDevice, O_RDWR);
00064
00065 if (! m_handle)
00066 {
00067 std::cout << "Cannot open CAN on USB: " << strerror(errno) << std::endl;
00068 ret = false;
00069 }
00070 else
00071 {
00072 ret = initCAN();
00073 }
00074
00075 return ret;
00076 }
00077
00078
00079 void CANPeakSysUSB::init()
00080 {
00081 std::string sCanDevice;
00082
00083 if( m_IniFile.GetKeyString( "TypeCan", "DevicePath", &sCanDevice, false) != 0) {
00084 sCanDevice = "/dev/pcan32";
00085 } else std::cout << "CAN-device path read from ini-File: " << sCanDevice << std::endl;
00086
00087
00088 m_handle = LINUX_CAN_Open(sCanDevice.c_str(), O_RDWR);
00089
00090 if (! m_handle)
00091 {
00092
00093 std::cout << "Cannot open CAN on USB: " << strerror(errno) << std::endl;
00094 sleep(3);
00095 exit(0);
00096 }
00097
00098 m_iBaudrateVal = 0;
00099 m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &m_iBaudrateVal, true);
00100
00101 initCAN();
00102 }
00103
00104
00105 bool CANPeakSysUSB::transmitMsg(CanMsg CMsg, bool bBlocking)
00106 {
00107 TPCANMsg TPCMsg;
00108 bool bRet = true;
00109
00110 if (m_bInitialized == false) return false;
00111
00112
00113 TPCMsg.LEN = CMsg.getLength();
00114 TPCMsg.ID = CMsg.getID();
00115 TPCMsg.MSGTYPE = CMsg.getType();
00116 for(int i=0; i<8; i++)
00117 TPCMsg.DATA[i] = CMsg.getAt(i);
00118
00119
00120
00121 int iRet;
00122
00123 iRet = LINUX_CAN_Write_Timeout(m_handle, &TPCMsg, 25);
00124
00125 if(iRet != CAN_ERR_OK) {
00126 #ifdef __DEBUG__
00127 std::cout << "CANPeakSysUSB::transmitMsg An error occured while sending..." << iRet << std::endl;
00128 outputDetailedStatus();
00129 #endif
00130 bRet = false;
00131 }
00132
00133 #ifdef __DEBUG__
00134
00135 iRet = CAN_Status(m_handle);
00136
00137 if(iRet < 0)
00138 {
00139 std::cout << "CANPeakSysUSB::transmitMsg, system error: " << iRet << std::endl;
00140 bRet = false;
00141 } else if((iRet & CAN_ERR_BUSOFF) != 0) {
00142 std::cout << "CANPeakSysUSB::transmitMsg, BUSOFF detected" << std::endl;
00143
00144 std::cout << "Trying to re-init Hardware..." << std::endl;
00145 bRet = initCAN();
00146
00147 } else if((iRet & CAN_ERR_ANYBUSERR) != 0) {
00148 std::cout << "CANPeakSysUSB::transmitMsg, ANYBUSERR" << std::endl;
00149
00150 } else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) {
00151 std::cout << "CANPeakSysUSB::transmitMsg, CAN_STATUS: " << iRet << std::endl;
00152 bRet = false;
00153 }
00154 #endif
00155
00156 return bRet;
00157 }
00158
00159
00160 bool CANPeakSysUSB::receiveMsg(CanMsg* pCMsg)
00161 {
00162 TPCANRdMsg TPCMsg;
00163 TPCMsg.Msg.LEN = 8;
00164 TPCMsg.Msg.MSGTYPE = 0;
00165 TPCMsg.Msg.ID = 0;
00166
00167 int iRet = CAN_ERR_OK;
00168
00169 bool bRet = false;
00170
00171 if (m_bInitialized == false) return false;
00172
00173 iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
00174
00175 if (iRet == CAN_ERR_OK)
00176 {
00177 pCMsg->setID(TPCMsg.Msg.ID);
00178 pCMsg->setLength(TPCMsg.Msg.LEN);
00179 pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
00180 TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
00181 bRet = true;
00182 }
00183 else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0)
00184 {
00185 std::cout << "CANPeakSysUSB::receiveMsg, CAN_STATUS: " << iRet << std::endl;
00186 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00187 }
00188
00189
00190 if( TPCMsg.Msg.MSGTYPE == MSGTYPE_STATUS ) {
00191 std::cout << "CANPeakSysUSB::receiveMsg, status message catched:\nData is (CAN_ERROR_...) " << TPCMsg.Msg.DATA[3] << std::endl;
00192 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00193 }
00194
00195 return bRet;
00196 }
00197
00198
00199 bool CANPeakSysUSB::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
00200 {
00201 int i, iRet;
00202
00203 TPCANRdMsg TPCMsg;
00204 TPCMsg.Msg.LEN = 8;
00205 TPCMsg.Msg.MSGTYPE = 0;
00206 TPCMsg.Msg.ID = 0;
00207
00208 if (m_bInitialized == false) return false;
00209
00210
00211 bool bRet = true;
00212 iRet = CAN_ERR_OK;
00213 i=0;
00214 do
00215 {
00216 iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
00217
00218 if(iRet == CAN_ERR_OK)
00219 break;
00220
00221 i++;
00222 usleep(10000);
00223 }
00224 while(i < iNrOfRetry);
00225
00226
00227 if(iRet != CAN_ERR_OK)
00228 {
00229 std::cout << "CANPeakSysUSB::receiveMsgRetry, errorcode= " << nGetLastError() << std::endl;
00230 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00231 bRet = false;
00232 }
00233 else
00234 {
00235 pCMsg->setID(TPCMsg.Msg.ID);
00236 pCMsg->setLength(TPCMsg.Msg.LEN);
00237 pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
00238 TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
00239 }
00240
00241 return bRet;
00242 }
00243
00244
00245 bool CANPeakSysUSB::receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout)
00246 {
00247 int iRet = CAN_ERR_OK;
00248
00249 TPCANRdMsg TPCMsg;
00250 TPCMsg.Msg.LEN = 8;
00251 TPCMsg.Msg.MSGTYPE = 0;
00252 TPCMsg.Msg.ID = 0;
00253
00254 if (m_bInitialized == false) return false;
00255
00256 bool bRet = true;
00257
00258 iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, nMicroSecTimeout);
00259
00260
00261 if(iRet != CAN_ERR_OK)
00262 {
00263 std::cout << "CANPeakSysUSB::receiveMsgTimeout, errorcode= " << nGetLastError() << std::endl;
00264 pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
00265 bRet = false;
00266 }
00267 else
00268 {
00269 pCMsg->setID(TPCMsg.Msg.ID);
00270 pCMsg->setLength(TPCMsg.Msg.LEN);
00271 pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
00272 TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
00273 }
00274
00275 return bRet;
00276 }
00277
00278 bool CANPeakSysUSB::initCAN() {
00279 int ret = CAN_ERR_OK;
00280 bool bRet = true;
00281
00282 switch(m_iBaudrateVal)
00283 {
00284 case CANITFBAUD_1M:
00285 ret = CAN_Init(m_handle, CAN_BAUD_1M, CAN_INIT_TYPE_ST);
00286 break;
00287 case CANITFBAUD_500K:
00288 ret = CAN_Init(m_handle, CAN_BAUD_500K, CAN_INIT_TYPE_ST);
00289 break;
00290 case CANITFBAUD_250K:
00291 ret = CAN_Init(m_handle, CAN_BAUD_250K, CAN_INIT_TYPE_ST);
00292 break;
00293 case CANITFBAUD_125K:
00294 ret = CAN_Init(m_handle, CAN_BAUD_125K, CAN_INIT_TYPE_ST);
00295 break;
00296 case CANITFBAUD_50K:
00297 ret = CAN_Init(m_handle, CAN_BAUD_50K, CAN_INIT_TYPE_ST);
00298 break;
00299 case CANITFBAUD_20K:
00300 ret = CAN_Init(m_handle, CAN_BAUD_20K, CAN_INIT_TYPE_ST);
00301 break;
00302 case CANITFBAUD_10K:
00303 ret = CAN_Init(m_handle, CAN_BAUD_10K, CAN_INIT_TYPE_ST);
00304 break;
00305 }
00306
00307 if(ret)
00308 {
00309 std::cout << "CANPeakSysUSB::CANPeakSysUSB(), error in init" << std::endl;
00310 m_bInitialized = false;
00311 bRet = false;
00312 }
00313 else
00314 {
00315 std::cout << "CANPeakSysUSB::CanpeakSys(), init ok" << std::endl;
00316 m_bInitialized = true;
00317 bRet = true;
00318 }
00319
00320 return bRet;
00321 }
00322
00323 void CANPeakSysUSB::outputDetailedStatus() {
00324 TPDIAG diag;
00325
00326 LINUX_CAN_Statistics(m_handle, &diag);
00327
00328 std::cout << "*************************\n"
00329 << "*** Detailed status output of CANPeakSys\n"
00330 << "*************************"
00331 << "\nIRQ-Level: " << diag.wIrqLevel
00332 << "\nNo reads: " << diag.dwReadCounter
00333 << "\nNo writes: " << diag.dwWriteCounter
00334 << "\nNo interrupts: " << diag.dwIRQcounter
00335 << "\nNo errors: " << diag.dwErrorCounter
00336 << "\nError flag: " << diag.wErrorFlag
00337 << "\nLast error: " << diag.nLastError
00338 << std::endl;
00339 }