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

Last change on this file since 13640 was 13223, checked in by tbretz, 14 years ago
Unintentionally changed the allowed states for TRACK_SOURCE with the last commit
File size: 40.2 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 typedef map<string,pair<double,double>> sources;
736 sources fSources;
737
738 // Status 0: Error
739 // Status 1: Unlocked
740 // Status 2: Locked
741 // Status 3: Stopping || Moving
742 // Status 4: Tracking
743
744 bool CheckEventSize(size_t has, const char *name, size_t size)
745 {
746 if (has==size)
747 return true;
748
749 ostringstream msg;
750 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
751 T::Fatal(msg);
752 return false;
753 }
754
755 enum Coordinates
756 {
757 kPoint,
758 kTrackSlow,
759 kTrackFast
760 };
761
762 string AngleToStr(double angle)
763 {
764 /* Handle sign */
765 const char sgn = angle<0?'-':'+';
766
767 /* Round interval and express in smallest units required */
768 double a = round(3600. * fabs(angle)); // deg to seconds
769
770 /* Separate into fields */
771 const double ad = trunc(a/3600.);
772 a -= ad * 3600.;
773 const double am = trunc(a/60.);
774 a -= am * 60.;
775 const double as = trunc(a);
776
777 /* Return results */
778 ostringstream str;
779 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
780 return str.str();
781 }
782
783 int SendCommand(const string &str, bool upd=true)
784 {
785 fDrive.PostMessage(str);
786 T::Message("Sending: "+str);
787
788 if (upd)
789 fDrive.UpdateSource();
790
791 return T::GetCurrentState();
792 }
793
794 int SendCoordinates(const EventImp &evt, const Coordinates type)
795 {
796 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
797 return T::kSM_FatalError;
798
799 const double *dat = evt.Ptr<double>();
800
801 string command;
802
803 switch (type)
804 {
805 case kPoint: command += "ZDAZ "; break;
806 case kTrackSlow: command += "RADEC "; break;
807 case kTrackFast: command += "GRB "; break;
808 }
809
810 if (type!=kPoint)
811 {
812 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
813 fDrive.UpdateSource(dim);
814 }
815
816 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
817 return SendCommand(command, type==kPoint);
818 }
819
820 int StartWobble(const double &srcra, const double &srcdec,
821 const double &woboff, const double &wobang,
822 const string name="")
823 {
824 const double ra = srcra *M_PI/12;
825 const double dec = srcdec*M_PI/180;
826 const double off = woboff*M_PI/180;
827 const double dir = wobang*M_PI/180;
828
829 const double cosdir = cos(dir);
830 const double sindir = sin(dir);
831 const double cosoff = cos(off);
832 const double sinoff = sin(off);
833 const double cosdec = cos(dec);
834 const double sindec = sin(dec);
835
836 if (off==0)
837 {
838 const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
839 fDrive.UpdateSource(dim, name);
840
841 string command = "RADEC ";
842 command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
843 return SendCommand(command, false);
844 }
845
846 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
847 if (sintheta >= 1)
848 {
849 T::Error("cos(Zd) > 1");
850 return T::GetCurrentState();
851 }
852
853 const double costheta = sqrt(1 - sintheta*sintheta);
854
855 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
856 const double sindeltara = sindir*sinoff/costheta;
857
858 const double ndec = asin(sintheta)*180/M_PI;
859 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
860
861 const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
862 fDrive.UpdateSource(dim, name);
863
864 string command = "RADEC ";
865 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
866 return SendCommand(command, false);
867 }
868
869 int Wobble(const EventImp &evt)
870 {
871 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
872 return T::kSM_FatalError;
873
874 const double *dat = evt.Ptr<double>();
875
876 return StartWobble(dat[0], dat[1], dat[2], dat[3]);
877 }
878
879 int Track(const EventImp &evt)
880 {
881 if (evt.GetSize()<=16)
882 {
883 ostringstream msg;
884 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
885 T::Fatal(msg);
886 return false;
887 }
888
889 const double *dat = evt.Ptr<double>();
890 const char *ptr = evt.Ptr<char>(16);
891 const char *last = ptr+evt.GetSize()-16;
892
893 if (find(ptr, last, '\0')==last)
894 {
895 T::Fatal("Track - The name transmitted by dim is not null-terminated.");
896 return false;
897 }
898
899 const string name(Tools::TrimQuotes(ptr));
900
901 const sources::const_iterator it = fSources.find(name);
902 if (it==fSources.end())
903 {
904 T::Error("Source '"+name+"' not found in list.");
905 return false;
906 }
907
908 const double &ra = it->second.first;
909 const double &dec = it->second.second;
910
911 return StartWobble(ra, dec, dat[0], dat[1], name);
912 }
913
914 int SetLedBrightness(const EventImp &evt)
915 {
916 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
917 return T::kSM_FatalError;
918
919 const uint32_t *led = evt.Ptr<uint32_t>();
920
921 ostringstream cmd;
922 cmd << "LEDS " << led[0] << " " << led[1];
923 return SendCommand(cmd.str(), false);
924 }
925
926
927 int SetVerbosity(const EventImp &evt)
928 {
929 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
930 return T::kSM_FatalError;
931
932 fDrive.SetVerbose(evt.GetBool());
933
934 return T::GetCurrentState();
935 }
936
937 int Print()
938 {
939 for (sources::const_iterator it=fSources.begin();
940 it!=fSources.end(); it++)
941 {
942 const string &name = it->first;
943 const double &ra = it->second.first;
944 const double &dec = it->second.second;
945
946 T::Out() << name << "," << ra << "," << dec << endl;
947 }
948 return T::GetCurrentState();
949 }
950
951 int Disconnect()
952 {
953 // Close all connections
954 fDrive.PostClose(false);
955
956 /*
957 // Now wait until all connection have been closed and
958 // all pending handlers have been processed
959 poll();
960 */
961
962 return T::GetCurrentState();
963 }
964
965 int Reconnect(const EventImp &evt)
966 {
967 // Close all connections to supress the warning in SetEndpoint
968 fDrive.PostClose(false);
969
970 // Now wait until all connection have been closed and
971 // all pending handlers have been processed
972 poll();
973
974 if (evt.GetBool())
975 fDrive.SetEndpoint(evt.GetString());
976
977 // Now we can reopen the connection
978 fDrive.PostClose(true);
979
980 return T::GetCurrentState();
981 }
982
983 int Execute()
984 {
985 // Dispatch (execute) at most one handler from the queue. In contrary
986 // to run_one(), it doesn't wait until a handler is available
987 // which can be dispatched, so poll_one() might return with 0
988 // handlers dispatched. The handlers are always dispatched/executed
989 // synchronously, i.e. within the call to poll_one()
990 poll_one();
991
992 return fDrive.GetState();
993 }
994
995
996public:
997 StateMachineDrive(ostream &out=cout) :
998 T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
999 fDrive(*this, *this)
1000 {
1001 // ba::io_service::work is a kind of keep_alive for the loop.
1002 // It prevents the io_service to go to stopped state, which
1003 // would prevent any consecutive calls to run()
1004 // or poll() to do nothing. reset() could also revoke to the
1005 // previous state but this might introduce some overhead of
1006 // deletion and creation of threads and more.
1007
1008 // State names
1009 AddStateName(kStateDisconnected, "Disconnected",
1010 "No connection to cosy");
1011
1012 AddStateName(kStateConnected, "Connected",
1013 "Cosy connected, drive stopped");
1014
1015 AddStateName(kStateNotReady, "NotReady",
1016 "Drive system not ready for movement");
1017
1018 AddStateName(kStateReady, "Ready",
1019 "Drive system ready for movement");
1020
1021 AddStateName(kStateArmed, "Armed",
1022 "Cosy armed, drive stopped");
1023
1024 AddStateName(kStateMoving, "Moving",
1025 "Telescope moving");
1026
1027 AddStateName(kStateTracking, "Tracking",
1028 "Telescope tracking");
1029
1030 // kStateIdle
1031 // kStateArmed
1032 // kStateMoving
1033 // kStateTracking
1034
1035 // Init
1036 // -----------
1037 // "ARM lock"
1038 // "STGMD off"
1039
1040 /*
1041 [ ] WAIT -> WM_WAIT
1042 [x] STOP! -> WM_STOP
1043 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
1044 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
1045 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
1046 [ ] CELEST id offset angle
1047 [ ] MOON wobble offset
1048 [ ] PREPS string
1049 [ ] TPOIN star mag
1050 [ ] ARM lock/unlock
1051 [ ] STGMD on/off
1052 */
1053
1054 // Drive Commands
1055 T::AddEvent("MOVE_TO", "D:2", kStateArmed) // ->ZDAZ
1056 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
1057 ("Move the telescope to the given local coordinates"
1058 "|Zd[deg]:Zenith distance"
1059 "|Az[deg]:Azimuth");
1060
1061 T::AddEvent("TRACK", "D:2", kStateArmed) // ->RADEC/GRB
1062 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
1063 ("Move the telescope to the given sky coordinates and start tracking them"
1064 "|Ra[h]:Right ascension"
1065 "|Dec[deg]:Declination");
1066
1067 T::AddEvent("WOBBLE", "D:4", kStateArmed) // ->RADEC/GRB
1068 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
1069 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
1070 "|Ra[h]:Right ascension"
1071 "|Dec[deg]:Declination"
1072 "|Offset[deg]:Wobble offset"
1073 "|Angle[deg]:Wobble angle");
1074
1075 T::AddEvent("TRACK_SOURCE", "D:2;C", kStateArmed) // ->RADEC/GRB
1076 (bind(&StateMachineDrive::Track, this, placeholders::_1))
1077 ("Move the telescope to the given wobble position around the given source and start tracking"
1078 "|Offset[deg]:Wobble offset"
1079 "|Angle[deg]:Wobble angle"
1080 "|Name[string]:Source name");
1081
1082 T::AddEvent("MOON", kStateArmed)
1083 (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
1084 ("Start tracking the moon");
1085 T::AddEvent("VENUS", kStateArmed)
1086 (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
1087 ("Start tracking Venus");
1088 T::AddEvent("MARS", kStateArmed)
1089 (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
1090 ("Start tracking Mars");
1091 T::AddEvent("JUPITER", kStateArmed)
1092 (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
1093 ("Start tracking Jupiter");
1094 T::AddEvent("SATURN", kStateArmed)
1095 (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
1096 ("Start tracking Saturn");
1097
1098 T::AddEvent("TAKE_TPOINT")
1099 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
1100 ("Take a TPoint");
1101
1102 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1103 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1104 ("Set the LED brightness of the top and bottom leds"
1105 "|top[au]:Allowed range 0-32767 for top LEDs"
1106 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1107
1108 T::AddEvent("LEDS_OFF")
1109 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
1110 ("Switch off TPoint LEDs");
1111
1112 T::AddEvent("STOP")
1113 (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
1114 ("Stop any kind of movement.");
1115
1116// T::AddEvent("ARM", kStateConnected)
1117// (bind(&StateMachineDrive::SendCommand, this, "ARM lock"))
1118// ("");
1119
1120
1121 // Verbosity commands
1122 T::AddEvent("SET_VERBOSE", "B")
1123 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1124 ("set verbosity state"
1125 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1126
1127 // Conenction commands
1128 AddEvent("DISCONNECT", kStateConnected, kStateArmed)
1129 (bind(&StateMachineDrive::Disconnect, this))
1130 ("disconnect from ethernet");
1131
1132 AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected, kStateArmed)
1133 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1134 ("(Re)connect ethernet connection to FTM, a new address can be given"
1135 "|[host][string]:new ethernet address in the form <host:port>");
1136
1137
1138 T::AddEvent("PRINT")
1139 (bind(&StateMachineDrive::Print, this))
1140 ("Print source list.");
1141
1142 fDrive.StartConnect();
1143 }
1144
1145 void SetEndpoint(const string &url)
1146 {
1147 fDrive.SetEndpoint(url);
1148 }
1149
1150 void ReadDatabase(const string &database)
1151 {
1152 //static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))?");
1153 static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))");
1154 // 2: user
1155 // 4: pass
1156 // 5: server
1157 // 7: port
1158 // 9: db
1159
1160 boost::smatch what;
1161 if (!boost::regex_match(database, what, expr, boost::match_extra))
1162 throw runtime_error("Couldn't parse '"+database+"'.");
1163
1164 if (what.size()!=10)
1165 throw runtime_error("Error parsing '"+database+"'.");
1166
1167 const string user = what[2];
1168 const string passwd = what[4];
1169 const string server = what[5];
1170 const string db = what[9];
1171 const int port = atoi(string(what[7]).c_str());
1172
1173 ostringstream out;
1174 out << "Connecting to '";
1175 if (!user.empty())
1176 out << user << "@";
1177 out << server;
1178 if (port)
1179 out << ":" << port;
1180 if (!db.empty())
1181 out << "/" << db;
1182
1183 T::Message(out);
1184
1185 mysqlpp::Connection conn(db.c_str(), server.c_str(), user.c_str(), passwd.c_str(), port);
1186 /* throws exceptions
1187 if (!conn.connected())
1188 {
1189 cout << "MySQL connection error: " << conn.error() << endl;
1190 throw;
1191 }*/
1192
1193 const mysqlpp::StoreQueryResult res =
1194 conn.query("SELECT fSourceName, fRightAscension, fDeclination FROM source").store();
1195 /* throws exceptions
1196 if (!res)
1197 {
1198 cout << "MySQL query failed: " << query.error() << endl;
1199 throw;
1200 }*/
1201
1202 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
1203 {
1204 const string name = (*v)[0].c_str();
1205 const double ra = (*v)[1];
1206 const double dec = (*v)[2];
1207
1208 // FIXME: Check double names
1209 fSources[name] = make_pair(ra, dec);
1210
1211 ostringstream msg;
1212 msg << " " << name << setprecision(8) << ": Ra=" << ra << "h Dec=" << dec << "deg";
1213 T::Message(msg);
1214 }
1215 }
1216
1217 int EvalOptions(Configuration &conf)
1218 {
1219 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1220
1221 const vector<string> &vec = conf.Vec<string>("source");
1222
1223 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
1224 {
1225 istringstream stream(*it);
1226
1227 string name;
1228 double ra=0;
1229 double dec=0;
1230
1231 int i=0;
1232
1233 string buffer;
1234 while (getline(stream, buffer, ','))
1235 {
1236 istringstream is(buffer);
1237
1238 switch (i++)
1239 {
1240 case 0: name = buffer; break;
1241 case 1: ra = ConnectionDrive::ReadAngle(is); break;
1242 case 2: dec = ConnectionDrive::ReadAngle(is); break;
1243 }
1244
1245 if (is.fail())
1246 break;
1247 }
1248
1249 if (i==3)
1250 {
1251 // FIXME: Check double names
1252 fSources[name] = make_pair(ra, dec);
1253 }
1254 }
1255
1256 if (conf.Has("source-database"))
1257 ReadDatabase(conf.Get<string>("source-database"));
1258
1259 // The possibility to connect should be last, so that
1260 // everything else is already initialized.
1261 SetEndpoint(conf.Get<string>("addr"));
1262
1263 return -1;
1264 }
1265};
1266
1267// ------------------------------------------------------------------------
1268
1269#include "Main.h"
1270
1271
1272template<class T, class S, class R>
1273int RunShell(Configuration &conf)
1274{
1275 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1276}
1277
1278void SetupConfiguration(Configuration &conf)
1279{
1280 po::options_description control("Drive control options");
1281 control.add_options()
1282 ("no-dim,d", po_switch(), "Disable dim services")
1283 ("addr,a", var<string>("localhost:7404"), "Network address of Cosy")
1284 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1285 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
1286 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
1287 ;
1288
1289 conf.AddOptions(control);
1290}
1291
1292/*
1293 Extract usage clause(s) [if any] for SYNOPSIS.
1294 Translators: "Usage" and "or" here are patterns (regular expressions) which
1295 are used to match the usage synopsis in program output. An example from cp
1296 (GNU coreutils) which contains both strings:
1297 Usage: cp [OPTION]... [-T] SOURCE DEST
1298 or: cp [OPTION]... SOURCE... DIRECTORY
1299 or: cp [OPTION]... -t DIRECTORY SOURCE...
1300 */
1301void PrintUsage()
1302{
1303 cout <<
1304 "The drivectrl is an interface to cosy.\n"
1305 "\n"
1306 "The default is that the program is started without user intercation. "
1307 "All actions are supposed to arrive as DimCommands. Using the -c "
1308 "option, a local shell can be initialized. With h or help a short "
1309 "help message about the usuage can be brought to the screen.\n"
1310 "\n"
1311 "Usage: drivectrl [-c type] [OPTIONS]\n"
1312 " or: drivectrl [OPTIONS]\n";
1313 cout << endl;
1314}
1315
1316void PrintHelp()
1317{
1318 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
1319
1320 /* Additional help text which is printed after the configuration
1321 options goes here */
1322
1323 /*
1324 cout << "bla bla bla" << endl << endl;
1325 cout << endl;
1326 cout << "Environment:" << endl;
1327 cout << "environment" << endl;
1328 cout << endl;
1329 cout << "Examples:" << endl;
1330 cout << "test exam" << endl;
1331 cout << endl;
1332 cout << "Files:" << endl;
1333 cout << "files" << endl;
1334 cout << endl;
1335 */
1336}
1337
1338int main(int argc, const char* argv[])
1339{
1340 Configuration conf(argv[0]);
1341 conf.SetPrintUsage(PrintUsage);
1342 Main::SetupConfiguration(conf);
1343 SetupConfiguration(conf);
1344
1345 if (!conf.DoParse(argc, argv, PrintHelp))
1346 return -1;
1347
1348 //try
1349 {
1350 // No console access at all
1351 if (!conf.Has("console"))
1352 {
1353 if (conf.Get<bool>("no-dim"))
1354 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1355 else
1356 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1357 }
1358 // Cosole access w/ and w/o Dim
1359 if (conf.Get<bool>("no-dim"))
1360 {
1361 if (conf.Get<int>("console")==0)
1362 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1363 else
1364 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1365 }
1366 else
1367 {
1368 if (conf.Get<int>("console")==0)
1369 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1370 else
1371 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1372 }
1373 }
1374 /*catch (std::exception& e)
1375 {
1376 cerr << "Exception: " << e.what() << endl;
1377 return -1;
1378 }*/
1379
1380 return 0;
1381}
Note: See TracBrowser for help on using the repository browser.