source: pacpussensors/trunk/Sick/SickLDMRSSensor.cpp

Last change on this file was 72, checked in by DHERBOMEZ Gérald, 10 years ago
  • Correction of a little bug in gps component (property recording bad)
  • Improvement of sick lms151 component
  • Correction of bug in sick LDRMS : buffer overflow in a memcpy
File size: 14.9 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#ifdef SICKLDMRS_SH_MEM
129 shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
130#endif
131
132}
133
134
135void SickLDMRSSensor::stopActivity()
136{
137 LOG_TRACE("destructor (" << this->name_ << ")");
138
139 S_socket->sendToServer(QString((uint32_t)0x0021));
140
141 S_socket->closeSocket();
142
143 if (recording_) {
144 LOG_TRACE("(Recording) Recording stopped");
145 dbtFile_.close();
146 dataFile_.close();
147 }
148
149#ifdef SICKLDMRS_SH_MEM
150 delete shmem_;
151#endif
152 // delete generator;
153
154}
155
156
157
158uint32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
159{
160 if (length < 4) {
161 return -1;
162 }
163
164 unsigned long i = 0;
165 while(*((uint32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
166 if (i == length) {
167 return -1;
168 }
169 ++i;
170 }
171 return i;
172
173}
174
175
176uint32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
177{
178
179 // we need at least 12 bytes
180 if (length < 12) {
181 return 0;
182 }
183 return ((*(message+magicWordIndex+11))&0x000000FF)
184 + ((*(message+magicWordIndex+10)<<8)&0x0000FF00)
185 + ((*(message+magicWordIndex+9)<<16)&0x00FF0000)
186 + ((*(message+magicWordIndex+8)<<24)&0xFF000000)
187 + 24 ;
188}
189
190
191bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
192{
193 if (size <= length) {
194 return true;
195 }
196 return false;
197}
198
199
200void SickLDMRSSensor::fillScanHeader( MessageLDMRS &msg )
201{
202 // address = base + Data header size (24-byte long) + offset
203 msg.hScan.scanNumber = *((uint16_t*)(msg.body+24));
204 msg.hScan.scannerStatus = *((uint16_t*)(msg.body+24+2));
205 msg.hScan.phaseOffset = *((uint16_t*)(msg.body+24+4));
206 msg.hScan.startNtpTime = *((uint64_t*)(msg.body+24+6));
207 msg.hScan.endNtpTime = *((uint64_t*)(msg.body+24+14));
208 msg.hScan.ticksPerRot= *((uint16_t*)(msg.body+24+22)); // needed to compute angle (°)
209 msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
210 msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
211 msg.hScan.numPoints = *((uint16_t*)(msg.body+24+28));
212
213// msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
214// msg.hScan.mountingPitchAngle = *((int16_t*)(msg.body+24+32));
215// msg.hScan.mountingRollAngle = *((int16_t*)(msg.body+24+34));
216// msg.hScan.mountingX = *((int16_t*)(msg.body+24+36));
217// msg.hScan.mountingY = *((int16_t*)(msg.body+24+38));
218// msg.hScan.mountingZ = *((int16_t*)(msg.body+24+40));
219
220
221 LOG_TRACE("(hScan) Scan header data parsed, scan number " << msg.hScan.scanNumber);
222 LOG_TRACE("(hScan) Number of scanned points : " << msg.hScan.numPoints);
223}
224
225
226void SickLDMRSSensor::fillDataHeader(MessageLDMRS &msg)
227{
228
229 msg.hData.magicWord = ((*(msg.body+3))&0x000000FF) +
230 ((*(msg.body+2)<<8)&0x0000FF00) +
231 ((*(msg.body+1)<<16)&0x00FF0000)+
232 ((*(msg.body)<<24)&0xFF000000);
233 LOG_TRACE("(hData) Magic word read "<<msg.hData.magicWord);
234 // TODO check if OK
235
236 msg.hData.sizePreviousMessage = ((*(msg.body+7))&0x000000FF)+
237 ((*(msg.body+6)<<8)&0x0000FF00)+
238 ((*(msg.body+5)<<16)&0x00FF0000)+
239 ((*(msg.body+4)<<24)&0xFF000000);
240 LOG_TRACE("(hData) previous message size : "<<msg.hData.sizePreviousMessage);
241
242 msg.hData.sizeCurrentMessage =((*(msg.body+11))&0x000000FF)+
243 ((*(msg.body+10)<<8)&0x0000FF00)+
244 ((*(msg.body+9)<<16)&0x00FF0000)+
245 ((*(msg.body+8)<<24)&0xFF000000);
246
247 LOG_TRACE("(hData) current message size : "<<msg.hData.sizeCurrentMessage);
248
249
250 msg.hData.deviceId = *(msg.body+13);
251
252 msg.hData.dataType = ((*(msg.body+15))&0x000000FF)+((*(msg.body+14)<<8)&0x0000FF00);
253 LOG_TRACE("(hData) Data type : " << msg.hData.dataType);
254
255
256 msg.hData.ntpTime = ((*(msg.body+15))&0x000000FF)+
257 ((*(msg.body+14)<<8)&0x0000FF00)+
258 ((*(msg.body+13)<<16)&0x00FF0000)+
259 ((*(msg.body+12)<<24)&0xFF000000)+
260 ((*(msg.body+11))&0x000000FF)+
261 ((*(msg.body+10)<<8)&0x0000FF00)+
262 ((*(msg.body+9)<<16)&0x00FF0000)+
263 ((*(msg.body+8)<<24)&0xFF000000);
264}
265
266
267void SickLDMRSSensor::storePendingBytes(road_time_t time)
268{
269 if (!pendingBytes.previousData)
270 {
271 pendingBytes.time = time;
272 pendingBytes.previousData = true;
273 }
274}
275
276
277void SickLDMRSSensor:: splitPacket(const char * packet, const int length, road_time_t time)
278{
279 long index = 0;
280 long msgSize = 0;
281 bool msgComplete = false;
282
283 // we are working on the previous not decoded data + the actual incoming packet
284 pendingBytes.data.append(packet,length);
285 LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
286
287
288 while (pendingBytes.data.size() > 0)
289 {
290 // we are looking for the MagicWord
291 index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
292 LOG_TRACE("(Packet reconstitution) MagicWord index : " << index );
293 if (index == -1)
294 {
295 storePendingBytes(time);
296 // exit the while loop
297 break;
298 }
299
300
301 // we are looking for the size of the message (scan data + scan points)
302 msgSize = getMessageSize(pendingBytes.data.c_str(), pendingBytes.data.size(), index);
303 if (msgSize == 0)
304 {
305 storePendingBytes(time);
306 // exit the while loop
307 break;
308 }
309
310 LOG_TRACE("(Packet reconstitution) Message size (data header(24) + size of the message) : " << msgSize );
311
312 // we are verifying if the message is complete
313 msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
314 if (msgComplete == false)
315 {
316 storePendingBytes(time);
317 // exit the while loop
318 break;
319 }
320
321 LOG_TRACE("(Packet reconstitution) Message complete ! ");
322 // we have a complete message available that we can add to the list
323 MessageLDMRS msg;
324 if (msgSize <= sizeof(msg.body))
325 memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize);
326 else
327 LOG_ERROR("Impossible to copy the pending bytes in the MessageLDMRS, checked the size of the body in the code! Data might be corrupted...");
328// msg->body = messageData;
329
330 // we set the timestamp of the message
331 if (pendingBytes.previousData)
332 {
333 // the timestamp is the one of the previous packet
334 msg.time = pendingBytes.time;
335 pendingBytes.previousData = false;
336 }
337 else
338 {
339 // the timestamp is the one of the actual received packet
340 msg.time = time;
341 }
342
343 // we add the message to the list
344 msgList.push_back(msg);
345 // and we suppress the processed bytes of the pending data
346 pendingBytes.data.erase(0, msgSize);
347 }
348}
349
350
351unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
352{
353 fillDataHeader(msg);
354 if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
355 LOG_TRACE("(Process Message) Scan Data Type!");
356 fillScanHeader(msg);
357
358 int index = 24 + 44; // data header + scan header
359
360 if(sizeof(ScanPoint) * msg.hScan.numPoints > BODY_MAX_SIZE){
361 LOG_FATAL("Size of the message is too long !");
362 return 0;
363 }
364
365 ScanPoint * scanPoints = new ScanPoint[msg.hScan.numPoints];
366
367 // replace memory with structured data
368 for (int i = 0; i < msg.hScan.numPoints; ++i) {
369 scanPoints[i].layerEcho = *((uchar*)(msg.body + index));
370 scanPoints[i].flags = *((uchar*)(msg.body + index + 1));
371 scanPoints[i].angle = *((int16_t*)(msg.body + index + 2));
372 scanPoints[i].distance = *((uint16_t*)(msg.body + index + 4));
373 scanPoints[i].echoPulseWidth = *((uint16_t*)(msg.body + index + 6));
374 index+=10;
375 }
376 memcpy(msg.body, scanPoints, sizeof(ScanPoint) * msg.hScan.numPoints);
377
378 delete[] scanPoints;
379 }
380 else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
381 LOG_TRACE("(Process Message) Object Data Type!");
382
383 // TODO
384 }
385 else {// irrelevant data type
386 // TODO
387 }
388 return msg.hData.dataType;
389}
390
391
392void SickLDMRSSensor::writeData(MessageLDMRS &msg)
393{
394 // record the data in a dbt file. The dbt data is only the structure DataHeader
395 // scan data are recorded in the sickldmrs_data.utc file
396
397 SickLDMRS_dbt entry;
398 entry.timeStartFromSensor = msg.hData.ntpTime;
399 entry.hScan = msg.hScan;
400 entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
401// entry.time = msg.time;
402// entry.timerange = msg.timerange;
403
404 LOG_TRACE("Writing into DBT + UTC files..");
405
406 // write DBT
407 try {
408 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
409 } catch (DbiteException & e) {
410 cerr << "error writing data: " << e.what() << endl;
411 return;
412 }
413
414 // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
415 for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
416 dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
417 }
418 // add a magic word to delimit the block of data
419 int32_t utcMagicWord = UTC_MAGIC_WORD;
420 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
421
422 LOG_TRACE("Writing done !");
423
424}
425
426
427
428void SickLDMRSSensor::configure(){
429 // Start measuring
430 // S_socket->sendToServer(QString((uint32_t)0x0020));
431
432 // LOG_TRACE(this->name_ +" configured.");
433}
434
435
436void SickLDMRSSensor::customEvent(QEvent * e)
437{
438 SickFrame * frame = ((SickFrameEvent*)e)->frame;
439
440 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
441 splitPacket(frame->msg, frame->size, frame->time);
442
443 // we delete the heap variable
444 delete frame;
445
446 // we test if we have some messages to decode
447 while ( !msgList.empty() )
448 {
449 LOG_TRACE("Message waiting");
450 // get the first (the eldest) message and process it
451 MessageLDMRS msgToProcess = msgList.front();
452 unsigned long type = processMessage(msgToProcess);
453 LOG_TRACE("Message processed !");
454
455 if (type == SICKLDMRS_SCANDATA_TYPE)
456 {
457// setState(ComponentBase::MONITOR_OK);
458 // write data on the disk
459 if (recording_)
460 writeData(msgToProcess);
461
462#ifdef SICKLDMRS_SH_MEM
463 /// NOT TESTED !
464 /// push data in shared memory
465 // First the scan info
466 SickLDMRS_shMem toWrite;
467 toWrite.time = msgToProcess.time;
468 toWrite.timerange = msgToProcess.timerange;
469 toWrite.scanInfo = msgToProcess.hScan;
470 shmem_->write(toWrite, sizeof(SickLDMRS_shMem));
471
472 // Then, the points
473 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
474 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
475 }
476#endif
477
478 }
479 else if (type == SICKLDMRS_OBJECTDATA_TYPE)
480 {
481 // Handled via CAN bus ?
482 }
483
484 // removes the processed item of the list
485 // free(msgList.front().body);
486 msgList.pop_front();
487 }
488}
489
490} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.