source: trunk/Cosy/main/MTracking.cc@ 10535

Last change on this file since 10535 was 10024, checked in by tbretz, 15 years ago
Finally fixed MTracking to get the tracking of celestial objects working.
File size: 17.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 fCosy->fRaDec = fSlalib.GetRaDecRad();
295 fCosy->fHourAngle = fSlalib.GetHourAngle();
296
297 return kTRUE;
298}
299
300Bool_t MTracking::UpdateSlalib(SlaPlanets &sla, Double_t mjd)
301{
302 sla.SetMjd(mjd);
303 return UpdateSlalib(sla);
304}
305
306bool MTracking::Move()
307{
308 const RaDec &dst = fSlalib.GetRaDecRad();
309
310 ZdAz dest = fSlalib.GetZdAzRad();
311
312 if (fTrackType>=0)
313 gLog << all << fSlalib.GetTime() << ": Tracking Planet with Id " << fTrackType << " (" << fWobbleOffset << ", " << fWobbleAngle << ")" << endl;
314 gLog << all << fSlalib.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;
315
316 // If the star is culminating behind the zenith (South) we want to
317 // align the azimuth angle between -180 and 180deg. If the star is
318 // culminating before the zenith (north) we want the star to be
319 // aligned between -180 and 180deg (which is the default of CalcZdAz)
320 if (fSlalib.GetPhi()>dst.Dec() && dest.Az()<0)
321 {
322 // align az from -180/180 to 0/360
323 gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
324 dest.Az(dest.Az() + TMath::TwoPi());
325 }
326
327 // Position the telescope to the current local position of the
328 // star. Do not reposition but start the tracking after the
329 // first positioning step
330 if (!SetPosition(dest, kTRUE))
331 {
332 gLog << err << "ERROR - Cannot start tracking, positioning failed." << endl;
333 return false;
334 }
335
336 return true;
337}
338
339void MTracking::TrackPosition(const RaDec &dst)
340{
341 fTrackPos = dst;
342 fTrackType = -1;
343
344 // Just for convenience
345 fMoon->SetOffsetShadow(0);
346 fMoon->SetOffsetWobble(0);
347
348 // Start tracking
349 Track();
350}
351
352void MTracking::TrackPlanet(ePlanets_t planet)
353{
354 fTrackPos = RaDec();
355 fTrackType = planet;
356
357 // Just for convenience
358 fMoon->SetOffsetShadow(0);
359 fMoon->SetOffsetWobble(0);
360
361 // Start tracking
362 Track();
363}
364
365void MTracking::TrackMoon(Double_t wobble, Double_t offset)
366{
367 fTrackPos = RaDec();
368 fTrackType = kEMoon|0x100;
369
370 fWobbleOffset = TMath::DegToRad()*wobble;
371 fWobbleAngle = TMath::DegToRad()*offset;
372
373 fMoon->SetOffsetShadow(fWobbleAngle);
374 fMoon->SetOffsetWobble(fWobbleOffset);
375
376 // Start tracking
377 Track();
378}
379
380void MTracking::Track() // ra, dec [rad]
381{
382 // Position to corrent nominal position
383 if (!UpdateSlalib())
384 return;
385
386 if (!Move())
387 return;
388
389 //fCosy->fRaDec = fSlalib.GetRaDec();
390
391 // Initialize Tracker (slalib or starguider)
392 RunThread();
393
394 //
395 // Init accelerations and Rpm Mode
396 //
397 if (!InitTracking())
398 {
399 fCosy->StopMovement();
400 return;
401 }
402
403 // Get current nominal local position
404 if (!UpdateSlalib())
405 {
406 fCosy->StopMovement();
407 return;
408 }
409
410 ZdAz pos = fSlalib.GetZdAzRad();
411
412 // Some output
413 XY xy(TMath::RadToDeg()*fCosy->fRaDec.Ra()/15, TMath::RadToDeg()*fCosy->fRaDec.Dec());
414 gLog << all << fSlalib.GetTime() << " - Start Tracking: Ra=" << xy.X() << "h Dec=";
415 gLog << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
416
417 //
418 // We want to reach the theoretical position exactly in about 0.5s
419 //
420 // *OLD*const float dt = 1; // 1 second
421 const float dt = 5;//3; // 2 second
422 while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
423 {
424 /*
425 // Code for position control mode
426 // Set: S-0-0001
427 // Set: S-0-0002
428 // Set: P-0-0099
429 // Set: P-0-0187 (Spline)
430 // Replace S-0-0258 by S-0-0047 in communication
431
432 sla.Now();
433 const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
434 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
435 dest = fCosy->fBending(dest); // [rad]
436 if (!fCosy->CheckRange(dest))
437 break;
438 dest *= 1./TMath::TwoPi(); //[rev]
439 fCosy->fMac2->SendSDO(0x6004, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
440 fCosy->fMac1->SendSDO(0x6004, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
441 usleep(10000); // 10ms
442 continue;
443 */
444
445 //
446 // Request Target position for Now+dt
447 //
448 if (!UpdateSlalib(dt))
449 break;
450
451 //
452 // Request nominal position for this time in the future (To+dt)
453 //
454 const ZdAz pointing = fSlalib.GetZdAzRad();
455 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
456
457 //ZdAz repos;
458 dest = fCosy->fBending(dest); // [rad]
459 if (!fCosy->CheckRange(dest))
460 break;
461
462 // Destination position at t+dt in re-units
463 dest *= TMath::RadToDeg(); // [re]
464
465 const ZdAz sepos = fCosy->GetSePos()*360; // [deg]
466
467 // Now calculate the distance to move from now
468 // to a time in t+dt.
469 dest -= sepos;
470
471 //
472 // Velocity to go [re/min] to reach the right position at time t+dt
473 // correct for the duration of RaDec2AltAz
474 //
475 const ZdAz v = dest * 60/dt; // [deg/min]
476 const ZdAz vt = v/360; // [rpm]
477
478 //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
479 //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
480 //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
481 //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));
482
483 if (LimitSpeed(v))
484 {
485 gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
486 gLog << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
487 }
488
489 //
490 // check if the drive is fast enough to follow the star
491 //
492 if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
493 TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
494 {
495 gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
496 gLog << "Zd: " << vt.Zd() << " Az: " << vt.Az() << endl;
497 break;
498 }
499
500 //
501 // Set theoretical velocity (as early after calculation as possible)
502 // Maybe we should attenuate the changes
503 //
504 if (!SetVelocity(vt))
505 break;
506
507 // Now the tracking procedure is finished and we have some time
508 // left to set the nominal pointing position for the starguider
509 // if available. This should be thread safe. Doing this in
510 // UpdateSlalib would also update it from the Thread
511 // which leads to problems.
512 if (fCosy->fStarguider)
513 fCosy->fStarguider->SetPointingPosition(fSlalib.GetRaDec());
514
515 //
516 // Update speed as often as possible.
517 // make sure, that dt is around 10 times larger than the
518 // update time
519 //
520 // The loop should not be executed faster than the ramp of
521 // a change in the velocity can be followed.
522 // (This is important on fast machines >500MHz)
523 //
524 usleep(1000000); // 1s
525
526 //
527 // If we would do it in the beginnign of the loop it can happen
528 // that we check before we got the feedback through the pdo
529 //
530 if (!fCosy->fMac1->IsRpmActive() || !fCosy->fMac2->IsRpmActive())
531 break;
532 }
533
534 fSlalib.Now();
535
536 CancelThread();
537
538 // If CancelPoints are used we have to make this a Cleanup!
539 fCosy->StopMovement();
540
541 gLog << all << fSlalib.GetTime() << " - Tracking stopped @ Zd=";
542 gLog << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az=";
543 gLog << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << endl;
544}
545
546Int_t MTracking::Thread()
547{
548 if (!fCosy->fMac1 || !fCosy->fMac2)
549 return 3;
550
551 gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;
552
553 //SlaPlanets sla(fSlalib.GetObservatoryKey());
554 SlaPlanets sla(MObservatory::kMagic1);
555 sla.Now();
556
557 if (!UpdateSlalib(sla))
558 gLog << err << "UpdateSlalib in Thread failed." << endl;
559
560 //
561 // only update fTrackingError while tracking
562 //
563 bool phca1=false;
564 bool phcaz=false;
565
566 while (1)
567 {
568 // Make changes (eg wind) smoother - attenuation of control function
569 // This is the time constant which defines how fast
570 // you correct for external influences (like wind)
571 //const float weight = 1.; //0.3;
572
573 // Check for changes of the shaftencoder values
574 fCosy->fMac1->ResetHasChangedPos2();
575 fCosy->fMac2->ResetHasChangedPos2();
576 do
577 {
578 phcaz = fCosy->fMac1->HasChangedPos2();
579 phca1 = fCosy->fMac2->HasChangedPos2();
580
581 usleep(1);
582
583 TThread::CancelPoint();
584
585 } while (!phca1 && !phcaz);
586
587 // get current position and corresponding time of shaftencoders
588 const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi(); // [deg]
589
590 const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
591 const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();
592
593
594 // calculate offset for both axis (only one is needed)
595 // if Shaftencoder changed position, calculate nominal position
596 if (phca1)
597 {
598 UpdateSlalib(sla, mjdzd);
599
600 ZdAz dummy = sla.GetZdAzRad();
601 dummy = fCosy->AlignTrackingPos(dummy);
602 fCosy->fZdAzSoll.Zd(dummy.Zd());
603 fCosy->fTrackingError.Zd(fCosy->fBending(dummy).Zd()-istse.Zd());
604
605 TThread::CancelPoint();
606 }
607 if (phcaz)
608 {
609 UpdateSlalib(sla, mjdaz);
610
611 ZdAz dummy = sla.GetZdAzRad();
612 dummy = fCosy->AlignTrackingPos(dummy);
613 fCosy->fZdAzSoll.Az(dummy.Az());
614 fCosy->fTrackingError.Az(fCosy->fBending(dummy).Az()-istse.Az());
615
616 TThread::CancelPoint();
617 }
618 }
619
620 gLog << inf2 << "- Tracking Thread done. (" << MTime(-1) << ")" << endl;
621 return 0;
622}
Note: See TracBrowser for help on using the repository browser.