source: pacpussensors/trunk/Sick/SickLMSSensor.cpp@ 73

Last change on this file since 73 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: 25.3 KB
RevLine 
[37]1/*********************************************************************
2// created: 2014/02/11 - 12:08
3// filename: SickLMSSensor.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 SickLMS sensor
11//
12*********************************************************************/
13
14#include "SickLMSSensor.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 <boost/algorithm/string/split.hpp>
24#include <boost/algorithm/string/classification.hpp>
25
26#include <iostream>
27#include <QTcpSocket>
28#include <string>
29#include <vector>
30
31using namespace std;
32
33// #define SICKLMS_SH_MEM
34
35namespace pacpus {
36
37static const int STX_CHAR = 0x02;
38static const int ETX_CHAR = 0x03;
39
40DECLARE_STATIC_LOGGER("pacpus.base.SickLMSSensor");
41
42// Commands which can be taken into account
43static const std::string SICKLMS_SCANDATA_MESSAGE = "LMDscandata";
44static const std::string SICKLMS_SCANCONFIG_MESSAGE = "LMPscancfg";
45static const int MINIMUM_PARAMETERS_MSG = 6; // ScanConfig message is the smallest relevant message
46
47//////////////////////////////////////////////////////////////////////////
48/// Constructor.
49SickLMSSensor::SickLMSSensor(QObject *parent)
50 : ipaddr_("192.168.0.50"),
51 port_(1211),
52 name_("SickLMSDefault"),
53 recording_(false)
54{
55 LOG_TRACE("constructor ("<<this->name_ << "), default settings");
56
57 S_socket = new SickSocket(this);
58
59 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
60
61 pendingBytes.time = 0;
62 pendingBytes.previousData = false;
63
64 this->kSickDbtFileName = name_.toStdString()+".dbt";
65 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
66}
67
68SickLMSSensor::SickLMSSensor(QObject *parent, QString name, QString ip, int port, int recording)
69 : ipaddr_(ip),
70 port_(port),
71 name_(name),
72 recording_((bool)recording)
73{
74
75 LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);
76
77 S_socket = new SickSocket(this);
78
79 connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
80
81 pendingBytes.time = 0;
82 pendingBytes.previousData = false;
83
84 this->kSickDbtFileName = name_.toStdString()+".dbt";
85 this->kSickUtcFileName = name_.toStdString()+"_data.utc";
86}
87
[42]88
[37]89SickLMSSensor::~SickLMSSensor()
90{
91 LOG_TRACE("destructor ("<< this->name_ << ")");
92 delete S_socket;
93}
94
[42]95
[37]96void SickLMSSensor::startActivity()
97{
98
99 LOG_TRACE("Start Activity ("<< this->name_ <<")");
100
101 S_socket->connectToServer(ipaddr_, port_);
102
103 // config can be computed for each measure
104 // askScanCfg();
105
106
107 char buf[100];
108
109// ///LOGIN
110// sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
111// S_socket->sendToServer(QString(buf));
112
113
114// sprintf(buf, "%c%s%c", 0x02, "sWN LMDscandatacfg 03 00 1 1 0 00 00 0 0 0 0 1 1", 0x03);
115// S_socket->sendToServer(QString(buf));
116
117//// sprintf(buf, "%c%s%c", 0x02, "sWN FREchoFilter 1", 0x03);
118//// S_socket->sendToServer(QString(buf));
119
120/////// SAVE PERMANENT
121// sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
122// S_socket->sendToServer(QString(buf));
123
124//// /// LOGOUT
125// sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
126// S_socket->sendToServer(QString(buf));
127
128
129 // Start measurement
130 sprintf(buf, "%c%s%c", 0x02, "sEN LMDscandata 1", 0x03);
131 S_socket->sendToServer(QString(buf));
132
133 /// TODO get response from sensor and analyse it to know if measuring has started
134 /// See p23 telegram
135 if(0)
136 LOG_INFO("(Measuring) Data sent permanently");
137
138
139 if (recording_) {
140 LOG_INFO("(Recording) Recording is on.");
141
142 try {
[42]143 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LMS, sizeof(SickLMS_dbt));
[37]144 } catch (DbiteException & e) {
145 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
146 return;
147 }
148
149 // FIXME: use ofstream
150 // open the file with C function to be sure that it will exist
151 FILE * stream;
152 if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
153 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
154 ::exit(-1);
155 } else {
156 fclose(stream);
157 }
158
159 dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
160 if (!dataFile_) {
161 LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
162 ::exit(-1);
163 }
164 }
165 else
166 LOG_INFO("(Recording) Not recording.");
167
168#ifdef SICKLMS_SH_MEM
169 shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
170#endif
171
172}
173
[42]174
175
[37]176void SickLMSSensor::stopActivity()
177{
178 LOG_TRACE("destructor (" << this->name_ << ")");
179
180 // Stop measurement. Not necessary : when socket is closed, data is no longer sent from sensor
181 char buf[64];
182 sprintf(buf, "%c%s%c", 0x02, "sEN LMDscandata 0", 0x03);
183 S_socket->sendToServer(QString(buf));
184
185 S_socket->closeSocket();
186
187 if (recording_) {
188 LOG_TRACE("(Recording) Recording stopped");
189 dbtFile_.close();
190 dataFile_.close();
191 }
192
193#ifdef SICKLMS_SH_MEM
194 delete shmem_;
195#endif
196 // delete generator;
197
198}
199
[42]200
[37]201void SickLMSSensor::configure(){
202 // Start measuring
[61]203 // S_socket->sendToServer(QString((uint32_t)0x0020));
[37]204
205 // LOG_TRACE(this->name_ +" configured.");
206}
207
208
209
210
[42]211/* Data must be parsed in slot customEvent */
[37]212void SickLMSSensor::askScanCfg(){
213 char buf[100];
214 sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
215
216 S_socket->sendToServer(QString(buf));
217}
218
219
220
[42]221
[37]222int SickLMSSensor::isMessageComplete(const char* packets, unsigned int size)
223{
224 for(int i = 0; i < size; ++i){
225 if(packets[i] == ETX_CHAR)
226 return i;
227 }
228 return -1;
229}
230
231
[42]232
[37]233int SickLMSSensor::findSTX(const char* packets, const unsigned int size ){
234 int i = 0;
235 while(i < size){
236 if(packets[i] == STX_CHAR)
237 return i;
238 i++;
239 }
240 return -1;
241}
242
243
244void SickLMSSensor::storePendingBytes(road_time_t time)
245{
246 if (!pendingBytes.previousData)
247 {
248 pendingBytes.time = time;
249 pendingBytes.previousData = true;
250 }
251}
252
[42]253
[37]254void SickLMSSensor::reconstituteMessage(const char * packet, const int length, road_time_t time)
255{
256 long indexSTX = 0;
257 long indexETX = 0;
258 long msgSize = 0;
259
260 // we are working on the previous not decoded data + the actual incoming packet
261 pendingBytes.data.append(packet,length);
262 LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
263
264
265 while (pendingBytes.data.size() > 0)
266 {
267 // we are looking for the start of frame <STX> (= 0x02)
268 indexSTX = findSTX(pendingBytes.data.c_str() , pendingBytes.data.size() );
269 LOG_TRACE("(Packet reconstitution) Start of text index : " << indexSTX );
270 if (indexSTX == -1)
271 {
272 storePendingBytes(time);
273 // exit the while loop
274 break;
275 }
276
277 // we are verifying if the message is complete
278 indexETX = isMessageComplete(pendingBytes.data.c_str(), pendingBytes.data.size());
279 LOG_TRACE("(Packet reconstitution) End of text index : " << indexETX );
280 if (indexETX == -1)
281 {
282 storePendingBytes(time);
283 // exit the while loop
284 break;
285 }
286
287 // we have a complete message available that we can add to the list
288 MessageLMS msg;
289
290 // we copy the bytes in the body message
291 msgSize = indexETX - indexSTX + 1;
292 char* messageData = (char*)malloc(msgSize+1);
293 memcpy(messageData, pendingBytes.data.c_str() + indexSTX, msgSize);
294
295 msg.body = messageData;
296 msg.msgSize = msgSize;
297
298 // we set the timestamp of the message
299 if (pendingBytes.previousData)
300 {
301 // the timestamp is the one of the previous packet
302 msg.time = pendingBytes.time;
303 pendingBytes.previousData = false;
304 }
305 else
306 {
307 // the timestamp is the one of the actual received packet
308 msg.time = time;
309 }
310
311 // we add the message to the list
312 msgList.push_back(msg);
313 // and we suppress the processed bytes of the pending data
314 pendingBytes.data.erase(0, msgSize);
315 }
316}
317
318
319int SickLMSSensor::splitMessage(MessageLMS* message){
320
321 message->splitMessage = new std::vector<std::string>();
322
[72]323 for(int i=0; i < message->msgSize; ++i){
[37]324 std::string* str = new std::string();
325 while(message->body[i] != ' ' && i < message->msgSize){
326 str->push_back(message->body[i]);
327 ++i;
328 }
329 message->splitMessage->push_back(*str);
330 delete str;
331 }
332 LOG_TRACE("(splitMessage) Number of parameters into the message : "<< (int)message->splitMessage->size());
333
334 return (int) message->splitMessage->size();
335}
336
[42]337
338
[37]339long SickLMSSensor::xstol(std::string str){
340 long ret = 0;
341 for (std::string::iterator it=str.begin(); it!=str.end(); ++it){
342 if(*it >= 'A' && *it <= 'F'){
343 ret *= 16;
344 ret += (*it - 'A' + 10);
345 }
346 else if (*it >= '0' && *it <= '9'){
347 ret *= 16;
348 ret += (*it - '0');
349 }
350 else
351 LOG_WARN("(conversion) String is not a hex value");
352 }
353 return ret;
354}
355
356
357int SickLMSSensor::processScanData(MessageLMS* msg)
358{
359 // just non-empty arrays will be saved in DBT
360 // so first, we initialize these values to 0 and then fill them if message contains corresponding data
361 msg->data.dist_len1 = 0;
362 msg->data.dist_len2 = 0;
363 msg->data.dist_len3 = 0;
364 msg->data.dist_len4 = 0;
365 msg->data.dist_len5 = 0;
366 msg->data.rssi_len1 = 0;
367 msg->data.rssi_len2 = 0;
368
369
370// for(std::vector<std::string>::iterator it = msg->splitMessage->begin(); it != msg->splitMessage->end(); it++){
371// printf("%s ", (*it).c_str());
372// }
373// printf("\n");
374
375 // 0 //Type of command
376 // 1 //Command
377 // 2 //VersionNumber
378 // 3 //DeviceNumber
379 // 4 //Serial number
380 // 5-6 //DeviceStatus
381 // 00 00 OK
382 // 00 01 Error
383 // 00 02 Pollution Warning
384 // 00 04 Pollution Error
385
386 // 7 //MessageCounter
387
388 LOG_TRACE("(Parsing) Message number 0x"<< msg->splitMessage->at(7).c_str());
389
390 // 8 //ScanCounter
391 // 9 //PowerUpDuration
392 // 10 //TransmissionDuration
393 // 11-12 //InputStatus
394 // 13-14 //OutputStatus
395 // 15 //ReservedByteA
396 // 16 //ScanningFrequency
397 msg->data.scanFrequency = xstol(msg->splitMessage->at(16));
398 LOG_TRACE("(Parsing) Scan frequency "<< msg->data.scanFrequency <<" [1/100 Hz]");
399
400 // 17 //MeasurementFrequency
401
402 // 18 //NumberEncoders
403 int NumberEncoders = xstol(msg->splitMessage->at(18));
404 LOG_TRACE("(Parsing) Number Encoders "<< NumberEncoders);
405
406 for (int i = 0; i < NumberEncoders; i++) {
407 // //EncoderPosition
408 // //EncoderSpeed
409 }
410
411
412 // 18+NumberEncoders*2+1 //NumberChannels16Bit
413 int NumberChannels16Bit = xstol(msg->splitMessage->at(18+NumberEncoders*2+1));
414 LOG_TRACE("(Parsing) Number channels 16Bit : "<<NumberChannels16Bit);
415
416 int totalData16 = 0;
417
418 for (int i = 0; i < NumberChannels16Bit; i++) {
419 int type = -1; // 0 DIST1 1 DIST2 2 (LMS1xx & LMS5xx)
420 // RSSI1 3 RSSI2 (LMS1xx)
421 // 4 DIST3 5 DIST4 6 DIST5 (LMS5xx)
422
423
424 int NumberData;
425
426 std::string content = msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+1);
427 // 19+NumberEncoders*2+i*6+totalData16+1 //MeasuredDataContent
428
429 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Measured Data Content : " << content);
430 if (content == "DIST1") {
431 type = 0;
432 } else if (content == "DIST2") {
433 type = 1;
434 } else if (content == "RSSI1") {
435 type = 2;
436 } else if (content == "RSSI2") {
437 type = 3;
438 } else if (content == "DIST3") {
439 type = 4;
440 } else if (content == "DIST4") {
441 type = 5;
442 } else if (content == "DIST5") {
443 type = 6;
444 }
445
446// 19+NumberEncoders*2+i*6+totalData16+2 //ScalingFactor
447 int scalingFactor = 1;
448 if(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+2) == "40000000")
449 scalingFactor = 2;
450 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Scaling factor x"<< scalingFactor);
451
452// 19+NumberEncoders*2+i*6+totalData16+3 //ScalingOffset
453// 19+NumberEncoders*2+i*6+totalData16+4 //Starting angle
454 msg->data.startAngle = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+4));
455 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Start angle "<< msg->data.startAngle << " [1/10000 degree]");
456
457// 19+NumberEncoders*2+i*6+totalData16+5 //Angular step width
458 msg->data.angleResolution = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+5));
459 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Angular step width "<< msg->data.angleResolution<<" [1/10000 degree]");
460
461// 19+NumberEncoders*2+i*6+totalData16+6 //NumberData
462 NumberData = xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+6));
463
464 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Number Data for "<<content<<" : "<<NumberData);
465
466 LOG_TRACE("(Parsing 16bit channel #"<<i<<") First data "<<content<<" (index "<<(19+NumberEncoders*2+i*6+totalData16+7)<<") : "<<xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7)));
467 LOG_TRACE("(Parsing 16bit channel #"<<i<<") Last data "<<content<<" (index "<<(19+NumberEncoders*2+i*6+totalData16+7+NumberData-1)<<") : "<<xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7+NumberData-1)));
468
469 uint16_t* distPoints = (uint16_t*) malloc(NumberData * sizeof(uint16_t));
470 for (int j = 0; j < NumberData; j++) {
471 distPoints[j] = scalingFactor * xstol(msg->splitMessage->at(19+NumberEncoders*2+i*6+totalData16+7+j));
472 }
473 if (type == 0) {
474 msg->data.dist1 = distPoints;
475 msg->data.dist_len1 = NumberData;
476 } else if (type == 1) {
477 msg->data.dist2 = distPoints;
478 msg->data.dist_len2 = NumberData;
479 } else if (type == 2) {
480 msg->data.rssi1 = distPoints;
481 msg->data.rssi_len1 = NumberData;
482 } else if (type == 3) {
483 msg->data.rssi2 = distPoints;
484 msg->data.rssi_len2 = NumberData;
485 } else if (type == 4) {
486 msg->data.dist3 = distPoints;
487 msg->data.dist_len3 = NumberData;
488 } else if (type == 5) {
489 msg->data.dist4 = distPoints;
490 msg->data.dist_len4 = NumberData;
491 } else if (type == 6) {
492 msg->data.dist5 = distPoints;
493 msg->data.dist_len5 = NumberData;
494 }
495 totalData16 += NumberData;
496
497 }
498
499
500// 19+NumberEncoders*2+NumberChannels16Bit*6+totalData16+1 //NumberChannels8Bit
501 int NumberChannels8Bit = xstol(msg->splitMessage->at(19+NumberEncoders*2+NumberChannels16Bit*6+totalData16+1));
502 LOG_TRACE("(Processing) Number channels 8Bit : "<<NumberChannels8Bit);
503
504 LOG_INFO("(Parsing) 8bit channel not implemented yet !");
505
506/*
507// int totalData8 = 0;
508// for (int i = 0; i < NumberChannels8Bit; i++) {
509
510// int type = -1;
511
512// std::string content = msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6);
513// LOG_TRACE("(Parsing 8bit channel) Measured Data Content : " << content);
514// if (content == "DIST1") {
515// type = 0;
516// } else if (content=="DIST2") {
517// type = 1;
518// } else if (content=="RSSI1") {
519// type = 2;
520// } else if (content == "RSSI2") {
521// type = 3;
522// }
523
524// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+1 //ScalingFactor
525// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+2 //ScalingOffset
526// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+3 //Starting angle
527// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+4 //Angular step width
528// 21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+i*6+5 //NumberData
529
530// int NumberData = xstol(msg->splitMessage->at(21+NumberEncoders*2+NumberChannels16Bit*6+totalData16+totalData8+i*6+5));
531// LOG_TRACE("(Parsing 16bit channel) Number Data for "<<content<<" : "<<NumberData);
532
533// sscanf(tok, "%X", &NumberData);
534
535// LOG_TRACE("(Processing) Number data : "<< NumberData);
536
537// if (type == 0) {
538// msg->data.dist_len1 = NumberData;
539// } else if (type == 1) {
540// msg->data.dist_len2 = NumberData;
541// } else if (type == 2) {
542// msg->data.rssi_len1 = NumberData;
543// } else if (type == 3) {
544// msg->data.rssi_len2 = NumberData;
545// }
546// for (int i = 0; i < NumberData; i++) {
547// int dat;
548// tok = strtok(NULL, " "); //data
549// sscanf(tok, "%X", &dat);
550
551// if (type == 0) {
552// msg->data.dist1[i] = dat;
553// } else if (type == 1) {
554// msg->data.dist2[i] = dat;
555// } else if (type == 2) {
556// msg->data.rssi1[i] = dat;
557// } else if (type == 3) {
558// msg->data.rssi2[i] = dat;
559// }
560// }
561 } // 8bit channel
562*/
563
[71]564 return 0;
[37]565}
566
[42]567
[37]568void SickLMSSensor::writeData(MessageLMS &msg)
569{
570 SickLMS_dbt entry;
571 entry.angleResolution = msg.data.angleResolution;
572 entry.scanNumber = xstol(msg.splitMessage->at(7));
573 entry.scannerStatus = xstol(msg.splitMessage->at(6));
[42]574 /* time in SickLMS_dbt is used uniquely when DBT file read */
575 // entry.time = msg.time;
576 // entry.timerange = msg.timerange;
[37]577 entry.scanFrequency = msg.data.scanFrequency;
578 entry.angleResolution = msg.data.angleResolution;
579 entry.startAngle = msg.data.startAngle;
580
[42]581 // Initialisation
582 entry.dist_len1 = entry.dataPos_dist1 = 0;
583 entry.dist_len2 = entry.dataPos_dist2 = 0;
584 entry.dist_len3 = entry.dataPos_dist3 = 0;
585 entry.dist_len4 = entry.dataPos_dist4 = 0;
586 entry.dist_len5 = entry.dataPos_dist5 = 0;
587 entry.rssi_len1 = entry.dataPos_rssi1 = 0;
588 entry.rssi_len2 = entry.dataPos_rssi2 = 0;
589
[37]590 if(msg.data.dist_len1){
591 entry.dist_len1 = msg.data.dist_len1;
592 entry.dataPos_dist1 = dataFile_.tellp();
593
594 for (unsigned int i = 0 ; i < msg.data.dist_len1; ++i) {
[61]595 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist1[i])), sizeof(uint16_t));
[37]596 }
597
598 free(msg.data.dist1);
599 }
600 if(msg.data.dist_len2){
601 entry.dist_len2 = msg.data.dist_len2;
602 entry.dataPos_dist2 = dataFile_.tellp();
603
604 for (unsigned int i = 0 ; i < msg.data.dist_len2; ++i) {
[61]605 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist2[i])), sizeof(uint16_t));
[37]606 }
607
608 free(msg.data.dist2);
609 }
610 if(msg.data.dist_len3){
611 entry.dist_len3 = msg.data.dist_len3;
612 entry.dataPos_dist3 = dataFile_.tellp();
613
614 for (unsigned int i = 0 ; i < msg.data.dist_len3; ++i) {
[61]615 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist3[i])), sizeof(uint16_t));
[37]616 }
617
618 free(msg.data.dist3);
619 }
620 if(msg.data.dist_len4){
621 entry.dist_len4 = msg.data.dist_len4;
622 entry.dataPos_dist4 = dataFile_.tellp();
623
624 for (unsigned int i = 0 ; i < msg.data.dist_len4; ++i) {
[61]625 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist4[i])), sizeof(uint16_t));
[37]626 }
627
628 free(msg.data.dist4);
629 }
630 if(msg.data.dist_len5){
631 entry.dist_len5 = msg.data.dist_len5;
632 entry.dataPos_dist5 = dataFile_.tellp();
633
634 for (unsigned int i = 0 ; i < msg.data.dist_len5; ++i) {
[61]635 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist5[i])), sizeof(uint16_t));
[37]636 }
637
638 free(msg.data.dist5);
639 }
640 if(msg.data.rssi_len1){
641 entry.rssi_len1 = msg.data.rssi_len1;
642 entry.dataPos_rssi1 = dataFile_.tellp();
643
644 for (unsigned int i = 0 ; i < msg.data.rssi_len1; ++i) {
[61]645 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi1[i])), sizeof(uint16_t));
[37]646 }
647
648 free(msg.data.rssi1);
649 }
650 if(msg.data.rssi_len2){
651 entry.rssi_len2 = msg.data.rssi_len2;
652 entry.dataPos_rssi2 = dataFile_.tellp();
653
654 for (unsigned int i = 0 ; i < msg.data.rssi_len2; ++i) {
[61]655 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi2[i])), sizeof(uint16_t));
[37]656 }
657
658 free(msg.data.rssi2);
659 }
660 // add a magic word to delimit the block of data
661 int32_t utcMagicWord = UTC_MAGIC_WORD;
662 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
663
664 // write DBT
665 try {
666 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLMS_dbt));
667 } catch (DbiteException & e) {
668 cerr << "error writing data: " << e.what() << endl;
669 return;
670 }
671
672}
673
674
675
[42]676
[37]677void SickLMSSensor::customEvent(QEvent * e)
678{
679 char answerMsg1[10];
680 sprintf(answerMsg1, "%c%s", 0x02, "sRA");
681 char answerMsg2[10];
682 sprintf(answerMsg2, "%c%s", 0x02, "sSN");
683
684 SickFrame * frame = ((SickFrameEvent*)e)->frame;
685
686 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
687 reconstituteMessage(frame->msg, frame->size, frame->time);
688
689 // we delete the heap variable
690 delete frame;
691
692 // we test if we have some messages to decode
693 while ( !msgList.empty() )
694 {
695 // get the first (the eldest) message and process it
696 MessageLMS* msgToProcess = &(msgList.front());
697
698 // verify if the message is worth to be decoded
699 if(!strncmp(answerMsg1, msgToProcess->body, 4) || !strncmp(answerMsg2, msgToProcess->body, 4)){
700
701 if(splitMessage(msgToProcess) >= MINIMUM_PARAMETERS_MSG){
702
703 std::string type = msgToProcess->splitMessage->at(1);
704
705 LOG_TRACE("(Message type) "<< type);
706
707 if (type == SICKLMS_SCANDATA_MESSAGE)
708 {
709 LOG_TRACE("Scan data message !");
710
711 processScanData(msgToProcess);
712
713 // write data on the disk (dbt + utc)
714 if (recording_)
715 writeData(msgList.front());
716
717
718 #ifdef SICKLMS_SH_MEM
719 /// push data in shared memory
720 // First the scan info
721 SickLMS_shMem toWrite;
722 toWrite.time = msgToProcess.time;
723 toWrite.timerange = msgToProcess.timerange;
724 toWrite.scanInfo = msgToProcess.hScan;
725 shmem_->write(toWrite, sizeof(SickLMS_shMem));
726
727 // Then, the points
728 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
729 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
730 }
731 #endif
732 }
733 else if (type == SICKLMS_SCANCONFIG_MESSAGE){
734 LOG_TRACE("Scan configuration message !");
735
736 // get the values as (unsigned or signed) integer
737 mainCfg.scaningFrequency = xstol(msgToProcess->splitMessage->at(2));
738 mainCfg.angleResolution = xstol(msgToProcess->splitMessage->at(4));
739 mainCfg.startAngle = xstol(msgToProcess->splitMessage->at(5));
740 mainCfg.stopAngle = xstol(msgToProcess->splitMessage->at(6));
741 LOG_TRACE("(Scan config) Frequency : "<<mainCfg.scaningFrequency<<", Resolution : "<<mainCfg.angleResolution);
742 LOG_TRACE("(Scan config) Start angle : "<<mainCfg.startAngle<<", Stop angle : "<<mainCfg.stopAngle);
743 }
744 }
745
746 delete msgToProcess->splitMessage;
747 }
748
749
750 // (malloced memory) raw data no longer needed
751 free(msgToProcess->body);
752
753
754 // removes the processed item of the list
755 msgList.pop_front();
756 }
757}
758
759} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.