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

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