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

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