source: pacpussensors/trunk/Sick/SickLDMRSSensor.cpp@ 61

Last change on this file since 61 was 61, checked in by DHERBOMEZ Gérald, 10 years ago

commit after experiments on track
Issue with GPS receiver, no data recorded
File size: 15.0 KB
Line 
1/*********************************************************************
2// created: 2014/02/02 - 12:08
3// filename: SickLDMRSSensor.cpp
4//
5// author: Cyril Fougeray
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: The acquisition class of the SickLDMRS sensor
11//
12*********************************************************************/
13
14#include "SickLDMRSSensor.h"
15
16#include "SickSocket.h"
17// #include "Pacpus/kernel/ComponentFactory.h"
18#include "Pacpus/kernel/DbiteException.h"
19#include "Pacpus/kernel/DbiteFileTypes.h"
20#include "Pacpus/kernel/Log.h"
21#include "Pacpus/PacpusTools/ShMem.h"
22
23#include <iostream>
24#include <QTcpSocket>
25#include <string>
26
27using namespace std;
28
29// #define SICKLDMRS_SH_MEM
30
31namespace pacpus {
32
33DECLARE_STATIC_LOGGER("pacpus.base.SickLDMRSSensor");
34
35
36static const string kSickMemoryName = "SickLDMRS";
37
38
39static const long MagicWord = 0xAFFEC0C2; // Sick LDMRS
40
41
42static const int SICKLDMRS_SCANDATA_TYPE = 0x2202;
43static const int SICKLDMRS_OBJECTDATA_TYPE = 0x2221;
44
45
46SickLDMRSSensor::SickLDMRSSensor(QObject *parent)
47 : ipaddr_("192.168.0.1"),
48 port_(12002),
49 name_("SickLDMRSDefault")
50{
51 LOG_TRACE("constructor ("<< this->name_ << "), default settings");
52
53 S_socket = new SickSocket(this);
54
55 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
56
57 pendingBytes.time = 0;
58 pendingBytes.previousData = false;
59
60 this->kSickDbtFileName = name_.toStdString()+".dbt";
61 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
62}
63
64SickLDMRSSensor::SickLDMRSSensor(QObject *parent, QString name, QString ip, int port, int recording)
65 : ipaddr_(ip),
66 port_(port),
67 name_(name),
68 recording_((bool)recording)
69{
70
71 LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);
72
73 S_socket = new SickSocket(this);
74
75 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
76
77 pendingBytes.time = 0;
78 pendingBytes.previousData = false;
79
80 this->kSickDbtFileName = name_.toStdString()+".dbt";
81 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
82}
83
84
85SickLDMRSSensor::~SickLDMRSSensor()
86{
87 LOG_TRACE("destructor ("<< this->name_ << ")");
88 delete S_socket;
89}
90
91
92void SickLDMRSSensor::startActivity()
93{
94
95 LOG_TRACE("Start Activity ("<< this->name_ <<")");
96
97 S_socket->connectToServer(ipaddr_, port_);
98
99 if (recording_) {
100 LOG_TRACE("(Recording) Starting recording...");
101
102 try {
103 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LDMRS, sizeof(SickLDMRS_dbt));
104 } catch (DbiteException & e) {
105 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
106 return;
107 }
108
109 // FIXME: use ofstream
110 // open the file with C function to be sure that it will exist
111 FILE * stream;
112 if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
113 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
114 ::exit(-1);
115 } else {
116 fclose(stream);
117 }
118
119 dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
120 if (!dataFile_) {
121 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
122 ::exit(-1);
123 }
124 }
125 else
126 LOG_TRACE("(Recording) Not recording...");
127
128
129#ifdef SICKLDMRS_SH_MEM
130 shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
131#endif
132
133}
134
135
136void SickLDMRSSensor::stopActivity()
137{
138 LOG_TRACE("destructor (" << this->name_ << ")");
139
140 S_socket->sendToServer(QString((uint32_t)0x0021));
141
142 S_socket->closeSocket();
143
144 if (recording_) {
145 LOG_TRACE("(Recording) Recording stopped");
146 dbtFile_.close();
147 dataFile_.close();
148 }
149
150#ifdef SICKLDMRS_SH_MEM
151 delete shmem_;
152#endif
153 // delete generator;
154
155}
156
157
158
159uint32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
160{
161 if (length < 4) {
162 return -1;
163 }
164
165 unsigned long i = 0;
166 while(*((uint32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
167 if (i == length) {
168 return -1;
169 }
170 ++i;
171 }
172 return i;
173
174}
175
176
177uint32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
178{
179
180 // we need at least 12 bytes
181 if (length < 12) {
182 return 0;
183 }
184 return ((*(message+magicWordIndex+11))&0x000000FF)
185 + ((*(message+magicWordIndex+10)<<8)&0x0000FF00)
186 + ((*(message+magicWordIndex+9)<<16)&0x00FF0000)
187 + ((*(message+magicWordIndex+8)<<24)&0xFF000000)
188 + 24 ;
189}
190
191
192bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
193{
194 if (size <= length) {
195 return true;
196 }
197 return false;
198}
199
200
201void SickLDMRSSensor::fillScanHeader( MessageLDMRS &msg )
202{
203 // address = base + Data header size (24-byte long) + offset
204 msg.hScan.scanNumber = *((uint16_t*)(msg.body+24));
205 msg.hScan.scannerStatus = *((uint16_t*)(msg.body+24+2));
206 msg.hScan.phaseOffset = *((uint16_t*)(msg.body+24+4));
207 msg.hScan.startNtpTime = *((uint64_t*)(msg.body+24+6));
208 msg.hScan.endNtpTime = *((uint64_t*)(msg.body+24+14));
209 msg.hScan.ticksPerRot= *((uint16_t*)(msg.body+24+22)); // needed to compute angle (°)
210 msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
211 msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
212 msg.hScan.numPoints = *((uint16_t*)(msg.body+24+28));
213
214// msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
215// msg.hScan.mountingPitchAngle = *((int16_t*)(msg.body+24+32));
216// msg.hScan.mountingRollAngle = *((int16_t*)(msg.body+24+34));
217// msg.hScan.mountingX = *((int16_t*)(msg.body+24+36));
218// msg.hScan.mountingY = *((int16_t*)(msg.body+24+38));
219// msg.hScan.mountingZ = *((int16_t*)(msg.body+24+40));
220
221
222 LOG_TRACE("(hScan) Scan header data parsed, scan number " << msg.hScan.scanNumber);
223 LOG_TRACE("(hScan) Number of scanned points : " << msg.hScan.numPoints);
224}
225
226
227void SickLDMRSSensor::fillDataHeader(MessageLDMRS &msg)
228{
229
230 msg.hData.magicWord = ((*(msg.body+3))&0x000000FF) +
231 ((*(msg.body+2)<<8)&0x0000FF00) +
232 ((*(msg.body+1)<<16)&0x00FF0000)+
233 ((*(msg.body)<<24)&0xFF000000);
234 LOG_TRACE("(hData) Magic word read "<<msg.hData.magicWord);
235 // TODO check if OK
236
237 msg.hData.sizePreviousMessage = ((*(msg.body+7))&0x000000FF)+
238 ((*(msg.body+6)<<8)&0x0000FF00)+
239 ((*(msg.body+5)<<16)&0x00FF0000)+
240 ((*(msg.body+4)<<24)&0xFF000000);
241 LOG_TRACE("(hData) previous message size : "<<msg.hData.sizePreviousMessage);
242
243 msg.hData.sizeCurrentMessage =((*(msg.body+11))&0x000000FF)+
244 ((*(msg.body+10)<<8)&0x0000FF00)+
245 ((*(msg.body+9)<<16)&0x00FF0000)+
246 ((*(msg.body+8)<<24)&0xFF000000);
247
248 LOG_TRACE("(hData) current message size : "<<msg.hData.sizeCurrentMessage);
249
250
251 msg.hData.deviceId = *(msg.body+13);
252
253 msg.hData.dataType = ((*(msg.body+15))&0x000000FF)+((*(msg.body+14)<<8)&0x0000FF00);
254 LOG_TRACE("(hData) Data type : " << msg.hData.dataType);
255
256
257 msg.hData.ntpTime = ((*(msg.body+15))&0x000000FF)+
258 ((*(msg.body+14)<<8)&0x0000FF00)+
259 ((*(msg.body+13)<<16)&0x00FF0000)+
260 ((*(msg.body+12)<<24)&0xFF000000)+
261 ((*(msg.body+11))&0x000000FF)+
262 ((*(msg.body+10)<<8)&0x0000FF00)+
263 ((*(msg.body+9)<<16)&0x00FF0000)+
264 ((*(msg.body+8)<<24)&0xFF000000);
265}
266
267
268void SickLDMRSSensor::storePendingBytes(road_time_t time)
269{
270 if (!pendingBytes.previousData)
271 {
272 pendingBytes.time = time;
273 pendingBytes.previousData = true;
274 }
275}
276
277
278void SickLDMRSSensor:: splitPacket(const char * packet, const int length, road_time_t time)
279{
280 long index = 0;
281 long msgSize = 0;
282 bool msgComplete = false;
283
284 // we are working on the previous not decoded data + the actual incoming packet
285 pendingBytes.data.append(packet,length);
286 LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
287
288
289 while (pendingBytes.data.size() > 0)
290 {
291 // we are looking for the MagicWord
292 index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
293 LOG_TRACE("(Packet reconstitution) MagicWord index : " << index );
294 if (index == -1)
295 {
296 storePendingBytes(time);
297 // exit the while loop
298 break;
299 }
300
301
302 // we are looking for the size of the message (scan data + scan points)
303 msgSize = getMessageSize(pendingBytes.data.c_str(), pendingBytes.data.size(), index);
304 if (msgSize == 0)
305 {
306 storePendingBytes(time);
307 // exit the while loop
308 break;
309 }
310
311 LOG_TRACE("(Packet reconstitution) Message size (data header(24) + size of the message) : " << msgSize );
312
313 // we are verifying if the message is complete
314 msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
315 if (msgComplete == false)
316 {
317 storePendingBytes(time);
318 // exit the while loop
319 break;
320 }
321
322 LOG_TRACE("(Packet reconstitution) Message complete ! ");
323 // we have a complete message available that we can add to the list
324 MessageLDMRS msg;
325
326 // we copy the bytes in the body message
327// char* messageData = (char*)malloc(msgSize);
328// if(messageData == NULL){
329// LOG_FATAL("(Packet reconstitution) Malloc FAILED. Packet lost.");
330// return;
331// }
332 memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize);
333
334// msg->body = messageData;
335
336 // we set the timestamp of the message
337 if (pendingBytes.previousData)
338 {
339 // the timestamp is the one of the previous packet
340 msg.time = pendingBytes.time;
341 pendingBytes.previousData = false;
342 }
343 else
344 {
345 // the timestamp is the one of the actual received packet
346 msg.time = time;
347 }
348
349 // we add the message to the list
350 msgList.push_back(msg);
351 // and we suppress the processed bytes of the pending data
352 pendingBytes.data.erase(0, msgSize);
353 }
354}
355
356
357unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
358{
359 fillDataHeader(msg);
360 if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
361 LOG_TRACE("(Process Message) Scan Data Type!");
362 fillScanHeader(msg);
363
364 int index = 24 + 44; // data header + scan header
365
366 if(sizeof(ScanPoint) * msg.hScan.numPoints > BODY_MAX_SIZE){
367 LOG_FATAL("Size of the message is too long !");
368 return 0;
369 }
370
371 ScanPoint * scanPoints = new ScanPoint[msg.hScan.numPoints];
372
373 // replace memory with structured data
374 for (int i = 0; i < msg.hScan.numPoints; ++i) {
375 scanPoints[i].layerEcho = *((uchar*)(msg.body + index));
376 scanPoints[i].flags = *((uchar*)(msg.body + index + 1));
377 scanPoints[i].angle = *((uint16_t*)(msg.body + index + 2));
378 scanPoints[i].distance = *((uint16_t*)(msg.body + index + 4));
379 scanPoints[i].echoPulseWidth = *((uint16_t*)(msg.body + index + 6));
380 }
381
382 memcpy(msg.body, scanPoints, sizeof(ScanPoint) * msg.hScan.numPoints);
383
384 delete[] scanPoints;
385 }
386 else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
387 LOG_TRACE("(Process Message) Object Data Type!");
388
389 // TODO
390 }
391 else {// irrelevant data type
392 // TODO
393 }
394
395
396 return msg.hData.dataType;
397}
398
399
400void SickLDMRSSensor::writeData(MessageLDMRS &msg)
401{
402 // record the data in a dbt file. The dbt data is only the structure DataHeader
403 // scan data are recorded in the sickldmrs_data.utc file
404
405 SickLDMRS_dbt entry;
406 entry.timeStartFromSensor = msg.hData.ntpTime;
407 entry.hScan = msg.hScan;
408 entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
409// entry.time = msg.time;
410// entry.timerange = msg.timerange;
411
412 LOG_TRACE("Writing into DBT + UTC files..");
413
414 // write DBT
415 try {
416 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
417 } catch (DbiteException & e) {
418 cerr << "error writing data: " << e.what() << endl;
419 return;
420 }
421
422 // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
423 for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
424 dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
425 }
426 // add a magic word to delimit the block of data
427 int32_t utcMagicWord = UTC_MAGIC_WORD;
428 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
429
430 LOG_TRACE("Writing done !");
431
432}
433
434
435
436void SickLDMRSSensor::configure(){
437 // Start measuring
438 // S_socket->sendToServer(QString((uint32_t)0x0020));
439
440 // LOG_TRACE(this->name_ +" configured.");
441}
442
443
444void SickLDMRSSensor::customEvent(QEvent * e)
445{
446 SickFrame * frame = ((SickFrameEvent*)e)->frame;
447
448 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
449 splitPacket(frame->msg, frame->size, frame->time);
450
451 // we delete the heap variable
452 delete frame;
453
454 // we test if we have some messages to decode
455 while ( !msgList.empty() )
456 {
457 LOG_TRACE("Message waiting");
458 // get the first (the eldest) message and process it
459 MessageLDMRS msgToProcess = msgList.front();
460 unsigned long type = processMessage(msgToProcess);
461 LOG_TRACE("Message processed !");
462
463 if (type == SICKLDMRS_SCANDATA_TYPE)
464 {
465// setState(ComponentBase::MONITOR_OK);
466 // write data on the disk
467 if (recording_)
468 writeData(msgToProcess);
469
470#ifdef SICKLDMRS_SH_MEM
471 /// NOT TESTED !
472 /// push data in shared memory
473 // First the scan info
474 SickLDMRS_shMem toWrite;
475 toWrite.time = msgToProcess.time;
476 toWrite.timerange = msgToProcess.timerange;
477 toWrite.scanInfo = msgToProcess.hScan;
478 shmem_->write(toWrite, sizeof(SickLDMRS_shMem));
479
480 // Then, the points
481 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
482 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
483 }
484#endif
485
486 }
487 else if (type == SICKLDMRS_OBJECTDATA_TYPE)
488 {
489 // Handled via CAN bus ?
490 }
491
492 // removes the processed item of the list
493 // free(msgList.front().body);
494 msgList.pop_front();
495 }
496}
497
498} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.