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

Last change on this file since 134 was 116, checked in by ydroniou, 9 years ago

Fix KVaser for Linux \o/

File size: 7.0 KB
Line 
1/********************************************************************
2// created: 2008/2/11 - 12:55
3// filename: CanGateway.cpp
4//
5// author: Gerald Dherbomez
6// Copyright Heudiasyc UMR UTC/CNRS 7253
7//
8// version: $Id: $
9//
10// purpose: Decoding of the CAN bus
11//
12*********************************************************************/
13
14
15#include "Pacpus/kernel/ComponentFactory.h"
16#include "Pacpus/kernel/DbiteFileTypes.h"
17
18using namespace pacpus;
19
20#include "CanGateway.h"
21
22#include <QDebug>
23
24#include <QSemaphore>
25#include <QList>
26
27#include "../CanGateway/CanDecoderBase.h"
28
29
30
31////////////////////////////////////////////////////////////////////////////////
32/// Construct the factory
33ComponentFactory<CanGateway> sFactory("CanGateway");
34
35
36////////////////////////////////////////////////////////////////////////////////
37/// Constructor
38CanGateway::CanGateway(QString name)
39 : ComponentBase(name)
40{
41 counter_ = 0;
42// tcpServer_ = NULL;
43 //subscribers_ = new QMultiHash<int, ComponentBase *>;
44}
45
46////////////////////////////////////////////////////////////////////////////////
47/// Destructor
48CanGateway::~CanGateway()
49{
50}
51
52/************************************************************************/
53/* Start function
54/************************************************************************/
55void CanGateway::startActivity()
56{
57 counter_ = 0;
58 THREAD_ALIVE = true;
59
60 // set the exhange parameters for incoming CAN frames
61 canIfRead_.setExchangeBuffer(incomingCanFrames_, INCOMINGCANFRAMES_SIZE);
62 canIfRead_.setSignalSempahore(&semaphore_);
63
64 if (source_ == "shMem") {
65 sameCanIf = true;
66 canIfRead_.setSource(Win32CanInterface::SharedMemory);
67 }
68 else if (source_ == "vector")
69 {
70 sameCanIf = true;
71 canIfRead_.setSource(Win32CanInterface::VectorCard);
72 // open the interface
73 if (!canIfRead_.openInterface(channel_, speed_))
74 qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
75 }
76 else if (source_ == "vectorXL")
77 {
78 sameCanIf = true;
79 canIfRead_.setSource(Win32CanInterface::XLVectorCard);
80 // open the interface
81 if (!canIfRead_.openInterface(channel_, speed_))
82 qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
83 }
84 else if (source_ == "peak")
85 {
86 sameCanIf = true;
87 canIfRead_.setSource(Win32CanInterface::PeakCard);
88 // open interface
89 if (canIfRead_.openInterface(port_, accessMode_)==0)
90 qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
91 }
92 else if (source_ == "igep")
93 {
94 sameCanIf = true;
95 canIfRead_.setSource(Win32CanInterface::igepCard);
96 // open interface
97 if (canIfRead_.openInterface(port_, accessMode_)==0)
98 qFatal("Failed to open the CAN interface port %s in %s mode",port_, accessMode_);
99 }
100 else if (source_ == "kvaser")
101 {
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)
111 qFatal("Failed to open the CAN interface num %d at speed %d",channel_,speed_);
112 }
113 else
114 {
115 qCritical("Error in the source property of the component, bad value");
116 return;
117 }
118
119 // start the 2 threads: reception thread and decoding thread
120 canIfRead_.start();
121
122 start();
123}
124
125
126
127/************************************************************************/
128/* Stop function
129/************************************************************************/
130void CanGateway::stopActivity()
131{
132 counter_ = 0;
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 }
141
142 canIfRead_.wait();
143
144 // we stop the decoding thread
145 THREAD_ALIVE = false;
146 semaphore_.release(); // to release the waiting of new CAN frame
147
148 if (!wait(1000))
149 {
150 terminate();
151 qDebug() << "The thread" << name() << "seems blocked, it has been killed";
152 }
153}
154
155
156
157/************************************************************************/
158/* Configuration of the component
159/************************************************************************/
160ComponentBase::COMPONENT_CONFIGURATION CanGateway::configureComponent(XmlComponentConfig config)
161{
162 // FIXME: use string instead of char[]
163 // Peak driver, default values
164 strcpy(port_,"/dev/pcanusb0");
165 strcpy(accessMode_,"ReadOnly");
166
167 channel_ = config.getProperty("channel").toInt() - 1;
168 speed_ = config.getProperty("speed").toInt() * 1000;
169 source_ = config.getProperty("source");
170 if (source_ =="peak")
171 {
172 if (config.getProperty("port")!= NULL)
173 strcpy(port_, config.getProperty("port").toStdString().c_str());
174 if (config.getProperty("mode")!= NULL)
175 strcpy(accessMode_, config.getProperty("mode").toStdString().c_str());
176 }
177 setRecording( config.getProperty("recording") == "true" ? true : false );
178
179 verbose_ = config.getProperty("verbose") == "true" ? true : false;
180
181 return ComponentBase::CONFIGURED_OK;
182}
183
184
185bool CanGateway::sendFrame(const CanFrame frame)
186{
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 }
199 else
200 {
201 if (canIfWrite_.canDriver_ == NULL)
202 return false;
203 else
204 {
205 if (canIfWrite_.canDriver_->sendFrame(frame))
206 return true;
207 else
208 return false;
209 }
210 }
211}
212
213
214
215/************************************************************************/
216/* The main loop of the thread
217/************************************************************************/
218void CanGateway::run()
219{
220 counter_ = 0;
221 qDebug() << name() << "thread is started";
222
223 if (isRecording()) {
224 rawCanFile_.open(name().toStdString() + "_rawcan.dbt", WriteMode, CARMEN_CAN_RAW , sizeof( CanFrame ) );
225 }
226
227 tic();
228
229 while (THREAD_ALIVE)
230 {
231 semaphore_.acquire();
232 if (!THREAD_ALIVE)
233 {
234 continue; // user asks stopping the thread
235 }
236
237 if (verbose_) displayData(incomingCanFrames_[counter_].frame.data, incomingCanFrames_[counter_].frame.dlc, incomingCanFrames_[counter_].frame.id);
238 if (isRecording())
239 {
240 rawCanFile_.writeRecord(incomingCanFrames_[counter_].time, incomingCanFrames_[counter_].timerange,
241 reinterpret_cast<const char *>(&(incomingCanFrames_[counter_].frame)), sizeof(CanFrame));
242 }
243
244 setState(ComponentBase::MONITOR_OK);
245
246 // printf("id:%x\n",incomingCanFrames_[counter_].frame.id);
247
248 /* switch (incomingCanFrames_[counter_].frame.id)
249 {
250 default:
251 // unknown identifier
252 break;
253 };
254
255 */
256
257 dispatchCanFrame(incomingCanFrames_[counter_]);
258
259 counter_++;
260 counter_ = counter_ % INCOMINGCANFRAMES_SIZE;
261 }
262
263 if (isRecording())
264 {
265 rawCanFile_.close();
266 }
267
268 qDebug() << name() << "thread is stopped";
269}
270
271
Note: See TracBrowser for help on using the repository browser.