source: pacpusframework/branches/2.0-beta1/src/TestComponents/Lidar/sensor/AlascaComponent_bak.cpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 14.6 KB
Line 
1/******************************************************/
2/* ACQUISITION DE DONNES TELEMETRE ALASCA */
3/******************************************************/
4
5
6#include "sensor/AlascaComponent.h"
7#include "sensor/AlascaSocket.h"
8#include <qsocket.h>
9#include <qapplication.h>
10#include "kernel/ComponentFactory.h"
11#include <iostream.h>
12#include <string.h>
13
14
15// Construct the factory
16ComponentFactory<AlascaComponent>* factory = new ComponentFactory<AlascaComponent>("AlascaComponent");
17
18
19
20//Constructeur
21AlascaComponent::AlascaComponent(const char * name) : ComponentBase(name)
22{
23 A_socket = new AlascaSocket(this);
24 //QApplication::connect(A_socket, SIGNAL(configuration()), this, SLOT(Config()) );
25}
26
27
28//Destructeur
29AlascaComponent::~AlascaComponent()
30{
31 delete A_socket;
32}
33
34
35
36
37void AlascaComponent::stopActivity()
38{
39 A_socket->closeSocket();
40}
41
42
43
44void AlascaComponent::startActivity()
45{
46 A_socket->connectToServer("172.17.130.205", 12000); // demarrage du telemetre et/ou envoi des donnees sur ethernet
47}
48
49
50
51
52void AlascaComponent::configureComponent(XmlComponentConfig config)
53{
54 param.localCopy(config.qDomElement());
55}
56
57
58
59
60
61
62/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63long AlascaComponent::findMagicWord(const char * message, const int length)
64{
65 unsigned long magicWord=0x00000000;
66 unsigned long i=0;
67
68 if (length < 4)
69 return -1;
70
71 magicWord = ( (*(message+i+3)) & 0x000000FF ) +
72 ( ((*(message+i+2))<<8) & 0x0000FF00 ) +
73 ( ((*(message+i+1))<<16) & 0x00FF0000 ) +
74 ( ((*(message+i))<<24) & 0xFF000000 );
75
76 while ( magicWord !=0xAFFEC0C0)
77 {
78 i++;
79 magicWord = ( (*(message+i+3)) & 0x000000FF ) +
80 ( ((*(message+i+2))<<8) & 0x0000FF00 ) +
81 ( ((*(message+i+1))<<16) & 0x00FF0000 ) +
82 ( ((*(message+i))<<24) & 0xFF000000 );
83
84 if (i==length)
85 {
86 return -1;
87 }
88 else continue ;
89 }
90
91 return i;
92}
93
94
95
96
97///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
98 // decoupe le paquet en messages et remplit la liste de 'Message'
99 // remplit le message header
100 // remplit le scan header
101 // détermine si le message est complet (isFull)
102 // renvoie le nombre de pending bytes
103
104
105unsigned long AlascaComponent:: splitPacket(const char * packet, const int length)
106{
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131 unsigned long ptr = 0;
132 long index;
133 Message msg; // the current message of the list, often the last
134
135 if ( !msgList.empty() )
136 {
137 if (msgList.back().isIncomplete == 1)
138 msg = msgList.back();
139 }
140
141
142 // Seulement si c'est un nouveau mesage qui n'est pas encore dans la liste
143 // ou si le dernier message de la liste est complet
144 if (msg.isIncomplete == -1)
145 {
146 // on cherche le magicword
147 index = findMagicWord(packet + ptr,length - ptr);
148 if ( index != -1)
149 {
150 // on vérifie que l'on a le message header (16 octets )
151 if (index + 16 <= length)
152 {
153 fillMessageHeader(msg, packet, index );
154 // test si le message est complet "MH + SH + body"
155 if (index + 32 + msg.hMsg.size <= length)
156 {
157 // message complet
158 msg.isIncomplete = 0;
159 memcpy(msg.body, packet + index, index + 32 + msg.hMsg.size);
160 msg.bodyPtr = index + 32 + msg.hMsg.size;
161 }
162 else
163 {
164 // message incomplet MH + une partie de 'SH+body'
165 msg.isIncomplete = 1;
166 memcpy(msg.body, packet + index, length - index);
167 msg.bodyPtr = length - index;
168 }
169 }
170 else
171 {
172 // message incomplet - MW sans le MH
173 memcpy(msg.body, packet + index, length - index);
174 msg.isIncomplete = 1;
175 msg.bodyPtr = length - index;
176 }
177 }
178 else
179 {
180 // pas de magic word
181 memcpy(msg.body, packet + ptr, length - ptr );
182 msg.isIncomplete = 1;
183 msg.bodyPtr = length - index;
184 }
185 }
186 else
187 {
188 // on travaille sur le dernier message de la liste qui n'est pas complet
189 // msg.isIncomplete == 1
190 if (msg.hMsg.magicWord != 0)
191 {
192 // on a deja trouvé le MW
193
194 }
195 else
196 {
197 char * packetCopy = new char [length + msg.bodyPtr];
198 memcpy(packetCopy, msg.body, msg.bodyPtr);
199 memcpy(packetCopy + msg.bodyPtr, packet, length);
200 index = findMagicWord(packetCopy ,sizeof(packetCopy));
201
202 if (index == -1)
203 {
204 // pas de magic word
205 memcpy(msg.body, packetCopy, sizeof(packetCopy) );
206 msg.isIncomplete = 1;
207 msg.bodyPtr = sizeof(packetCopy);
208 }
209
210 if (index != 0)
211 qDebug("Error at line %d - file %s",__LINE__, __FILE__ );
212 else
213 {
214 // tester si on a le MH
215 if (index + 16 <= sizeof(packetCopy))
216 {
217 fillMessageHeader(msg, packetCopy, index );
218 // test si le message est complet "MH + SH + body"
219 if (index + 32 + msg.hMsg.size <= sizeof(packetCopy))
220 {
221 // message complet
222 msg.isIncomplete = 0;
223 memcpy(msg.body, packetCopy + index, index + 32 + msg.hMsg.size);
224 msg.bodyPtr = index + 32 + msg.hMsg.size;
225 }
226 else
227 {
228 // message incomplet MH + une partie de 'SH+body'
229 msg.isIncomplete = 1;
230 memcpy(msg.body, packetCopy + index, sizeof(packetCopy) - index);
231 msg.bodyPtr = sizeof(packetCopy) - index;
232 }
233 }
234 else
235 {
236 // message incomplet - MW sans le MH
237 memcpy(msg.body, packet + index, length - index);
238 msg.isIncomplete = 1;
239 msg.bodyPtr = length - index;
240 }
241
242 }
243
244 }
245
246
247
248//
249//
250// if ( !msgList.empty() )
251// {
252// msg = msgList.back();
253// if ( (msg.isIncomplete == 1) || (msg.isIncomplete == 0) )
254// {
255// // je vais en (1)
256// }
257// else if (msg.isIncomplete == 2)
258// {
259// // je vais en (2)
260// }
261// else if (msg.isIncomplete == 3)
262// {
263// // je vais en (3)
264// }
265// else if (msg.isIncomplete == 4)
266// {
267// // je vais en (4)
268// }
269// else
270// {
271// qWarning("Error in the message list ! A message has been found with an incorrect status");
272// return 0;
273// }
274// }
275// else
276// {
277// msgList.push_back(msg);
278// }
279
280 // tester dans la liste qu'il n'y a pas un message incomplet
281 // si oui => le compléter
282 // sinon on continue
283 /*
284 // (1)
285 index = findMagicWord(packet,length );
286 if ( index == -1)
287 {
288 return length;
289 }
290
291
292 // (2)
293 // test if the message header is complete (16 bytes)
294 if (index + 16 <= length)
295 {
296 }
297 else
298 {
299 return length;
300 }
301
302
303 // (3)
304 // test if the scan header is complete (16 bytes)
305 if (index + 32 <= length)
306 {
307 fillScanHeader(msg, packet, index )
308 }
309 else
310 return length;
311
312
313 // (4)
314 // test if the message body is complete ( index + 32 + msg.hScan.numPoints * 12 <= length)
315 if (index + 32 + msg.hScan.numPoints * 12 <= length)
316 {
317 msg.isIncomplete = 0;
318 memcpy(msg.body, packet + index + 32, msg.hScan.numPoints * 12);
319 }
320 else
321 {
322 msg.isIncomplete = 4;
323 // on copie ce qu'il reste dans le paquet
324 memcpy(msg.body, packet + index + 32, length - index + 32);
325 }
326
327
328
329
330
331
332
333
334 msgList.push_back(msg);
335
336
337/*
338
339
340 // tabmessages [0]=i;
341 // nbmessages=1;
342 ptr=i+4;
343 while ( ptr<length)
344 {
345 i= findMagicWord(packet+ptr,length-ptr);
346 if(i==-1)
347 {
348 break;
349 }
350
351 tabmessages[ nbmessages ]=i+ptr;
352 ptr = ptr + i + 4;
353 nbmessages++;
354 }
355
356
357 /// a voir
358return nbmessages;*/
359return 0;
360}
361}
362
363
364void AlascaComponent::fillScanHeader(Message &msg, const char * packet, const int magicWordIndex )
365{
366 msg.hScan.version = *(packet+magicWordIndex+16);
367 msg.hScan.scannerType = *(packet+magicWordIndex+17);
368 msg.hScan.ecuId = *(packet+magicWordIndex+18);
369 // byte 19 is a padding byte
370
371 msg.hScan.timeStamp = ((*(packet+magicWordIndex+23))&0x000000FF)+
372 ((*(packet+magicWordIndex+22)<<8)&0x0000FF00)+
373 ((*(packet+magicWordIndex+21)<<16)&0x00FF0000)+
374 ((*(packet+magicWordIndex+20)<<24)&0xFF000000);
375
376 msg.hScan.startAngle = ((*(packet+magicWordIndex+25))&0x00FF)+
377 ((*(packet+magicWordIndex+24)<<8)&0xFF00);
378
379 msg.hScan.endAngle = ((*(packet+magicWordIndex+27))&0x00FF)+
380 ((*(packet+magicWordIndex+26)<<8)&0xFF00);
381
382 msg.hScan.scanCounter = ((*(packet+magicWordIndex+29))&0x00FF)+
383 ((*(packet+magicWordIndex+28)<<8)&0xFF00);
384
385 msg.hScan.numPoints = ((*(packet+magicWordIndex+31))&0x00FF)+
386 ((*(packet+magicWordIndex+30)<<8)&0xFF00);
387}
388
389
390
391void AlascaComponent::fillMessageHeader(Message &msg, const char * packet, const int magicWordIndex )
392{
393 msg.hMsg.magicWord = ((*(packet+magicWordIndex+3))&0x000000FF) +
394 ((*(packet+magicWordIndex+2)<<8)&0x0000FF00) +
395 ((*(packet+magicWordIndex+1)<<16)&0x00FF0000)+
396 ((*(packet+magicWordIndex)<<24)&0xFF000000);
397
398 msg.hMsg.size = ((*(packet+magicWordIndex+7))&0x000000FF)+
399 ((*(packet+magicWordIndex+6)<<8)&0x0000FF00)+
400 ((*(packet+magicWordIndex+5)<<16)&0x00FF0000)+
401 ((*(packet+magicWordIndex+4)<<24)&0xFF000000);
402
403 msg.hMsg.dataType = ((*(packet+magicWordIndex+11))&0x000000FF)+
404 ((*(packet+magicWordIndex+10)<<8)&0x0000FF00)+
405 ((*(packet+magicWordIndex+9)<<16)&0x00FF0000)+
406 ((*(packet+magicWordIndex+8)<<24)&0xFF000000);
407
408 msg.hMsg.timestamp =((*(packet+magicWordIndex+15))&0x000000FF)+
409 ((*(packet+magicWordIndex+14)<<8)&0x0000FF00)+
410 ((*(packet+magicWordIndex+13)<<16)&0x00FF0000)+
411 ((*(packet+magicWordIndex+12)<<24)&0xFF000000);
412}
413
414
415int AlascaComponent::splitMessage(const char * message, const int length)
416{
417
418 /*
419 unsigned long i;
420 unsigned long compteur;
421
422
423
424 // verification que le message est complet
425 // msgheader.size = taille de Message Body (16 octets pour le message header)
426 if (msgheader.size + 16 != length)
427 {
428 qDebug(" msgheader.size (%d) != length (%d) ", msgheader.size + 16, length);
429 return 0;
430 }
431
432
433 // 15 = FILE_TYPE_COMPRESSED_SCAN
434 if ((msgheader.dataType )!= 0x0F) {
435 printf ("-"); // affiche - si ce n'est pas un message SCAN
436 return -1;
437 }
438 else
439 {
440 printf("+"); // affiche + quand il y a un nouveau message SCAN
441
442
443 compteur=32;
444
445 nbOfPtsInScanList = sc.nbPoint;
446 //printf("nbOfPtsInScanList = %d\n",nbOfPtsInScanList);
447
448 for (i=0;i< sc.nbPoint;i++)
449 {
450 scanList[i].layerNumber = ((*(message+compteur))&(0xFF));
451 scanList[i].x= ((*(message+compteur+5))&(0x00FF))+
452 ((*(message+compteur+4)<<8)&(0xFF00));
453
454
455 scanList[i].y= ((*(message+compteur+7))&(0x00FF))+
456 ( (*(message+compteur+6)<<8)&(0xFF00));
457
458
459 scanList[i].z= ((*(message+compteur+9))&(0x00FF))+
460 ((*(message+compteur+8)<<8)&(0xFF00));
461
462
463
464 scanList[i].width= ((*(message+compteur+11))&(0x00FF))+
465 ((*(message+compteur+10)<<8)&(0xFF00));
466
467
468 compteur+=12;
469
470 }
471
472
473 return sc.nbPoint;
474 } //fin de else
475*/
476
477 return 0;
478
479}
480
481
482
483
484//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
485void AlascaComponent::customEvent(QCustomEvent * e)
486{
487 unsigned long j,ptr,nbpoints,taille,i,size;
488 int nbmessages ;
489
490 int type = e->type();
491
492 FRAME * frame= (FRAME*)e->data();
493 /*
494 // le paquet complet
495 char *messg = new char [frame->size];
496
497 memcpy(messg, frame->msg, frame->size);
498 delete [] frame->msg;
499
500 // resultat de splitPacket
501 char *messgtemp=new char[frame->size];
502 j=0;
503 size=frame->size;
504
505
506 nbmessages=splitPacket(messg,size);
507 if (nbmessages==0)
508 {
509 printf ("0");
510 delete [] messgtemp;
511 delete [] messg;
512 return;
513 }
514
515 for (i=0;i<(nbmessages-1);i++)
516 {
517 ptr=0;
518 while (j<tabmessages[i+1])
519 {
520 memcpy(messgtemp+ptr,messg+j,1);
521 ptr++;
522 j++;
523 }
524 nbpoints= splitMessage(messgtemp,tabmessages[i+1]-tabmessages[i]);
525 }
526
527
528 // tester si les octets restants sont completes pour constituer un message
529 // sinon on leur memorise pour que la fois prochaine on leur complete
530 memcpy(restOfBytes,messg+tabmessages[nbmessages-1],size-tabmessages[nbmessages-1]);
531 if ((size-tabmessages[nbmessages-1])>=8)
532 {
533 taille = (restOfBytes[4])&0x000000FF+
534 (restOfBytes[5]<<8)&0x0000FF00+
535 (restOfBytes[6]<<16)&0x00FF0000+
536 (restOfBytes[7]<<24)&0xFF000000;
537
538 if ( taille <= (size-tabmessages[nbmessages-1]))
539 nbpoints= splitMessage(restOfBytes,size-tabmessages[nbmessages-1]);
540 }
541
542 delete [] messgtemp;
543 delete [] messg;
544
545 */
546}
547
548
549
550
551void AlascaComponent::Config()
552{
553 double CalOffsetX; //Postponement in x-direction.
554 double CalOffsetY; //Postponement in y-direction.
555 double CalOffsetZ; //Postponement in z-direction.
556 double HAngleOffset; //Rotation horizontale du télémètre.
557 double VAngleOffset; //Rotation verticale du télémètre.
558 double StartAngle; //Angle de début.
559 double EndAngle; //Angle de fin.
560 double ScanFrequency; //Configure la fréquence de scan du télémètre.
561 //bool UpsideDown; //Specifie si le télémètre est monté à l'envers.
562
563
564 cout<<"Configuration du telemetre"<<endl;
565 cout<<"Offset x:"<<endl; cin>>CalOffsetX;
566 cout<<"Offset y:"<<endl; cin>>CalOffsetY;
567 cout<<"Offset z:"<<endl;; cin>>CalOffsetZ;
568 cout<<"Rotation vericale:"<<endl; cin>>HAngleOffset;
569 cout<<"Rotation horizontale:"<<endl; cin>>VAngleOffset;
570 cout<<"Angle de debut:"<<endl; cin>>StartAngle;
571 cout<<"Angle de fin:"<<endl; cin>>EndAngle;
572 cout<<"Frequence:"<<endl; cin>>ScanFrequency;
573 cout<<"Telemetre monte a l'envers? Oui=1, Non=0 :"<<endl;
574 cout<<"Configuration enregistree"<<endl;
575
576 //Il faut ensuite envoyer les données au télémètre pour réaliser la configuration
577 //A_socket->sendToServer(...);
578 // a voir si utile de renvoyer de nouveau la configuration au telemetre
579
580
581 cout<<"Telemetre configure.\nRedemarrer l'ECU pour la prise en compte de la configuration\n";
582
583}
Note: See TracBrowser for help on using the repository browser.