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