source: trunk/MagicSoft/Cosy/main/MTracking.cc@ 9555

Last change on this file since 9555 was 9555, checked in by tbretz, 16 years ago
*** empty log message ***
File size: 15.8 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
220void 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 MMoonPointing moon("MoonShadowOffsets.root");
230 if (moon.IsZombie())
231 {
232 gLog << warn << "WARNING - Calculation of moon shadow failed." << endl;
233 return;
234 }
235
236 moon.SetOffsetShadow(TMath::DegToRad()*fWobbleAngle);
237 moon.SetOffsetWobble(TMath::DegToRad()*fWobbleOffset);
238
239 ZdAz srcpos, pointpos;
240
241 const ZdAz za = sla.GetZdAzRad();
242 if (moon.CalcPosition(za, srcpos, pointpos))
243 sla.Set(pointpos);
244
245 // Ra/Dec, Zd/Az from pointpos
246 // NEW: Source pos from srcpos
247 }
248 /*
249 else
250 if (fWobbleOffset>0)
251 {
252 MPositionOffsetCalc calc(fWobbleOffset, fWobbleAngle);
253 const ZdAz offset = calc.GetOffset(sla.GetZdAzRad());
254 //if (srcpos.Zd()==0)
255 // return kFALSE;
256 sla.ApplyOffsetAltAz(offset);
257 }
258 */
259 // if (fCosy->fStarguider)
260 // fCosy->fStarguider->SetPointingPosition(sla.GetRaDec());
261}
262
263void MTracking::UpdateSlalib(Double_t dt)
264{
265 fSlalib.Now(dt);
266 UpdateSlalib(fSlalib);
267
268 fCosy->fRaDec = fSlalib.GetRaDecRad();
269 fCosy->fHourAngle = fSlalib.GetHourAngle();
270}
271
272void MTracking::UpdateSlalib(SlaPlanets &sla, Double_t mjd)
273{
274 sla.SetMjd(mjd);
275 UpdateSlalib(sla);
276}
277
278bool MTracking::Move()
279{
280 const RaDec &dst = fSlalib.GetRaDecRad();
281
282 ZdAz dest = fSlalib.GetZdAzRad();
283
284 if (fTrackType>=0)
285 gLog << all << fSlalib.GetTime() << ": Tracking Planet with Id " << fTrackType << endl;
286 gLog << all << fSlalib.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;
287
288 // If the star is culminating behind the zenith (South) we want to
289 // align the azimuth angle between -180 and 180deg. If the star is
290 // culminating before the zenith (north) we want the star to be
291 // aligned between -180 and 180deg (which is the default of CalcZdAz)
292 if (fSlalib.GetPhi()>dst.Dec() && dest.Az()<0)
293 {
294 // align az from -180/180 to 0/360
295 gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
296 dest.Az(dest.Az() + TMath::TwoPi());
297 }
298
299 // Position the telescope to the current local position of the
300 // star. Do not reposition but start the tracking after the
301 // first positioning step
302 if (!SetPosition(dest, kTRUE))
303 {
304 gLog << err << "ERROR - Cannot start tracking, positioning failed." << endl;
305 return false;
306 }
307
308 return true;
309}
310
311void MTracking::TrackPosition(const RaDec &dst)
312{
313 fTrackPos = dst;
314 fTrackType = -1;
315
316 // Start tracking
317 Track();
318}
319
320void MTracking::TrackPlanet(ePlanets_t planet)
321{
322 fTrackPos = RaDec();
323 fTrackType = planet;
324
325 // Start tracking
326 Track();
327}
328
329void MTracking::TrackMoon(Double_t wobble, Double_t offset)
330{
331 fTrackPos = RaDec();
332 fTrackType = kEMoon|0x100;
333
334 fWobbleOffset = TMath::DegToRad()*wobble;
335 fWobbleAngle = TMath::DegToRad()*offset;
336
337 // Start tracking
338 Track();
339}
340
341void MTracking::Track() // ra, dec [rad]
342{
343 // Position to corrent nominal position
344 UpdateSlalib();
345
346 if (!Move())
347 return;
348
349 //fCosy->fRaDec = fSlalib.GetRaDec();
350
351 // Initialize Tracker (slalib or starguider)
352 RunThread();
353
354 //
355 // Init accelerations and Rpm Mode
356 //
357 if (!InitTracking())
358 {
359 fCosy->StopMovement();
360 return;
361 }
362
363 // Get current nominal local position
364 UpdateSlalib();
365
366 ZdAz pos = fSlalib.GetZdAzRad();
367
368 // Some output
369 XY xy(TMath::RadToDeg()*fCosy->fRaDec.Ra()/15, TMath::RadToDeg()*fCosy->fRaDec.Dec());
370 gLog << all << fSlalib.GetTime() << " - Start Tracking: Ra=" << xy.X() << "h Dec=";
371 gLog << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
372
373 //
374 // We want to reach the theoretical position exactly in about 0.5s
375 //
376 // *OLD*const float dt = 1; // 1 second
377 const float dt = 5;//3; // 2 second
378 while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
379 {
380 /*
381 // Code for position control mode
382 // Set: S-0-0001
383 // Set: S-0-0002
384 // Set: P-0-0099
385 // Set: P-0-0187 (Spline)
386 // Replace S-0-0258 by S-0-0047 in communication
387
388 sla.Now();
389 const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
390 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
391 dest = fCosy->fBending(dest); // [rad]
392 if (!fCosy->CheckRange(dest))
393 break;
394 dest *= 1./TMath::TwoPi(); //[rev]
395 fCosy->fMac2->SendSDO(0x6004, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
396 fCosy->fMac1->SendSDO(0x6004, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
397 usleep(10000); // 10ms
398 continue;
399 */
400
401 //
402 // Request Target position for Now+dt
403 //
404 UpdateSlalib(dt);
405
406 //
407 // Request nominal position for this time in the future (To+dt)
408 //
409 const ZdAz pointing = fSlalib.GetZdAzRad();
410 ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity
411
412 //ZdAz repos;
413 dest = fCosy->fBending(dest); // [rad]
414 if (!fCosy->CheckRange(dest))
415 break;
416
417 // Destination position at t+dt in re-units
418 dest *= TMath::RadToDeg(); // [re]
419
420 const ZdAz sepos = fCosy->GetSePos()*360; // [deg]
421
422 // Now calculate the distance to move from now
423 // to a time in t+dt.
424 dest -= sepos;
425
426 //
427 // Velocity to go [re/min] to reach the right position at time t+dt
428 // correct for the duration of RaDec2AltAz
429 //
430 const ZdAz v = dest * 60/dt; // [deg/min]
431 const ZdAz vt = v/360; // [rpm]
432
433 //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
434 //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
435 //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
436 //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));
437
438 if (LimitSpeed(v))
439 {
440 gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
441 gLog << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
442 }
443
444 //
445 // check if the drive is fast enough to follow the star
446 //
447 if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
448 TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
449 {
450 gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
451 gLog << "Zd: " << vt.Zd() << " Az: " << vt.Az() << endl;
452 break;
453 }
454
455 //
456 // Set theoretical velocity (as early after calculation as possible)
457 // Maybe we should attenuate the changes
458 //
459 if (!SetVelocity(vt))
460 break;
461
462 //
463 // Update speed as often as possible.
464 // make sure, that dt is around 10 times larger than the
465 // update time
466 //
467 // The loop should not be executed faster than the ramp of
468 // a change in the velocity can be followed.
469 // (This is important on fast machines >500MHz)
470 //
471 usleep(1000000); // 1s
472
473 //
474 // If we would do it in the beginnign of the loop it can happen
475 // that we check before we got the feedback through the pdo
476 //
477 if (!fCosy->fMac1->IsRpmActive() || !fCosy->fMac2->IsRpmActive())
478 break;
479 }
480
481 fSlalib.Now();
482
483 CancelThread();
484
485 // If CancelPoints are used we have to make this a Cleanup!
486 fCosy->StopMovement();
487
488 gLog << all << fSlalib.GetTime() << " - Tracking stopped @ Zd=";
489 gLog << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az=";
490 gLog << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << endl;
491}
492
493Int_t MTracking::Thread()
494{
495 if (!fCosy->fMac1 || !fCosy->fMac2)
496 return 3;
497
498 gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;
499
500 //SlaPlanets sla(fSlalib.GetObservatoryKey());
501 SlaPlanets sla(MObservatory::kMagic1);
502
503 UpdateSlalib(sla);
504
505 //
506 // only update fTrackingError while tracking
507 //
508 bool phca1=false;
509 bool phcaz=false;
510
511 while (1)
512 {
513 // Make changes (eg wind) smoother - attenuation of control function
514 // This is the time constant which defines how fast
515 // you correct for external influences (like wind)
516 //const float weight = 1.; //0.3;
517
518 // Check for changes of the shaftencoder values
519 fCosy->fMac1->ResetHasChangedPos2();
520 fCosy->fMac2->ResetHasChangedPos2();
521 do
522 {
523 phcaz = fCosy->fMac1->HasChangedPos2();
524 phca1 = fCosy->fMac2->HasChangedPos2();
525
526 usleep(1);
527
528 TThread::CancelPoint();
529
530 } while (!phca1 && !phcaz);
531
532 // get current position and corresponding time of shaftencoders
533 const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi(); // [deg]
534
535 const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
536 const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();
537
538
539 // calculate offset for both axis (only one is needed)
540 // if Shaftencoder changed position, calculate nominal position
541 if (phca1)
542 {
543 UpdateSlalib(sla, mjdzd);
544
545 ZdAz dummy = sla.GetZdAzRad();
546 dummy = fCosy->AlignTrackingPos(dummy);
547 fCosy->fZdAzSoll.Zd(dummy.Zd());
548 fCosy->fTrackingError.Zd(fCosy->fBending(dummy).Zd()-istse.Zd());
549
550 TThread::CancelPoint();
551 }
552 if (phcaz)
553 {
554 UpdateSlalib(sla, mjdaz);
555
556 ZdAz dummy = sla.GetZdAzRad();
557 dummy = fCosy->AlignTrackingPos(dummy);
558 fCosy->fZdAzSoll.Az(dummy.Az());
559 fCosy->fTrackingError.Az(fCosy->fBending(dummy).Az()-istse.Az());
560
561 TThread::CancelPoint();
562 }
563 }
564
565 gLog << inf2 << "- Tracking Thread done. (" << MTime(-1) << ")" << endl;
566 return 0;
567}
Note: See TracBrowser for help on using the repository browser.