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

Last change on this file since 69 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: 25.2 KB
Line 
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
88
89SickLMSSensor::~SickLMSSensor()
90{
91 LOG_TRACE("destructor ("<< this->name_ << ")");
92 delete S_socket;
93}
94
95
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 {
143 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LMS, sizeof(SickLMS_dbt));
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
174
175
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
200
201void SickLMSSensor::configure(){
202 // Start measuring
203 // S_socket->sendToServer(QString((uint32_t)0x0020));
204
205 // LOG_TRACE(this->name_ +" configured.");
206}
207
208
209
210
211/* Data must be parsed in slot customEvent */
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
221
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
232
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
253
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
323 for(int i; i < message->msgSize; ++i){
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
337
338
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
564}
565
566
567void SickLMSSensor::writeData(MessageLMS &msg)
568{
569 SickLMS_dbt entry;
570 entry.angleResolution = msg.data.angleResolution;
571 entry.scanNumber = xstol(msg.splitMessage->at(7));
572 entry.scannerStatus = xstol(msg.splitMessage->at(6));
573 /* time in SickLMS_dbt is used uniquely when DBT file read */
574 // entry.time = msg.time;
575 // entry.timerange = msg.timerange;
576 entry.scanFrequency = msg.data.scanFrequency;
577 entry.angleResolution = msg.data.angleResolution;
578 entry.startAngle = msg.data.startAngle;
579
580 // Initialisation
581 entry.dist_len1 = entry.dataPos_dist1 = 0;
582 entry.dist_len2 = entry.dataPos_dist2 = 0;
583 entry.dist_len3 = entry.dataPos_dist3 = 0;
584 entry.dist_len4 = entry.dataPos_dist4 = 0;
585 entry.dist_len5 = entry.dataPos_dist5 = 0;
586 entry.rssi_len1 = entry.dataPos_rssi1 = 0;
587 entry.rssi_len2 = entry.dataPos_rssi2 = 0;
588
589 if(msg.data.dist_len1){
590 entry.dist_len1 = msg.data.dist_len1;
591 entry.dataPos_dist1 = dataFile_.tellp();
592
593 for (unsigned int i = 0 ; i < msg.data.dist_len1; ++i) {
594 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist1[i])), sizeof(uint16_t));
595 }
596
597 free(msg.data.dist1);
598 }
599 if(msg.data.dist_len2){
600 entry.dist_len2 = msg.data.dist_len2;
601 entry.dataPos_dist2 = dataFile_.tellp();
602
603 for (unsigned int i = 0 ; i < msg.data.dist_len2; ++i) {
604 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist2[i])), sizeof(uint16_t));
605 }
606
607 free(msg.data.dist2);
608 }
609 if(msg.data.dist_len3){
610 entry.dist_len3 = msg.data.dist_len3;
611 entry.dataPos_dist3 = dataFile_.tellp();
612
613 for (unsigned int i = 0 ; i < msg.data.dist_len3; ++i) {
614 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist3[i])), sizeof(uint16_t));
615 }
616
617 free(msg.data.dist3);
618 }
619 if(msg.data.dist_len4){
620 entry.dist_len4 = msg.data.dist_len4;
621 entry.dataPos_dist4 = dataFile_.tellp();
622
623 for (unsigned int i = 0 ; i < msg.data.dist_len4; ++i) {
624 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist4[i])), sizeof(uint16_t));
625 }
626
627 free(msg.data.dist4);
628 }
629 if(msg.data.dist_len5){
630 entry.dist_len5 = msg.data.dist_len5;
631 entry.dataPos_dist5 = dataFile_.tellp();
632
633 for (unsigned int i = 0 ; i < msg.data.dist_len5; ++i) {
634 dataFile_.write(reinterpret_cast<char*>(&(msg.data.dist5[i])), sizeof(uint16_t));
635 }
636
637 free(msg.data.dist5);
638 }
639 if(msg.data.rssi_len1){
640 entry.rssi_len1 = msg.data.rssi_len1;
641 entry.dataPos_rssi1 = dataFile_.tellp();
642
643 for (unsigned int i = 0 ; i < msg.data.rssi_len1; ++i) {
644 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi1[i])), sizeof(uint16_t));
645 }
646
647 free(msg.data.rssi1);
648 }
649 if(msg.data.rssi_len2){
650 entry.rssi_len2 = msg.data.rssi_len2;
651 entry.dataPos_rssi2 = dataFile_.tellp();
652
653 for (unsigned int i = 0 ; i < msg.data.rssi_len2; ++i) {
654 dataFile_.write(reinterpret_cast<char*>(&(msg.data.rssi2[i])), sizeof(uint16_t));
655 }
656
657 free(msg.data.rssi2);
658 }
659 // add a magic word to delimit the block of data
660 int32_t utcMagicWord = UTC_MAGIC_WORD;
661 dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
662
663 // write DBT
664 try {
665 dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLMS_dbt));
666 } catch (DbiteException & e) {
667 cerr << "error writing data: " << e.what() << endl;
668 return;
669 }
670
671}
672
673
674
675
676void SickLMSSensor::customEvent(QEvent * e)
677{
678 char answerMsg1[10];
679 sprintf(answerMsg1, "%c%s", 0x02, "sRA");
680 char answerMsg2[10];
681 sprintf(answerMsg2, "%c%s", 0x02, "sSN");
682
683 SickFrame * frame = ((SickFrameEvent*)e)->frame;
684
685 // we try to find some messages in the current packet + the pending bytes of the previous incoming data
686 reconstituteMessage(frame->msg, frame->size, frame->time);
687
688 // we delete the heap variable
689 delete frame;
690
691 // we test if we have some messages to decode
692 while ( !msgList.empty() )
693 {
694 // get the first (the eldest) message and process it
695 MessageLMS* msgToProcess = &(msgList.front());
696
697 // verify if the message is worth to be decoded
698 if(!strncmp(answerMsg1, msgToProcess->body, 4) || !strncmp(answerMsg2, msgToProcess->body, 4)){
699
700 if(splitMessage(msgToProcess) >= MINIMUM_PARAMETERS_MSG){
701
702 std::string type = msgToProcess->splitMessage->at(1);
703
704 LOG_TRACE("(Message type) "<< type);
705
706 if (type == SICKLMS_SCANDATA_MESSAGE)
707 {
708 LOG_TRACE("Scan data message !");
709
710 processScanData(msgToProcess);
711
712 // write data on the disk (dbt + utc)
713 if (recording_)
714 writeData(msgList.front());
715
716
717 #ifdef SICKLMS_SH_MEM
718 /// push data in shared memory
719 // First the scan info
720 SickLMS_shMem toWrite;
721 toWrite.time = msgToProcess.time;
722 toWrite.timerange = msgToProcess.timerange;
723 toWrite.scanInfo = msgToProcess.hScan;
724 shmem_->write(toWrite, sizeof(SickLMS_shMem));
725
726 // Then, the points
727 for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
728 shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
729 }
730 #endif
731 }
732 else if (type == SICKLMS_SCANCONFIG_MESSAGE){
733 LOG_TRACE("Scan configuration message !");
734
735 // get the values as (unsigned or signed) integer
736 mainCfg.scaningFrequency = xstol(msgToProcess->splitMessage->at(2));
737 mainCfg.angleResolution = xstol(msgToProcess->splitMessage->at(4));
738 mainCfg.startAngle = xstol(msgToProcess->splitMessage->at(5));
739 mainCfg.stopAngle = xstol(msgToProcess->splitMessage->at(6));
740 LOG_TRACE("(Scan config) Frequency : "<<mainCfg.scaningFrequency<<", Resolution : "<<mainCfg.angleResolution);
741 LOG_TRACE("(Scan config) Start angle : "<<mainCfg.startAngle<<", Stop angle : "<<mainCfg.stopAngle);
742 }
743 }
744
745 delete msgToProcess->splitMessage;
746 }
747
748
749 // (malloced memory) raw data no longer needed
750 free(msgToProcess->body);
751
752
753 // removes the processed item of the list
754 msgList.pop_front();
755 }
756}
757
758} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.