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

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