Context Navigation

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

Visit:

Last change on this file since 12582 was 10035, checked in by tbretz, 14 years ago
Fixed indention.
File size: 28.9 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
26	ClassImp(MCosy);
27
28	using namespace std;
29
30	typedef struct tm tm_t;
31
32	//#define EXPERT
33	#undef EXPERT
34
35	/*
36	ZdAz 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	//
118	ZdAz 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	//
130	int 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	//
142	void 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	//
172	void 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
215	Bool_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
252	ZdAz 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	//gLog << "ZD=-ZD Az+=180" << endl;
264	}
265
266	const ZdAz se = GetSePos()*TMath::TwoPi(); // [rad]
267	const ZdAz unbendedse = fBending.CorrectBack(se)*TMath::RadToDeg(); // ist pointing
268
269	//gLog << "Unbended: " << unbendedse.Zd() << " " << unbendedse.Az() << endl;
270
271	do
272	{
273	const Double_t d = unbendedse.Az() - pointing.Az();
274	if (d>-180 && d<=180)
275	break;
276
277	//gLog << "AZ += " << TMath::Sign(360., d) << endl;
278
279	pointing.Az(pointing.Az()+TMath::Sign(360., d));
280	} while (1);
281
282	return pointing/TMath::RadToDeg();
283	/*
284	const Bool_t rc = CheckRange(pointing);
285	za = pointing/TMath::RadToDeg(); // [rad]
286
287	if (!rc)
288	gLog << "Error: Aligned position out of Range." << endl;
289
290	return rc;*/
291	}
292
293	/*
294	Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
295	{
296	ifstream fin("pointingpos.txt");
297	if (!fin)
298	return -1;
299
300	Double_t mjd0, zd, az;
301	fin >> mjd0 >> zd >> az;
302
303	mjd0 += 52000;
304
305	if (mjd0+1./24/60 <mjd)
306	return -1;
307
308	ZdAz point=AlignTrackingPos(ZdAz(zd, az)/TMath::RadToDeg());
309
310	const ZdAz diff = (dest-point)*TMath::RadToDeg();
311
312	if (diff.Zd()>5 \|\| diff.Az()>5)
313	{
314	cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
315	return -1;
316	}
317
318	dest -= point;
319	dest *= -kGearTot/TMath::TwoPi(); // [re]
320
321	cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
322
323	return (mjd-mjd0) * (246060); // [s]
324	}
325	*/
326
327	// --------------------------------------------------------------------------
328	//
329	// Move the telescope to the given position. The position must be given in
330	// a ZdAz object in rad.
331	//
332	// The first positioning is done absolutely. If we didn't reach the
333	// correct psotion we try to correct for this by 10 relative position
334	// maneuvers. If this doesn't help positioning failed.
335	//
336	// As a reference the shaftencoder values are used.
337	//
338	int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
339	{
340	MSlewing point(this);
341
342	// Default: point.SetPointAcc(0.03, 0.01);
343	// Default: point.SetPointVelocity(0.3);
344	point.SetPointAcc(0.03, 0.01);
345	point.SetPointVelocity(0.3);
346
347	//point.SetPointAcc(0.09, 0.03);
348	//point.SetPointVelocity(1.0);
349
350	return point.SetPosition(dst, track);
351	}
352
353	void MCosy::TrackPlanet(Int_t id, Double_t offset, Double_t angle)
354	{
355	MTracking track(this);
356	track.SetOut(fOutRep);
357
358	track.SetPointAcc(0.03, 0.01);
359	track.SetPointVelocity(0.3);
360	track.SetTrackAcc(0.01, 0.01);
361
362	track.SetWobble(offset, angle);
363	track.TrackPlanet((ePlanets_t)id);
364	}
365
366	void MCosy::TrackMoon(Double_t wobble, Double_t offset)
367	{
368	MTracking track(this);
369	track.SetOut(fOutRep);
370
371	track.SetPointAcc(0.03, 0.01);
372	track.SetPointVelocity(0.3);
373	track.SetTrackAcc(0.01, 0.01);
374
375	track.TrackMoon(wobble, offset);
376	}
377
378	void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
379	{
380	MTracking track(this);
381	track.SetOut(fOutRep);
382
383	track.SetPointAcc(0.03, 0.01);
384	track.SetPointVelocity(0.3);
385	track.SetTrackAcc(0.01, 0.01);
386
387	//track.SetWobbleOffset(offset, angle);
388	track.TrackPosition(dst);
389	}
390
391	void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
392	{
393	TrackPosition(dst);
394	return;
395
396	MTracking track(this);
397	track.SetOut(fOutRep);
398	track.SetPointAcc(0.09, 0.03);
399	track.SetPointVelocity(1.0);
400	track.SetTrackAcc(0.01, 0.01);
401
402	//track.SetWobbleOffset(offset, angle);
403	track.TrackPosition(dst);
404	}
405
406	// --------------------------------------------------------------------------
407	//
408	// Stops the movement of both motors.
409	//
410	// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
411	// stops. Quits the revolution mode and wait for the end of the movement.
412	//
413	void MCosy::StopMovement()
414	{
415	//
416	// Set status to Stopping
417	//
418	SetStatus(MDriveCom::kStopping);
419
420	//
421	// set deceleration to 50%
422	//
423	gLog << inf2 << "Stopping movement..." << endl;
424	if (fMac1 && fMac2)
425	{
426	// FIXME: Makes sense?
427	fMac1->SetAcceleration(TMath::Nint(0.03*1000000000));
428	fMac2->SetAcceleration(TMath::Nint(0.09*1000000000));
429
430	fMac1->SetRpmMode(FALSE);
431	fMac2->SetRpmMode(FALSE);
432	}
433
434	//
435	// Wait for the movement to really be finished.
436	//
437	#ifdef EXPERT
438	cout << "Waiting for end of movement..." << endl;
439	#endif
440	WaitForEndMovement();
441
442	//
443	// Check whether everything works fine.
444	//
445	CheckForError();
446	#ifdef EXPERT
447	cout << "Movement stopped." << endl;
448	#endif
449	}
450
451	bool MCosy::CheckNetwork()
452	{
453	if (!HasConnection())
454	{
455	gLog << warn << "- No connection to network." << endl;
456	return false;
457	}
458
459	CheckForError();
460
461	if (HasZombie())
462	{
463	gLog << warn << "- Found Zombies in Network..." << endl;
464	if (!RebootZombies())
465	return false;
466	}
467
468	/*
469	FIXME HANDLING ERROR
470	*/
471	if (HasError())
472	{
473	fMac1->HandleError();
474	fMac2->HandleError();
475	if (HasError() \|\| HasZombie())
476	return false;
477	}
478
479	CheckForError();
480
481	return fMac1->IsOperative() && fMac2->IsOperative();
482	}
483
484	Int_t MCosy::Proc(int msg, void *mp)
485	{
486	switch (msg)
487	{
488	case WM_WAIT:
489	gLog << inf2 << "Wait for execution of Proc(WM_*, ): done." << endl;
490	return 0;
491
492	case WM_STOP:
493	//cout << "MCosy::Proc: Stop." << endl;
494	if (!CheckNetwork())
495	return 0xebb0;
496	StopMovement();
497	return 0;
498
499	case WM_TPOINT:
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(rdTMath::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	break;
548
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_ARM:
579	//cout << "WM_Position: start." << endl;
580	{
581	if (!CheckNetwork())
582	return 0xebb0;
583
584	const bool arm = mp ? ((bool)mp) : true;
585	if (arm)
586	{
587	fMac1->Arm();
588	fMac2->Arm();
589	}
590	else
591	{
592	fMac1->Disarm();
593	fMac2->Disarm();
594	}
595	}
596	//cout << "WM_Position: done. (return 0x7777)" << endl;
597	break;
598
599	case WM_POSITION:
600	//cout << "WM_Position: start." << endl;
601	{
602	if (!CheckNetwork())
603	return 0xebb0;
604
605	ZdAz dest = ((ZdAz)mp);
606	SetPosition(dest*TMath::DegToRad());
607	}
608	//cout << "WM_Position: done. (return 0x7777)" << endl;
609	break;
610
611	case WM_POSITION1:
612	//cout << "WM_Position1: start." << endl;
613	{
614	if (!CheckNetwork())
615	return 0xebb0;
616
617	ZdAz dest = ((ZdAz)mp);
618	SetPosition(dest*TMath::DegToRad(), kTRUE);
619	}
620	//cout << "WM_Position: done. (return 0x7777)" << endl;
621	break;
622
623	case WM_PREPS:
624	//cout << "WM_Track: START" << endl;
625	{
626	if (!CheckNetwork())
627	return 0xebb0;
628
629	const char preps = (const char)mp;
630	cout << "Preposition command to " << preps << " received." << endl;
631
632	ifstream fin(fFilePrepos);
633	if (!fin)
634	{
635	cout << "ERROR: cannot open " << fFilePrepos << endl;
636	return 0xebb1;
637	}
638
639	while (1)
640	{
641	Double_t zd, az;
642	fin >> zd >> az;
643
644	TString str;
645	str.ReadLine(fin);
646	if (!fin)
647	break;
648
649	str.ToLower();
650
651	if (str.Strip(TString::kBoth)==preps)
652	{
653	ZdAz dest(zd, az);
654	SetPosition(dest*TMath::DegToRad());
655	return 0x7979;
656	}
657	cout << "ERROR - Requested preposition not found in file..." << endl;
658	}
659	}
660	//cout << "WM_Track: done. (return 0x8888)" << endl;
661	break;
662	/*
663	case WM_TESTSE:
664	//cout << "WM_TestSe: start." << endl;
665	fBackground = mp ? kBgdSeTest : kBgdNone;
666	//cout << "WM_TestSe: done. (return 0x1e51)" << endl;
667	return 0x1e51;
668
669	case WM_GEAR:
670	//cout << "WM_Gear: start." << endl;
671	fBackground = mp ? kBgdGear : kBgdNone;
672	//cout << "WM_Gear: done. (return 0xfeaf)" << endl;
673	return 0xfeaf;
674
675	case WM_DISPLAY:
676	//cout << "WM_Display: start." << endl;
677	fTriggerDisplay = kTRUE;
678	//cout << "WM_Disply: done. (return 0xd1e1)" << endl;
679	return 0xd1e1;
680	*/
681	case WM_TRACK:
682	case WM_GRB:
683	//cout << "WM_Track/GRB: START" << endl;
684	{
685	RaDec dest = ((RaDec*)mp)[0];
686	if (!CheckNetwork())
687	return 0xebb0;
688
689	if (msg==WM_TRACK)
690	TrackPosition(dest*TMath::DegToRad());
691	else
692	TrackPositionGRB(dest*TMath::DegToRad());
693	}
694	//cout << "WM_Track/GRB: done. (return 0x8888)" << endl;
695	break;
696
697	case WM_CELEST:
698	//cout << "WM_PLANET: START" << endl;
699	{
700	if (!CheckNetwork())
701	return 0xebb1;
702
703	const Double_t d = (Double_t)mp;
704
705	const Int_t id = TMath::Nint(d[0]);
706	const Double_t offset = d[1];
707	const Double_t angle = d[2];
708
709	TrackPlanet(id, offset, angle);
710	}
711	//cout << "WM_PLANET: done. (return 0x8889)" << endl;
712	break;
713
714	case WM_MOON:
715	//cout << "WM_PLANET: START" << endl;
716	{
717	if (!CheckNetwork())
718	return 0xebb1;
719
720	const Double_t d = (Double_t)mp;
721
722	const Double_t wobble = d[0];
723	const Double_t offset = d[1];
724
725	TrackMoon(wobble, offset);
726	}
727	//cout << "WM_PLANET: done. (return 0x8889)" << endl;
728	break;
729	/*
730	case WM_NEWTRACK:
731	//cout << "WM_NewTrack: START" << endl;
732	fRaDec = ((RaDec)mp);
733	//cout << "WM_NewTrack: done. (return 0x9999)" << endl;
734	return 0x9999;
735	*/
736	case WM_LOADBENDING:
737	//cout << "WM_LoadBending: START" << endl;
738	fBending.Load("bending.txt");
739	//cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
740	break;
741
742	case WM_RESETBENDING:
743	//cout << "WM_ResetBending: START" << endl;
744	fBending.Reset();
745	//cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
746	break;
747
748	case WM_CALCALTAZ:
749	{
750	cout << endl;
751
752	SlaStars sla(fObservatory);
753	sla.Now();
754
755	XY xy = ((XY)mp);
756	RaDec rd(xy.X()*15., xy.Y()); // [deg]
757
758	ZdAz a1 = sla.CalcZdAz(rd*TMath::DegToRad()); // [rad]
759
760	cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
761	cout << "Zd/Az target: " << a1.Zd()TMath::RadToDeg() << "° " << a1.Az()TMath::RadToDeg() << "°" << endl;
762
763	if (fMac1 && fMac2)
764	a1 = AlignTrackingPos(a1);
765
766	a1 = fBending(a1);
767	CheckRange(a1);
768	a1 *= TMath::RadToDeg();
769
770	const ZdAz a2 = a1/360;
771
772	cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
773	cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
774	}
775	break;
776
777	case WM_ENDSWITCH:
778	{
779	ZdAz pos = GetSePos()*TMath::TwoPi();
780	pos = fBending.SubtractOffsets(pos)*TMath::RadToDeg();
781
782	cout << "Endswitch Position: Zd=" << pos.Zd() << "° Az=";
783	cout << pos.Az() << "°" << endl;
784	}
785	break;
786
787	case WM_QUIT:
788	cout << "WM_Quit: now." << endl;
789	if (!CheckNetwork())
790	{
791	gLog << err << "ERROR: Cannot shutdown network." << endl;
792	gLog << " Please shutdown the drive system manually" << endl;
793	}
794	TerminateApp();
795	cout << "WM_Quit: done." << endl;
796	break;
797
798	default:
799	cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
800	return 0xffffffff;
801	}
802
803	return msg;
804	}
805
806	void MCosy::ReadConfig(MEnv &env)
807	{
808	gLog << inf2 << "Reading telescope range..." << flush;
809	const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
810	const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
811	fMin.Set(zmin, amin);
812
813	const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
814	const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
815	fMax.Set(zmax, amax);
816	gLog << "done." << endl;
817
818	gLog << all << flush;
819
820	gLog << " * Min: " << zmin << "deg " << amin << "deg" << endl;
821	gLog << " * Max: " << zmax << "deg " << amax << "deg" << endl;
822
823	fMin = fBending.AddOffsets(fMin*TMath::DegToRad());
824	fMax = fBending.AddOffsets(fMax*TMath::DegToRad());
825
826	gLog << " * Min': " << fMin.Zd()TMath::RadToDeg() << "deg " << fMin.Az()TMath::RadToDeg() << "deg" << endl;
827	gLog << " * Max': " << fMax.Zd()TMath::RadToDeg() << "deg " << fMax.Az()TMath::RadToDeg() << "deg" << endl;
828	}
829
830	ZdAz MCosy::GetPointingPos() const
831	{
832	if (fMac1->IsZombieNode() \|\| fMac2->IsZombieNode())
833	return ZdAz(0, 0);
834
835	// GetPointingPos [deg]
836	const ZdAz seist = GetSePos()*TMath::TwoPi(); // [rad]
837	return fBending.CorrectBack(seist)*TMath::RadToDeg();
838	}
839
840	Bool_t MCosy::HandleTimer(TTimer *t)
841	{
842	const Int_t rc = fMutexGui.TryLock();
843	if (rc==13)
844	gLog << warn << "MCosy::HandleTimer - mutex is already locked by this thread" << endl;
845
846	if (rc)
847	{
848	gLog << warn << "* GUI update skipped due to locked mutex." << endl;
849	return kTRUE;
850	}
851
852	//
853	// Update Gui, foremer MTGui.
854	//
855	if (fMac1)
856	fMac1->DisplayVal();
857	if (fMac2)
858	fMac2->DisplayVal();
859
860	/*
861	Byte_t avail = 0;
862
863	avail \|= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
864	avail \|= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
865	// avail \|= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
866	*/
867	Bool_t armed = kTRUE;
868
869	armed &= fMac1 && fMac1->IsArmed();
870	armed &= fMac2 && fMac2->IsArmed();
871
872	if (fMac1 && fMac2)
873	{
874	SetStatus(MDriveCom::kStopped);
875	if (fMac1->IsPositioning() \|\| fMac2->IsPositioning())
876	SetStatus(MDriveCom::kMoving);
877	if (fMac1->IsRpmActive() \|\| fMac2->IsRpmActive())
878	SetStatus(MDriveCom::kTracking);
879	}
880
881	if (HasError())
882	SetStatus(MDriveCom::kError);
883
884	gLog.UpdateGui();
885
886	const TString stataz = fMac1 ? fMac1->GetStatusDKC() : "";
887	const TString statzd = fMac2 ? fMac2->GetStatusDKC() : "";
888
889	const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0;
890	const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0;
891
892	ZdAz bendist = GetPointingPos();
893
894	//cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
895
896	static MTimeout tout(0);
897	if (tout.HasTimedOut())
898	{
899	tout.Start(999);
900	fCom->SendReport(fStatus, fRaDec, fHourAngle, fZdAzSoll, bendist, fTrackingError, armed,
901	fStarguider ? fStarguider->GetStarguiderMode() : 0);
902	}
903
904	fWin->UpdateWeather(*fCom);
905	fWin->Update(bendist, fTrackingError, /fVelocity, fOffset,/
906	fRaDec, fZdAzSoll, fStatus, ((stat1&0xff)<<8)\|(stat2&0xff), HasConnection(), armed, statzd, stataz);
907
908	gLog.UpdateGui();
909
910	if (fMutexGui.UnLock()==13)
911	gLog << warn << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl;
912
913	return kTRUE;
914	}
915
916	// --------------------------------------------------------------------------
917	//
918	// Start the work of the application:
919	//
920	// Start the Can-Network.
921	// Start the MCosy::TalkThread thread.
922	// turn on the gui update
923	//
924	void MCosy::Start(MEnv &env)
925	{
926	// Don't call this function twice!
927	Network::Start();
928
929	CheckForError();
930
931	ReadConfig(env);
932
933	gLog << inf << "- Starting GUI update." << endl;
934	fUpdateGui->TurnOn();
935	}
936
937	// --------------------------------------------------------------------------
938	//
939	// Start the work of the application:
940	//
941	// Turn of the gui update
942	// stop the MCosy::TalkThread thread.
943	// Stop the network
944	//
945	void MCosy::Stop()
946	{
947	gLog << inf << "- Stopping GUI update." << endl;
948	fUpdateGui->TurnOff();
949	gLog << inf << "- GUI Update stopped." << endl;
950
951	gLog << inf << "- Stopping CAN network." << endl;
952	Network::Stop();
953	gLog << inf << "- CAN network stopped." << endl;
954
955	gLog << inf << "- Stopping message queue." << endl;
956	CancelThread();
957	gLog << inf << "- Message queue stopped." << endl;
958	}
959
960	TString MCosy::GetFileName(const char path, const char name, const char *ext)
961	{
962	// FIXME: Timeout missing
963
964	while (1)
965	{
966	MTime time(-1);
967
968	// This is the full qualified date which is part of the name
969	const TString clock = time.GetStringFmt("%Y%m%d_%H%M%S");
970
971	// This gives the night in which the date belongs to
972	time.SetMjd(TMath::Nint(time.GetMjd()));
973
974	const TString night = time.GetStringFmt("%Y_%m_%d");
975
976	const TString dir = Form("%s/%s", path, night.Data());
977	const TString fname = Form("%s_%s.%s", name, clock.Data(), ext);
978
979	const TString full = Form("%s/%s", dir.Data(), fname.Data());
980
981	gSystem->mkdir(dir, kTRUE);
982
983	if (gSystem->AccessPathName(full, kFileExists))
984	return full;
985
986	break;// !!!!!!!!!!!!!!!!!!!!!!!
987
988	usleep(1000);
989	}
990	return "";
991	}
992
993	MCosy::MCosy(MEnv &env, MDriveCom *com)
994	: Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
995	fMac1(0), fMac2(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
996	{
997	const Int_t id1 = env.GetValue("Az_Id", 1);
998	const Int_t id2 = env.GetValue("Zd_Id", 3);
999
1000	fFilePrepos = env.GetValue("FilePredefinedPositions", "prepos.txt");
1001
1002	TString name = GetFileName("rep", "cosy", "rep");
1003	gLog << inf << "Open Repfile: " << name << endl;
1004	fOutRep = new MLog(name, kTRUE);
1005	*fOutRep << all << "[Drive Report File]" << endl;
1006	*fOutRep << "Version <cvs>" << endl;
1007	*fOutRep << "Date " << MTime(-1) << endl;
1008	*fOutRep << "[Reports]" << endl;
1009
1010	const TString pointing = env.GetValue("PointingModel", "bending.txt");
1011
1012	gLog << all << "Reading pointing model from " << pointing << "..." << endl;
1013	if (fBending.Load(pointing))
1014	gLog << all << "Reading pointing model from " << pointing << " successfull." << endl;
1015	else
1016	gLog << err << "ERROR - Reading pointing model from " << pointing << endl;
1017
1018	//
1019	// Create Nodes
1020	//
1021	gLog << inf << "- Setting up network." << endl;
1022
1023	fMac1=new Dkc(id1, "DKC/Az");
1024	fMac2=new Dkc(id2, "DKC/Zd");
1025
1026	fMac1->SetReport(fOutRep);
1027	fMac2->SetReport(fOutRep);
1028
1029	gLog << inf << "- Connecting devices to network." << endl;
1030
1031	//
1032	// Connect the devices to the network
1033	//
1034	SetNode(fMac1);
1035	SetNode(fMac2);
1036
1037	//
1038	// Create Gui Event timer and Gui
1039	//
1040	gLog << inf << "- Initializing GUI Timer." << endl;
1041	fUpdateGui = new TTimer(this, 100); // 100ms
1042
1043	gLog << all << "- Starting GUI." << endl;
1044	fWin=new MGCosy(fObservatory, fFilePrepos, this);
1045
1046	gLog.SetOutputGui(fWin->GetLog(), kTRUE);
1047
1048	fMac2->SetDisplay(fWin->GetLabel2());
1049	fMac1->SetDisplay(fWin->GetLabel1());
1050
1051	fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
1052	fCom->SetOutRep(fOutRep);
1053	// fCom->Start();
1054	}
1055
1056	void MCosy::TerminateApp()
1057	{
1058	gLog << inf2 << "MCosy::TerminateApp()" << endl;
1059	/*
1060	Int_t rc;
1061	TGMessageBox msg(this, gClient->GetRoot(),
1062	"Information",
1063	"Cosy is shutting down the system - this may take wa while!",
1064	kMBIconExclamation,
1065	kMBOK, //kMBClose
1066	&rc, 0);
1067	*/
1068
1069	gLog.DisableOutputDevice(MLog::eGui);
1070	// FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
1071	// gLog.SetOutputGui(NULL, kFALSE);
1072
1073	gApplication->Terminate(0);
1074	}
1075
1076	MCosy::~MCosy()
1077	{
1078	if(fCom)
1079	{
1080	fCom->SetMsgQueue(NULL);
1081	fCom->SetOutRep(NULL);
1082	}
1083
1084	gLog << inf2 << "Deleting GUI timer." << endl;
1085	// FIXME: Wait until last Update was finished!!!
1086	delete fUpdateGui;
1087
1088	//fMutexGui.Lock();
1089
1090	// Now the files can safely be closed
1091	gLog << inf2 << "Closing output files." << endl;
1092	if (fOutTp)
1093	{
1094	*fOutTp << "END" << endl;
1095	delete fOutTp;
1096	}
1097
1098	delete fOutRep;
1099
1100	//gLog << inf2 << "Deleting CC communication." << endl;
1101	//delete fCom;
1102
1103	gLog << inf2 << "Deleting Nodes." << endl;
1104	fMac1->SetReport(0);
1105	fMac2->SetReport(0);
1106
1107	delete fMac1;
1108	delete fMac2;
1109
1110	gLog << inf2 << "Deleting MGCosy." << endl;
1111
1112	gLog.DisableOutputDevice(MLog::eGui);
1113
1114	delete fWin;
1115
1116	gLog << inf2 << "MGCosy destructed." << endl;
1117	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: