source: trunk/MagicSoft/Cosy/main/MCosy.cc@ 9403

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