source: trunk/Cosy/main/MCosy.cc@ 19372

Last change on this file since 19372 was 16763, checked in by tbretz, 12 years ago
Adapted the status setting to the IsPositioning information from the PDO3 for FACT; for FACT do not write communication to file.
File size: 29.7 KB
Line 
1#include "MCosy.h"
2#include "MCosy.h"
3
4#include <iomanip>
5#include <fstream>
6
7#include <TEnv.h>
8#include <TTimer.h>
9#include <TApplication.h>
10
11//#include "MLog.h"
12#include "MLogManip.h"
13
14#include "MEnv.h"
15#include "MTime.h"
16#include "MPointing.h"
17
18#include "MGCosy.h"
19#include "MDriveCom.h"
20#include "MStarguider.h"
21#include "SlaStars.h"
22#include "MTracking.h"
23
24#include "dkc.h"
25
26ClassImp(MCosy);
27
28using namespace std;
29
30typedef struct tm tm_t;
31
32//#define EXPERT
33#undef EXPERT
34#define FACT
35
36/*
37ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
38{
39 // CorrectTarget [se]
40
41 // src [se]
42 // dst [rad]
43
44 // fAltMax = 70
45 // fAltMin = -105/110
46 // fAzMin = -355
47 // fAzMax = 355
48
49 ZdAz source = src * 360.0/16384.0;
50 ZdAz dest = dst * TMath::RadToDeg();
51
52 if (dest.Zd()>-3 && dest.Zd()<3)
53 dest.Zd(dest.Zd()<0?-3:3);
54
55 if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
56 return dst*(16384.0/k2Pi);
57
58 const float fZdMin = -67;
59 const float fZdMax = 67;
60 const float fAzMin = -29;
61 const float fAzMax = 423;
62
63 //
64 // This corrects to target for the shortest distance, not for the fastest move!
65 //
66 ZdAz s = source-dest;
67
68 float min = s.Sqr();
69
70 //
71 // Is it enought to search inside one revolution?
72 //
73 ZdAz ret = dest;
74
75 for (int i=-5; i<5+1; i++)
76 {
77 const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
78
79 //
80 // Range Check
81 //
82 if (p.Zd()<fZdMin || p.Zd()>fZdMax)
83 continue;
84
85 if (p.Az()<fAzMin || p.Az()>fAzMax)
86 continue;
87
88 //
89 // Calculate distance
90 //
91 s = source-p;
92
93 const float dist = s.Sqr();
94
95 if (dist > min)
96 continue;
97
98 //
99 // New shortest distance
100 //
101 ret = p;
102 min = dist;
103 }
104 return ret*(16384.0/360.0);
105}
106*/
107// --------------------------------------------------------------------------
108//
109// GetSePos, reads the Shaftencoder positions from the Can-drivers
110// for the shaftencoders. The two shaft encoders at the elevation axis
111// are avaraged. The values are returned as a ZdAz object.
112//
113// If one of the two shaftencoders on the elevation axis is missing
114// the other one's position is returned.
115//
116// The positions are alway up-to-date because the shaftencoders are
117// sending all changes immediatly.
118//
119ZdAz MCosy::GetSePos() const
120{
121 const Double_t pa = fMac1 ? (Double_t)fMac1->GetPdoPos2()/fMac1->GetPosRes() : 0;
122 const Double_t p1 = fMac2 ? (Double_t)fMac2->GetPdoPos2()/fMac2->GetPosRes() : 0;
123
124 return ZdAz(p1, pa);
125}
126
127// --------------------------------------------------------------------------
128//
129// check for a break-signal (from the msgqueue) and errors.
130//
131int MCosy::StopWaitingForSDO() const
132{
133 return 0/*Break() || HasError()*/;
134}
135
136// --------------------------------------------------------------------------
137//
138// Waits for a movement to become finished.
139//
140// First waits for all peding Sdos, then waits until both motors are stopped
141// or waiting for SDOs was stopped (either by an error or by Break)
142//
143void MCosy::WaitForEndMovement()
144{
145 // FIXME, what when waiting times out (Zombie)
146 if (!fMac1 || !fMac2)
147 return;
148
149 while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
150 !(Break() || HasError() || HasZombie()))
151 usleep(1);
152
153 if (!Break() && !HasError() && !HasZombie())
154 return;
155
156 MTime t(-1);
157 gLog << inf << t << " - MCosy::WaitForEndMovement aborted...";
158 if (Break())
159 gLog << " Break signal...";
160 if (HasError())
161 gLog << " Network has error...";
162 if (HasZombie())
163 gLog << " Network has zombie...";
164 gLog << endl;
165}
166
167// --------------------------------------------------------------------------
168//
169// Check for an error...
170//
171// This is ment for usage after the Action: All Motors Stop.
172//
173void MCosy::CheckForError()
174{
175 //
176 // Check all Can-Nodes for an Error. If there is no error the motor
177 // status is set to stopped.
178 //
179 if (HasError() || HasZombie())
180 {
181 SetStatus(MDriveCom::kError);
182 return;
183 }
184
185 if (fMac1->IsPositioning() || fMac2->IsPositioning())
186 SetStatus(MDriveCom::kMoving);
187 else
188 SetStatus(MDriveCom::kStopped);
189
190 //
191 // If there is an error, the error status is set to Error.
192 //
193
194 /*
195 FIXME: HANDLINGE ERROR
196
197 //
198 // Now try to handle the error.
199 //
200 fMac1->HandleError();
201 fMac2->HandleError();
202
203 //
204 // If the error couldn't get solved return
205 //
206 if (HasError())
207 return;
208
209 //
210 // Set motor status to stopped
211 //
212 SetStatus(MDriveCom::kStopped);
213 */
214}
215
216Bool_t MCosy::CheckRange(const ZdAz &d) const
217{
218 // d [rad]
219
220 if (d.Zd()<fMin.Zd())
221 {
222 gLog << err << "ERROR: Requested Zenith Angle " << d.Zd()*TMath::RadToDeg() << " below negative endswitch " << fMin.Zd()*TMath::RadToDeg() << endl;
223 return kFALSE;
224 }
225
226 if (d.Zd()>fMax.Zd())
227 {
228 gLog << err << "ERROR: Requested Zenith Angle " << d.Zd()*TMath::RadToDeg() << " behind positive endswitch " << fMax.Zd()*TMath::RadToDeg() << endl;
229 return kFALSE;
230 }
231
232 if (d.Az()<fMin.Az())
233 {
234 gLog << err << "ERROR: Requested Azimuth Angle " << d.Az()*TMath::RadToDeg() << " below negative endswitch " << fMin.Az()*TMath::RadToDeg() << endl;
235 if (TMath::Abs(d.Zd())<1)
236 gLog << " Remember that there is a small inaccessible region around zenith!" << endl;
237
238 return kFALSE;
239 }
240
241 if (d.Az()>fMax.Az())
242 {
243 gLog << err << "ERROR: Requested Azimuth Angle " << d.Az()*TMath::RadToDeg() << " behind positive endswitch " << fMax.Az()*TMath::RadToDeg() << endl;
244 if (TMath::Abs(d.Zd())<1)
245 gLog << " Remember that there is a small inaccessible region around zenith!" << endl;
246 return kFALSE;
247 }
248
249
250 return kTRUE;
251}
252
253ZdAz MCosy::AlignTrackingPos(ZdAz pointing) const
254{
255 // pointing [rad]
256 // AlignTrackingPos [deg]
257
258 pointing *= TMath::RadToDeg();
259
260 if (pointing.Zd()<0)
261 {
262 pointing.Zd(-pointing.Zd());
263 pointing.Az(pointing.Az()+180);
264 }
265
266 const ZdAz se = GetSePos()*TMath::TwoPi(); // [rad]
267 const ZdAz unbendedse = fBending.CorrectBack(se)*TMath::RadToDeg(); // ist pointing
268
269 do
270 {
271 const Double_t d = unbendedse.Az() - pointing.Az();
272 if (d>-180 && d<=180)
273 break;
274
275 pointing.Az(pointing.Az()+TMath::Sign(360., d));
276
277 } while (1);
278
279 return pointing/TMath::RadToDeg();
280/*
281 const Bool_t rc = CheckRange(pointing);
282 za = pointing/TMath::RadToDeg(); // [rad]
283
284 if (!rc)
285 gLog << "Error: Aligned position out of Range." << endl;
286
287 return rc;*/
288}
289
290/*
291Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
292{
293 ifstream fin("pointingpos.txt");
294 if (!fin)
295 return -1;
296
297 Double_t mjd0, zd, az;
298 fin >> mjd0 >> zd >> az;
299
300 mjd0 += 52000;
301
302 if (mjd0+1./24/60 <mjd)
303 return -1;
304
305 ZdAz point=AlignTrackingPos(ZdAz(zd, az)/TMath::RadToDeg());
306
307 const ZdAz diff = (dest-point)*TMath::RadToDeg();
308
309 if (diff.Zd()>5 || diff.Az()>5)
310 {
311 cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
312 return -1;
313 }
314
315 dest -= point;
316 dest *= -kGearTot/TMath::TwoPi(); // [re]
317
318 cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
319
320 return (mjd-mjd0) * (24*60*60); // [s]
321}
322*/
323
324// --------------------------------------------------------------------------
325//
326// Move the telescope to the given position. The position must be given in
327// a ZdAz object in rad.
328//
329// The first positioning is done absolutely. If we didn't reach the
330// correct psotion we try to correct for this by 10 relative position
331// maneuvers. If this doesn't help positioning failed.
332//
333// As a reference the shaftencoder values are used.
334//
335int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
336{
337 MSlewing point(this);
338
339 // Default: point.SetPointAcc(0.03, 0.01);
340 // Default: point.SetPointVelocity(0.3);
341 point.SetPointAcc(0.03, 0.01);
342 point.SetPointVelocity(0.3);
343
344 //point.SetPointAcc(0.09, 0.03);
345 //point.SetPointVelocity(1.0);
346
347 return point.SetPosition(dst, track);
348}
349
350void MCosy::TrackPlanet(Int_t id, Double_t offset, Double_t angle)
351{
352 MTracking track(this);
353 track.SetOut(fOutRep);
354
355 track.SetPointAcc(0.03, 0.01);
356 track.SetPointVelocity(0.3);
357 track.SetTrackAcc(0.01, 0.01);
358
359 track.SetWobble(offset, angle);
360 track.TrackPlanet((ePlanets_t)id);
361}
362
363void MCosy::TrackMoon(Double_t wobble, Double_t offset)
364{
365 MTracking track(this);
366 track.SetOut(fOutRep);
367
368 track.SetPointAcc(0.03, 0.01);
369 track.SetPointVelocity(0.3);
370 track.SetTrackAcc(0.01, 0.01);
371
372 track.TrackMoon(wobble, offset);
373}
374
375void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
376{
377 MTracking track(this);
378 track.SetOut(fOutRep);
379
380 track.SetPointAcc(0.03, 0.01);
381 track.SetPointVelocity(0.3);
382 track.SetTrackAcc(0.01, 0.01);
383
384 //track.SetWobbleOffset(offset, angle);
385 track.TrackPosition(dst);
386}
387
388void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
389{
390 TrackPosition(dst);
391 return;
392
393 MTracking track(this);
394 track.SetOut(fOutRep);
395 track.SetPointAcc(0.09, 0.03);
396 track.SetPointVelocity(1.0);
397 track.SetTrackAcc(0.01, 0.01);
398
399 //track.SetWobbleOffset(offset, angle);
400 track.TrackPosition(dst);
401}
402
403// --------------------------------------------------------------------------
404//
405// Stops the movement of both motors.
406//
407// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
408// stops. Quits the revolution mode and wait for the end of the movement.
409//
410void MCosy::StopMovement()
411{
412 //
413 // Set status to Stopping
414 //
415 SetStatus(MDriveCom::kStopping);
416
417 //
418 // set deceleration to 50%
419 //
420 gLog << inf2 << "Stopping movement..." << endl;
421 if (fMac1 && fMac2)
422 {
423 // FIXME: Makes sense?
424 fMac1->SetAcceleration(TMath::Nint(0.03*1000000000));
425 fMac2->SetAcceleration(TMath::Nint(0.09*1000000000));
426
427 fMac1->SetRpmMode(FALSE);
428 fMac2->SetRpmMode(FALSE);
429 }
430
431 //
432 // Wait for the movement to really be finished.
433 //
434#ifdef EXPERT
435 cout << "Waiting for end of movement..." << endl;
436#endif
437 WaitForEndMovement();
438
439 //
440 // Check whether everything works fine.
441 //
442 CheckForError();
443#ifdef EXPERT
444 cout << "Movement stopped." << endl;
445#endif
446}
447
448bool MCosy::CheckNetwork()
449{
450 if (!HasConnection())
451 {
452 gLog << warn << "- No connection to network." << endl;
453 return false;
454 }
455
456 CheckForError();
457
458 if (HasZombie())
459 {
460 gLog << warn << "- Found Zombies in Network..." << endl;
461 if (!RebootZombies())
462 return false;
463 }
464
465 /*
466 FIXME HANDLING ERROR
467 */
468 if (HasError())
469 {
470 fMac1->HandleError();
471 fMac2->HandleError();
472 if (HasError() || HasZombie())
473 return false;
474 }
475
476 CheckForError();
477
478 return fMac1->IsOperative() && fMac2->IsOperative();
479}
480
481Int_t MCosy::Proc(int msg, void *mp)
482{
483 cout << "*** Received " << hex << msg << endl;
484 switch (msg)
485 {
486 case WM_WAIT:
487 gLog << inf2 << "Wait for execution of Proc(WM_*, ): done." << endl;
488 return 0;
489
490 case WM_STOP:
491 //cout << "MCosy::Proc: Stop." << endl;
492 if (!CheckNetwork())
493 return 0xebb0;
494 StopMovement();
495 return 0;
496
497 case WM_TPOINT:
498#ifdef FACT
499 fStarguider->fTPoint->SetDown(kTRUE);
500#else
501 {
502 //cout << "WM_TPoint: start." << endl;
503 SlaStars sla(fObservatory);
504 sla.Now();
505
506 RaDec rd = *((RaDec*)mp);
507 cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
508
509 AltAz za=sla.CalcAltAz(rd*TMath::DegToRad())*TMath::RadToDeg();
510
511 if (!fOutTp)
512 {
513 //
514 // open tpoint file
515 //
516 const TString name = GetFileName("tpoint", "old-tpoint", "txt");
517 cout << "TPoint-Cosy File ********* " << name << " ********** " << endl;
518
519 fOutTp = new ofstream(name);
520 *fOutTp << "Magic Model TPOINT data file" << endl;
521 *fOutTp << ": ALTAZ" << endl;
522 *fOutTp << "49 48 0 ";
523 *fOutTp << sla.GetTime().Year() << " " << sla.GetTime().Month() << " " << sla.GetTime().Day() << " ";
524 *fOutTp << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
525 // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
526 }
527
528 cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
529 *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";
530
531 ZdAz sepos = GetSePos()*TMath::TwoPi();
532 za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
533 za *= TMath::RadToDeg();
534
535 cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
536 *fOutTp << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
537
538 if (fStarguider)
539 {
540 XY tp = fStarguider->GetCoordinates();
541 *fOutTp << 90-tp.X() << " " << tp.Y() << " ";
542 }
543
544 *fOutTp << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
545
546 //cout << "WM_TPoint: done. (return 0xaffe)" << endl;
547 }
548#endif
549 break;
550 case WM_STARGTPOINT:
551 if (fStarguider)
552 fStarguider->StartTPoint((char*)mp);
553 break;
554
555 case WM_STARGMODE:
556 if (fStarguider)
557 fStarguider->StartStarguider(*((bool*)mp));
558 break;
559
560 case WM_TRACKPOS:
561 //cout << "WM_TrackPosition: start." << endl;
562 {
563 if (!CheckNetwork())
564 return 0xebb0;
565
566 ZdAz dest = *((ZdAz*)mp) * TMath::DegToRad();
567 //if (!SetPosition(dest, kTRUE))
568 // return 0x1234;
569
570 SlaStars sla(fObservatory);
571 sla.Now();
572
573 RaDec rd = sla.CalcRaDec(dest);
574 TrackPosition(rd);
575 }
576 //cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
577 break;
578
579 case WM_LEDS:
580 {
581 if (!CheckNetwork())
582 return 0xebb0;
583
584 const Long_t *u = (Long_t*)mp;
585
586 fMac1->SetLedVoltage(u[0]);
587 fMac2->SetLedVoltage(u[1]);
588 }
589 break;
590
591 case WM_ARM:
592 //cout << "WM_Position: start." << endl;
593 {
594 if (!CheckNetwork())
595 return 0xebb0;
596
597 const bool arm = mp ? *((bool*)mp) : true;
598 if (arm)
599 {
600 fMac1->Arm();
601 fMac2->Arm();
602 }
603 else
604 {
605 fMac1->Disarm();
606 fMac2->Disarm();
607 }
608 }
609 //cout << "WM_Position: done. (return 0x7777)" << endl;
610 break;
611
612 case WM_POSITION:
613 //cout << "WM_Position: start." << endl;
614 {
615 if (!CheckNetwork())
616 return 0xebb0;
617
618 ZdAz dest = *((ZdAz*)mp);
619 SetPosition(dest*TMath::DegToRad());
620 }
621 //cout << "WM_Position: done. (return 0x7777)" << endl;
622 break;
623
624 case WM_POSITION1:
625 //cout << "WM_Position1: start." << endl;
626 {
627 if (!CheckNetwork())
628 return 0xebb0;
629
630 ZdAz dest = *((ZdAz*)mp);
631 SetPosition(dest*TMath::DegToRad(), kTRUE);
632 }
633 //cout << "WM_Position: done. (return 0x7777)" << endl;
634 break;
635
636 case WM_PREPS:
637 //cout << "WM_Track: START" << endl;
638 {
639 if (!CheckNetwork())
640 return 0xebb0;
641
642 const char *preps = (const char*)mp;
643 cout << "Preposition command to " << preps << " received." << endl;
644
645 ifstream fin(fFilePrepos);
646 if (!fin)
647 {
648 cout << "ERROR: cannot open " << fFilePrepos << endl;
649 return 0xebb1;
650 }
651
652 while (1)
653 {
654 Double_t zd, az;
655 fin >> zd >> az;
656
657 TString str;
658 str.ReadLine(fin);
659 if (!fin)
660 break;
661
662 str.ToLower();
663
664 if (str.Strip(TString::kBoth)==preps)
665 {
666 ZdAz dest(zd, az);
667 SetPosition(dest*TMath::DegToRad());
668 return 0x7979;
669 }
670 cout << "ERROR - Requested preposition not found in file..." << endl;
671 }
672 }
673 //cout << "WM_Track: done. (return 0x8888)" << endl;
674 break;
675/*
676 case WM_TESTSE:
677 //cout << "WM_TestSe: start." << endl;
678 fBackground = mp ? kBgdSeTest : kBgdNone;
679 //cout << "WM_TestSe: done. (return 0x1e51)" << endl;
680 return 0x1e51;
681
682 case WM_GEAR:
683 //cout << "WM_Gear: start." << endl;
684 fBackground = mp ? kBgdGear : kBgdNone;
685 //cout << "WM_Gear: done. (return 0xfeaf)" << endl;
686 return 0xfeaf;
687
688 case WM_DISPLAY:
689 //cout << "WM_Display: start." << endl;
690 fTriggerDisplay = kTRUE;
691 //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
692 return 0xd1e1;
693 */
694 case WM_TRACK:
695 case WM_GRB:
696 //cout << "WM_Track/GRB: START" << endl;
697 {
698 RaDec dest = ((RaDec*)mp)[0];
699 if (!CheckNetwork())
700 return 0xebb0;
701
702 if (msg==WM_TRACK)
703 TrackPosition(dest*TMath::DegToRad());
704 else
705 TrackPositionGRB(dest*TMath::DegToRad());
706 }
707 //cout << "WM_Track/GRB: done. (return 0x8888)" << endl;
708 break;
709
710 case WM_CELEST:
711 //cout << "WM_PLANET: START" << endl;
712 {
713 if (!CheckNetwork())
714 return 0xebb1;
715
716 const Double_t *d = (Double_t*)mp;
717
718 const Int_t id = TMath::Nint(d[0]);
719 const Double_t offset = d[1];
720 const Double_t angle = d[2];
721
722 TrackPlanet(id, offset, angle);
723 }
724 //cout << "WM_PLANET: done. (return 0x8889)" << endl;
725 break;
726
727 case WM_MOON:
728 //cout << "WM_PLANET: START" << endl;
729 {
730 if (!CheckNetwork())
731 return 0xebb1;
732
733 const Double_t *d = (Double_t*)mp;
734
735 const Double_t wobble = d[0];
736 const Double_t offset = d[1];
737
738 TrackMoon(wobble, offset);
739 }
740 //cout << "WM_PLANET: done. (return 0x8889)" << endl;
741 break;
742/*
743 case WM_NEWTRACK:
744 //cout << "WM_NewTrack: START" << endl;
745 fRaDec = *((RaDec*)mp);
746 //cout << "WM_NewTrack: done. (return 0x9999)" << endl;
747 return 0x9999;
748*/
749 case WM_LOADBENDING:
750 //cout << "WM_LoadBending: START" << endl;
751 fBending.Load("bending.txt");
752 //cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
753 break;
754
755 case WM_RESETBENDING:
756 //cout << "WM_ResetBending: START" << endl;
757 fBending.Reset();
758 //cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
759 break;
760
761 case WM_CALCALTAZ:
762 {
763 cout << endl;
764
765 SlaStars sla(fObservatory);
766 sla.Now();
767
768 XY xy = *((XY*)mp);
769 RaDec rd(xy.X()*15., xy.Y()); // [deg]
770
771 ZdAz a1 = sla.CalcZdAz(rd*TMath::DegToRad()); // [rad]
772
773 cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
774 cout << "Zd/Az target: " << a1.Zd()*TMath::RadToDeg() << "° " << a1.Az()*TMath::RadToDeg() << "°" << endl;
775
776 if (fMac1 && fMac2)
777 a1 = AlignTrackingPos(a1);
778
779 a1 = fBending(a1);
780 CheckRange(a1);
781 a1 *= TMath::RadToDeg();
782
783 const ZdAz a2 = a1/360;
784
785 cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
786 cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
787 }
788 break;
789
790 case WM_ENDSWITCH:
791 {
792 ZdAz pos = GetSePos()*TMath::TwoPi();
793 pos = fBending.SubtractOffsets(pos)*TMath::RadToDeg();
794
795 cout << "Endswitch Position: Zd=" << pos.Zd() << "° Az=";
796 cout << pos.Az() << "°" << endl;
797 }
798 break;
799
800 case WM_QUIT:
801 cout << "WM_Quit: now." << endl;
802 if (!CheckNetwork())
803 {
804 gLog << err << "ERROR: Cannot shutdown network." << endl;
805 gLog << " Please shutdown the drive system manually" << endl;
806 }
807 TerminateApp();
808 cout << "WM_Quit: done." << endl;
809 break;
810
811 default:
812 cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
813 return 0xffffffff;
814 }
815
816 return msg;
817}
818
819void MCosy::ReadConfig(MEnv &env)
820{
821 gLog << inf2 << "Reading telescope range..." << flush;
822 const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
823 const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
824 fMin.Set(zmin, amin);
825
826 const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
827 const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
828 fMax.Set(zmax, amax);
829 gLog << "done." << endl;
830
831 gLog << all << flush;
832
833 gLog << " * Min: " << zmin << "deg " << amin << "deg" << endl;
834 gLog << " * Max: " << zmax << "deg " << amax << "deg" << endl;
835
836 fMin = fBending.AddOffsets(fMin*TMath::DegToRad());
837 fMax = fBending.AddOffsets(fMax*TMath::DegToRad());
838
839 gLog << " * Min': " << fMin.Zd()*TMath::RadToDeg() << "deg " << fMin.Az()*TMath::RadToDeg() << "deg" << endl;
840 gLog << " * Max': " << fMax.Zd()*TMath::RadToDeg() << "deg " << fMax.Az()*TMath::RadToDeg() << "deg" << endl;
841}
842
843ZdAz MCosy::GetPointingPos() const
844{
845 if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
846 return ZdAz(0, 0);
847
848 // GetPointingPos [deg]
849 const ZdAz seist = GetSePos()*TMath::TwoPi(); // [rad]
850 return fBending.CorrectBack(seist)*TMath::RadToDeg();
851}
852
853Bool_t MCosy::HandleTimer(TTimer *t)
854{
855 const Int_t rc = fMutexGui.TryLock();
856 if (rc==13)
857 gLog << warn << "MCosy::HandleTimer - mutex is already locked by this thread" << endl;
858
859 if (rc)
860 {
861 gLog << warn << "* GUI update skipped due to locked mutex." << endl;
862 return kTRUE;
863 }
864
865 //
866 // Update Gui, foremer MTGui.
867 //
868 if (fMac1)
869 fMac1->DisplayVal();
870 if (fMac2)
871 fMac2->DisplayVal();
872
873 /*
874 Byte_t avail = 0;
875
876 avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
877 avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
878// avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
879 */
880 Bool_t armed = kTRUE;
881
882 armed &= fMac1 && fMac1->IsArmed();
883 armed &= fMac2 && fMac2->IsArmed();
884
885#ifndef FACT // FIXME: Maybe this is also correct for magic?
886 if (fMac1 && fMac2)
887 {
888 SetStatus(MDriveCom::kStopped);
889 if (fMac1->IsPositioning() || fMac2->IsPositioning())
890 SetStatus(MDriveCom::kMoving);
891 if (fMac1->IsRpmActive() || fMac2->IsRpmActive())
892 SetStatus(MDriveCom::kTracking);
893 }
894#else
895 if (fMac1 && fMac2)
896 {
897 // Handle remote control introduced state changed
898 // FIXME: Use a bit in the PD3 to signal remote control operation!
899 if (fStatus==MDriveCom::kStopped && (fMac1->IsPositioning() || fMac2->IsPositioning()))
900 SetStatus(MDriveCom::kMoving);
901 if (fStatus==MDriveCom::kMoving && !fMac1->IsPositioning() && !fMac2->IsPositioning())
902 SetStatus(MDriveCom::kStopped);
903 }
904#endif
905
906 if (HasError())
907 SetStatus(MDriveCom::kError);
908
909 gLog.UpdateGui();
910
911 const TString stataz = fMac1 ? fMac1->GetStatusDKC() : "";
912 const TString statzd = fMac2 ? fMac2->GetStatusDKC() : "";
913
914 const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0;
915 const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0;
916
917 ZdAz bendist = GetPointingPos();
918
919 //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
920
921 static MTimeout tout(0);
922 if (tout.HasTimedOut())
923 {
924 tout.Start(999);
925 fCom->SendReport(fStatus, fMJD, fRaDec, fHourAngle, fZdAzSoll, bendist, fTrackingError, armed,
926 fStarguider ? fStarguider->GetStarguiderMode() : 0, ((stat1&0xffff)<<16)|(stat2&0xffff));
927 }
928
929 fWin->UpdateWeather(*fCom);
930 fWin->Update(bendist, fTrackingError, /*fVelocity, fOffset,*/
931 fRaDec, fZdAzSoll, fStatus, ((stat1&0xffff)<<16)|(stat2&0xffff), HasConnection(), armed, statzd, stataz);
932
933 gLog.UpdateGui();
934
935 if (fMutexGui.UnLock()==13)
936 gLog << warn << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl;
937
938 return kTRUE;
939}
940
941// --------------------------------------------------------------------------
942//
943// Start the work of the application:
944//
945// Start the Can-Network.
946// Start the MCosy::TalkThread thread.
947// turn on the gui update
948//
949void MCosy::Start(MEnv &env)
950{
951 // Don't call this function twice!
952 Network::Start();
953
954 CheckForError();
955
956 ReadConfig(env);
957
958 gLog << inf << "- Starting GUI update." << endl;
959 fUpdateGui->TurnOn();
960}
961
962// --------------------------------------------------------------------------
963//
964// Start the work of the application:
965//
966// Turn of the gui update
967// stop the MCosy::TalkThread thread.
968// Stop the network
969//
970void MCosy::Stop()
971{
972 gLog << inf << "- Stopping GUI update." << endl;
973 fUpdateGui->TurnOff();
974 gLog << inf << "- GUI Update stopped." << endl;
975
976 gLog << inf << "- Stopping CAN network." << endl;
977 Network::Stop();
978 gLog << inf << "- CAN network stopped." << endl;
979
980 gLog << inf << "- Stopping message queue." << endl;
981 CancelThread();
982 gLog << inf << "- Message queue stopped." << endl;
983}
984
985TString MCosy::GetFileName(const char *path, const char *name, const char *ext)
986{
987 // FIXME: Timeout missing
988
989 while (1)
990 {
991 MTime time(-1);
992
993 // This is the full qualified date which is part of the name
994 const TString clock = time.GetStringFmt("%Y%m%d_%H%M%S");
995
996 // This gives the night in which the date belongs to
997 time.SetMjd(TMath::Nint(time.GetMjd()));
998
999 const TString night = time.GetStringFmt("%Y_%m_%d");
1000
1001 const TString dir = Form("%s/%s", path, night.Data());
1002 const TString fname = Form("%s_%s.%s", name, clock.Data(), ext);
1003
1004 const TString full = Form("%s/%s", dir.Data(), fname.Data());
1005
1006 gSystem->mkdir(dir, kTRUE);
1007
1008 if (gSystem->AccessPathName(full, kFileExists))
1009 return full;
1010
1011 break;// !!!!!!!!!!!!!!!!!!!!!!!
1012
1013 usleep(1000);
1014 }
1015 return "";
1016}
1017
1018MCosy::MCosy(MEnv &env, MDriveCom *com)
1019: Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
1020fMac1(0), fMac2(0), fMJD(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
1021{
1022 const Int_t id1 = env.GetValue("Az_Id", 1);
1023 const Int_t id2 = env.GetValue("Zd_Id", 3);
1024
1025 fFilePrepos = env.GetValue("FilePredefinedPositions", "prepos.txt");
1026
1027 TString name = GetFileName("rep", "cosy", "rep");
1028 gLog << inf << "Open Repfile: " << name << endl;
1029 fOutRep = new MLog(name, kTRUE);
1030 *fOutRep << all << "[Drive Report File]" << endl;
1031 *fOutRep << "Version <cvs>" << endl;
1032 *fOutRep << "Date " << MTime(-1) << endl;
1033 *fOutRep << "[Reports]" << endl;
1034
1035 const TString pointing = env.GetValue("PointingModel", "bending.txt");
1036
1037 gLog << all << "Reading pointing model from " << pointing << "..." << endl;
1038 if (fBending.Load(pointing))
1039 gLog << all << "Reading pointing model from " << pointing << " successfull." << endl;
1040 else
1041 gLog << err << "ERROR - Reading pointing model from " << pointing << endl;
1042
1043 //
1044 // Create Nodes
1045 //
1046 gLog << inf << "- Setting up network." << endl;
1047
1048 fMac1=new Dkc(id1, "DKC/Az");
1049 fMac2=new Dkc(id2, "DKC/Zd");
1050
1051 fMac1->SetReport(fOutRep);
1052 fMac2->SetReport(fOutRep);
1053
1054 gLog << inf << "- Connecting devices to network." << endl;
1055
1056 //
1057 // Connect the devices to the network
1058 //
1059 SetNode(fMac1);
1060 SetNode(fMac2);
1061
1062 //
1063 // Create Gui Event timer and Gui
1064 //
1065 gLog << inf << "- Initializing GUI Timer." << endl;
1066 fUpdateGui = new TTimer(this, 100); // 100ms
1067
1068 gLog << all << "- Starting GUI." << endl;
1069 fWin=new MGCosy(fObservatory, fFilePrepos, this);
1070
1071 gLog.SetOutputGui(fWin->GetLog(), kTRUE);
1072
1073 fMac2->SetDisplay(fWin->GetLabel2());
1074 fMac1->SetDisplay(fWin->GetLabel1());
1075
1076 fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
1077#ifndef FACT
1078 fCom->SetOutRep(fOutRep);
1079#endif
1080 // fCom->Start();
1081}
1082
1083void MCosy::TerminateApp()
1084{
1085 gLog << inf2 << "MCosy::TerminateApp()" << endl;
1086/*
1087 Int_t rc;
1088 TGMessageBox msg(this, gClient->GetRoot(),
1089 "Information",
1090 "Cosy is shutting down the system - this may take wa while!",
1091 kMBIconExclamation,
1092 kMBOK, //kMBClose
1093 &rc, 0);
1094*/
1095
1096 gLog.DisableOutputDevice(MLog::eGui);
1097 // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
1098 // gLog.SetOutputGui(NULL, kFALSE);
1099
1100 gApplication->Terminate(0);
1101}
1102
1103MCosy::~MCosy()
1104{
1105 if(fCom)
1106 {
1107 fCom->SetMsgQueue(NULL);
1108 fCom->SetOutRep(NULL);
1109 }
1110
1111 gLog << inf2 << "Deleting GUI timer." << endl;
1112 // FIXME: Wait until last Update was finished!!!
1113 delete fUpdateGui;
1114
1115 //fMutexGui.Lock();
1116
1117 // Now the files can safely be closed
1118 gLog << inf2 << "Closing output files." << endl;
1119 if (fOutTp)
1120 {
1121 *fOutTp << "END" << endl;
1122 delete fOutTp;
1123 }
1124
1125 delete fOutRep;
1126
1127 //gLog << inf2 << "Deleting CC communication." << endl;
1128 //delete fCom;
1129
1130 gLog << inf2 << "Deleting Nodes." << endl;
1131 fMac1->SetReport(0);
1132 fMac2->SetReport(0);
1133
1134 delete fMac1;
1135 delete fMac2;
1136
1137 gLog << inf2 << "Deleting MGCosy." << endl;
1138
1139 gLog.DisableOutputDevice(MLog::eGui);
1140
1141 delete fWin;
1142
1143 gLog << inf2 << "MGCosy destructed." << endl;
1144}
Note: See TracBrowser for help on using the repository browser.