source: Sophya/trunk/Poubelle/archediab.old/archediab.sources/c/senseur_stellaire.c

Last change on this file was 649, checked in by ansari, 26 years ago

archediab 28 vol
File size: 16.7 KB
Line 
1#include "diabolo.h"
2#include "senseur_stellaire.h"
3#include "compress.h"
4#include "recons_sst.h"
5#include <math.h>
6
7#define M_PI 3.1415926535
8
9//-------------------- fonction exec de la fenetre : senseur_stellaire -------------------------------
10
11static int diodes[48];
12
13#define DIODE_UNUSED_1 3
14#define DIODE_UNUSED_2 7
15
16
17int diodesbuffer[dbufsz][48];int off[48];
18float zfoundstars[20];
19float mfoundstars[20];
20double tfoundstars[20];
21int nfoundstars;
22int seuilSST=5;
23
24static void init_sst_buffers(void);
25static void remove_diode_offset(void);
26static void find_stars(double secondes);
27
28static void trace_lin(int fen, double secondes);
29static void trace_radar(int fen, double secondes);
30static void init_radar(void);
31static void setup_gra(int fen);
32static void clean_radar(float phase,int fen);
33
34static double radarPer = 30.; // periode du radar, en secondes.
35static double radarTUp = 0; // le temps de reference pour pointage "NORD"
36static int radarSens = 1;
37static int sstHas2Bars = false;
38
39//static double tm = 0;
40
41void exec_senseur_stellaire(int fen,int item,double valeur,...)
42{
43if(item>1000) item-=1000; // pour appeler le case pour tous les cara d'un edit texte
44switch(item)
45 {
46 case ouverture :
47 selectgra(fen);
48 setup_gra(fen);
49 init_sst_buffers();
50 radarPer = litD(fenetre_senseur_stellaire,sst_periode,0);
51 radarTUp = litD(fenetre_senseur_stellaire,sst_phase,0);
52
53 break;
54 case fermeture :
55 break;
56 case sst_txt :
57 break;
58 case sst_twobars :
59 sstHas2Bars = litD(fenetre_senseur_stellaire,sst_twobars,0);
60 break;
61 case sst_radar :
62 setup_gra(fen);
63 break;
64 case sst_seuil : {
65 seuilSST = litD(fenetre_senseur_stellaire,sst_seuil,0);
66 if (seuilSST <=0) {
67 seuilSST = 1;
68 }
69 }
70 break;
71 case sst_periode :
72 case sst_sens_horaire:
73 case sst_phase : {
74 double sns = litD(fenetre_senseur_stellaire,sst_sens_horaire,0);
75 double phs = litD(fenetre_senseur_stellaire,sst_phase,0);
76 radarPer = litD(fenetre_senseur_stellaire,sst_periode,0);
77 radarTUp = phs/360. * radarPer;
78 selectgra(fen);
79 if (radarSens != (sns ? -1 : 1)) efface(fen);
80 radarSens = sns ? -1 : 1;
81 break;
82 }
83 case sst_efface :
84 setup_gra(fen);
85 efface(fen);
86 break;
87 case tache_de_fond:
88 selectgra(fen);
89 if(litD(fenetre_senseur_stellaire,sst_radar,0)) {
90 trace_radar(fen, valeur);
91 } else {
92 trace_lin(fen, valeur);
93 }
94 break;
95 default : break;
96 }
97}
98
99void setup_gra(fen) {
100 selectgra(fen);
101 if(litD(fenetre_senseur_stellaire,sst_radar,0)) {
102 graph->ymin=-60;
103 graph->ymax=60;
104 graph->ypas=0;
105 graph->xmin=-60;
106 graph->xmax=60;
107 graph->xpas=0;
108 graph->grille=0;
109 graph->graduations=0;
110 graph->taille_graduations=0;
111 graph->sans_image=1;
112 graph->avec_icones=0;
113 strcpy(graph->xtitre, "");
114 graph->ytitre[0]=0;
115 init_radar();
116 } else {
117 graph->ymin=0;
118 graph->ymax=48;
119 graph->ypas=5;
120 graph->xmin=0;
121 graph->xmax=240;
122 graph->xpas=30;
123 graph->grille=0;
124 graph->graduations=1;
125 graph->taille_graduations=10;
126 graph->sans_image=0;
127 graph->avec_icones=0;
128 strcpy(graph->xtitre, "T");
129 graph->ytitre[0]=0;
130 }
131 efface(fen);
132}
133
134#define seuil1 500
135#define seuil2 250
136#define seuil3 120
137#define seuil4 60
138#define seuil5 30
139#define seuil6 10
140
141#define frc(x) ((x) - (int)(x))
142
143void trace_lin(int fen, double secondes) {
144 int i;
145 //tm += gg->periode_echantillonage;
146 //if (tm>graph->xmax) tm = 0;
147 double tm = secondes - (int)(secondes/graph->xmax)*graph->xmax;
148 if(!litD(fenetre_senseur_stellaire,sst_etoiles,0)) {
149 for (i=0; i<48; i++) {
150 if(fabs(off[i])>1000.) continue;
151 if (-diodes[i] +off[i]> seuil1)
152 symbole(fen, tm, i, 5, rondplein, 0, rouge);
153 else if (-diodes[i]+off[i] > seuil2)
154 symbole(fen, tm, i, 4, rondplein, 0, rouge);
155 else if (-diodes[i]+off[i] > seuil3)
156 symbole(fen, tm, i, 3, rondplein, 0, rouge);
157 else if (-diodes[i]+off[i] > seuil4)
158 symbole(fen, tm, i, 2, rondplein, 0, rouge);
159 else if (-diodes[i]+off[i] > seuil5)
160 symbole(fen, tm, i, 2, point, 0, rouge);
161 else if (-diodes[i]+off[i] > seuil6)
162 symbole(fen, tm, i, 2, point, 0, noir);
163 }
164 } else {
165 for (i=0; i<nfoundstars; i++) {
166 float z = zfoundstars[i];
167 if (mfoundstars[i] > seuil1) {
168 symbole(fen, tm, z, 5, rondplein, 0, rouge);
169 } else if (mfoundstars[i] > seuil2) {
170 symbole(fen, tm, z, 4, rondplein, 0, rouge);
171 } else if (mfoundstars[i] > seuil3) {
172 symbole(fen, tm, z, 3, rondplein, 0, rouge);
173 } else if (mfoundstars[i] > seuil4) {
174 symbole(fen, tm, z, 2, rondplein, 0, rouge);
175 } else if (mfoundstars[i] > seuil5) {
176 symbole(fen, tm, z, 2, point, 0, rouge);
177 } else
178 symbole(fen, tm, z, 2, point, 0, noir);
179 }
180 }
181}
182
183#define NMAXRADARSTAR 1000
184
185struct radarStar {
186 float phase;
187 float rayon;
188 float flux;
189};
190
191static struct radarStar* radarstars = 0;
192static double lastPhase = 0;
193
194void init_radar() {
195 int i;
196 if (!radarstars) {
197 radarstars = (struct radarStar*) malloc(NMAXRADARSTAR*sizeof(struct radarStar));
198 }
199 for (i=0; i<NMAXRADARSTAR; i++) {
200 radarstars[i].phase = -1;
201 radarstars[i].rayon = -1;
202 }
203
204}
205
206
207// Nettoyage radar pour trace jusqu'a phase
208
209static void clean_radar(float phase, int fen) {
210 int i;
211 double x,y,r,th;
212 for (i=0; i<NMAXRADARSTAR; i++) {
213 // if (radarstars[i].phase >= 0 && radarstars[i].phase < 10 &&
214 // ((phase > lastPhase && radarstars[i].phase > lastPhase &&
215 // radarstars[i].phase <= phase) ||
216 // ((phase < lastPhase && (radarstars[i].phase > lastPhase ||
217 // radarstars[i].phase <= phase))))) {
218 if (radarstars[i].phase >= 0 && radarstars[i].phase < phase-1) {
219 r = (60-48) + radarstars[i].rayon;
220 th = (frc(radarstars[i].phase)*radarSens+.25) * 2 * M_PI ;
221 x = r * cos(th);
222 y = r * sin(th);
223 if (radarstars[i].flux > seuil1) {
224 symbole(fen, x, y, 5, rondplein, 0, blanc);
225 } else if (radarstars[i].flux > seuil2) {
226 symbole(fen, x, y, 4, rondplein, 0, blanc);
227 } else if (radarstars[i].flux > seuil3) {
228 symbole(fen, x, y, 3, rondplein, 0, blanc);
229 } else if (radarstars[i].flux > seuil4) {
230 symbole(fen, x, y, 2, rondplein, 0, blanc);
231 } else {
232 symbole(fen, x, y, 2, point, 0, blanc);
233 }
234 radarstars[i].phase = -1;
235 }
236 //if (radarstars[i].phase >= 10) {
237 // radarstars[i].phase -= 10;
238 //}
239 }
240}
241
242static long lastRadarTrace = 0;
243
244void trace_radar(int fen, double secondes) {
245 // On a des nouvelles etoiles...
246 double phase;
247 int i,j;
248 long tk;
249
250 phase = (secondes - radarTUp)/radarPer;
251 //phase = phase - (int)(phase);
252
253 for (i=0; i<nfoundstars; i++) {
254 for (j=0; j<NMAXRADARSTAR; j++) {
255 if (radarstars[j].phase < 0) {
256 radarstars[j].phase = phase;// +10; // >=10 : nouvelle...
257 radarstars[j].rayon = zfoundstars[i];
258 radarstars[j].flux = mfoundstars[i];
259 break;
260 }
261 }
262 }
263
264 // Le trace...
265 // mais on ne trace pas tout le temps...
266 tk = TickCount();
267 if (tk - lastRadarTrace < 10) return;
268 lastRadarTrace = tk;
269
270 if (litD(fenetre_senseur_stellaire,sst_autolock,0)) {
271 float per,phs,sns;
272 per = DonnePeriod();
273 phs = DonnePhase();
274 sns = DonneSens();
275 if (per>0) {
276 ecritD(fenetre_senseur_stellaire, sst_periode, "%8.3f",per);
277 radarPer = per;
278 }
279 if (phs>-9000) {
280 ecritD(fenetre_senseur_stellaire, sst_phase, "%8.1f",phs);
281 radarTUp = phs/360. * radarPer;
282 }
283 if (sns > -9000) {
284 radarSens = -sns;
285 ecritC(fenetre_senseur_stellaire, sst_sens_horaire, sns > 0);
286 }
287 }
288
289 //cercle(fen,-(60-48),-(60-48),(60-48),(60-48),jaune); // trace le cercle x1,y1 - x2,y2
290
291 modtrace(fen,1,blanc);
292 segment(fen, (60-48)*cos((lastPhase*radarSens+.25)*M_PI*2),
293 (60-48)*sin((lastPhase*radarSens+.25)*M_PI*2),
294 60*cos((lastPhase*radarSens+.25)*M_PI*2),
295 60*sin((lastPhase*radarSens+.25)*M_PI*2));
296
297 clean_radar(phase, fen);
298
299 modtrace(fen,1,jaune);
300 segment(fen, (60-48)*cos((frc(phase)*radarSens+.25)*M_PI*2),
301 (60-48)*sin((frc(phase)*radarSens+.25)*M_PI*2),
302 60*cos((frc(phase)*radarSens+.25)*M_PI*2),
303 60*sin((frc(phase)*radarSens+.25)*M_PI*2));
304
305 for (i=0; i<NMAXRADARSTAR; i++) {
306 if (radarstars[i].phase >= 0) {
307 float x,y,r,th;
308 r = (60-48) + radarstars[i].rayon;
309 th = (frc(radarstars[i].phase)*radarSens+.25) * 2 * M_PI ;
310 x = r * cos(th);
311 y = r * sin(th);
312 if (radarstars[i].flux > seuil1) {
313 symbole(fen, x, y, 5, rondplein, 0, rouge);
314 } else if (radarstars[i].flux > seuil2) {
315 symbole(fen, x, y, 4, rondplein, 0, rouge);
316 } else if (radarstars[i].flux > seuil3) {
317 symbole(fen, x, y, 3, rondplein, 0, rouge);
318 } else if (radarstars[i].flux > seuil4) {
319 symbole(fen, x, y, 2, rondplein, 0, rouge);
320 } else if (radarstars[i].flux > seuil5) {
321 symbole(fen, x, y, 2, point, 0, rouge);
322 } else
323 symbole(fen, x, y, 2, point, 0, noir);
324 }
325 }
326 lastPhase = frc(phase);
327
328}
329
330
331
332//#define bit_sst(i,j,k) (((blk->sst[i][i*3+k/4])>>(j+8*k%4))&1)
333
334
335// i est le numero du point dans le block de 72
336// j est le numero de la diode de 0 a 47
337// j%8 est la place du bit dans le mot de 8 bit
338// j/8 (de 0 a 5 ) est la serie de mesure
339// k est le numero du bit dans le mot de 12 bits
340// les k se suivent dans les données
341
342// k est le numero du point dans le block de 72
343// i est le numero de la diode de 0 a 47
344// j est le paquet de 4 bits de 0 a 2
345
346
347// on prend des paquets de 4 bits
348// chaque diode est formee de 3 paquets
349// dans l'ordre : les paquets de 8 diodes (8 paquets)
350// recommencer 3 fois (24 paquets)
351// enfin, faire 6 fois cette operation (144 paquets)
352
353// soit q la place du paquet i,j,k
354#define place_paquet(i,j) (i/8) * 24 + j*8 + (i%8)
355
356
357void decode_sst(block_type_sst* blk, int i, int* diodes); // diodes = tableau a 48 entrees
358
359void decode_sst(block_type_sst* blk, int i, int* diodes) {
360 int j; // 0-5 : numero du bloc de 8 diodes
361 int k; // 0-2 : indice du bloc de 4 bits (une diode = 12 bits = 3 blocs de 4 bits)
362 int l; // 0-7 : indice de la diode dans son bloc (8 diodes * 4 bits = 1 mot de 32 bits)
363
364 // numero de la diode (0-47) = j*8+l;
365 // indice dans le bloc sst du mot de 32 bits (0-17) = j*3+k;
366 // indice dans mot de 32 bits du premier bit utile = 4*l;
367
368 for (j=0; j<48; j++) diodes[j] = 0;
369
370 for (j=0; j<6; j++)
371 for (k=0; k<3; k++)
372 for (l=0; l<8; l++) {
373 long word = blk->sst[i][j*3+k];
374 word = (word >> (4*l)) & 0xF;
375 //printf("diode %d mot %d valeur %d\n", j*8+l, k, word);
376 diodes[j*8+l] = (diodes[j*8+l] << 4) + word;
377 }
378
379 //for (j=0; j<48; j++) if (diodes[j]>2047) diodes[j] -= 4096;
380 for (j=0; j<48; j++) diodes[j] -= 2048;
381}
382
383
384
385//#define place_paquet(i,j) ((i/8) * 24 + j*8 + (i%8) )
386
387void traite_block_sst_comprime (block_type_sst_comprime* blk){
388 block_type_sst blk2;
389 unsigned long sst_vrai[nb_per_block*2];
390 int j,jc,i,k;
391 unsigned long a,b0,b1,b2;
392
393
394 for (j=0; j<18; j++)
395 for (i=0; i<nb_per_block*2; i++)
396 blk2.sst[i][j] = 0;
397
398 jc=0;
399 for(j=0;j<48;j++) {
400 if ((j!=0) && (j!=4))
401 {
402 decompress_4_1((long*)blk->sst[jc],(long*)sst_vrai,nb_per_block*2);
403 for(k=0;k<nb_per_block*2;k++) {
404 b2 = sst_vrai[k] & 0xf;
405 b1 = (sst_vrai[k] >> 4) & 0xf;
406 b0 = (sst_vrai[k] >> 8) & 0xf;
407 a=place_paquet(j,0);
408 blk2.sst[k][a/8] |= (b0 << (a%8)*4);
409 a=place_paquet(j,1);
410 blk2.sst[k][a/8] |= (b1 << (a%8)*4);
411 a=place_paquet(j,2);
412 blk2.sst[k][a/8] |= (b2 << (a%8)*4);
413 }
414 jc++;
415 }
416 }
417 valide_block((block_type_modele*)&blk2,block_sst,numero_block(blk));
418 traite_block_sst(&blk2);
419}
420
421
422void traite_block_sst(block_type_sst* blk)
423{
424int i,k;
425//double y[15];
426//double x;
427
428int temps_cntl;
429double secondes;
430//int a,b,b0,b1,b2;
431//char tab[5000];
432
433for (i=0; i<nb_per_block*2; i++) {
434 k=0;
435 decode_sst(blk, i, diodes);
436 temps_cntl=numero_block(blk)*nb_per_block*2+i;
437 secondes = temps_cntl*gg->periode_echantillonage;
438
439 // 1. Suppression d'offset sur la rangee de diodes, et remise en ordre
440 remove_diode_offset();
441 // 2. Suppression des doubles impulsions et detection des etoiles
442 find_stars(secondes);
443 exec_recons_sst();
444
445 if(fenetre(fenetre_senseur_stellaire))
446 exec_senseur_stellaire(fenetre_senseur_stellaire,tache_de_fond,secondes);
447}
448}
449
450void init_sst_buffers(void) {
451 int i,j;
452 for (i=0; i<dbufsz; i++)
453 for (j=0; j<48; j++)
454 diodesbuffer[i][j] = 0;
455 nfoundstars = 0;
456}
457
458// sans objet a cause des filtres de l'electronique ?
459// se contente de permuter les diodes
460
461// diodpermut[i] = channel de la diode i
462static int diodpermut[46]=
463 { 8,24,40, 9,25,41,10,26,42,11,
464 27,43,16,32, 1,17,33, 2,18,34,
465 3,19,35,12,28,44,13,29,45,14,
466 30,46,15,31,47,20,36, 5,21,37,
467 6,22,38, 7,23,39};
468 // voies 0 et 4 non connectees, voie 1 en panne.
469
470void remove_diode_offset(void) {
471 int dd[46];
472 int i;
473 for (i=0; i<46; i++) {
474 dd[i] = diodes[i];
475 }
476 diodes[46] = diodes[47] = 0;
477 for (i=0; i<46; i++) {
478 diodes[i] = dd[diodpermut[i]];
479 }
480 return;
481/*
482 int i,j;
483 float m,sig;
484 // suppression des positions non utilisees. 3 et 7 ?
485 for (i=DIODE_UNUSED_1; i<46; i++)
486 diodes[i] = diodes[ i<DIODE_UNUSED_2-1 ? i+1 : i+2 ];
487
488 // calcul d'un fond sur la rangee. Moyenne clippee.
489 m = 0; sig = 1.e10;
490 for (i=0; i<2; i++) {
491 float s=0; float s2=0; int n=0;
492 for (j=0; j<46; j++) {
493 if (fabs(diodes[j]-m)<3*sig+1) {
494 s += diodes[j]; s2 += diodes[j]*diodes[j]; n++;
495 }
496 }
497 if (n>0) {
498 m = s/n; sig = sqrt(s2/n - m*m);
499 } else {
500 m = 0; break;
501 }
502 }
503 for (j=0; j<46; j++)
504 diodes[j] -= m;
505
506 diodes[46] = diodes[47] = 0;
507 */
508}
509
510
511void find_stars(double secondes) {
512 int i,j,feelasleep,sousoff;
513 float corrtemps,aufsete;
514 // Une etoile est validee seulement si impulsion dans meme canal
515 // ou dans canal juste au dessus dans les 4 echantillons qui precedent
516 // (en excluant le precedent).
517 // On demande aussi que le signal soit en train de remonter...
518 // en pratique on pourrait restreindre la contrainte avec une estimation
519 // de la vitesse de rotation. echantillon -2 ou -3...
520 nfoundstars = 0;
521 for (i=0; i<46; i++) {
522 // la diode 14 est morte (canal 1)...
523 if (i==14) continue;
524 aufsete=0.;feelasleep=0;
525 for (sousoff=0;sousoff<5;sousoff++)
526 {aufsete+=diodesbuffer[sousoff][i];feelasleep++;}
527 aufsete/=feelasleep;
528 off[i]=aufsete;
529 if ((diodes[i] -aufsete< -seuilSST ||
530 diodesbuffer[dbufsz-1][i]-aufsete< -seuilSST ||
531 diodesbuffer[dbufsz-2][i]-aufsete< -seuilSST)
532 && fabs(off[i])<1000.) {
533 if (sstHas2Bars) {
534 for (j=dbufsz-2; j>=0; j--) {
535 if (diodesbuffer[j][i] < -seuilSST) {
536 //printf("Found star same %d\n",dbufsz-j);
537 zfoundstars[nfoundstars] = i;
538 mfoundstars[nfoundstars] = diodes[i]-aufsete;
539 tfoundstars[nfoundstars] = secondes;
540 nfoundstars++;
541 if (nfoundstars >= MAXFOUNDSTARS) return;
542 break;
543 }
544 if (i < 45 && diodesbuffer[j][i+1] < -seuilSST) {
545 //printf("Found star decal %d\n",dbufsz-j);
546 zfoundstars[nfoundstars] = i+.5;
547 mfoundstars[nfoundstars] = diodes[i]-aufsete;
548 tfoundstars[nfoundstars] = secondes;
549 nfoundstars++;
550 if (nfoundstars >= MAXFOUNDSTARS) return;
551 break;
552 }
553 }
554 } else {
555 if ((diodes[i] > diodesbuffer[dbufsz-1][i])
556 && (diodesbuffer[dbufsz-1][i] <
557 .5*(diodesbuffer[dbufsz-2][i]+diodesbuffer[dbufsz-3][i]))) {
558 zfoundstars[nfoundstars] = i;
559 mfoundstars[nfoundstars] = -(diodesbuffer[dbufsz-1][i]+diodesbuffer[dbufsz-2][i])/2.+aufsete;
560 corrtemps=(diodesbuffer[dbufsz-2][i]-diodes[i])*gg->periode_echantillonage;
561 corrtemps=-corrtemps/
562 ((diodesbuffer[dbufsz-2][i]+diodesbuffer[dbufsz-1][i]+diodes[i])-3.*aufsete);
563// corrtemps=0;
564 tfoundstars[nfoundstars] = secondes+corrtemps;
565 nfoundstars++;
566 if (nfoundstars >= MAXFOUNDSTARS) return;
567 }
568 }
569 }
570 }
571
572 // remplissage buffer echantillons precedents
573 for (j=0; j<dbufsz-1; j++)
574 for (i=0; i<46; i++)
575 diodesbuffer[j][i] = diodesbuffer[j+1][i];
576 for (i=0; i<46; i++)
577 diodesbuffer[dbufsz-1][i] = diodes[i];
578}
579
580
581// Comparaison avec le GSC.
582// Preparer une carte pour une position et une heure.
Note: See TracBrowser for help on using the repository browser.