Changeset 116 in pacpussensors for trunk


Ignore:
Timestamp:
Dec 16, 2015, 4:57:50 PM (9 years ago)
Author:
ydroniou
Message:

Fix KVaser for Linux \o/

Location:
trunk/CanGateway
Files:
5 edited

Legend:

Unmodified
Added
Removed

  • trunk/CanGateway/CanGateway.cpp

    r91 r116  
    5959 
    6060        // set the exhange parameters for incoming CAN frames
    61         canIf_.setExchangeBuffer(incomingCanFrames_, INCOMINGCANFRAMES_SIZE);
    62         canIf_.setSignalSempahore(&semaphore_);
    63         if (source_ == "shMem")
    64                 canIf_.setSource(Win32CanInterface::SharedMemory);
     61        canIfRead_.setExchangeBuffer(incomingCanFrames_, INCOMINGCANFRAMES_SIZE);
     62        canIfRead_.setSignalSempahore(&semaphore_);
     63
     64        if (source_ == "shMem") {
     65                sameCanIf = true;
     66                canIfRead_.setSource(Win32CanInterface::SharedMemory);
     67        }
    6568        else if (source_ == "vector")
    6669        {
    67                 canIf_.setSource(Win32CanInterface::VectorCard);
     70                sameCanIf = true;
     71                canIfRead_.setSource(Win32CanInterface::VectorCard);
    6872                // open the interface
    69                 if (!canIf_.openInterface(channel_, speed_))
     73                if (!canIfRead_.openInterface(channel_, speed_))
    7074                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
    7175        }
    7276        else if (source_ == "vectorXL")
    7377        {
    74                 canIf_.setSource(Win32CanInterface::XLVectorCard);
     78                sameCanIf = true;
     79                canIfRead_.setSource(Win32CanInterface::XLVectorCard);
    7580                // open the interface
    76                 if (!canIf_.openInterface(channel_, speed_))
     81                if (!canIfRead_.openInterface(channel_, speed_))
    7782                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
    7883        }
    7984        else if (source_ == "peak")
    8085        {
    81                 canIf_.setSource(Win32CanInterface::PeakCard);
    82                 // open interface
    83                 if (canIf_.openInterface(port_, accessMode_)==0)
     86                sameCanIf = true;
     87                canIfRead_.setSource(Win32CanInterface::PeakCard);
     88                // open interface
     89                if (canIfRead_.openInterface(port_, accessMode_)==0)
    8490                        qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
    8591        }
    8692        else if (source_ == "igep")
    8793        {
    88                 canIf_.setSource(Win32CanInterface::igepCard);
    89                 // open interface
    90                 if (canIf_.openInterface(port_, accessMode_)==0)
     94                sameCanIf = true;
     95                canIfRead_.setSource(Win32CanInterface::igepCard);
     96                // open interface
     97                if (canIfRead_.openInterface(port_, accessMode_)==0)
    9198                        qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
    9299        }
    93100        else if (source_ == "kvaser")
    94101        {
    95                 canIf_.setSource(Win32CanInterface::KvaserCard);
    96                 // open interface
    97                 if (canIf_.openInterface(channel_,speed_)==0)
     102                sameCanIf = false;
     103                canIfRead_.setSource(Win32CanInterface::KvaserCard);
     104                // open interface
     105                if (canIfRead_.openInterface(channel_,speed_)==0)
     106                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
     107
     108                canIfWrite_.setSource(Win32CanInterface::KvaserCard);
     109                // open interface
     110                if (canIfWrite_.openInterface(channel_,speed_)==0)
    98111                        qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
    99112        }
    … …  
    105118
    106119        // start the 2 threads: reception thread and decoding thread
    107         canIf_.start();
     120        canIfRead_.start();
     121
    108122        start();
    109123}
    … …  
    117131{
    118132        counter_ = 0;
    119         canIf_.stop();
    120 
    121         if ((source_ == "vector")||(source_=="peak")||(source_=="vectorXL")||(source_=="igep")||(source_=="kvaser"))
    122                 canIf_.closeInterface(channel_);
     133
     134        canIfRead_.stop();
     135
     136        if ((source_ == "vector")||(source_=="peak")||(source_=="vectorXL")||(source_=="igep")||(source_=="kvaser")) {
     137                canIfRead_.closeInterface(channel_);
     138                if(!sameCanIf)
     139                        canIfWrite_.closeInterface(channel_);
     140        }
    123141       
    124         canIf_.wait();
     142        canIfRead_.wait();
    125143
    126144        // we stop the decoding thread
    … …  
    167185bool CanGateway::sendFrame(const CanFrame frame)
    168186{
    169         if (canIf_.canDriver_ == NULL)
    170                 return false;
     187        if(sameCanIf)
     188        {
     189                if (canIfRead_.canDriver_ == NULL)
     190                        return false;
     191                else
     192                {
     193                        if (canIfWrite_.canDriver_->sendFrame(frame))
     194                                return true;
     195                        else
     196                                return false;
     197                }
     198        }
    171199        else
    172200        {
    173                 if (canIf_.canDriver_->sendFrame(frame))
    174                         return true;
     201                if (canIfWrite_.canDriver_ == NULL)
     202                        return false;
    175203                else
    176                         return false;
     204                {
     205                        if (canIfWrite_.canDriver_->sendFrame(frame))
     206                                return true;
     207                        else
     208                                return false;
     209                }
    177210        }
    178211}

  • trunk/CanGateway/CanGateway.h

    r91 r116  
    6262  void toc(char * text) { printf("duration = %d %s\n", (int)(road_time() - tic_), text ); }
    6363
    64   Win32CanInterface canIf_;
     64  Win32CanInterface canIfRead_;
     65  Win32CanInterface canIfWrite_;
     66  bool sameCanIf;
    6567  QSemaphore semaphore_;
    6668  TimestampedCanFrame incomingCanFrames_[INCOMINGCANFRAMES_SIZE];

  • trunk/CanGateway/Win32CanInterface.cpp

    r94 r116  
    55//  author:     Gerald Dherbomez
    66//              Copyright Heudiasyc UMR UTC/CNRS 7253
    7 // 
     7//
    88//  version:    $Id: $
    99//
    … …  
    3737
    3838/************************************************************************/
    39 /// Opens the CAN interface of the number given in argument 
     39/// Opens the CAN interface of the number given in argument
    4040bool Win32CanInterface::openInterface(const int number, const unsigned int speed)
    4141{
    4242  canDriver_ = new CanDriver(number, speed);
    43            
    44   //connection to CAN   bus 
     43
     44  //connection to CAN   bus
    4545  if(canDriver_->initPort() != 0)
    4646  {
    … …  
    6969/************************************************************************/
    7070/// Close the CAN interface
    71 /// todo: close only the port identified by number 
    72 ///       make a function that close the driver or close the driver only if 
     71/// todo: close only the port identified by number
     72///       make a function that close the driver or close the driver only if
    7373///       there is no more pending connections
    7474bool Win32CanInterface::closeInterface(const int /*number*/)
    7575{
    76   if(canDriver_->cleanUpPort() != 0)
    77   {
    78     cout << "CAN disconnection fails" << endl;
    79     delete canDriver_;
    80     return false;
     76  if(canDriver_ != NULL) {
     77    if(canDriver_->cleanUpPort() != 0)
     78    {
     79      cout << "CAN disconnection fails" << endl;
     80      delete canDriver_;
     81      return false;
     82    }
     83    else
     84    {
     85      delete canDriver_;
     86      return true;
     87    }
    8188  }
    8289  else
    83   {
    84     delete canDriver_;
    8590    return true;
    86   }
    8791}
    8892
    … …  
    9599
    96100  if (!receivedFramesArraySize_)
    97     qFatal("receivedFramesArraySize_ not initialized, division by zero may occur if you continue. Context:%s:L%d",__FILE__, __LINE__); 
     101    qFatal("receivedFramesArraySize_ not initialized, division by zero may occur if you continue. Context:%s:L%d",__FILE__, __LINE__);
    98102
    99103  cout << "Win32CanInterface starts" << endl;
    … …  
    102106  {
    103107  case VectorCard:
    104     vectorLoop();
     108  case PeakCard:
     109  case igepCard:
     110  case XLVectorCard:
     111  case KvaserCard:
     112    defaultLoop();
    105113    break;
    106   // case SharedMemory:  // runtime crash because PosixShMem
    107   //   shMemLoop();
    108   //   break;
    109   case PeakCard:
    110     peakLoop();
    111     break;
    112   case igepCard:
    113     igepLoop();
    114     break;
    115   case XLVectorCard:
    116     vectorXlLoop();
    117   case KvaserCard:
    118     kvaserLoop();
    119     break;
    120   default:
     114  default:
    121115    break;
    122116  }
    … …  
    128122/************************************************************************/
    129123/// The loop used for waiting CAN data from Vector card
    130 void Win32CanInterface::vectorLoop()
    131 {
    132   while (continue_)
    133   {
    134     // Wait incoming data from the CAN bus
    135     if (canDriver_->receiveFrame(frame_) == 0) {
    136       receivedFrames_[counter_].time = road_time();
    137       receivedFrames_[counter_].timerange = 0;
    138       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
    139       semaphore_->release();
    140       counter_++;
    141       counter_ = counter_ % receivedFramesArraySize_;
    142     }
    143   }
    144 }
     124// Default for vector, vectorXL, kvaser, Peak, Igep
    145125
    146 /************************************************************************/
    147 /// The loop used for waiting CAN data from XL Vector card
    148 void Win32CanInterface::vectorXlLoop()
     126void Win32CanInterface::defaultLoop()
    149127{
    150128  while(continue_) {
    151     // Wait incoming data from the CAN bus 
     129    // Wait incoming data from the CAN bus
    152130    if (canDriver_->receiveFrame(frame_) == 0) {
    153131      receivedFrames_[counter_].time = road_time();
    … …  
    155133      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame));
    156134      semaphore_->release();
    157       counter_++;
    158       counter_ = counter_ % receivedFramesArraySize_;
    159     }
    160   }
    161 }
    162 
    163 /************************************************************************/
    164 /// The loop used for waiting CAN data from Kvaser card
    165 void Win32CanInterface::kvaserLoop()
    166 {
    167   while(continue_) {
    168     // Wait incoming data from the CAN bus
    169     if (canDriver_->receiveFrame(frame_) == 0) {
    170       receivedFrames_[counter_].time = road_time();
    171       receivedFrames_[counter_].timerange = 0;
    172       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame));
    173       semaphore_->release();
    174       counter_++;
     135      counter_++;
    175136      counter_ = counter_ % receivedFramesArraySize_;
    176137    }
    … …  
    195156//       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
    196157//       semaphore_->release();
    197 //       counter_++; 
     158//       counter_++;
    198159//       counter_ = counter_ % receivedFramesArraySize_;
    199160//     }
    200161//   } // END while (continue_)
    201  
     162
    202163//   delete shMem_;
    203164// }
    204165
    205 /************************************************************************/
    206 /// The loop used for waiting CAN data from Peak card
    207 void Win32CanInterface::peakLoop()
    208 {
    209   std::cout << "In peak loop" << std::endl;
    210   while(continue_)
    211   {
    212     // Wait incoming data from the CAN bus
    213     if ( canDriver_->receiveFrame(frame_) == 0 ) {
    214       receivedFrames_[counter_].time = road_time();
    215       receivedFrames_[counter_].timerange = 0;
    216       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
    217       semaphore_->release();
    218       counter_++;
    219       counter_ = counter_ % receivedFramesArraySize_;
    220     }
    221   }
    222 }
    223 
    224 /************************************************************************/
    225 /// The loop used for waiting CAN data from igep card
    226 void Win32CanInterface::igepLoop()
    227 {
    228   std::cout << "In igep loop" << std::endl;
    229 
    230   while(continue_)
    231   {
    232     // Wait incoming data from the CAN bus
    233     if ( canDriver_->receiveFrame(frame_) == 0 ) {
    234       receivedFrames_[counter_].time = road_time();
    235       receivedFrames_[counter_].timerange = 0;
    236       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
    237       semaphore_->release();
    238       counter_++;
    239       counter_ = counter_ % receivedFramesArraySize_;
    240     }
    241   }
    242 }
    243166/************************************************************************/
    244167/// Stops the thread
    … …  
    251174/// Defines the place where the incoming frames will be copied
    252175void Win32CanInterface::setExchangeBuffer(TimestampedCanFrame * framesArray, const int framesArraySize)
    253 { 
     176{
    254177  receivedFrames_ = framesArray;
    255178  receivedFramesArraySize_ = framesArraySize;

  • trunk/CanGateway/Win32CanInterface.h

    r94 r116  
    5959protected:
    6060  void run();
    61   void vectorLoop();
     61
    6262  //void shMemLoop(); // runtime crash because PosixShMem
    63   void peakLoop();
    64   void igepLoop();
    65   void vectorXlLoop();
    66   void kvaserLoop();
     63  void defaultLoop();
    6764
    6865

  • trunk/CanGateway/driver/KVaserCanDriver.cpp

    r94 r116  
    44
    55#include "KVaserCanDriver.h"
     6/*
    67#ifdef WIN32
    78#include "kvaser/windows/canlib.h"
    … …  
    910#include "kvaser/linux/canlib.h"
    1011#endif
     12*/
     13
     14
     15#include <canlib.h>
     16
    1117#include <cassert>
    1218#include <cstdio>
    … …  
    4450// Check a status code and issue an error message if the code isn't canOK.
    4551//
    46 void ErrorExit(char* id, canStatus stat)
     52void ErrorExit(string id, canStatus stat)
    4753{
    4854    char buf[50];
    … …  
    123129
    124130
    125 void KVaserCanDriver::displayHardware() 
     131void KVaserCanDriver::displayHardware()
    126132{
    127133        LOG_INFO("----------------------------------------------------------");
    … …  
    139145  canHandle_ = canOpenChannel(hwChannel, 0);
    140146    if (canHandle_ < 0) {
    141         ErrorExit("canOpenChannel", (canStatus)canHandle_);
     147        ErrorExit("canOpenChannel", (canStatus) canHandle_);
    142148    }
    143         else 
     149        else
    144150        {
    145151                int ret = 0;
    … …  
    153159                                break;
    154160                        case 500000:
    155                                 ret = canSetBusParams(canHandle_, canBITRATE_500K, 0, 0, 0, 0, 0);
     161                                ret = canSetBusParams(canHandle_, canBITRATE_500K, 4, 3, 1, 1, 0);
    156162                                break;
    157163                        default:
    … …  
    166172                if (ret < 0) {
    167173                        ErrorExit("canBusOn", (canStatus)ret);
    168        
     174
    169175                }
    170176        }
    … …  
    184190    canInitializeLibrary();
    185191#endif
    186    
     192
    187193        // open the physical CAN interface
    188194        initialize(kvaserHardwareType_, canChannel_, canBitRate_);
    … …  
    199205short KVaserCanDriver::cleanUpPort (void)
    200206{
    201        
     207
    202208        //traceXLCommand("xlCloseDriver", xlCloseDriver());
    203209
    … …  
    215221{
    216222        canStatus stat = canWrite(canHandle_, frame.id,frame.data, frame.dlc, 0);
    217         if (stat == canOK ) 
     223        if (stat == canOK )
    218224        {
    219225                return 0;
    220226        } else {
    221227                LOG_WARN("Kvaser driver - sendFrame method - Failed to send the CAN frame");
    222                 return 1; 
     228                return 1;
    223229        }
    224230}
    … …  
    236242        //DWORD timestamp;
    237243        unsigned long timestamp;
    238        
     244
    239245        canStatus stat = canReadWait(canHandle_, &id, msg, &dlc, &flags, &timestamp, WAIT_RECEIVING_FRAME_TIMEOUT);
    240         if (stat == canOK ) 
     246        if (stat == canOK )
    241247        {
    242                 if ((flags & canMSG_ERROR_FRAME) == 0) 
     248                if ((flags & canMSG_ERROR_FRAME) == 0)
    243249                {
    244250                        // Get the message data
    … …  
    254260                        memcpy(frame.data, msg, frame.dlc);
    255261                        return 0;
    256                 } 
    257         } 
     262                }
     263        }
    258264        else if (stat == canERR_NOMSG ) {
    259265                        // timeout occurs
    260266                        LOG_WARN("Kvaser card - receiveFrame() method - TIMEOUT");
    261267                        return 1;
    262         } 
     268        }
    263269        else {
    264270                        // An error frame.
Note: See TracChangeset for help on using the changeset viewer.