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

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