source: pacpussensors/trunk/CanGateway/CanGateway.cpp@ 41

/********************************************************************
//  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
  canIf_.setExchangeBuffer(incomingCanFrames_, INCOMINGCANFRAMES_SIZE);
  canIf_.setSignalSempahore(&semaphore_);
  if (source_ == "shMem")
    canIf_.setSource(Win32CanInterface::SharedMemory);
  else if (source_ == "vector")
  {
    canIf_.setSource(Win32CanInterface::VectorCard);
    // open the interface
    if (!canIf_.openInterface(channel_, speed_))
      qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
  }
  else if (source_ == "vectorXL")
  {
          canIf_.setSource(Win32CanInterface::XLVectorCard);
    // open the interface
    if (!canIf_.openInterface(channel_, speed_))
      qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
  }
  else if (source_ == "peak")
  {
    canIf_.setSource(Win32CanInterface::PeakCard);
    // open interface
    if (canIf_.openInterface(port_, accessMode_)==0)
      qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
  }
  else if (source_ == "igep")
  {
    canIf_.setSource(Win32CanInterface::igepCard);
    // open interface
    if (canIf_.openInterface(port_, accessMode_)==0)
      qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
  }
  else 
  {
    qCritical("Error in the source property of the component, bad value");
    return;
  }

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


}



/************************************************************************/
/* Stop function
/************************************************************************/
void CanGateway::stopActivity()
{
  counter_ = 0;
  canIf_.stop();
  if ((source_ == "vector")||(source_=="peak")||(source_=="vectorXL")||(source_=="igep"))
    canIf_.closeInterface(channel_);
  canIf_.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" << componentName << "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_ = param.getProperty("channel").toInt() - 1;
  speed_ = param.getProperty("speed").toInt() * 1000;
  source_ = param.getProperty("source");
  if (source_ =="peak")
  {
    if (param.getProperty("port")!= NULL)
        strcpy(port_,param.getProperty("port").toStdString().c_str());
    if (param.getProperty("mode")!= NULL)
        strcpy(accessMode_,param.getProperty("mode").toStdString().c_str());        
  }
  recording = (param.getProperty("recording") == "true" ? true : false);

  return ComponentBase::CONFIGURED_OK;
}


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



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

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

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

    //displayData(incomingCanFrames_[counter_].frame.data, incomingCanFrames_[counter_].frame.dlc, incomingCanFrames_[counter_].frame.id);
    if (recording) 
        {
        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 (recording) 
  {
      rawCanFile_.close();
        }
  
  qDebug() << componentName << "thread is stopped";
}