cob_generic_can: CanESD.cpp Source 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: This class implements the interface to an ESD can node
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: April 2010
00021  * ToDo: - Remove Mutex.h search for a Boost lib
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 // general includes
00055 
00056 // Headers provided by other cob-packages
00057 #include <cob_generic_can/CanESD.h>
00058 
00059 // Headers provided by other cob-packages which should be avoided/removed
00060 
00061 
00062 //-----------------------------------------------
00063 CanESD::CanESD(const char* cIniFile, bool bObjectMode)
00064 {
00065         m_bObjectMode = bObjectMode;
00066         m_bIsTXError = false;
00067         m_IniFile.SetFileName(cIniFile, "CanESD.cpp");
00068         initIntern();
00069 }
00070 
00071 //-----------------------------------------------
00076 CanESD::~CanESD()
00077 {
00078         std::cout << "Closing CAN handle" << std::endl;
00079         canClose(m_Handle);
00080 }
00081 
00082 
00083 //-----------------------------------------------
00084 void CanESD::initIntern()
00085 {       
00086         int ret=0;      
00087         ret = 0;
00088         int iCanNet = 1;
00089         m_IniFile.GetKeyInt( "CanCtrl", "NetESD", &iCanNet, true);
00090         
00091         int iBaudrateVal = 2;
00092         m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true);
00093 
00094         std::cout << "Initializing CAN network with id =" << iCanNet << ", baudrate=" << iBaudrateVal << std::endl;
00095 
00096         int iRet;
00097         if( m_bObjectMode )
00098                 iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 1000, 0, &m_Handle);
00099         else
00100                 iRet = canOpen(iCanNet, 0, 10000, 10000, 1000, 0, &m_Handle);
00101         Sleep(300);
00102 
00103         if(iRet == NTCAN_SUCCESS)
00104                 std::cout << "CanESD::CanESD(), init ok" << std::endl;
00105         else
00106                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00107 
00108         iRet = canSetBaudrate(m_Handle, iBaudrateVal);
00109         if(iRet == NTCAN_SUCCESS)
00110                 std::cout << "CanESD::CanESD(), canSetBaudrate ok" << std::endl;
00111         else
00112                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00113         Sleep(300);
00114 
00115         long lArg;
00116         iRet = canIoctl(m_Handle, NTCAN_IOCTL_FLUSH_RX_FIFO, NULL);
00117 
00118         // MMB/24.02.2006: Add all 11-bit identifiers as there is no loss in performance.
00119         for( int i=0; i<=0x7FF; ++i ) {
00120                 iRet = canIdAdd( m_Handle, i );
00121                 if(iRet != NTCAN_SUCCESS)
00122                         std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
00123         }
00124 
00125 
00126         Sleep(300);
00127 
00128         m_LastID = -1;
00129 }
00130 
00131 //-----------------------------------------------
00138 bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
00139 {
00140         CMSG NTCANMsg;
00141         NTCANMsg.id = CMsg.m_iID;
00142         NTCANMsg.len = CMsg.m_iLen;
00143 
00144         for(int i=0; i<8; i++)
00145                 NTCANMsg.data[i] = CMsg.getAt(i);
00146         
00147         int ret;
00148         int32_t len;
00149         bool bRet = true;
00150         
00151         len = 1;
00152 
00153         if (bBlocking)
00154                 ret = canWrite(m_Handle, &NTCANMsg, &len, NULL);
00155         else
00156                 ret = canSend(m_Handle, &NTCANMsg, &len);
00157 
00158         if( ret != NTCAN_SUCCESS)
00159         {
00160                 std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
00161                 bRet = false;
00162         }
00163 
00164         m_LastID = (int)NTCANMsg.data[0];
00165 
00166         m_bIsTXError = !bRet;
00167         return bRet;
00168 }
00169 
00170 //-----------------------------------------------
00171 bool CanESD::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
00172 {
00173         int id = pCMsg->m_iID;
00174         CMSG NTCANMsg;
00175         NTCANMsg.len = 8;
00176         
00177         int32_t len;
00178         int i, ret;
00179         bool bRet = true;
00180         
00181         i=0;
00182         
00183         len = 1;
00184 
00185         do
00186         {
00187                 len = 1;
00188                 ret = canTake(m_Handle, &NTCANMsg, &len);
00189                 i++;
00190                 Sleep(10);
00191         }
00192 
00193         while((len == 0) && (i < iNrOfRetry));
00194         
00195         if(i == iNrOfRetry)
00196         {
00197                 if( ret != NTCAN_SUCCESS )
00198                         std::cout << "error in CANESD::receiveMsgRetry: " << GetErrorStr(ret) << std::endl;
00199 
00200                 bRet = false;
00201         }
00202         else
00203         {
00204                 pCMsg->m_iID = NTCANMsg.id;
00205                 pCMsg->m_iLen = NTCANMsg.len;
00206                 pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00207                         NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00208         }
00209 
00210         return bRet;
00211 }
00212 
00213 //-----------------------------------------------
00214 bool CanESD::receiveMsg(CanMsg* pCMsg)
00215 {
00216         CMSG NTCANMsg;
00217         NTCANMsg.len = 8;
00218 
00219         int ret;
00220         int32_t len;
00221         bool bRet = true;
00222         
00223         len = 1;
00224 
00225         // Debug valgrind
00226         NTCANMsg.data[0] = 0;
00227         NTCANMsg.data[1] = 0;
00228         NTCANMsg.data[2] = 0;
00229         NTCANMsg.data[3] = 0;
00230         NTCANMsg.data[4] = 0;
00231         NTCANMsg.data[5] = 0;
00232         NTCANMsg.data[6] = 0;
00233         NTCANMsg.data[7] = 0;
00234         NTCANMsg.msg_lost = 0;
00235         NTCANMsg.id = 0;
00236         NTCANMsg.len = 0;
00237 
00238         pCMsg->set(0,0,0,0,0,0,0,0);
00239 
00240         
00241         if( !isObjectMode() ) {
00242                 pCMsg->m_iID = 0;
00243         } else {
00244                 NTCANMsg.id = pCMsg->m_iID;
00245         }
00246 
00247         ret = canTake(m_Handle, &NTCANMsg, &len);
00248 
00249         if( !isObjectMode() ) {
00250                 if( (len == 1) && (ret == NTCAN_SUCCESS) )
00251                 {
00252                         // message received
00253                         pCMsg->m_iID = NTCANMsg.id;
00254                         pCMsg->m_iLen = NTCANMsg.len;
00255                         pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00256                                 NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00257                         bRet = true;
00258                 }
00259                 else
00260                 {
00261                         // no message
00262                         if( ret != NTCAN_SUCCESS)
00263                         {
00264                                 // error
00265                                 std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(ret) << std::endl;
00266                         }
00267         
00268                         pCMsg->m_iID = NTCANMsg.id;
00269                         pCMsg->set(0,0,0,0,0,0,0,0);
00270 
00271                         bRet = false;
00272                 }
00273         } else {
00274                 if( len == 16 ) {
00275                         // No message was received yet.
00276                         pCMsg->m_iID = NTCANMsg.id;
00277                         pCMsg->set(0,0,0,0,0,0,0,0);
00278                         bRet = false;
00279                 } else {
00280                         pCMsg->m_iID = NTCANMsg.id;
00281                         pCMsg->m_iLen = NTCANMsg.len;
00282                         pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
00283                                    NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
00284                         bRet = true;
00285                 }
00286         }
00287         
00288         if( NTCANMsg.msg_lost != 0 )
00289                 std::cout << (int)(NTCANMsg.msg_lost) << " messages lost!" << std::endl;
00290 
00291         return bRet;
00292 }
00293 
00294 
00303 int CanESD::canIdAddGroup(NTCAN_HANDLE handle, int id)
00304 {
00305         int result = NTCAN_SUCCESS;
00306         int i = 0;
00307         int iRes = 0;
00308         int cmd_id = invert(id);
00309 
00310         for( i=0; i<8; ++i) {
00311                 iRes = canIdAdd(m_Handle, cmd_id+i);
00312 
00313                 if( iRes != NTCAN_SUCCESS ) {
00314                         std::cout << "Adding CAN ID " << cmd_id+i << " failed with errorcode: " << iRes << " " << GetErrorStr(iRes) << std::endl;
00315                         result = iRes;
00316                 }
00317         }
00318 
00319         return result;
00320 }
00321 
00322 //-----------------------------------------------
00323 std::string CanESD::GetErrorStr(int ntstatus) const
00324 {
00325         switch (ntstatus)
00326         {
00327         case NTCAN_SUCCESS:                     return "NTCAN_SUCCESS";
00328         case NTCAN_RX_TIMEOUT:                  return "NTCAN_RX_TIMEOUT";
00329         case NTCAN_TX_TIMEOUT:                  return "NTCAN_TX_TIMEOUT";
00330         case NTCAN_TX_ERROR:                    return "NTCAN_TX_ERROR";
00331         case NTCAN_CONTR_OFF_BUS:               return "NTCAN_CONTR_OFF_BUS";
00332         case NTCAN_CONTR_BUSY:                  return "NTCAN_CONTR_BUSY";
00333         case NTCAN_CONTR_WARN:                  return "NTCAN_CONTR_WARN";
00334         case NTCAN_NO_ID_ENABLED:               return "NTCAN_NO_ID_ENABLED";
00335         case NTCAN_ID_ALREADY_ENABLED:          return "NTCAN_ID_ALREADY_ENABLED";
00336         case NTCAN_ID_NOT_ENABLED:              return "NTCAN_ID_NOT_ENABLED";
00337 
00338         case NTCAN_INVALID_FIRMWARE:            return "NTCAN_INVALID_FIRMWARE";
00339         case NTCAN_MESSAGE_LOST:                return "NTCAN_MESSAGE_LOST";
00340         case NTCAN_INVALID_HARDWARE:            return "NTCAN_INVALID_HARDWARE";
00341 
00342         case NTCAN_PENDING_WRITE:               return "NTCAN_PENDING_WRITE";
00343         case NTCAN_PENDING_READ:                return "NTCAN_PENDING_READ";
00344         case NTCAN_INVALID_DRIVER:              return "NTCAN_INVALID_DRIVER";
00345 
00346         case NTCAN_INVALID_PARAMETER:           return "NTCAN_INVALID_PARAMETER";
00347         case NTCAN_INVALID_HANDLE:              return "NTCAN_INVALID_HANDLE";
00348         case NTCAN_NET_NOT_FOUND:               return "NTCAN_NET_NOT_FOUND";
00349         case NTCAN_INSUFFICIENT_RESOURCES:      return "NTCAN_INSUFFICIENT_RESOURCES";
00350         
00351         case NTCAN_OPERATION_ABORTED:           return "NTCAN_OPERATION_ABORTED";
00352         }
00353         char msg[100];
00354         sprintf(msg, "unknown error code %d", ntstatus);
00355         return msg;
00356 }
00357 
00367 int CanESD::readEvent()
00368 {
00369         EVMSG evmsg;
00370         int iRet = 0;
00371         int ret;
00372 
00373         ret = canReadEvent(m_Handle, &evmsg, NULL);
00374         
00375         if(ret == NTCAN_SUCCESS)
00376         {
00377                 if( (int)evmsg.evid == NTCAN_EV_CAN_ERROR ) {
00378                         switch( evmsg.evdata.s[0] ) {
00379                                 case 0x00:
00380                                         iRet = 0;
00381                                         break;
00382                                 case 0xC0:
00383                                         iRet = -6;
00384                                         std::cout << "BUS OFF" << std::endl;
00385                                         break;
00386                                 case 0x40:
00387                                         iRet = -7;
00388                                         std::cout << "ERROR PASSIVE" << std::endl;
00389                                         break;
00390                         }
00391                         if( evmsg.evdata.s[3] != 0 ) {
00392                                 iRet = -3;
00393                                 std::cout << "Lost " << (int)evmsg.evdata.s[3] << " messages" << std::endl;
00394                         } else if( evmsg.evdata.s[5] != 0 ) {
00395                                 iRet = -5;
00396                                 std::cout << "Lost " << (int)evmsg.evdata.s[5] << " messages from fifo" << std::endl;
00397                         }
00398                 }
00399         }
00400         return iRet;
00401 }
00402