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

Last change on this file since 46 was 42, checked in by cfougera, 10 years ago

Last version
File size: 14.8 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((u_int32_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
159u_int32_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(*((u_int32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
167 if (i == length) {
168 return -1;
169 }
170 ++i;
171 }
172 return i;
173
174}
175
176
177u_int32_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 = *((u_int16_t*)(msg.body+24));
205 msg.hScan.scannerStatus = *((u_int16_t*)(msg.body+24+2));
206 msg.hScan.phaseOffset = *((u_int16_t*)(msg.body+24+4));
207 msg.hScan.startNtpTime = *((u_int64_t*)(msg.body+24+6));
208 msg.hScan.endNtpTime = *((u_int64_t*)(msg.body+24+14));
209 msg.hScan.ticksPerRot= *((u_int16_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 = *((u_int16_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 // we have a complete message available that we can add to the list
323 MessageLDMRS msg;
324
325 // we copy the bytes in the body message
326 char* messageData = (char*)malloc(msgSize);
327 memcpy(messageData, pendingBytes.data.c_str() + index, msgSize);
328
329 msg.body = messageData;
330
331 // we set the timestamp of the message
332 if (pendingBytes.previousData)
333 {
334 // the timestamp is the one of the previous packet
335 msg.time = pendingBytes.time;
336 pendingBytes.previousData = false;
337 }
338 else
339 {
340 // the timestamp is the one of the actual received packet
341 msg.time = time;
342 }
343
344 // we add the message to the list
345 msgList.push_back(msg);
346 // and we suppress the processed bytes of the pending data
347 pendingBytes.data.erase(0, msgSize);
348 }
349}
350
351
352unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
353{
354 fillDataHeader(msg);
355 if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
356 LOG_TRACE("(Process Message) Scan Data Type!");
357 fillScanHeader(msg);
358
359 int index = 24 + 44; // data header + scan header
360 ScanPoint* scanPoints = (ScanPoint*) malloc(sizeof(ScanPoint) * msg.hScan.numPoints);
361 int position = 0;
362
363 for (int i = 0; i < msg.hScan.numPoints; ++i) {
364 ((ScanPoint*)(scanPoints + position))->layerEcho = *((uchar*)(msg.body + index));
365 ((ScanPoint*)(scanPoints + position))->flags = *((uchar*)(msg.body + index + 1));
366 ((ScanPoint*)(scanPoints + position))->angle = *((u_int16_t*)(msg.body + index + 2));
367 ((ScanPoint*)(scanPoints + position))->distance = *((u_int16_t*)(msg.body + index + 4));
368 ((ScanPoint*)(scanPoints + position))->echoPulseWidth = *((u_int16_t*)(msg.body + index + 6));
369 index += 10;
370 position += sizeof(ScanPoint);
371 }
372
373 // raw data (message) no longer needed, free memory and replace with structured data
374 free(msg.body);
375 msg.body = (char*) scanPoints;
376 }
377 else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
378 LOG_TRACE("(Process Message) Object Data Type!");
379
380
381
382 // TODO
383
384 // raw message no longer needed, free memory
385 free(msg.body);
386 // msg.body = (char*) scanObjects;
387 }
388 else // irrelevant data type
389 free(msg.body); // free raw data
390
391 return msg.hData.dataType;
392}
393
394
395void SickLDMRSSensor::writeData(MessageLDMRS &msg)
396{
397 // record the data in a dbt file. The dbt data is only the structure DataHeader
398 // scan data are recorded in the sickldmrs_data.utc file
399
400 SickLDMRS_dbt entry;
401 entry.timeStartFromSensor = msg.hData.ntpTime;
402 entry.hScan = msg.hScan;
403 entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
404// entry.time = msg.time;
405// entry.timerange = msg.timerange;
406
407 // write DBT
408 try {
409 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
410 } catch (DbiteException & e) {
411 cerr << "error writing data: " << e.what() << endl;
412 return;
413 }
414
415 // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
416 for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
417 dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
418 }
419 // add a magic word to delimit the block of data
420 int32_t utcMagicWord = UTC_MAGIC_WORD;
421 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
422
423}
424
425
426
427void SickLDMRSSensor::configure(){
428 // Start measuring
429 // S_socket->sendToServer(QString((u_int32_t)0x0020));
430
431 // LOG_TRACE(this->name_ +" configured.");
432}
433
434
435void SickLDMRSSensor::customEvent(QEvent * e)
436{
437 SickFrame * frame = ((SickFrameEvent*)e)->frame;
438
439 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
440 splitPacket(frame->msg, frame->size, frame->time);
441
442 // we delete the heap variable
443 delete frame;
444
445 // we test if we have some messages to decode
446 while ( !msgList.empty() )
447 {
448 // get the first (the eldest) message and process it
449 MessageLDMRS msgToProcess = msgList.front();
450 unsigned long type = processMessage(msgToProcess);
451
452 if (type == SICKLDMRS_SCANDATA_TYPE)
453 {
454// setState(ComponentBase::MONITOR_OK);
455 // write data on the disk
456 if (recording_)
457 writeData(msgToProcess);
458
459#ifdef SICKLDMRS_SH_MEM
460 /// NOT TESTED !
461 /// push data in shared memory
462 // First the scan info
463 SickLDMRS_shMem toWrite;
464 toWrite.time = msgToProcess.time;
465 toWrite.timerange = msgToProcess.timerange;
466 toWrite.scanInfo = msgToProcess.hScan;
467 shmem_->write(toWrite, sizeof(SickLDMRS_shMem));
468
469 // Then, the points
470 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
471 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
472 }
473#endif
474
475 }
476 else if (type == SICKLDMRS_OBJECTDATA_TYPE)
477 {
478 // Handled via CAN bus ?
479 }
480
481 // removes the processed item of the list
482 msgList.pop_front();
483 }
484}
485
486} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.