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

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