Changeset 116 in pacpussensors for trunk/CanGateway/Win32CanInterface.cpp

Timestamp:

12/16/15 16:57:50 (9 years ago)

Author:

ydroniou

Message:

Fix KVaser for Linux \o/

File:

• trunk/CanGateway/Win32CanInterface.cpp (modified) (9 diffs)

trunk/CanGateway/Win32CanInterface.cpp

@@ -5 +5 @@
 //  author:     Gerald Dherbomez
 //              Copyright Heudiasyc UMR UTC/CNRS 7253
-// 
+//
 //  version:    $Id: $
 //
@@ -37 +37 @@
 
 /************************************************************************/
-/// Opens the CAN interface of the number given in argument 
+/// Opens the CAN interface of the number given in argument
 bool Win32CanInterface::openInterface(const int number, const unsigned int speed)
 {
   canDriver_ = new CanDriver(number, speed);
-            
-  //connection to CAN   bus 
+
+  //connection to CAN   bus
   if(canDriver_->initPort() != 0)
   {
@@ -69 +69 @@
 /************************************************************************/
 /// Close the CAN interface
-/// todo: close only the port identified by number 
-///       make a function that close the driver or close the driver only if 
+/// todo: close only the port identified by number
+///       make a function that close the driver or close the driver only if
 ///       there is no more pending connections
 bool Win32CanInterface::closeInterface(const int /*number*/)
 {
-  if(canDriver_->cleanUpPort() != 0)
-  {
-    cout << "CAN disconnection fails" << endl;
-    delete canDriver_;
-    return false;
+  if(canDriver_ != NULL) {
+    if(canDriver_->cleanUpPort() != 0)
+    {
+      cout << "CAN disconnection fails" << endl;
+      delete canDriver_;
+      return false;
+    }
+    else
+    {
+      delete canDriver_;
+      return true;
+    }
   }
   else
-  {
-    delete canDriver_;
     return true;
-  }
 }
 
@@ -95 +99 @@
 
   if (!receivedFramesArraySize_)
-    qFatal("receivedFramesArraySize_ not initialized, division by zero may occur if you continue. Context:%s:L%d",__FILE__, __LINE__);  
+    qFatal("receivedFramesArraySize_ not initialized, division by zero may occur if you continue. Context:%s:L%d",__FILE__, __LINE__);
 
   cout << "Win32CanInterface starts" << endl;
@@ -102 +106 @@
   {
   case VectorCard:
-    vectorLoop();
+  case PeakCard:
+  case igepCard:
+  case XLVectorCard:
+  case KvaserCard:
+    defaultLoop();
     break;
-  // case SharedMemory:  // runtime crash because PosixShMem
-  //   shMemLoop();
-  //   break;
-  case PeakCard:
-    peakLoop();
-    break;
-  case igepCard:
-    igepLoop();
-    break;
-  case XLVectorCard:
-    vectorXlLoop();
-  case KvaserCard:
-    kvaserLoop();
-    break;
-  default: 
+  default:
     break;
   }
@@ -128 +122 @@
 /************************************************************************/
 /// The loop used for waiting CAN data from Vector card
-void Win32CanInterface::vectorLoop()
-{
-  while (continue_)
-  {
-    // Wait incoming data from the CAN bus 
-    if (canDriver_->receiveFrame(frame_) == 0) {
-      receivedFrames_[counter_].time = road_time();
-      receivedFrames_[counter_].timerange = 0;
-      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
-      semaphore_->release();
-      counter_++; 
-      counter_ = counter_ % receivedFramesArraySize_;
-    }
-  }
-}
+// Default for vector, vectorXL, kvaser, Peak, Igep
 
-/************************************************************************/
-/// The loop used for waiting CAN data from XL Vector card
-void Win32CanInterface::vectorXlLoop()
+void Win32CanInterface::defaultLoop()
 {
   while(continue_) {
-    // Wait incoming data from the CAN bus 
+    // Wait incoming data from the CAN bus
     if (canDriver_->receiveFrame(frame_) == 0) {
       receivedFrames_[counter_].time = road_time();
@@ -155 +133 @@
       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame));
       semaphore_->release();
-      counter_++; 
-      counter_ = counter_ % receivedFramesArraySize_;
-    }
-  }
-}
-
-/************************************************************************/
-/// The loop used for waiting CAN data from Kvaser card
-void Win32CanInterface::kvaserLoop()
-{
-  while(continue_) {
-    // Wait incoming data from the CAN bus 
-    if (canDriver_->receiveFrame(frame_) == 0) {
-      receivedFrames_[counter_].time = road_time();
-      receivedFrames_[counter_].timerange = 0;
-      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame));
-      semaphore_->release();
-      counter_++; 
+      counter_++;
       counter_ = counter_ % receivedFramesArraySize_;
     }
@@ -195 +156 @@
 //       memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
 //       semaphore_->release();
-//       counter_++; 
+//       counter_++;
 //       counter_ = counter_ % receivedFramesArraySize_;
 //     }
 //   } // END while (continue_)
-  
+
 //   delete shMem_;
 // }
 
-/************************************************************************/
-/// The loop used for waiting CAN data from Peak card
-void Win32CanInterface::peakLoop()
-{
-  std::cout << "In peak loop" << std::endl;
-  while(continue_)
-  {
-    // Wait incoming data from the CAN bus
-    if ( canDriver_->receiveFrame(frame_) == 0 ) {
-      receivedFrames_[counter_].time = road_time();
-      receivedFrames_[counter_].timerange = 0;
-      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
-      semaphore_->release();
-      counter_++;
-      counter_ = counter_ % receivedFramesArraySize_;
-    }
-  }
-}
-
-/************************************************************************/
-/// The loop used for waiting CAN data from igep card
-void Win32CanInterface::igepLoop()
-{
-  std::cout << "In igep loop" << std::endl;
-
-  while(continue_)
-  {
-    // Wait incoming data from the CAN bus
-    if ( canDriver_->receiveFrame(frame_) == 0 ) {
-      receivedFrames_[counter_].time = road_time();
-      receivedFrames_[counter_].timerange = 0;
-      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
-      semaphore_->release();
-      counter_++;
-      counter_ = counter_ % receivedFramesArraySize_;
-    }
-  }
-}
 /************************************************************************/
 /// Stops the thread
@@ -251 +174 @@
 /// Defines the place where the incoming frames will be copied
 void Win32CanInterface::setExchangeBuffer(TimestampedCanFrame * framesArray, const int framesArraySize)
-{ 
+{
   receivedFrames_ = framesArray;
   receivedFramesArraySize_ = framesArraySize;