source: Sophya/trunk/Poubelle/archTOI.old/starmatcher.cc@ 3554

Last change on this file since 3554 was 635, checked in by ansari, 26 years ago

warnings linux egcs, et struct Horloge
File size: 25.2 KB
Line 
1// starmatcher.cc
2// Eric Aubourg CEA/DAPNIA/SPP novembre 1999
3
4#include "starmatcher.h"
5#include "sststarfinder.h"
6#include "toimanager.h"
7#include "archexc.h"
8#include "archparam.h"
9#include "gondolageom.h"
10#include "polfitclip.h"
11
12#define STARDUMP
13
14#define TEchan TFin
15
16#include <math.h>
17
18extern "C" {
19#include "aa_hadec.h"
20#define NRANSI
21#include "nrutil.h"
22
23#ifndef M_PI
24#define M_PI 3.1415926535
25#endif
26
27void lfit(double x[], double y[], double sig[], int ndat, double a[], int ia[],
28 int ma, double **covar, double *chisq, void (*funcs)(double, double [], int));
29
30void polfunc(double x, double afunc[], int ma);
31void sinfunc(double x, double afunc[], int ma);
32}
33
34void polfunc(double x, double afunc[], int ma) {
35 afunc[1] = 1;
36 for (int i=2; i<=ma; i++)
37 afunc[i] = afunc[i-1]*x;
38}
39
40void sinfunc(double x, double afunc[], int /*ma*/) {
41 afunc[1] = cos(x);
42 afunc[2] = sin(x);
43 afunc[3] = 1;
44}
45
46
47double polval(double x, double a[], int ma);
48
49double polval(double x, double a[], int ma) {
50 double r = a[ma];
51 for (int i=ma-1; i>0; i--) {
52 r = r*x+a[i];
53 }
54 return r;
55}
56
57#include <stdio.h>
58
59#ifdef __DECCXX
60#define SWAP
61#endif
62#if defined(Linux) || defined(linux)
63#define SWAP
64#endif
65
66typedef unsigned int4 uint_4;
67typedef unsigned short uint_2;
68
69static inline void bswap4(void* p)
70{
71 uint_4 tmp = *(uint_4*)p;
72 *(uint_4*)p = ((tmp >> 24) & 0x000000FF) |
73 ((tmp >> 8) & 0x0000FF00) |
74 ((tmp & 0x0000FF00) << 8) |
75 ((tmp & 0x000000FF) << 24);
76}
77
78static inline void bswap2(void* p)
79{
80 uint_2 tmp = *(uint_2*)p;
81 *(uint_2*)p = ((tmp >> 8) & 0x00FF) |
82 ((tmp & 0x00FF) << 8);
83}
84
85
86#define azimuthPendul "azimuthPendul"
87#define anglePendul "anglePendul"
88#define azimuthAxis "azimuthAxis"
89#define elvAxis "deltaZenith"
90#define alphaAxis "alphaZenith"
91#define deltaAxis "deltaZenith"
92#define azimuthFPC "azimuthFPC"
93#define elvFPC "elvFPC"
94#define alphaFPC "alphaFPC"
95#define deltaFPC "deltaFPC"
96#define azimuthBolo "azimuthBolo"
97#define elvBolo "elvBolo"
98#define alphaBolo "alphaBolo"
99#define deltaBolo "deltaBolo"
100#define azimuthSST "azimuthSST"
101#define elvSST "elvSST"
102#define alphaSST "alphaSST"
103#define deltaSST "deltaSST"
104
105
106StarMatcher::StarMatcher() {
107 possibleTOIs.insert(TOI(azimuthSST, TOI::unspec, "interp", "degrees","sstmatch"));
108 possibleTOIs.insert(TOI(elvSST, TOI::unspec, "interp", "degrees","sstmatch"));
109 possibleTOIs.insert(TOI(alphaSST, TOI::unspec, "interp", "hours","sstmatch"));
110 possibleTOIs.insert(TOI(deltaSST, TOI::unspec, "interp", "degrees","sstmatch"));
111 possibleTOIs.insert(TOI(azimuthAxis, TOI::unspec, "interp", "degrees","sstmatch"));
112 possibleTOIs.insert(TOI(elvAxis, TOI::unspec, "interp", "degrees","sstmatch"));
113 possibleTOIs.insert(TOI(alphaAxis, TOI::unspec, "interp", "hours","sstmatch"));
114 possibleTOIs.insert(TOI(deltaAxis, TOI::unspec, "interp", "degrees","sstmatch"));
115 possibleTOIs.insert(TOI(azimuthPendul, TOI::unspec, "interp", "degrees","sstmatch"));
116 possibleTOIs.insert(TOI(anglePendul, TOI::unspec, "interp", "degrees","sstmatch"));
117 possibleTOIs.insert(TOI(azimuthFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
118 possibleTOIs.insert(TOI(elvFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
119 possibleTOIs.insert(TOI(alphaFPC, TOI::unspec, "interp", "hours", "sstmatch"));
120 possibleTOIs.insert(TOI(deltaFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
121 possibleTOIs.insert(TOI(azimuthBolo, TOI::all, "interp", "degrees", "sstmatch"));
122 possibleTOIs.insert(TOI(elvBolo, TOI::all, "interp", "degrees", "sstmatch"));
123 possibleTOIs.insert(TOI(alphaBolo, TOI::all, "interp", "hours", "sstmatch"));
124 possibleTOIs.insert(TOI(deltaBolo, TOI::all, "interp", "degrees", "sstmatch"));
125
126 FILE* f;
127
128 f = fopen("gsc7.dat","r");
129 if (!f) throw ArchExc("Error opening gsc7.dat");
130
131 int4 n4;
132 fread(&n4, sizeof(int4), 1 , f);
133
134#ifdef SWAP
135 bswap4(&n4);
136#endif
137 nstars = n4;
138
139 stars = new gscStar[nstars];
140 char* compdata = new char[10*nstars];
141 fread(compdata, 10, nstars, f);
142 fclose(f);
143
144 for (int i=0; i<nstars; i++) {
145#ifdef SWAP
146 ((char*)&(stars[i].ra))[0] = compdata[10*i+3];
147 ((char*)&(stars[i].ra))[1] = compdata[10*i+2];
148 ((char*)&(stars[i].ra))[2] = compdata[10*i+1];
149 ((char*)&(stars[i].ra))[3] = compdata[10*i+0];
150 ((char*)&(stars[i].dec))[0] = compdata[10*i+7];
151 ((char*)&(stars[i].dec))[1] = compdata[10*i+6];
152 ((char*)&(stars[i].dec))[2] = compdata[10*i+5];
153 ((char*)&(stars[i].dec))[3] = compdata[10*i+4];
154 ((char*)&(stars[i].mag))[0] = compdata[10*i+9];
155 ((char*)&(stars[i].mag))[1] = compdata[10*i+8];
156#else
157 ((char*)&(stars[i].ra))[0] = compdata[10*i+0];
158 ((char*)&(stars[i].ra))[1] = compdata[10*i+1];
159 ((char*)&(stars[i].ra))[2] = compdata[10*i+2];
160 ((char*)&(stars[i].ra))[3] = compdata[10*i+3];
161 ((char*)&(stars[i].dec))[0] = compdata[10*i+4];
162 ((char*)&(stars[i].dec))[1] = compdata[10*i+5];
163 ((char*)&(stars[i].dec))[2] = compdata[10*i+6];
164 ((char*)&(stars[i].dec))[3] = compdata[10*i+7];
165 ((char*)&(stars[i].mag))[0] = compdata[10*i+8];
166 ((char*)&(stars[i].mag))[1] = compdata[10*i+9];
167#endif
168 }
169
170 delete[] compdata;
171
172 TOIProducer* prod = TOIManager::findTOIProducer(TOI("sstStarCount"));
173 if (!prod) {
174 cerr << "StarMatcher : cannot find producer for sstStarCount" << endl;
175 exit(-1);
176 }
177
178 SSTStarFinder* sprod = dynamic_cast<SSTStarFinder*>(prod);
179 if (!sprod) {
180 cerr << "StarMatcher : producer for sstStarCount is not a SSTStarFinder" << endl;
181 exit(-1);
182 }
183
184 lastSeq = 0;
185
186 sprod->registerProcessor(this);
187
188}
189
190string StarMatcher::getName() {
191 return("StarMatcher 1.0");
192}
193
194#ifdef STARDUMP
195static ofstream starstream("stars.dat");
196static ofstream cstarstream("cstars.dat");
197static ofstream pendstream("pendul.dat");
198#endif
199static ofstream logstream("starmatch.log");
200
201void StarMatcher::dataFeed(SSTEtoile const& x) {
202 lastStars.push_back(x);
203}
204
205static long lastCleanSave=0;
206
207void nrerror(char * error_text) {
208 throw(string(error_text));
209}
210
211
212void StarMatcher::processStars() {
213
214 if (lastStars.empty()) return;
215
216 map<TOI, TOIProducer*> & m = (*neededTOIs.begin()).second;
217 while (!lastStars.empty()) {
218 SSTEtoile lastStar = lastStars.front();
219 lastStars.pop_front();
220
221 double lat, lon, ts, alpha, delta, az, rspeed;
222
223 long snstar = (long) lastStar.TEchan;
224
225 for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
226 TOI const& inToi = (*i).first;
227 TOIProducer* prod = (*i).second;
228 if (inToi.name == "latitude") lat = prod->getValue(snstar, inToi);
229 if (inToi.name == "longitude") lon = prod->getValue(snstar, inToi);
230 if (inToi.name == "tsid") ts = prod->getValue(snstar, inToi);
231 if (inToi.name == "alphaSST") alpha = prod->getValue(snstar, inToi);
232 if (inToi.name == "deltaSST") delta = prod->getValue(snstar, inToi);
233 if (inToi.name == "azimuthSST") az = prod->getValue(snstar, inToi);
234 if (inToi.name == "rotSpeed") rspeed = prod->getValue(snstar, inToi);
235 }
236
237 // correct azimuth using fractional value of TEchan
238
239 az -= rspeed * archParam.acq.perEch * (lastStar.TEchan-snstar);
240
241 // find all stars +- 2 deg boresight
242 double dist = 2;
243 double dmin = delta - dist; if (dmin<-90) dmin=-90;
244 double dmax = delta + dist; if (dmax> 90) dmax= 90;
245 double amin = alpha - dist / cos(delta * M_PI/180) / 15.;
246 if (amin<0) amin += 24;
247 double amax = alpha + dist / cos(delta * M_PI/180) / 15.;
248 if (amax>24) amax -= 24;
249
250 int a,b,c;
251 a=0; c=nstars-1;
252 while (a+1<c) {
253 b = (a+c)/2;
254 if (stars[b].dec < dmin) a=b; else c=b;
255 }
256 int imin = a;
257 a=0; c=nstars;
258 while (a+1<c) {
259 b = (a+c)/2;
260 if (stars[b].dec < dmax) a=b; else c=b;
261 }
262 int imax = c;
263
264 for (int i=imin; i<=imax; i++) {
265 if (stars[i].ra >= amin && stars[i].ra <= amax) {
266 double ha = (ts/3600. - stars[i].ra) * 15. * M_PI/180.;
267 double elv, azim;
268 hadec_aa(lat * M_PI/180., ha, stars[i].dec * M_PI/180.,
269 &elv, &azim);
270 elv *= 180/M_PI;
271 azim *= 180/M_PI;
272 if (azim<0) azim += 360;
273
274 double da = azim-az; if (da>360) da -= 360;
275 // if (da < -0.6 || da > 0.4) continue; // appropriate for TEchan
276 if (da < -0.7 || da > 0.3) continue; // appropriate for TFin
277 double elv0 = elv - GondolaGeom::sstPixelHeight * lastStar.NoDiode;
278 if (fabs(elv0-GondolaGeom::elevSST0) > 0.25) continue; // Might be too strong
279
280#ifdef STARDUMP
281 starstream << setprecision(10) << lastStar.TEchan << " " <<
282 lastStar.NoDiode << " " <<
283 alpha << " " << delta << " " <<
284 az << " " <<
285 stars[i].ra << " " << stars[i].dec << " " <<
286 elv << " " << azim << " " <<
287 lastStar.InpCurrent << " " << stars[i].mag << "\n";
288#endif
289
290 matchStar s;
291 lastSeq++;
292 s.SN = lastStar.TEchan;
293 s.raGSC = stars[i].ra;
294 s.decGSC = stars[i].dec;
295 s.azGSC = azim;
296 s.elvGSC = elv;
297 s.nDiode = lastStar.NoDiode;
298 s.ok = true;
299 s.seq = lastSeq;
300 s.lon = lon;
301 s.lat = lat;
302 s.ts = ts;
303
304 matchStars.push_back(s);
305 }
306 }
307 }
308
309 // new set of matched stars... Clean, and get parameters...
310 // We don't want more than 30 seconds of data
311
312 if (matchStars.empty()) return;
313
314
315 double snEnd = matchStars.back().SN;
316 deque<matchStar>::iterator it;
317 for (it = matchStars.begin(); it!=matchStars.end(); it++) {
318 if ((snEnd - (*it).SN)*archParam.acq.perEch < 30 ||
319 (*it).seq > lastCleanSave)
320 break;
321 }
322 if (it != matchStars.begin()) {
323 it--;
324 }
325 if (it != matchStars.begin()) {
326 matchStars.erase(matchStars.begin(), it);
327 }
328
329 // we want to clean on the last 5 seconds of data.
330
331 int nskip=0;
332 for (it = matchStars.begin(); it!=matchStars.end(); it++) {
333 if ((snEnd - (*it).SN)*archParam.acq.perEch < 5)
334 break;
335 nskip++;
336 }
337
338 if (matchStars.size()-nskip < 30) return; // pas assez d'etoiles
339
340 // we remove "bursts" of stars, ie more than 4 stars in the same samplenum
341
342 double lastSN = 0;
343 deque<matchStar>::iterator lastIt = it;
344 long burstLen = 0;
345 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
346 matchStar s = (*it1);
347 if ((long) s.SN == lastSN) {
348 burstLen++;
349 continue;
350 }
351 if (burstLen >= 4) {
352 for (deque<matchStar>::iterator it2=lastIt; it2 != it1; it2++) {
353 //if ((*it2).ok)
354 // logstream << " kill " << (*it2).seq << " " << setprecision(11) << (*it2).SN << " burst" << '\n';
355 (*it2).ok=false;
356 }
357 }
358 burstLen = 1;
359 lastIt = it1;
360 lastSN = s.SN;
361 }
362 // we fit the data to a polynomial, with clipping...
363
364 //double* sn = ::dvector(1, matchStars.size());
365 double* elv0 = ::dvector(1, matchStars.size());
366 double* azi = ::dvector(1, matchStars.size());
367 double* sig = ::dvector(1, matchStars.size());
368 //double* ae = ::dvector(1, 3);
369 double* aa = ::dvector(1, 3);
370 int* ia = ivector(1, 3);
371 double** cov = matrix(1, 3, 1, 3);
372 int ndata;
373
374 //long sn0 = matchStars.front().SN;
375 double sn0 = (*it).SN;
376 PolFitClip2 fitElvAz(matchStars.size(), 2); fitElvAz.setClip(0.1,0,2,3);
377 ndata = 0;
378
379 double oldAz = -1;
380 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
381 matchStar s1 = (*it1);
382 if (!s1.ok) continue;
383 double az = s1.azGSC;
384 if (ndata > 0 && az - oldAz > 180) az -= 360;
385 if (ndata > 0 && az - oldAz < -180) az += 360;
386 fitElvAz.addData(s1.SN-sn0, s1.elvGSC - s1.nDiode*GondolaGeom::sstPixelHeight, az);
387 oldAz = az;
388 ndata++;
389 }
390
391 double celv[3], caz[3];
392 if (fitElvAz.doFit(celv,caz)) return;
393 //if (fitElvAz.doFit()) return;
394
395 //logstream << "*** Fit sig=" << fitElvAz.getSigmaY() << " " << fitElvAz.getSigmaZ()
396 // << " n =" << fitElvAz.getNData() << " " << fitElvAz.getNDataUsed()
397 // << " SN :" << fitElvAz.getXMin() + sn0 << " - " << fitElvAz.getXMax() + sn0 << '\n';
398 //logstream << " sn0 = " << sn0 << "; snmin =" << fitElvAz.getXMin() + sn0 << "; snmax ="
399 // << fitElvAz.getXMax() + sn0 << '\n';
400 //logstream << " fitelv[x_] := " << celv[2] << " x^2 + " << celv[1] << " x + " << celv[0] << '\n';
401 //logstream << " fitaz[x_] := " << caz[2] << " x^2 + " << caz[1] << " x + " << caz[0] << '\n';
402
403 //if (fitElvAz.getSigmaY() > 0.05 || fitElvAz.getSigmaZ() > 0.05) return;
404 if (fitElvAz.getNDataUsed() < 5 ||
405 (double)fitElvAz.getNDataUsed()/fitElvAz.getNData() < .5) return;
406
407 double dcutElv = fitElvAz.getSigmaY()*3;
408 double dcutAz = fitElvAz.getSigmaZ()*3;
409
410 if (dcutElv < 0.05) dcutElv = 0.05;
411 if (dcutAz < 0.05) dcutAz = 0.05;
412
413 // don't kill borders of fit....
414 //if (matchStars.end() - it > 6)
415 // for (deque<matchStar>::iterator it1 = it+3 ; it1!=matchStars.end()-3; it1++) {
416 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
417 matchStar sss = (*it1);
418 if (!sss.ok) continue;
419 if (fabs(fitElvAz.valueY(sss.SN-sn0)-
420 (sss.elvGSC - sss.nDiode*GondolaGeom::sstPixelHeight)) > dcutElv) {
421 (*it1).ok = false;
422 //logstream << " kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
423 // << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << '\n';
424 continue;
425 }
426 double daz = fitElvAz.valueZ(sss.SN-sn0) - sss.azGSC;
427 if (daz>=180) daz -= 360;
428 if (daz<-180) daz += 360;
429 if (fabs(daz) > dcutAz) (*it1).ok = false;
430 if (!(*it1).ok) {
431 //logstream << " kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
432 // << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << " "
433 // << daz << '\n';
434 }
435 }
436
437 bool gotNewStars = false;
438 for (deque<matchStar>::iterator it1 = matchStars.begin() ; it1!=it; it1++) {
439 if ((*it1).ok && (*it1).seq > lastCleanSave) {
440 gotNewStars = true;
441 lastCleanSave = (*it1).seq;
442#ifdef STARDUMP
443 cstarstream << (*it1).seq << "\n";
444#endif
445 posInfo info;
446 info.SN = (*it1).SN;
447 info.azStar = (*it1).azGSC;
448 info.elvStar = (*it1).elvGSC;
449 info.diodStar= (*it1).nDiode;
450 info.lon = (*it1).lon;
451 info.lat = (*it1).lat;
452 info.ts = (*it1).ts;
453 posInfos[info.SN] = info;
454 }
455 }
456
457 if (!gotNewStars) return;
458
459 // On a des etoiles nettoyees, on va trouver amplitude et phase du
460 // signal en elevation, ce qui va nous donner les deux angles d'Euler
461 // de la pendulation (au premier ordre en theta)
462
463 // Il faut avoir une periode entiere ou pas loin, sinon on ne peut
464 // rien dire simplement.... -> we want to run on the last 18 seconds of
465 // data before the last fully cleaned star (it).
466
467 deque<matchStar>::iterator itstart;
468
469 for (itstart = matchStars.begin(); itstart != it; itstart++) {
470 if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 19)
471 break;
472 }
473
474 if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 15) return;
475
476
477 // it = matchStars.end(); it--;
478 // if (((*it).SN - matchStars.front().SN)*archParam.acq.perEch < 17) return;
479
480 // $CHECK$ utiliser plutot le SN moyen/median de tous les points effectivement utilises.
481 double snmid = ((*it).SN + (*itstart).SN)/2;
482
483 ndata=0;
484 double snmean = 0;
485
486 logstream << "PendFit : " << setprecision(11) << (*itstart).SN << '-' << (*it).SN << " "
487 << setprecision(4)
488 << ((*it).SN - (*itstart).SN)*archParam.acq.perEch << " " ;
489
490 for (deque<matchStar>::iterator it1 = itstart ; it1!=it; it1++) {
491 matchStar st = *it1;
492 if (!st.ok) continue;
493 ndata++;
494 snmean += st.SN;
495 azi[ndata] = st.azGSC * M_PI/180;
496 elv0[ndata] = st.elvGSC - st.nDiode*GondolaGeom::sstPixelHeight;
497 sig[ndata] = 0.01;
498 }
499 if (ndata) snmean /= ndata;
500
501 ia[1] = ia[2] = 1;
502 ia[3] = 0;
503 aa[3] = GondolaGeom::elevSST0;// do not fit elv0
504
505 if (ndata<5) return;
506 double chi2;
507 try {
508 lfit(azi, elv0, sig, ndata, aa, ia, 3, cov, &chi2, sinfunc);
509 } catch(string st) {
510 return;
511 }
512
513 double cc = aa[1];
514 double ss = aa[2];
515
516 logstream << setprecision(11) << snmean << setprecision(4)
517 << " cs=" << cc << " " << ss << " chi2r=" << chi2/ndata
518 << " cov " << cov[1][1] << " " << cov[2][2] << '\n';
519
520 // Get rid of bad fits. The cuts are rather ad hoc
521
522 //if (aa[3] < 39.64 || aa[3] > 39.68) return;
523 if (chi2/ndata > 9) return;
524 if (cov[1][1] > 0.0001) return;
525 if (cov[2][2] > 0.0001) return;
526
527 double ampl = sqrt(cc*cc+ss*ss);
528 double phase = atan2(cc,ss)/(M_PI/180);
529
530
531 pendulInfo info;
532 info.SN = snmean;
533 info.azPendul = 180-phase;
534 if (info.azPendul > 360) info.azPendul -= 360;
535 if (info.azPendul < 0) info.azPendul += 360;
536 info.angPendul = ampl;
537 pendulInfos[info.SN] = info;
538
539#ifdef STARDUMP
540 pendstream << setprecision(11) << snmean << " "
541 << setprecision(4) << ampl << " " << info.azPendul << " " << ndata << " " << chi2/ndata << " "
542 << cov[1][1] << " " << cov[2][2] << '\n';
543#endif
544 /*
545 double snum = (matchStars.front().SN + matchStars.back().SN)/2-sn0;
546 if (snmin > snum || snmax < snum) return;
547 double elsst = polval(snum, ae, 3);
548 double azsst = polval(snum, aa, 3);
549
550 if (azsst > 360) azsst -= 360;
551 if (azsst < 0 ) azsst += 360;
552 */
553
554// for (set<TOI>::iterator i = producedTOIs.begin(); i!=producedTOIs.end(); i++) {
555// if ((*i).name == "azimuthSST") computedValue((*i), snum+sn0, azsst);
556// if ((*i).name == "elvSST") computedValue((*i), snum+sn0, elsst);
557// }
558
559 //free_vector(sn, 1, matchStars.size());
560 free_vector(elv0, 1, matchStars.size());
561 free_vector(azi, 1, matchStars.size());
562 free_vector(sig, 1, matchStars.size());
563 //free_vector(ae, 1, 3);
564 free_vector(aa, 1, 3);
565 free_ivector(ia, 1, matchStars.size());
566 free_matrix(cov, 1, 3, 1, 3);
567}
568
569
570// $CHECK$ do a polynomial fit with several points...
571int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
572
573 static double lastSN = -1;
574 static pendulInfo lastPendul;
575
576 if (sampleNum == lastSN) {
577 info = lastPendul;
578 return 0;
579 }
580
581 PolFitClip2 fitPendul(30,2);
582
583 map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
584 if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
585 if (i == pendulInfos.end()) return -1;
586 map<double, pendulInfo>::iterator last = pendulInfos.end();
587 if (last == pendulInfos.begin()) return -1;
588 last--;
589 if (i == last && (*i).second.SN <= sampleNum) return -1;
590
591 if ((*i).second.SN > sampleNum) i--; // i just before us...
592
593 //$CHECK$ reject if too large a gap...
594 if (sampleNum - (*i).second.SN > 1000) return -1;
595 last = i; last++;
596 if ((*last).second.SN - sampleNum > 1000) return -1;
597
598 int nn=0;
599 double aziprev=0, azicur=0, azi0=0;
600 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
601 pendulInfo inf1 = (*ii).second;
602 if (fabs(inf1.SN - sampleNum) > 1000) continue;
603 aziprev = azicur;
604 azicur = inf1.azPendul;
605 nn++;
606 if (nn==1) azi0 = azicur;
607 if (nn>1 && azicur - aziprev > 180) azicur -= 360;
608 if (nn>1 && azicur - aziprev < -180) azicur += 360;
609 fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
610 if (nn>=5) break;
611 }
612
613 azicur = azi0;
614 if (i != pendulInfos.end()) i++;
615 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
616 pendulInfo inf1 = (*ii).second;
617 if (fabs(inf1.SN - sampleNum) > 1000) continue;
618 aziprev = azicur;
619 azicur = inf1.azPendul;
620 nn++;
621 if (nn>1 && azicur - aziprev > 180) azicur -= 360;
622 if (nn>1 && azicur - aziprev < -180) azicur += 360;
623 fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
624 if (nn>=10) break;
625 }
626
627 if (fitPendul.doFit()) return -1;
628
629 info.SN = sampleNum;
630 info.azPendul = fitPendul.valueZ(sampleNum);
631 if (info.azPendul > 360) info.azPendul -= 360;
632 if (info.azPendul < 0) info.azPendul += 360;
633 info.angPendul = fitPendul.valueY(sampleNum);
634
635 lastSN = sampleNum;
636 lastPendul = info;
637
638 return 0;
639}
640
641
642double StarMatcher::getValue(long sampleNum, TOI const& toi) {
643 processStars();
644
645 // 1. Interpoler la valeur de pendulation
646 // 2. Interpoler la position en azimuth avec les etoiles encadrant
647
648 pendulInfo pendul;
649 int rc = getPendulInfo(sampleNum, pendul);
650 if (rc) return -99999;
651 if (toi.name == azimuthPendul) return pendul.azPendul;
652 if (toi.name == anglePendul) return pendul.angPendul;
653
654 // find nearest matched star
655 map<double, posInfo>::iterator i = posInfos.lower_bound(sampleNum);
656 if (i == posInfos.begin() && (*i).second.SN >= sampleNum) return -1;
657 if (i == posInfos.end() && (*i).second.SN <= sampleNum) return -1;
658 if ((*i).second.SN > sampleNum) i--;
659
660 // $CHECK$ if time spent here, can keep a GondolaGeom object for several
661 // samples...
662 GondolaGeom geom;
663 geom.setEarthPos((*i).second.lon,(*i).second.lat);
664 geom.setTSid((*i).second.ts);
665 geom.setPendulation(pendul.azPendul, pendul.angPendul);
666
667 int ns=0;
668 for (map<double, posInfo>::iterator it=i; it != posInfos.end(); it++) {
669 posInfo s = (*it).second;
670 double delsn = s.SN - sampleNum;
671 ns++;
672 //if (delsn * archParam.acq.perEch > 1 && ns > 4) break;
673 if (delsn * archParam.acq.perEch > 5) break;
674 geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
675 }
676
677 if (i != posInfos.begin()) i--;
678 ns = 0;
679 for (map<double, posInfo>::iterator it=i; it != posInfos.begin(); it--) {
680 posInfo s = (*it).second;
681 double delsn = s.SN - sampleNum;
682 ns++;
683 //if (-delsn * archParam.acq.perEch > 1 && ns > 4) break;
684 if (-delsn * archParam.acq.perEch > 5) break;
685 geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
686 }
687
688 if (geom.solveStars()) return -99999;
689
690 if (toi.name == azimuthAxis) return geom.getAzimutAxis();
691 if (toi.name == elvAxis) return geom.getElvAxis();
692 if (toi.name == alphaAxis) return geom.getAlphaAxis();
693 if (toi.name == deltaAxis) return geom.getDeltaAxis();
694
695 if (toi.name == azimuthSST) return geom.getAzimutSST();
696 if (toi.name == elvSST) return geom.getElvSST();
697 if (toi.name == alphaSST) return geom.getAlphaSST();
698 if (toi.name == deltaSST) return geom.getDeltaSST();
699
700 if (toi.name == azimuthFPC) return geom.getAzimutCenter();
701 if (toi.name == elvFPC) return geom.getElvCenter();
702 if (toi.name == alphaFPC) return geom.getAlphaCenter();
703 if (toi.name == deltaFPC) return geom.getDeltaCenter();
704
705 if (toi.name == azimuthBolo) return geom.getAzimutBolo(toi.index);
706 if (toi.name == elvBolo) return geom.getElvBolo(toi.index);
707 if (toi.name == alphaBolo) return geom.getAlphaBolo(toi.index);
708 if (toi.name == deltaBolo) return geom.getDeltaBolo(toi.index);
709
710 return -99999;
711}
712
713bool StarMatcher::canGetValue(long sampleNum, TOI const& /*toi*/) {
714 processStars();
715
716 map<double, pendulInfo>::iterator i = pendulInfos.begin();
717 if (i == pendulInfos.end()) return false;
718 if (sampleNum < (*i).second.SN) return false;
719 i = pendulInfos.end(); i--;
720 if (sampleNum > (*i).second.SN) return false;
721
722 return true;
723}
724
725bool StarMatcher::canGetValueLater(long sampleNum, TOI const& /*toi*/) {
726 processStars();
727
728 map<double, pendulInfo>::iterator i = pendulInfos.end();
729 if (i == pendulInfos.begin()) return true;
730 i--;
731 return (sampleNum+4000> (*i).second.SN);
732}
733
734
735
736set<TOI> StarMatcher::reqTOIFor(TOI const&) {
737 set<TOI> t;
738 t.insert(TOI("latitude", TOI::unspec, "interp"));
739 t.insert(TOI("longitude", TOI::unspec, "interp"));
740 t.insert(TOI("tsid", TOI::unspec));
741 t.insert(TOI("alphaSST", TOI::unspec, "galcross0"));
742 t.insert(TOI("deltaSST", TOI::unspec, "galcross0"));
743 t.insert(TOI("azimuthSST",TOI::unspec, "galcross0"));
744 t.insert(TOI("elvSST", TOI::unspec, "galcross0"));
745 t.insert(TOI("rotSpeed", TOI::unspec, "galcross0"));
746 return t;
747}
748
749void StarMatcher::propagateLowBound(TOI const& toi, long sampleNum) {
750 // we want to keep some past information to interpolate...
751 // keep 1000 sampleNums (easier than a number of posinfos...)
752
753 sampleNum -= 4000;
754
755 map<double, posInfo>::iterator i = posInfos.begin();
756 while (i != posInfos.end() && (*i).first < sampleNum) i++;
757 if (i != posInfos.begin()) {
758 i--;
759 posInfos.erase(posInfos.begin(), i);
760 }
761
762 map<double, pendulInfo>::iterator j = pendulInfos.begin();
763 while (j != pendulInfos.end() && (*j).first < sampleNum) j++;
764 if (j != pendulInfos.begin()) {
765 j--;
766 pendulInfos.erase(pendulInfos.begin(), j);
767 }
768
769 TOIDerivProducer::propagateLowBound(toi, sampleNum);
770}
771
772
773// 1. processStars seulement quand au moins 10 etoiles nouvelles
774// 2. Nettoyer avec fit parabolique sur les 5 dernieres seconde de donnees
775// 3. Garder periodeRotation secondes de donnees nettoyees
776// 4. TF ordre 0 sur ces donnees, amplitude et phase -> theta et phi pendulation,
777// elevationSST = elv-theta Sin[azimut-phi]
778// azimutSST = azimut+theta Cos[azimut-phi] Tan[elv] (+ OFFSET GALCROSS)
779// le signal le plus propre est l'elevation -> fit dessus, puis
780// correction azimut SST a partir seconde equation, sans utiliser azimut galcross
781
782
Note: See TracBrowser for help on using the repository browser.