source: pacpussensors/trunk/CanGateway/CanGateway.cpp

/********************************************************************
//  created:    2008/2/11 - 12:55
//  filename:   CanGateway.cpp
//
//  author:     Gerald Dherbomez
//              Copyright Heudiasyc UMR UTC/CNRS 7253
// 
//  version:    $Id: $
//
//  purpose:    Decoding of the CAN bus 
//
*********************************************************************/


#include "Pacpus/kernel/ComponentFactory.h"
#include "Pacpus/kernel/DbiteFileTypes.h"

using namespace pacpus;

#include "CanGateway.h"

#include <QDebug>

#include <QSemaphore>
#include <QList>

#include "../CanGateway/CanDecoderBase.h"



////////////////////////////////////////////////////////////////////////////////
/// Construct the factory
ComponentFactory<CanGateway> sFactory("CanGateway");


////////////////////////////////////////////////////////////////////////////////
/// Constructor
CanGateway::CanGateway(QString name)
    : ComponentBase(name)
{
  counter_ = 0;
//  tcpServer_ = NULL;
  //subscribers_ = new QMultiHash<int, ComponentBase *>;
}

////////////////////////////////////////////////////////////////////////////////
/// Destructor
CanGateway::~CanGateway()
{  
}

/************************************************************************/
/* Start function
/************************************************************************/
void CanGateway::startActivity()
{
        counter_ = 0;
        THREAD_ALIVE = true;
  
        // set the exhange parameters for incoming CAN frames
        canIfRead_.setExchangeBuffer(incomingCanFrames_, INCOMINGCANFRAMES_SIZE);
        canIfRead_.setSignalSempahore(&semaphore_);

        if (source_ == "shMem") {
                sameCanIf = true;
                canIfRead_.setSource(Win32CanInterface::SharedMemory);
        }
        else if (source_ == "vector")
        {
                sameCanIf = true;
                canIfRead_.setSource(Win32CanInterface::VectorCard);
                // open the interface
                if (!canIfRead_.openInterface(channel_, speed_))
                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
        }
        else if (source_ == "vectorXL")
        {
                sameCanIf = true;
                canIfRead_.setSource(Win32CanInterface::XLVectorCard);
                // open the interface
                if (!canIfRead_.openInterface(channel_, speed_))
                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
        }
        else if (source_ == "peak")
        {
                sameCanIf = true;
                canIfRead_.setSource(Win32CanInterface::PeakCard);
                // open interface
                if (canIfRead_.openInterface(port_, accessMode_)==0)
                        qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
        }
        else if (source_ == "igep")
        {
                sameCanIf = true;
                canIfRead_.setSource(Win32CanInterface::igepCard);
                // open interface
                if (canIfRead_.openInterface(port_, accessMode_)==0)
                        qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
        }
        else if (source_ == "kvaser")
        {
                sameCanIf = false;
                canIfRead_.setSource(Win32CanInterface::KvaserCard);
                // open interface
                if (canIfRead_.openInterface(channel_,speed_)==0)
                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);

                canIfWrite_.setSource(Win32CanInterface::KvaserCard);
                // open interface
                if (canIfWrite_.openInterface(channel_,speed_)==0)
                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
        }
        else 
        {
                qCritical("Error in the source property of the component, bad value");
                return;
        }

        // start the 2 threads: reception thread and decoding thread
        canIfRead_.start();

        start();
}



/************************************************************************/
/* Stop function
/************************************************************************/
void CanGateway::stopActivity()
{
        counter_ = 0;

        canIfRead_.stop();

        if ((source_ == "vector")||(source_=="peak")||(source_=="vectorXL")||(source_=="igep")||(source_=="kvaser")) {
                canIfRead_.closeInterface(channel_);
                if(!sameCanIf)
                        canIfWrite_.closeInterface(channel_);
        }
        
        canIfRead_.wait();

        // we stop the decoding thread
        THREAD_ALIVE = false;
        semaphore_.release(); // to release the waiting of new CAN frame

        if (!wait(1000))
        {
                terminate();
                qDebug() << "The thread" << name() << "seems blocked, it has been killed";
        }
}



/************************************************************************/
/* Configuration of the component
/************************************************************************/
ComponentBase::COMPONENT_CONFIGURATION CanGateway::configureComponent(XmlComponentConfig config)
{
        // FIXME: use string instead of char[]
        // Peak driver, default values
        strcpy(port_,"/dev/pcanusb0");
        strcpy(accessMode_,"ReadOnly");

        channel_ = config.getProperty("channel").toInt() - 1;
        speed_ = config.getProperty("speed").toInt() * 1000;
        source_ = config.getProperty("source");
        if (source_ =="peak")
        {
                if (config.getProperty("port")!= NULL)
                        strcpy(port_, config.getProperty("port").toStdString().c_str());
                if (config.getProperty("mode")!= NULL)
                        strcpy(accessMode_, config.getProperty("mode").toStdString().c_str());        
        }
        setRecording( config.getProperty("recording") == "true" ? true : false );

        verbose_ = config.getProperty("verbose") == "true" ? true : false;

        return ComponentBase::CONFIGURED_OK;
}


bool CanGateway::sendFrame(const CanFrame frame)
{
        if(sameCanIf) 
        {
                if (canIfRead_.canDriver_ == NULL)
                        return false;
                else
                {
                        if (canIfWrite_.canDriver_->sendFrame(frame))
                                return true;
                        else
                                return false;
                }
        } 
        else
        {
                if (canIfWrite_.canDriver_ == NULL)
                        return false;
                else
                {
                        if (canIfWrite_.canDriver_->sendFrame(frame))
                                return true;
                        else
                                return false;
                }
        }
}



/************************************************************************/
/* The main loop of the thread
/************************************************************************/
void CanGateway::run()
{
        counter_ = 0; 
        qDebug() << name() << "thread is started";

        if (isRecording()) {
                rawCanFile_.open(name().toStdString() + "_rawcan.dbt", WriteMode, CARMEN_CAN_RAW , sizeof( CanFrame ) );
        }

        tic();
   
        while (THREAD_ALIVE) 
        {
                semaphore_.acquire();
                if (!THREAD_ALIVE)
                {
                        continue; // user asks stopping the thread
                }

        if (verbose_) displayData(incomingCanFrames_[counter_].frame.data, incomingCanFrames_[counter_].frame.dlc, incomingCanFrames_[counter_].frame.id);
        if (isRecording()) 
        {
                rawCanFile_.writeRecord(incomingCanFrames_[counter_].time, incomingCanFrames_[counter_].timerange,
                                                                reinterpret_cast<const char *>(&(incomingCanFrames_[counter_].frame)), sizeof(CanFrame));
        }

        setState(ComponentBase::MONITOR_OK);

        //      printf("id:%x\n",incomingCanFrames_[counter_].frame.id);

        /* switch (incomingCanFrames_[counter_].frame.id)
        { 
        default:
                // unknown identifier
                break;
        };

        */

        dispatchCanFrame(incomingCanFrames_[counter_]);

        counter_++; 
        counter_ = counter_ % INCOMINGCANFRAMES_SIZE;
        }

        if (isRecording()) 
        {
      rawCanFile_.close();
        }
  
        qDebug() << name() << "thread is stopped";
}