source: trunk/FACT++/src/drivectrl.cc@ 12804

Last change on this file since 12804 was 12770, checked in by tbretz, 13 years ago
Added a new dim service SOURCE_POSITION and a new command WOBBLE.
File size: 34.1 KB
Line 
1#include <boost/bind.hpp>
2
3#include "FACT.h"
4#include "Dim.h"
5#include "Event.h"
6#include "Shell.h"
7#include "StateMachineDim.h"
8#include "Connection.h"
9#include "Configuration.h"
10#include "Timers.h"
11#include "Console.h"
12#include "Converter.h"
13
14#include "tools.h"
15
16
17namespace ba = boost::asio;
18namespace bs = boost::system;
19namespace dummy = ba::placeholders;
20
21using namespace std;
22
23// ------------------------------------------------------------------------
24
25namespace Drive
26{
27 struct DimPointing
28 {
29 } __attribute__((__packed__));
30
31 struct DimTracking
32 {
33 } __attribute__((__packed__));
34
35 struct DimStarguider
36 {
37 double fMissZd;
38 double fMissAz;
39
40 double fNominalZd;
41 double fNominalAz;
42
43 double fCenterX;
44 double fCenterY;
45
46 double fBrightness;
47
48 uint16_t fNumCorrelated;
49 uint16_t fNumLeds;
50 uint16_t fNumRings;
51 uint16_t fNumStars;
52
53 } __attribute__((__packed__));
54
55 struct DimTPoint
56 {
57 double fRa;
58 double fDec;
59
60 double fNominalZd;
61 double fNominalAz;
62
63 double fPointingZd;
64 double fPointingAz;
65
66 double fFeedbackZd;
67 double fFeedbackAz;
68
69 uint16_t fNumLeds;
70 uint16_t fNumRings;
71
72 double fCenterX;
73 double fCenterY;
74 double fCenterMag;
75
76 double fStarX;
77 double fStarY;
78 double fStarMag;
79
80 double fRealMag;
81
82 } __attribute__((__packed__));
83};
84
85
86
87// ------------------------------------------------------------------------
88
89class ConnectionDrive : public Connection
90{
91 int fState;
92
93 bool fIsVerbose;
94
95 // --verbose
96 // --hex-out
97 // --dynamic-out
98 // --load-file
99 // --leds
100 // --trigger-interval
101 // --physcis-coincidence
102 // --calib-coincidence
103 // --physcis-window
104 // --physcis-window
105 // --trigger-delay
106 // --time-marker-delay
107 // --dead-time
108 // --clock-conditioner-r0
109 // --clock-conditioner-r1
110 // --clock-conditioner-r8
111 // --clock-conditioner-r9
112 // --clock-conditioner-r11
113 // --clock-conditioner-r13
114 // --clock-conditioner-r14
115 // --clock-conditioner-r15
116 // ...
117
118 virtual void UpdatePointing(const Time &, const array<double, 2> &)
119 {
120 }
121
122 virtual void UpdateTracking(const Time &, const array<double, 7> &)
123 {
124 }
125
126 virtual void UpdateStatus(const Time &, const array<uint8_t, 3> &)
127 {
128 }
129
130 virtual void UpdateStarguider(const Time &, const Drive::DimStarguider &)
131 {
132 }
133
134 virtual void UpdateTPoint(const Time &, const Drive::DimTPoint &, const string &)
135 {
136 }
137
138public:
139 virtual void UpdateSource()
140 {
141 }
142 virtual void UpdateSource(const array<double, 6> &)
143 {
144 }
145
146protected:
147 map<uint16_t, int> fCounter;
148
149 ba::streambuf fBuffer;
150
151 Time ReadTime(istream &in)
152 {
153 uint16_t y, m, d, hh, mm, ss, ms;
154 in >> y >> m >> d >> hh >> mm >> ss >> ms;
155
156 return Time(y, m, d, hh, mm, ss, ms*1000);
157 }
158
159 double ReadAngle(istream &in)
160 {
161 char sgn;
162 uint16_t d, m;
163 float s;
164
165 in >> sgn >> d >> m >> s;
166
167 const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
168 return sgn=='-' ? -ret : ret;
169 }
170
171 void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received)
172 {
173 // Do not schedule a new read if the connection failed.
174 if (bytes_received==0 || err)
175 {
176 if (err==ba::error::eof)
177 Warn("Connection closed by remote host (FTM).");
178
179 // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
180 // 125: Operation canceled
181 if (err && err!=ba::error::eof && // Connection closed by remote host
182 err!=ba::error::basic_errors::not_connected && // Connection closed by remote host
183 err!=ba::error::basic_errors::operation_aborted) // Connection closed by us
184 {
185 ostringstream str;
186 str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
187 Error(str);
188 }
189 PostClose(err!=ba::error::basic_errors::operation_aborted);
190 return;
191 }
192
193 istream is(&fBuffer);
194
195 string line;
196 getline(is, line);
197
198 if (fIsVerbose)
199 Out() << line << endl;
200
201 StartReadReport();
202
203 if (line.substr(0, 13)=="DRIVE-STATUS ")
204 {
205 Message(line.substr(70));
206 return;
207 }
208
209 if (line.substr(0, 13)=="STARG-REPORT ")
210 {
211 istringstream stream(line.substr(16));
212
213 // 0: Error
214 // 1: Standby
215 // 2: Monitoring
216 uint16_t status1;
217 stream >> status1;
218 const Time t1 = ReadTime(stream);
219
220 uint16_t status2;
221 stream >> status2;
222 const Time t2 = ReadTime(stream);
223
224 double misszd, missaz;
225 stream >> misszd >> missaz;
226
227 const double zd = ReadAngle(stream);
228 const double az = ReadAngle(stream);
229
230 double cx, cy;
231 stream >> cx >> cy;
232
233 int ncor;
234 stream >> ncor;
235
236 double bright, mjd;
237 stream >> bright >> mjd;
238
239 int nled, nring, nstars;
240 stream >> nled >> nring >> nstars;
241
242 if (stream.fail())
243 return;
244
245 Drive::DimStarguider data;
246
247 data.fMissZd = misszd;
248 data.fMissAz = missaz;
249 data.fNominalZd = zd;
250 data.fNominalAz = az;
251 data.fCenterX = cx;
252 data.fCenterY = cy;
253 data.fNumCorrelated = ncor;
254 data.fBrightness = bright;
255 data.fNumLeds = nled;
256 data.fNumRings = nring;
257 data.fNumStars = nstars;
258
259 UpdateStarguider(Time(mjd), data);
260
261 return;
262
263 }
264
265 if (line.substr(0, 14)=="TPOINT-REPORT ")
266 {
267 istringstream stream(line.substr(17));
268
269 uint16_t status1;
270 stream >> status1;
271 const Time t1 = ReadTime(stream);
272
273 uint16_t status2;
274 stream >> status2;
275 const Time t2 = ReadTime(stream);
276
277 char type;
278 stream >> type;
279 if (type != 'T')
280 return;
281
282 double az1, alt1, az2, alt2, ra, dec, dzd, daz;
283 stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
284
285 // c: center, s:start
286 double mjd, cmag, smag, cx, cy, sx, sy;
287 stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
288
289 int nled, nring, nstar, ncor;
290 stream >> nled >> nring >> nstar >> ncor;
291
292 double bright, mag;
293 stream >> bright >> mag;
294
295 string name;
296 stream >> name;
297
298 if (stream.fail())
299 return;
300
301 Drive::DimTPoint tpoint;
302
303 tpoint.fRa = ra;
304 tpoint.fDec = dec;
305
306 tpoint.fNominalZd = 90-alt1-dzd;
307 tpoint.fNominalAz = 90-az1 +daz;
308
309 tpoint.fPointingZd = 90-alt1;
310 tpoint.fPointingAz = az1;
311
312 tpoint.fFeedbackZd = 90-alt2;
313 tpoint.fFeedbackAz = az2;
314
315 tpoint.fNumLeds = nled;
316 tpoint.fNumRings = nring;
317
318 tpoint.fCenterX = cx;
319 tpoint.fCenterY = cy;
320 tpoint.fCenterMag = cmag;
321
322 tpoint.fStarX = sx;
323 tpoint.fStarY = sy;
324 tpoint.fStarMag = smag;
325
326 tpoint.fRealMag = mag;
327
328 UpdateTPoint(t1, tpoint, name);
329
330 return;
331 }
332
333 if (line.substr(0, 13)=="DRIVE-REPORT ")
334 {
335 // DRIVE-REPORT M1
336 // 01 2011 05 14 11 31 19 038
337 // 02 1858 11 17 00 00 00 000
338 // + 000 00 000 + 000 00 000
339 // + 000 00 000
340 // 55695.480081
341 // + 000 00 000 + 000 00 000
342 // + 000 00 000 + 000 00 000
343 // 0000.000 0000.000
344 // 0 2
345
346 // status
347 // year month day hour minute seconds millisec
348 // year month day hour minute seconds millisec
349 // ra(+ h m s) dec(+ d m s) ha(+ h m s)
350 // mjd
351 // zd(+ d m s) az(+ d m s)
352 // zd(+ d m s) az(+ d m s)
353 // zd_err az_err
354 // armed(0=unlocked, 1=locked)
355 // stgmd(0=none, 1=starguider, 2=starguider off)
356 istringstream stream(line.substr(16));
357
358 uint16_t status1;
359 stream >> status1;
360 const Time t1 = ReadTime(stream);
361
362 uint16_t status2;
363 stream >> status2;
364 const Time t2 = ReadTime(stream);
365
366 const double ra = ReadAngle(stream);
367 const double dec = ReadAngle(stream);
368 const double ha = ReadAngle(stream);
369
370 double mjd;
371 stream >> mjd;
372
373 const double zd1 = ReadAngle(stream);
374 const double az1 = ReadAngle(stream);
375 const double zd2 = ReadAngle(stream);
376 const double az2 = ReadAngle(stream);
377
378 double zd_err, az_err;
379 stream >> zd_err;
380 stream >> az_err;
381
382 uint16_t armed, stgmd;
383 stream >> armed;
384 stream >> stgmd;
385
386 uint32_t pdo3;
387 stream >> hex >> pdo3;
388
389 if (stream.fail())
390 return;
391
392 // Status 0: Error
393 // Status 1: Stopped
394 // Status 3: Stopping || Moving
395 // Status 4: Tracking
396 if (status1==0)
397 status1 = 99;
398
399 const bool ready = (pdo3&0xef00ef)==0xef00ef;
400 if (!ready)
401 fState = 0;
402 else
403 fState = status1==1 ? armed+1 : status1;
404
405 // 1: Not ready
406 // 2: Not armed
407 // 3: Armed
408 // 4: Moving
409 // 5: Tracking
410 // 99: Error
411
412
413
414 // pdo3:
415 // 1 Ab
416 // 2 1
417 // 4 Emergency
418 // 8 OverVolt
419 // 10 Move (Drehen-soll)
420 // 20 Af
421 // 40 1
422 // 80 Power on Az
423 // ------------------
424 // 100 NOT UPS Alarm
425 // 200 UPS on Battery
426 // 400 UPS charging
427
428 // Power cut: 2ef02ef
429 // charging: 4ef04ef
430
431 const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
432 UpdateStatus(t1, state);
433
434 const array<double, 2> point = {{ zd2, az2 }};
435 UpdatePointing(t1, point);
436
437 const array<double, 7> track =
438 {{
439 ra, dec, ha,
440 zd1, az1,
441 zd_err/3600, az_err/3600
442 }};
443 if (mjd>0)
444 UpdateTracking(Time(mjd), track);
445
446 // ---- DIM ----> t1 as event time
447 // status1
448 // mjd
449 // ra/dec/ha
450 // zd/az (nominal)
451 // zd/az (current)
452 // err(zd/az)
453 // [armed] [stgmd]
454
455 // Maybe:
456 // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
457 //
458 // if (mjd>0)
459 // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
460 // ra/dec, ha(not well defined),
461 // [Nominal + Error == Current]
462
463 // MJD is the time which corresponds to the nominal position
464 // t1 is the time which corresponds to the current position/HA
465
466 return;
467 }
468 }
469
470 void StartReadReport()
471 {
472 boost::asio::async_read_until(*this, fBuffer, '\n',
473 boost::bind(&ConnectionDrive::HandleReceivedReport, this,
474 dummy::error, dummy::bytes_transferred));
475 }
476
477 boost::asio::deadline_timer fKeepAlive;
478
479 void KeepAlive()
480 {
481 PostMessage(string("KEEP_ALIVE"));
482
483 fKeepAlive.expires_from_now(boost::posix_time::seconds(10));
484 fKeepAlive.async_wait(boost::bind(&ConnectionDrive::HandleKeepAlive,
485 this, dummy::error));
486 }
487
488 void HandleKeepAlive(const bs::error_code &error)
489 {
490 // 125: Operation canceled (bs::error_code(125, bs::system_category))
491 if (error && error!=ba::error::basic_errors::operation_aborted)
492 {
493 ostringstream str;
494 str << "Write timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
495 Error(str);
496
497 PostClose(false);
498 return;
499 }
500
501 if (!is_open())
502 {
503 // For example: Here we could schedule a new accept if we
504 // would not want to allow two connections at the same time.
505 return;
506 }
507
508 // Check whether the deadline has passed. We compare the deadline
509 // against the current time since a new asynchronous operation
510 // may have moved the deadline before this actor had a chance
511 // to run.
512 if (fKeepAlive.expires_at() > ba::deadline_timer::traits_type::now())
513 return;
514
515 KeepAlive();
516 }
517
518
519private:
520 // This is called when a connection was established
521 void ConnectionEstablished()
522 {
523 StartReadReport();
524 KeepAlive();
525 }
526
527 /*
528 void HandleReadTimeout(const bs::error_code &error)
529 {
530 if (error && error!=ba::error::basic_errors::operation_aborted)
531 {
532 stringstream str;
533 str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
534 Error(str);
535
536 PostClose();
537 return;
538
539 }
540
541 if (!is_open())
542 {
543 // For example: Here we could schedule a new accept if we
544 // would not want to allow two connections at the same time.
545 return;
546 }
547
548 // Check whether the deadline has passed. We compare the deadline
549 // against the current time since a new asynchronous operation
550 // may have moved the deadline before this actor had a chance
551 // to run.
552 if (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
553 return;
554
555 Error("Timeout reading data from "+URL());
556
557 PostClose();
558 }*/
559
560
561public:
562
563 static const uint16_t kMaxAddr;
564
565public:
566 ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
567 fState(-1), fIsVerbose(true), fKeepAlive(ioservice)
568 {
569 SetLogStream(&imp);
570 }
571
572 void SetVerbose(bool b)
573 {
574 fIsVerbose = b;
575 }
576
577 int GetState() const
578 {
579 if (!IsConnected())
580 return 1;
581 if (IsConnected() && fState<0)
582 return 2;
583 return fState+3;
584 }
585};
586
587const uint16_t ConnectionDrive::kMaxAddr = 0xfff;
588
589// ------------------------------------------------------------------------
590
591#include "DimDescriptionService.h"
592
593class ConnectionDimDrive : public ConnectionDrive
594{
595private:
596 DimDescribedService fDimPointing;
597 DimDescribedService fDimTracking;
598 DimDescribedService fDimSource;
599 DimDescribedService fDimTPoint;
600 DimDescribedService fDimStatus;
601
602 void UpdatePointing(const Time &t, const array<double, 2> &arr)
603 {
604 fDimPointing.setData(arr);
605 fDimPointing.Update(t);
606 }
607
608 void UpdateTracking(const Time &t,const array<double, 7> &arr)
609 {
610 fDimTracking.setData(arr);
611 fDimTracking.Update(t);
612 }
613
614 void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
615 {
616 fDimStatus.setData(arr);
617 fDimStatus.Update(t);
618 }
619
620 void UpdateTPoint(const Time &t, const Drive::DimTPoint &data,
621 const string &name)
622 {
623 vector<char> dim(sizeof(data)+name.length()+1);
624 memcpy(dim.data(), &data, sizeof(data));
625 memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1);
626
627 fDimTPoint.setData(dim);
628 fDimTPoint.Update(t);
629 }
630
631public:
632 ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
633 ConnectionDrive(ioservice, imp),
634 fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1",
635 "|Zd[deg]:Zenith distance (encoder readout)"
636 "|Az[deg]:Azimuth angle (encoder readout)"),
637 fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1",
638 "|Ra[h]:Command right ascension"
639 "|Dec[deg]:Command declination"
640 "|Ha[h]:Corresponding hour angle"
641 "|Zd[deg]:Nominal zenith distance"
642 "|Az[deg]:Nominal azimuth angle"
643 "|dZd[deg]:Control deviation Zd"
644 "|dAz[deg]:Control deviation Az"),
645 fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1",
646 "|Ra_cmd[h]:Command right ascension"
647 "|Dec_cmd[deg]:Command declination"
648 "|Ra_src[h]:Source right ascension"
649 "|Dec_src[deg]:Source declination"
650 "|Offfset[deg]:Wobble offset"
651 "|Angle[deg]:Wobble angle"),
652 fDimTPoint("DRIVE_CONTROL/TPOINT", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;S:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;C",
653 "|Ra[h]:Command right ascension"
654 "|Dec[deg]:Command declination"
655 "|Zd_nom[deg]:Nominal zenith distance"
656 "|Az_nom[deg]:Nominal azimuth angle"
657 "|Zd_cur[deg]:Current zenith distance (calculated from image)"
658 "|Az_cur[deg]:Current azimuth angle (calculated from image)"
659 "|Zd_enc[deg]:Feedback zenith axis (from encoder)"
660 "|Az_enc[deg]:Feedback azimuth angle (from encoder)"
661 "|N_leds[cnt]:Number of detected LEDs"
662 "|N_rings[cnt]:Number of rings used to calculate the camera center"
663 "|Xc[pix]:X position of center in CCD camera frame"
664 "|Yc[pix]:Y position of center in CCD camera frame"
665 "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)"
666 "|Xs[pix]:X position of start in CCD camera frame"
667 "|Ys[pix]:Y position of star in CCD camera frame"
668 "|Ms[mag]:Artifical magnitude of star (calculated form image))"
669 "|Mc[mag]:Catalog magnitude of star"),
670 fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", "")
671
672 {
673 }
674
675 void UpdateSource()
676 {
677 const array<double, 6> arr = {{ 0, 0, 0, 0, 0, 0 }};
678 fDimSource.setQuality(0);
679 fDimSource.Update(arr);
680 }
681
682 void UpdateSource(const array<double, 6> &arr)
683 {
684 fDimSource.setQuality(1);
685 fDimSource.Update(arr);
686 }
687
688 // A B [C] [D] E [F] G H [I] J K [L] M N O P Q R [S] T U V W [X] Y Z
689};
690
691// ------------------------------------------------------------------------
692
693template <class T, class S>
694class StateMachineDrive : public T, public ba::io_service, public ba::io_service::work
695{
696 int Wrap(boost::function<void()> f)
697 {
698 f();
699 return T::GetCurrentState();
700 }
701
702 boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
703 {
704 return bind(&StateMachineDrive::Wrap, this, func);
705 }
706
707private:
708 S fDrive;
709
710 enum states_t
711 {
712 kStateDisconnected = 1,
713 kStateConnected,
714 kStateNotReady,
715 kStateReady,
716 kStateArmed,
717 kStateMoving,
718 kStateTracking,
719 };
720
721 // Status 0: Error
722 // Status 1: Unlocked
723 // Status 2: Locked
724 // Status 3: Stopping || Moving
725 // Status 4: Tracking
726
727 bool CheckEventSize(size_t has, const char *name, size_t size)
728 {
729 if (has==size)
730 return true;
731
732 ostringstream msg;
733 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
734 T::Fatal(msg);
735 return false;
736 }
737
738 enum Coordinates
739 {
740 kPoint,
741 kTrackSlow,
742 kTrackFast
743 };
744
745 string AngleToStr(double angle)
746 {
747 /* Handle sign */
748 const char sgn = angle<0?'-':'+';
749
750 /* Round interval and express in smallest units required */
751 double a = round(3600. * fabs(angle)); // deg to seconds
752
753 /* Separate into fields */
754 const double ad = trunc(a/3600.);
755 a -= ad * 3600.;
756 const double am = trunc(a/60.);
757 a -= am * 60.;
758 const double as = trunc(a);
759
760 /* Return results */
761 ostringstream str;
762 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
763 return str.str();
764 }
765
766 int SendCommand(const string &str, bool upd=true)
767 {
768 fDrive.PostMessage(str);
769 T::Message("Sending: "+str);
770
771 if (upd)
772 fDrive.UpdateSource();
773
774 return T::GetCurrentState();
775 }
776
777 int SendCoordinates(const EventImp &evt, const Coordinates type)
778 {
779 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
780 return T::kSM_FatalError;
781
782 const double *dat = evt.Ptr<double>();
783
784 string command;
785
786 switch (type)
787 {
788 case kPoint: command += "ZDAZ "; break;
789 case kTrackSlow: command += "RADEC "; break;
790 case kTrackFast: command += "GRB "; break;
791 }
792
793 if (type!=kPoint)
794 {
795 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
796 fDrive.UpdateSource(dim);
797 }
798
799 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
800 return SendCommand(command, type==kPoint);
801 }
802
803 int Wobble(const EventImp &evt)
804 {
805 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
806 return T::kSM_FatalError;
807
808 const double *dat = evt.Ptr<double>();
809
810 const double ra = dat[0]*M_PI/12;
811 const double dec = dat[1]*M_PI/180;
812 const double off = dat[2]*M_PI/180;
813 const double dir = dat[3]*M_PI/180;
814
815 const double cosdir = cos(dir);
816 const double sindir = sin(dir);
817 const double cosoff = cos(off);
818 const double sinoff = sin(off);
819 const double cosdec = cos(dec);
820 const double sindec = sin(dec);
821
822 if (off==0)
823 {
824 const array<double, 6> dim = {{ ra, dec, ra, dec, 0, 0 }};
825 fDrive.UpdateSource(dim);
826
827 string command = "RADEC ";
828 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
829 return SendCommand(command, false);
830 }
831
832 const double costheta = cosdec*cosoff + sindec*sinoff*cosdir;
833 if (costheta >= 1)
834 {
835 T::Error("cos(Zd) > 1");
836 return T::GetCurrentState();
837 }
838
839 const double sintheta = sqrt(1 - costheta*costheta);
840
841 const double cosdeltara = (cosoff - cosdec*costheta)/(sindec*sintheta);
842 const double sindeltara = sindir*sinoff/sintheta;
843
844 const double ndec = acos(costheta)*180/M_PI;
845 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
846
847 const array<double, 6> dim = {{ ra, dec, nra, ndec, off, dir }};
848 fDrive.UpdateSource(dim);
849
850 string command = "RADEC ";
851 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
852 return SendCommand(command, false);
853 }
854
855 int SetLedBrightness(const EventImp &evt)
856 {
857 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
858 return T::kSM_FatalError;
859
860 const uint32_t *led = evt.Ptr<uint32_t>();
861
862 ostringstream cmd;
863 cmd << "LEDS " << led[0] << " " << led[1];
864 return SendCommand(cmd.str(), false);
865 }
866
867
868 int SetVerbosity(const EventImp &evt)
869 {
870 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
871 return T::kSM_FatalError;
872
873 fDrive.SetVerbose(evt.GetBool());
874
875 return T::GetCurrentState();
876 }
877
878 int Disconnect()
879 {
880 // Close all connections
881 fDrive.PostClose(false);
882
883 /*
884 // Now wait until all connection have been closed and
885 // all pending handlers have been processed
886 poll();
887 */
888
889 return T::GetCurrentState();
890 }
891
892 int Reconnect(const EventImp &evt)
893 {
894 // Close all connections to supress the warning in SetEndpoint
895 fDrive.PostClose(false);
896
897 // Now wait until all connection have been closed and
898 // all pending handlers have been processed
899 poll();
900
901 if (evt.GetBool())
902 fDrive.SetEndpoint(evt.GetString());
903
904 // Now we can reopen the connection
905 fDrive.PostClose(true);
906
907 return T::GetCurrentState();
908 }
909
910 int Execute()
911 {
912 // Dispatch (execute) at most one handler from the queue. In contrary
913 // to run_one(), it doesn't wait until a handler is available
914 // which can be dispatched, so poll_one() might return with 0
915 // handlers dispatched. The handlers are always dispatched/executed
916 // synchronously, i.e. within the call to poll_one()
917 poll_one();
918
919 return fDrive.GetState();
920 }
921
922
923public:
924 StateMachineDrive(ostream &out=cout) :
925 T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
926 fDrive(*this, *this)
927 {
928 // ba::io_service::work is a kind of keep_alive for the loop.
929 // It prevents the io_service to go to stopped state, which
930 // would prevent any consecutive calls to run()
931 // or poll() to do nothing. reset() could also revoke to the
932 // previous state but this might introduce some overhead of
933 // deletion and creation of threads and more.
934
935 // State names
936 AddStateName(kStateDisconnected, "Disconnected",
937 "No connection to cosy");
938
939 AddStateName(kStateConnected, "Connected",
940 "Cosy connected, drive stopped");
941
942 AddStateName(kStateNotReady, "NotReady",
943 "Drive system not ready for movement");
944
945 AddStateName(kStateReady, "Ready",
946 "Drive system ready for movement");
947
948 AddStateName(kStateArmed, "Armed",
949 "Cosy armed, drive stopped");
950
951 AddStateName(kStateMoving, "Moving",
952 "Telescope moving");
953
954 AddStateName(kStateTracking, "Tracking",
955 "Telescope tracking");
956
957 // kStateIdle
958 // kStateArmed
959 // kStateMoving
960 // kStateTracking
961
962 // Init
963 // -----------
964 // "ARM lock"
965 // "STGMD off"
966
967 /*
968 [ ] WAIT -> WM_WAIT
969 [x] STOP! -> WM_STOP
970 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
971 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
972 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
973 [ ] CELEST id offset angle
974 [ ] MOON wobble offset
975 [ ] PREPS string
976 [ ] TPOIN star mag
977 [ ] ARM lock/unlock
978 [ ] STGMD on/off
979 */
980
981 // Drive Commands
982 T::AddEvent("MOVE_TO", "D:2", kStateArmed) // ->ZDAZ
983 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
984 ("Move the telescope to the given local coordinates"
985 "|Zd[deg]:Zenith distance"
986 "|Az[deg]:Azimuth");
987
988 T::AddEvent("TRACK", "D:2", kStateArmed) // ->RADEC/GRB
989 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
990 ("Move the telescope to the given sky coordinates and start tracking them"
991 "|Ra[h]:Right ascension"
992 "|Dec[deg]:Declination");
993
994 T::AddEvent("WOBBLE", "D:4", kStateArmed) // ->RADEC/GRB
995 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
996 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
997 "|Ra[h]:Right ascension"
998 "|Dec[deg]:Declination"
999 "|Offset[deg]:Wobble offset"
1000 "|Angle[deg]:Wobble angle");
1001
1002 T::AddEvent("MOON", kStateArmed)
1003 (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
1004 ("Start tracking the moon");
1005 T::AddEvent("VENUS", kStateArmed)
1006 (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
1007 ("Start tracking Venus");
1008 T::AddEvent("MARS", kStateArmed)
1009 (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
1010 ("Start tracking Mars");
1011 T::AddEvent("JUPITER", kStateArmed)
1012 (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
1013 ("Start tracking Jupiter");
1014 T::AddEvent("SATURN", kStateArmed)
1015 (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
1016 ("Start tracking Saturn");
1017
1018 T::AddEvent("TAKE_TPOINT")
1019 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
1020 ("Take a TPoint");
1021
1022 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1023 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1024 ("Set the LED brightness of the top and bottom leds"
1025 "|top[au]:Allowed range 0-32767 for top LEDs"
1026 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1027
1028 T::AddEvent("LEDS_OFF")
1029 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
1030 ("Switch off TPoint LEDs");
1031
1032 T::AddEvent("STOP")
1033 (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
1034 ("Stop any kind of movement.");
1035
1036// T::AddEvent("ARM", kStateConnected)
1037// (bind(&StateMachineDrive::SendCommand, this, "ARM lock"))
1038// ("");
1039
1040
1041 // Verbosity commands
1042 T::AddEvent("SET_VERBOSE", "B")
1043 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1044 ("set verbosity state"
1045 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1046
1047 // Conenction commands
1048 AddEvent("DISCONNECT", kStateConnected, kStateArmed)
1049 (bind(&StateMachineDrive::Disconnect, this))
1050 ("disconnect from ethernet");
1051
1052 AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected, kStateArmed)
1053 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1054 ("(Re)connect ethernet connection to FTM, a new address can be given"
1055 "|[host][string]:new ethernet address in the form <host:port>");
1056
1057 fDrive.StartConnect();
1058 }
1059
1060 void SetEndpoint(const string &url)
1061 {
1062 fDrive.SetEndpoint(url);
1063 }
1064
1065 int EvalOptions(Configuration &conf)
1066 {
1067 SetEndpoint(conf.Get<string>("addr"));
1068
1069 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1070
1071 return -1;
1072 }
1073};
1074
1075// ------------------------------------------------------------------------
1076
1077#include "Main.h"
1078
1079
1080template<class T, class S, class R>
1081int RunShell(Configuration &conf)
1082{
1083 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1084}
1085
1086void SetupConfiguration(Configuration &conf)
1087{
1088 po::options_description control("Drive control options");
1089 control.add_options()
1090 ("no-dim,d", po_switch(), "Disable dim services")
1091 ("addr,a", var<string>("localhost:7404"), "Network address of Cosy")
1092 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1093 ;
1094
1095 conf.AddOptions(control);
1096}
1097
1098/*
1099 Extract usage clause(s) [if any] for SYNOPSIS.
1100 Translators: "Usage" and "or" here are patterns (regular expressions) which
1101 are used to match the usage synopsis in program output. An example from cp
1102 (GNU coreutils) which contains both strings:
1103 Usage: cp [OPTION]... [-T] SOURCE DEST
1104 or: cp [OPTION]... SOURCE... DIRECTORY
1105 or: cp [OPTION]... -t DIRECTORY SOURCE...
1106 */
1107void PrintUsage()
1108{
1109 cout <<
1110 "The drivectrl is an interface to cosy.\n"
1111 "\n"
1112 "The default is that the program is started without user intercation. "
1113 "All actions are supposed to arrive as DimCommands. Using the -c "
1114 "option, a local shell can be initialized. With h or help a short "
1115 "help message about the usuage can be brought to the screen.\n"
1116 "\n"
1117 "Usage: drivectrl [-c type] [OPTIONS]\n"
1118 " or: drivectrl [OPTIONS]\n";
1119 cout << endl;
1120}
1121
1122void PrintHelp()
1123{
1124// Main::PrintHelp<StateMachineFTM<StateMachine, ConnectionFTM>>();
1125
1126 /* Additional help text which is printed after the configuration
1127 options goes here */
1128
1129 /*
1130 cout << "bla bla bla" << endl << endl;
1131 cout << endl;
1132 cout << "Environment:" << endl;
1133 cout << "environment" << endl;
1134 cout << endl;
1135 cout << "Examples:" << endl;
1136 cout << "test exam" << endl;
1137 cout << endl;
1138 cout << "Files:" << endl;
1139 cout << "files" << endl;
1140 cout << endl;
1141 */
1142}
1143
1144int main(int argc, const char* argv[])
1145{
1146 Configuration conf(argv[0]);
1147 conf.SetPrintUsage(PrintUsage);
1148 Main::SetupConfiguration(conf);
1149 SetupConfiguration(conf);
1150
1151 if (!conf.DoParse(argc, argv, PrintHelp))
1152 return -1;
1153
1154 //try
1155 {
1156 // No console access at all
1157 if (!conf.Has("console"))
1158 {
1159 if (conf.Get<bool>("no-dim"))
1160 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1161 else
1162 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1163 }
1164 // Cosole access w/ and w/o Dim
1165 if (conf.Get<bool>("no-dim"))
1166 {
1167 if (conf.Get<int>("console")==0)
1168 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1169 else
1170 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1171 }
1172 else
1173 {
1174 if (conf.Get<int>("console")==0)
1175 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1176 else
1177 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1178 }
1179 }
1180 /*catch (std::exception& e)
1181 {
1182 cerr << "Exception: " << e.what() << endl;
1183 return -1;
1184 }*/
1185
1186 return 0;
1187}
Note: See TracBrowser for help on using the repository browser.