CanESD.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering   
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob_drivers
 * ROS package name: cob_generic_can
 * Description: This class implements the interface to an ESD can node
 *                                                              
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *                      
 * Author: Christian Connette, email:christian.connette@ipa.fhg.de
 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
 *
 * Date of creation: April 2010
 * ToDo: - Remove Mutex.h search for a Boost lib
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Fraunhofer Institute for Manufacturing 
 *       Engineering and Automation (IPA) nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 of the 
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

// general includes

// Headers provided by other cob-packages
#include <cob_generic_can/CanESD.h>

// Headers provided by other cob-packages which should be avoided/removed


//-----------------------------------------------
CanESD::CanESD(const char* cIniFile, bool bObjectMode)
{
        m_bObjectMode = bObjectMode;
        m_bIsTXError = false;
        m_IniFile.SetFileName(cIniFile, "CanESD.cpp");
        initIntern();
}

//-----------------------------------------------
CanESD::~CanESD()
{
        std::cout << "Closing CAN handle" << std::endl;
        canClose(m_Handle);
}


//-----------------------------------------------
void CanESD::initIntern()
{       
        int ret=0;      
        ret = 0;
        int iCanNet = 1;
        m_IniFile.GetKeyInt( "CanCtrl", "NetESD", &iCanNet, true);
        
        int iBaudrateVal = 2;
        m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true);

        std::cout << "Initializing CAN network with id =" << iCanNet << ", baudrate=" << iBaudrateVal << std::endl;

        int iRet;
        if( m_bObjectMode )
                iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 1000, 0, &m_Handle);
        else
                iRet = canOpen(iCanNet, 0, 10000, 10000, 1000, 0, &m_Handle);
        Sleep(300);

        if(iRet == NTCAN_SUCCESS)
                std::cout << "CanESD::CanESD(), init ok" << std::endl;
        else
                std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;

        iRet = canSetBaudrate(m_Handle, iBaudrateVal);
        if(iRet == NTCAN_SUCCESS)
                std::cout << "CanESD::CanESD(), canSetBaudrate ok" << std::endl;
        else
                std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
        Sleep(300);

        //long lArg;
        iRet = canIoctl(m_Handle, NTCAN_IOCTL_FLUSH_RX_FIFO, NULL);

        // MMB/24.02.2006: Add all 11-bit identifiers as there is no loss in performance.
        for( int i=0; i<=0x7FF; ++i ) {
                iRet = canIdAdd( m_Handle, i );
                if(iRet != NTCAN_SUCCESS)
                        std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl;
        }


        Sleep(300);

        m_LastID = -1;
}

//-----------------------------------------------
bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
{
        CMSG NTCANMsg;
        NTCANMsg.id = CMsg.m_iID;
        NTCANMsg.len = CMsg.m_iLen;

        for(int i=0; i<8; i++)
                NTCANMsg.data[i] = CMsg.getAt(i);
        
        int ret;
        int32_t len;
        bool bRet = true;
        
        len = 1;

        if (bBlocking)
                ret = canWrite(m_Handle, &NTCANMsg, &len, NULL);
        else
                ret = canSend(m_Handle, &NTCANMsg, &len);

        if( ret != NTCAN_SUCCESS)
        {
                std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
                bRet = false;
        }

        m_LastID = (int)NTCANMsg.data[0];

        m_bIsTXError = !bRet;
        return bRet;
}

//-----------------------------------------------
bool CanESD::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry)
{
        //int id = pCMsg->m_iID;
        CMSG NTCANMsg;
        NTCANMsg.len = 8;
        
        int32_t len;
        int i, ret;
        bool bRet = true;
        
        i=0;
        
        len = 1;

        do
        {
                len = 1;
                ret = canTake(m_Handle, &NTCANMsg, &len);
                i++;
                Sleep(10);
        }

        while((len == 0) && (i < iNrOfRetry));
        
        if(i == iNrOfRetry)
        {
                if( ret != NTCAN_SUCCESS )
                        std::cout << "error in CANESD::receiveMsgRetry: " << GetErrorStr(ret) << std::endl;

                bRet = false;
        }
        else
        {
                pCMsg->m_iID = NTCANMsg.id;
                pCMsg->m_iLen = NTCANMsg.len;
                pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
                        NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
        }

        return bRet;
}

//-----------------------------------------------
bool CanESD::receiveMsg(CanMsg* pCMsg)
{
        CMSG NTCANMsg;
        NTCANMsg.len = 8;

        int ret;
        int32_t len;
        bool bRet = true;
        
        len = 1;

        // Debug valgrind
        NTCANMsg.data[0] = 0;
        NTCANMsg.data[1] = 0;
        NTCANMsg.data[2] = 0;
        NTCANMsg.data[3] = 0;
        NTCANMsg.data[4] = 0;
        NTCANMsg.data[5] = 0;
        NTCANMsg.data[6] = 0;
        NTCANMsg.data[7] = 0;
        NTCANMsg.msg_lost = 0;
        NTCANMsg.id = 0;
        NTCANMsg.len = 0;

        pCMsg->set(0,0,0,0,0,0,0,0);

        
        if( !isObjectMode() ) {
                pCMsg->m_iID = 0;
        } else {
                NTCANMsg.id = pCMsg->m_iID;
        }

        ret = canTake(m_Handle, &NTCANMsg, &len);

        if( !isObjectMode() ) {
                if( (len == 1) && (ret == NTCAN_SUCCESS) )
                {
                        // message received
                        pCMsg->m_iID = NTCANMsg.id;
                        pCMsg->m_iLen = NTCANMsg.len;
                        pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
                                NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
                        bRet = true;
                }
                else
                {
                        // no message
                        if( ret != NTCAN_SUCCESS)
                        {
                                // error
                                std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(ret) << std::endl;
                        }
        
                        pCMsg->m_iID = NTCANMsg.id;
                        pCMsg->set(0,0,0,0,0,0,0,0);

                        bRet = false;
                }
        } else {
                if( len == 16 ) {
                        // No message was received yet.
                        pCMsg->m_iID = NTCANMsg.id;
                        pCMsg->set(0,0,0,0,0,0,0,0);
                        bRet = false;
                } else {
                        pCMsg->m_iID = NTCANMsg.id;
                        pCMsg->m_iLen = NTCANMsg.len;
                        pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3],
                                   NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]);
                        bRet = true;
                }
        }
        
        if( NTCANMsg.msg_lost != 0 )
                std::cout << (int)(NTCANMsg.msg_lost) << " messages lost!" << std::endl;

        return bRet;
}


int CanESD::canIdAddGroup(NTCAN_HANDLE handle, int id)
{
        int result = NTCAN_SUCCESS;
        int i = 0;
        int iRes = 0;
        int cmd_id = invert(id);

        for( i=0; i<8; ++i) {
                iRes = canIdAdd(m_Handle, cmd_id+i);

                if( iRes != NTCAN_SUCCESS ) {
                        std::cout << "Adding CAN ID " << cmd_id+i << " failed with errorcode: " << iRes << " " << GetErrorStr(iRes) << std::endl;
                        result = iRes;
                }
        }

        return result;
}

//-----------------------------------------------
std::string CanESD::GetErrorStr(int ntstatus) const
{
        switch (ntstatus)
        {
        case NTCAN_SUCCESS:                     return "NTCAN_SUCCESS";
        case NTCAN_RX_TIMEOUT:                  return "NTCAN_RX_TIMEOUT";
        case NTCAN_TX_TIMEOUT:                  return "NTCAN_TX_TIMEOUT";
        case NTCAN_TX_ERROR:                    return "NTCAN_TX_ERROR";
        case NTCAN_CONTR_OFF_BUS:               return "NTCAN_CONTR_OFF_BUS";
        case NTCAN_CONTR_BUSY:                  return "NTCAN_CONTR_BUSY";
        case NTCAN_CONTR_WARN:                  return "NTCAN_CONTR_WARN";
        case NTCAN_NO_ID_ENABLED:               return "NTCAN_NO_ID_ENABLED";
        case NTCAN_ID_ALREADY_ENABLED:          return "NTCAN_ID_ALREADY_ENABLED";
        case NTCAN_ID_NOT_ENABLED:              return "NTCAN_ID_NOT_ENABLED";

        case NTCAN_INVALID_FIRMWARE:            return "NTCAN_INVALID_FIRMWARE";
        case NTCAN_MESSAGE_LOST:                return "NTCAN_MESSAGE_LOST";
        case NTCAN_INVALID_HARDWARE:            return "NTCAN_INVALID_HARDWARE";

        case NTCAN_PENDING_WRITE:               return "NTCAN_PENDING_WRITE";
        case NTCAN_PENDING_READ:                return "NTCAN_PENDING_READ";
        case NTCAN_INVALID_DRIVER:              return "NTCAN_INVALID_DRIVER";

        case NTCAN_INVALID_PARAMETER:           return "NTCAN_INVALID_PARAMETER";
        case NTCAN_INVALID_HANDLE:              return "NTCAN_INVALID_HANDLE";
        case NTCAN_NET_NOT_FOUND:               return "NTCAN_NET_NOT_FOUND";
        case NTCAN_INSUFFICIENT_RESOURCES:      return "NTCAN_INSUFFICIENT_RESOURCES";
        
        case NTCAN_OPERATION_ABORTED:           return "NTCAN_OPERATION_ABORTED";
        }
        char msg[100];
        sprintf(msg, "unknown error code %d", ntstatus);
        return msg;
}

int CanESD::readEvent()
{
        EVMSG evmsg;
        int iRet = 0;
        int ret;

        ret = canReadEvent(m_Handle, &evmsg, NULL);
        
        if(ret == NTCAN_SUCCESS)
        {
                if( (int)evmsg.evid == NTCAN_EV_CAN_ERROR ) {
                        switch( evmsg.evdata.s[0] ) {
                                case 0x00:
                                        iRet = 0;
                                        break;
                                case 0xC0:
                                        iRet = -6;
                                        std::cout << "BUS OFF" << std::endl;
                                        break;
                                case 0x40:
                                        iRet = -7;
                                        std::cout << "ERROR PASSIVE" << std::endl;
                                        break;
                        }
                        if( evmsg.evdata.s[3] != 0 ) {
                                iRet = -3;
                                std::cout << "Lost " << (int)evmsg.evdata.s[3] << " messages" << std::endl;
                        } else if( evmsg.evdata.s[5] != 0 ) {
                                iRet = -5;
                                std::cout << "Lost " << (int)evmsg.evdata.s[5] << " messages from fifo" << std::endl;
                        }
                }
        }
        return iRet;
}