source: trunk/Cosy/main/MTracking.cc

Last change on this file was 12595, checked in by tbretz, 13 years ago
Added some more output. Fill fCosy->fMJD and update nominal RaDec during tracking; changed the control loop parameters and the position alignment for FACT
File size: 18.7 KB
Line 
1#include "MTracking.h"
2
3#include "MLogManip.h"
4
5#include "dkc.h"
6
7#include "SlaStars.h"
8
9#include "MCosy.h"
10#include "MStarguider.h"
11
12#include "MDriveCom.h"
13
14#include "MMoonPointing.h"
15
16ClassImp(MTracking);
17
18using namespace std;
19
20//#define EXPERT
21#undef EXPERT
22
23MTracking::MTracking(MCosy *cosy)
24 : MSlewing(cosy), MThread("MTracking"), fSlalib(fCosy->fObservatory),
25 fTrackAcc(0, 0), fWobbleOffset(-1), fWobbleAngle(0), fOut(0)
26{
27 fMoon = new MMoonPointing("MoonShadowOffsets.root");
28}
29
30MTracking::~MTracking()
31{
32 delete fMoon;
33}
34
35// --------------------------------------------------------------------------
36//
37// Sets the tracking velocity
38//
39// The velocities are given in a ZdAz object in re/min. Return kTRUE
40// in case of success, kFALSE in case of failure.
41//
42Bool_t MTracking::SetVelocity(const ZdAz &v)
43{
44 const Double_t vrzd = fCosy->fMac2->GetVelRes();
45 const Double_t vraz = fCosy->fMac1->GetVelRes();
46
47 //
48 // Send the new velocities for both axes.
49 //
50 fCosy->fMac2->SendSDO(0x3007, (LWORD_t)(v.Zd()*vrzd)); // SetRpmVelocity [re/min]
51 fCosy->fMac1->SendSDO(0x3007, (LWORD_t)(v.Az()*vraz)); // SetRpmVelocity [re/min]
52
53 //
54 // Wait for the objects to be acknoledged.
55 //
56 fCosy->fMac2->WaitForSdo(0x3007, 0, 100);
57 fCosy->fMac1->WaitForSdo(0x3007, 0, 100);
58
59 //
60 // If the waiting for the objects wasn't interrupted return kTRUE
61 //
62 if (!Break())
63 return kTRUE;
64
65 fCosy->PrintError();
66
67 //
68 // print a message if the interruption was due to a Can-node Error
69 //
70 if (fCosy->HasError())
71 gLog << err << "ERROR - while setting tracking velocity (SDO #3006)" << endl;
72
73 return kFALSE;
74}
75
76// --------------------------------------------------------------------------
77//
78// Initializes Tracking mode
79//
80// Initializes the accelerations of both axes with 90% of the maximum
81// acceleration. Set the status for moving and tracking and starts thr
82// revolution mode.
83//
84bool MTracking::InitTracking()
85{
86 // FIXME? Handling of Zombie OK?
87 if (fCosy->fMac1->IsZombieNode() || fCosy->fMac2->IsZombieNode())
88 return false;
89
90 //
91 // Start revolution mode
92 //
93 if (!SetAcc(fCosy->fMac2, fTrackAcc.Zd()))
94 return false;
95
96 if (!SetAcc(fCosy->fMac1, fTrackAcc.Az()))
97 return false;
98
99 fCosy->fMac2->SetRpmMode(TRUE);
100 if (fCosy->fMac2->IsZombieNode())
101 return false;
102
103 fCosy->fMac1->SetRpmMode(TRUE);
104 if (fCosy->fMac1->IsZombieNode())
105 return false;
106
107 fCosy->SetStatus(MDriveCom::kMoving | MDriveCom::kTracking);
108
109 return true;
110}
111
112/*
113void MTracking::StopTracking()
114{
115 //
116 // Set status to Stopping
117 //
118 fCosy->SetStatus(MDriveCom::kStopping);
119
120 //
121 // set deceleration to 50%
122 //
123 cout << "Stopping tracking (dec=20%)..." << endl;
124 fCosy->fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
125 fCosy->fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
126
127 fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
128 fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
129 fCosy->fMac2->WaitForSdo(0x3006, 1);
130 fCosy->fMac1->WaitForSdo(0x3006, 1);
131
132 cout << "Waiting for end of movement..." << endl;
133 fCosy->WaitForEndMovement();
134
135 //
136 // Wait for the objects to be OKed.
137 //
138 fCosy->fMac1->SetRpmMode(FALSE);
139 fCosy->fMac2->SetRpmMode(FALSE);
140
141 //
142 // Wait for the movement to really be finished.
143 //
144 //cout << "Waiting for end of movement..." << endl;
145 //WaitForEndMovement();
146
147 //
148 // Check whether everything works fine.
149 //
150 fCosy->CheckForError();
151 cout << "Movement stopped." << endl;
152}
153*/
154
155// --------------------------------------------------------------------------
156//
157// Limits the speed.
158//
159// This function should work as a limiter. If a tracking error is too large
160// to be corrected fast enough we would get enormous velocities. These
161// velocities are limited to the maximum velocity.
162//
163Bool_t MTracking::LimitSpeed(const ZdAz &vt) const
164{
165 // vt [deg/min]
166
167 // We can set a maximum speed here
168 // And we can limit the change of the speed (which is done
169 // by acceleration in the drive anyway)
170
171 return kTRUE;
172/*
173 // vt[re/min]
174
175 // Calculate approximate velocity of both axis
176 ZdAz vcalc = fSlalib.GetApproxVel(fCosy->fRaDec); // [rad/rad]
177
178 //vcalc *= 1./(24*60); // [U_tel/min]
179 //vcalc *= fCosy->kGearTot; // [U_mot/min]
180 //vcalc *= fCosy->kResRE; // [re/min]
181
182 vcalc *= fCosy->kGearTot*fCosy->kResRE/(24*60); // [re/min]
183
184 // Set return code
185 Bool_t rc = kFALSE;
186
187 //
188 // How to limit the speed. If the wind comes and blowes
189 // we cannot forbid changing of the sign. But on the other hand
190 // we don't want fast changes!
191 //
192 ULong_t vrzd = fCosy->fMac1->GetVelRes();
193 ULong_t vraz = fCosy->fMac2->GetVelRes();
194
195#define sgn(x) (x<0?-1:1)
196
197 //
198 // When speed changes sign, the maximum allowed speed
199 // is 25% of the |v|
200 //
201 //const Float_t limit = 0.25;
202
203 //
204 // The maximum allowed speed while tracking is 10%
205 //
206 const Float_t maxtrack = 0.1;
207
208 if (fabs(vt->Az()) > maxtrack*vraz)
209 {
210 vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
211 gLog << warn << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl;
212 gLog << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
213 rc=kTRUE;
214 }
215 if (fabs(vt->Zd()) > maxtrack*vrzd)
216 {
217 vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
218 gLog << warn << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl;
219 gLog << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
220 rc=kTRUE;
221 }
222 return rc;
223 */
224}
225
226Bool_t MTracking::UpdateSlalib(SlaPlanets &sla)
227{
228 if (fTrackType<0)
229 sla.Set(fTrackPos/TMath::DegToRad());
230 else
231 sla.SetPlanet((ePlanets_t)(fTrackType&0xff));
232
233 if (fTrackType==(kEMoon|0x100))
234 {
235 if (fMoon->IsZombie())
236 {
237 gLog << err << "ERROR - Could not initialize MMoonPointing." << endl;
238 return kFALSE;
239 }
240
241 //moon.SetOffsetShadow(fWobbleAngle);
242 //moon.SetOffsetWobble(fWobbleOffset);
243
244 const ZdAz za = sla.GetZdAzRad();
245
246 ZdAz srcpos, pointpos;
247 if (!fMoon->CalcPosition(za, srcpos, pointpos))
248 {
249 gLog << err << "ERROR - Calculation of moon shadow pointing position failed." << endl;
250 gLog << " WoAngle =" << fWobbleAngle << endl;
251 gLog << " WoOffset =" << fWobbleOffset << endl;
252 gLog << " TrackType =" << fTrackType << endl;
253 gLog << " TT&0xff =" << (fTrackType&0xff) << endl;
254 gLog << " mjd =" << sla.GetMjd() << endl;
255 gLog << " za.Zd =" << za.Zd() << endl;
256 gLog << " za.Az =" << za.Az() << endl;
257 gLog << " srcpos.Zd =" << srcpos.Zd() << endl;
258 gLog << " srcpos.Az =" << srcpos.Az() << endl;
259 gLog << " pointpos.Zd=" << pointpos.Zd() << endl;
260 gLog << " pointpos.Az=" << pointpos.Az() << endl;
261 return kFALSE;
262 }
263
264 sla.Set(pointpos/TMath::DegToRad());
265
266// return kTRUE;
267
268 //RaDec rd = sla.GetRaDec();
269 //ZdAz zd = sla.GetZdAz();
270
271 // Ra/Dec, Zd/Az from pointpos
272 // NEW: Source pos from srcpos
273 }
274 /*
275 else
276 if (fWobbleOffset>0)
277 {
278 MPositionOffsetCalc calc(fWobbleOffset, fWobbleAngle);
279 const ZdAz offset = calc.GetOffset(sla.GetZdAzRad());
280 //if (srcpos.Zd()==0)
281 // return kFALSE;
282 sla.ApplyOffsetAltAz(offset);
283 }
284 */
285 return kTRUE;
286}
287
288Bool_t MTracking::UpdateSlalib(Double_t dt)
289{
290 fSlalib.Now(dt);
291 if (!UpdateSlalib(fSlalib))
292 return kFALSE;
293
294 return kTRUE;
295}
296
297Bool_t MTracking::UpdateSlalib(SlaPlanets &sla, Double_t mjd)
298{
299 sla.SetMjd(mjd);
300 return UpdateSlalib(sla);
301}
302
303bool MTracking::Move()
304{
305 const RaDec &dst = fSlalib.GetRaDecRad();
306
307 ZdAz dest = fSlalib.GetZdAzRad();
308
309 if (fTrackType>=0)
310 gLog << all << fSlalib.GetTime() << ": Tracking Planet with Id " << fTrackType << " (" << fWobbleOffset << ", " << fWobbleAngle << ")" << endl;
311 gLog << all << fSlalib.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;
312
313 // If the star is culminating behind the zenith (South) we want to
314 // align the azimuth angle between -180 and 180deg. If the star is
315 // culminating before the zenith (north) we want the star to be
316 // aligned between -180 and 180deg (which is the default of CalcZdAz)
317
318#ifdef FACT
319 if (fSlalib.GetPhi()>dst.Dec() && dest.Az()>0)
320 {
321 // align az from -180/180 to 0/360
322 gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
323 dest.Az(dest.Az() - TMath::TwoPi());
324 }
325#else
326 if (fSlalib.GetPhi()>dst.Dec() && dest.Az()<0)
327 {
328 // align az from -180/180 to 0/360
329 gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
330 dest.Az(dest.Az() + TMath::TwoPi());
331 }
332#endif
333
334 // Position the telescope to the current local position of the
335 // star. Do not reposition but start the tracking after the
336 // first positioning step
337 if (!SetPosition(dest, kTRUE))
338 {
339 gLog << err << "ERROR - Cannot start tracking, positioning failed." << endl;
340 return false;
341 }
342
343 return true;
344}
345
346void MTracking::TrackPosition(const RaDec &dst)
347{
348 fTrackPos = dst;
349 fTrackType = -1;
350
351 // Just for convenience
352 fMoon->SetOffsetShadow(0);
353 fMoon->SetOffsetWobble(0);
354
355 // Start tracking
356 Track();
357}
358
359void MTracking::TrackPlanet(ePlanets_t planet)
360{
361 fTrackPos = RaDec();
362 fTrackType = planet;
363
364 // Just for convenience
365 fMoon->SetOffsetShadow(0);
366 fMoon->SetOffsetWobble(0);
367
368 // Start tracking
369 Track();
370}
371
372void MTracking::TrackMoon(Double_t wobble, Double_t offset)
373{
374 fTrackPos = RaDec();
375 fTrackType = kEMoon|0x100;
376
377 fWobbleOffset = TMath::DegToRad()*wobble;
378 fWobbleAngle = TMath::DegToRad()*offset;
379
380 fMoon->SetOffsetShadow(fWobbleAngle);
381 fMoon->SetOffsetWobble(fWobbleOffset);
382
383 // Start tracking
384 Track();
385}
386
387void MTracking::Track() // ra, dec [rad]
388{
389 // Position to corrent nominal position
390 if (!UpdateSlalib())
391 return;
392
393 if (!Move())
394 return;
395
396 //fCosy->fRaDec = fSlalib.GetRaDec();
397
398 // Initialize Tracker (slalib or starguider)
399 RunThread();
400
401 //
402 // Init accelerations and Rpm Mode
403 //
404 if (!InitTracking())
405 {
406 fCosy->StopMovement();
407 return;
408 }
409
410 // Get current nominal local position
411 if (!UpdateSlalib())
412 {
413 fCosy->StopMovement();
414 return;
415 }
416
417 ZdAz pos = fSlalib.GetZdAzRad();
418
419 // Some output
420 XY xy(TMath::RadToDeg()*fCosy->fRaDec.Ra()/15, TMath::RadToDeg()*fCosy->fRaDec.Dec());
421 gLog << all << fSlalib.GetTime() << " - Start Tracking: Ra=" << xy.X() << "h Dec=";
422 gLog << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
423
424 //
425 // We want to reach the theoretical position exactly in about 0.5s
426 //
427 // *OLD*const float dt = 1; // 1 second
428#ifdef FACT
429 const float dt = 7.6;//3; // 2 second
430#else
431 const float dt = 5;//3; // 2 second
432#endif
433 while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
434 {
435 /*
436 // Code for position control mode
437 // Set: S-0-0001
438 // Set: S-0-0002
439 // Set: P-0-0099
440 // Set: P-0-0187 (Spline)
441 // Replace S-0-0258 by S-0-0047 in communication
442
443 sla.Now();
444 const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
445 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
446 dest = fCosy->fBending(dest); // [rad]
447 if (!fCosy->CheckRange(dest))
448 break;
449 dest *= 1./TMath::TwoPi(); //[rev]
450 fCosy->fMac2->SendSDO(0x6004, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
451 fCosy->fMac1->SendSDO(0x6004, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
452 usleep(10000); // 10ms
453 continue;
454 */
455
456 //
457 // Request Target position for Now+dt
458 //
459 if (!UpdateSlalib(dt))
460 break;
461
462 //
463 // Request nominal position for this time in the future (To+dt)
464 //
465 const ZdAz pointing = fSlalib.GetZdAzRad();
466 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
467
468 //ZdAz repos;
469 dest = fCosy->fBending(dest); // [rad]
470 if (!fCosy->CheckRange(dest))
471 break;
472
473 // Destination position at t+dt in re-units
474 dest *= TMath::RadToDeg(); // [re]
475
476 const ZdAz sepos = fCosy->GetSePos()*360; // [deg]
477
478 // Now calculate the distance to move from now
479 // to a time in t+dt.
480 dest -= sepos;
481
482 //
483 // Velocity to go [re/min] to reach the right position at time t+dt
484 // correct for the duration of RaDec2AltAz
485 //
486 const ZdAz v = dest * 60/dt; // [deg/min]
487 const ZdAz vt = v/360; // [rpm]
488
489 //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
490 //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
491 //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
492 //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));
493
494 if (LimitSpeed(v))
495 {
496 gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
497 gLog << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
498 }
499
500 //
501 // check if the drive is fast enough to follow the star
502 //
503 if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
504 TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
505 {
506 gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
507 gLog << "Zd: " << vt.Zd() << " Az: " << vt.Az() << endl;
508 break;
509 }
510
511 //
512 // Set theoretical velocity (as early after calculation as possible)
513 // Maybe we should attenuate the changes
514 //
515 if (!SetVelocity(vt))
516 break;
517
518 // Now the tracking procedure is finished and we have some time
519 // left to set the nominal pointing position for the starguider
520 // if available. This should be thread safe. Doing this in
521 // UpdateSlalib would also update it from the Thread
522 // which leads to problems.
523 if (fCosy->fStarguider)
524 fCosy->fStarguider->SetPointingPosition(fSlalib.GetRaDec());
525
526 //
527 // Update speed as often as possible.
528 // make sure, that dt is around 10 times larger than the
529 // update time
530 //
531 // The loop should not be executed faster than the ramp of
532 // a change in the velocity can be followed.
533 // (This is important on fast machines >500MHz)
534 //
535#ifdef FACT
536 usleep(760000/4); // 1.4s
537#else
538 usleep(1000000);
539#endif
540
541 //
542 // If we would do it in the beginnign of the loop it can happen
543 // that we check before we got the feedback through the pdo
544 //
545 if (!fCosy->fMac1->IsRpmActive() || !fCosy->fMac2->IsRpmActive())
546 {
547 gLog << warn << fSlalib.GetTime() << " - RPM mode not active anymore." << endl;
548 break;
549 }
550 }
551
552 if (fCosy->Break())
553 gLog << all << fSlalib.GetTime() << " - Break signal received." << endl;
554 if (fCosy->HasError())
555 gLog << all << fSlalib.GetTime() << " - HasError received." << endl;
556 if (fCosy->HasZombie())
557 gLog << all << fSlalib.GetTime() << " - HasZombie received." << endl;
558
559 fSlalib.Now();
560
561 CancelThread();
562 fCosy->fMJD = 0;
563
564 // If CancelPoints are used we have to make this a Cleanup!
565 fCosy->StopMovement();
566
567 gLog << all << fSlalib.GetTime() << " - Tracking stopped @ Zd=";
568 gLog << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az=";
569 gLog << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << endl;
570}
571
572Int_t MTracking::Thread()
573{
574 if (!fCosy->fMac1 || !fCosy->fMac2)
575 return 3;
576
577 gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;
578
579 //SlaPlanets sla(fSlalib.GetObservatoryKey());
580 SlaPlanets sla(MObservatory::kMagic1);
581 sla.Now();
582
583 if (!UpdateSlalib(sla))
584 gLog << err << "UpdateSlalib in Thread failed." << endl;
585
586 //
587 // only update fTrackingError while tracking
588 //
589 bool phca1=false;
590 bool phcaz=false;
591
592 while (1)
593 {
594 // Make changes (eg wind) smoother - attenuation of control function
595 // This is the time constant which defines how fast
596 // you correct for external influences (like wind)
597 //const float weight = 1.; //0.3;
598
599 // Check for changes of the shaftencoder values
600 fCosy->fMac1->ResetHasChangedPos2();
601 fCosy->fMac2->ResetHasChangedPos2();
602 do
603 {
604 phcaz = fCosy->fMac1->HasChangedPos2();
605 phca1 = fCosy->fMac2->HasChangedPos2();
606
607 usleep(1);
608
609 TThread::CancelPoint();
610
611 } while (!phca1 && !phcaz);
612
613 // get current position and corresponding time of shaftencoders
614 const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi(); // [deg]
615
616 const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
617 const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();
618
619
620 // calculate offset for both axis (only one is needed)
621 // if Shaftencoder changed position, calculate nominal position
622 if (phca1)
623 {
624 UpdateSlalib(sla, mjdzd);
625
626 ZdAz dummy = sla.GetZdAzRad();
627 dummy = fCosy->AlignTrackingPos(dummy);
628 fCosy->fRaDec = sla.GetRaDecRad();
629 fCosy->fHourAngle = sla.GetHourAngle();
630 fCosy->fMJD = mjdaz;
631 fCosy->fZdAzSoll.Zd(dummy.Zd());
632 fCosy->fTrackingError.Zd(fCosy->fBending(dummy).Zd()-istse.Zd());
633
634 TThread::CancelPoint();
635 }
636 if (phcaz)
637 {
638 UpdateSlalib(sla, mjdaz);
639
640 ZdAz dummy = sla.GetZdAzRad();
641 dummy = fCosy->AlignTrackingPos(dummy);
642 fCosy->fRaDec = sla.GetRaDecRad();
643 fCosy->fHourAngle = sla.GetHourAngle();
644 fCosy->fMJD = mjdaz;
645 fCosy->fZdAzSoll.Az(dummy.Az());
646 fCosy->fTrackingError.Az(fCosy->fBending(dummy).Az()-istse.Az());
647
648 TThread::CancelPoint();
649 }
650 }
651
652 gLog << inf2 << "- Tracking Thread done. (" << MTime(-1) << ")" << endl;
653 return 0;
654}
Note: See TracBrowser for help on using the repository browser.