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

Last change on this file since 13142 was 12989, checked in by tbretz, 13 years ago
Replaces status==0 but 0x100-3 (ERROR) instead of 99, leading to a state 102
File size: 39.6 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 const double *dat = evt.Ptr<double>();
882 const string name = evt.Ptr<char>(16);
883
884 const sources::const_iterator it = fSources.find(name);
885 if (it==fSources.end())
886 return T::Error("Source '"+name+"' not found in list.");
887
888 const double &ra = it->second.first;
889 const double &dec = it->second.second;
890
891 return StartWobble(ra, dec, dat[0], dat[1], name);
892 }
893
894 int SetLedBrightness(const EventImp &evt)
895 {
896 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
897 return T::kSM_FatalError;
898
899 const uint32_t *led = evt.Ptr<uint32_t>();
900
901 ostringstream cmd;
902 cmd << "LEDS " << led[0] << " " << led[1];
903 return SendCommand(cmd.str(), false);
904 }
905
906
907 int SetVerbosity(const EventImp &evt)
908 {
909 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
910 return T::kSM_FatalError;
911
912 fDrive.SetVerbose(evt.GetBool());
913
914 return T::GetCurrentState();
915 }
916
917 int Print()
918 {
919 for (sources::const_iterator it=fSources.begin();
920 it!=fSources.end(); it++)
921 {
922 const string &name = it->first;
923 const double &ra = it->second.first;
924 const double &dec = it->second.second;
925
926 T::Out() << name << "," << ra << "," << dec << endl;
927 }
928 return T::GetCurrentState();
929 }
930
931 int Disconnect()
932 {
933 // Close all connections
934 fDrive.PostClose(false);
935
936 /*
937 // Now wait until all connection have been closed and
938 // all pending handlers have been processed
939 poll();
940 */
941
942 return T::GetCurrentState();
943 }
944
945 int Reconnect(const EventImp &evt)
946 {
947 // Close all connections to supress the warning in SetEndpoint
948 fDrive.PostClose(false);
949
950 // Now wait until all connection have been closed and
951 // all pending handlers have been processed
952 poll();
953
954 if (evt.GetBool())
955 fDrive.SetEndpoint(evt.GetString());
956
957 // Now we can reopen the connection
958 fDrive.PostClose(true);
959
960 return T::GetCurrentState();
961 }
962
963 int Execute()
964 {
965 // Dispatch (execute) at most one handler from the queue. In contrary
966 // to run_one(), it doesn't wait until a handler is available
967 // which can be dispatched, so poll_one() might return with 0
968 // handlers dispatched. The handlers are always dispatched/executed
969 // synchronously, i.e. within the call to poll_one()
970 poll_one();
971
972 return fDrive.GetState();
973 }
974
975
976public:
977 StateMachineDrive(ostream &out=cout) :
978 T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
979 fDrive(*this, *this)
980 {
981 // ba::io_service::work is a kind of keep_alive for the loop.
982 // It prevents the io_service to go to stopped state, which
983 // would prevent any consecutive calls to run()
984 // or poll() to do nothing. reset() could also revoke to the
985 // previous state but this might introduce some overhead of
986 // deletion and creation of threads and more.
987
988 // State names
989 AddStateName(kStateDisconnected, "Disconnected",
990 "No connection to cosy");
991
992 AddStateName(kStateConnected, "Connected",
993 "Cosy connected, drive stopped");
994
995 AddStateName(kStateNotReady, "NotReady",
996 "Drive system not ready for movement");
997
998 AddStateName(kStateReady, "Ready",
999 "Drive system ready for movement");
1000
1001 AddStateName(kStateArmed, "Armed",
1002 "Cosy armed, drive stopped");
1003
1004 AddStateName(kStateMoving, "Moving",
1005 "Telescope moving");
1006
1007 AddStateName(kStateTracking, "Tracking",
1008 "Telescope tracking");
1009
1010 // kStateIdle
1011 // kStateArmed
1012 // kStateMoving
1013 // kStateTracking
1014
1015 // Init
1016 // -----------
1017 // "ARM lock"
1018 // "STGMD off"
1019
1020 /*
1021 [ ] WAIT -> WM_WAIT
1022 [x] STOP! -> WM_STOP
1023 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
1024 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
1025 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
1026 [ ] CELEST id offset angle
1027 [ ] MOON wobble offset
1028 [ ] PREPS string
1029 [ ] TPOIN star mag
1030 [ ] ARM lock/unlock
1031 [ ] STGMD on/off
1032 */
1033
1034 // Drive Commands
1035 T::AddEvent("MOVE_TO", "D:2", kStateArmed) // ->ZDAZ
1036 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
1037 ("Move the telescope to the given local coordinates"
1038 "|Zd[deg]:Zenith distance"
1039 "|Az[deg]:Azimuth");
1040
1041 T::AddEvent("TRACK", "D:2", kStateArmed) // ->RADEC/GRB
1042 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
1043 ("Move the telescope to the given sky coordinates and start tracking them"
1044 "|Ra[h]:Right ascension"
1045 "|Dec[deg]:Declination");
1046
1047 T::AddEvent("WOBBLE", "D:4", kStateArmed) // ->RADEC/GRB
1048 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
1049 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
1050 "|Ra[h]:Right ascension"
1051 "|Dec[deg]:Declination"
1052 "|Offset[deg]:Wobble offset"
1053 "|Angle[deg]:Wobble angle");
1054
1055 T::AddEvent("TRACK_SOURCE", "D:2;W", kStateArmed) // ->RADEC/GRB
1056 (bind(&StateMachineDrive::Track, this, placeholders::_1))
1057 ("Move the telescope to the given wobble position around the given source and start tracking"
1058 "|Offset[deg]:Wobble offset"
1059 "|Angle[deg]:Wobble angle"
1060 "|Name[string]:Source name");
1061
1062 T::AddEvent("MOON", kStateArmed)
1063 (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
1064 ("Start tracking the moon");
1065 T::AddEvent("VENUS", kStateArmed)
1066 (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
1067 ("Start tracking Venus");
1068 T::AddEvent("MARS", kStateArmed)
1069 (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
1070 ("Start tracking Mars");
1071 T::AddEvent("JUPITER", kStateArmed)
1072 (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
1073 ("Start tracking Jupiter");
1074 T::AddEvent("SATURN", kStateArmed)
1075 (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
1076 ("Start tracking Saturn");
1077
1078 T::AddEvent("TAKE_TPOINT")
1079 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
1080 ("Take a TPoint");
1081
1082 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1083 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1084 ("Set the LED brightness of the top and bottom leds"
1085 "|top[au]:Allowed range 0-32767 for top LEDs"
1086 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1087
1088 T::AddEvent("LEDS_OFF")
1089 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
1090 ("Switch off TPoint LEDs");
1091
1092 T::AddEvent("STOP")
1093 (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
1094 ("Stop any kind of movement.");
1095
1096// T::AddEvent("ARM", kStateConnected)
1097// (bind(&StateMachineDrive::SendCommand, this, "ARM lock"))
1098// ("");
1099
1100
1101 // Verbosity commands
1102 T::AddEvent("SET_VERBOSE", "B")
1103 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1104 ("set verbosity state"
1105 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1106
1107 // Conenction commands
1108 AddEvent("DISCONNECT", kStateConnected, kStateArmed)
1109 (bind(&StateMachineDrive::Disconnect, this))
1110 ("disconnect from ethernet");
1111
1112 AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected, kStateArmed)
1113 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1114 ("(Re)connect ethernet connection to FTM, a new address can be given"
1115 "|[host][string]:new ethernet address in the form <host:port>");
1116
1117
1118 T::AddEvent("PRINT")
1119 (bind(&StateMachineDrive::Print, this))
1120 ("Print source list.");
1121
1122 fDrive.StartConnect();
1123 }
1124
1125 void SetEndpoint(const string &url)
1126 {
1127 fDrive.SetEndpoint(url);
1128 }
1129
1130 void ReadDatabase(const string &database)
1131 {
1132 //static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))?");
1133 static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))");
1134 // 2: user
1135 // 4: pass
1136 // 5: server
1137 // 7: port
1138 // 9: db
1139
1140 boost::smatch what;
1141 if (!boost::regex_match(database, what, expr, boost::match_extra))
1142 throw runtime_error("Couldn't parse '"+database+"'.");
1143
1144 if (what.size()!=10)
1145 throw runtime_error("Error parsing '"+database+"'.");
1146
1147 const string user = what[2];
1148 const string passwd = what[4];
1149 const string server = what[5];
1150 const string db = what[9];
1151 const int port = atoi(string(what[7]).c_str());
1152
1153 ostringstream out;
1154 out << "Connecting to '";
1155 if (!user.empty())
1156 out << user << "@";
1157 out << server;
1158 if (port)
1159 out << ":" << port;
1160 if (!db.empty())
1161 out << "/" << db;
1162
1163 T::Message(out);
1164
1165 mysqlpp::Connection conn(db.c_str(), server.c_str(), user.c_str(), passwd.c_str(), port);
1166 /* throws exceptions
1167 if (!conn.connected())
1168 {
1169 cout << "MySQL connection error: " << conn.error() << endl;
1170 throw;
1171 }*/
1172
1173 const mysqlpp::StoreQueryResult res =
1174 conn.query("SELECT fSourceName, fRightAscension, fDeclination FROM source").store();
1175 /* throws exceptions
1176 if (!res)
1177 {
1178 cout << "MySQL query failed: " << query.error() << endl;
1179 throw;
1180 }*/
1181
1182 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
1183 {
1184 const string name = (*v)[0].c_str();
1185 const double ra = (*v)[1];
1186 const double dec = (*v)[2];
1187
1188 // FIXME: Check double names
1189 fSources[name] = make_pair(ra, dec);
1190
1191 ostringstream msg;
1192 msg << " " << name << setprecision(8) << ": Ra=" << ra << "h Dec=" << dec << "deg";
1193 T::Message(msg);
1194 }
1195 }
1196
1197 int EvalOptions(Configuration &conf)
1198 {
1199 SetEndpoint(conf.Get<string>("addr"));
1200
1201 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1202
1203 const vector<string> &vec = conf.Vec<string>("source");
1204
1205 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
1206 {
1207 istringstream stream(*it);
1208
1209 string name;
1210 double ra=0;
1211 double dec=0;
1212
1213 int i=0;
1214
1215 string buffer;
1216 while (getline(stream, buffer, ','))
1217 {
1218 istringstream is(buffer);
1219
1220 switch (i++)
1221 {
1222 case 0: name = buffer; break;
1223 case 1: ra = ConnectionDrive::ReadAngle(is); break;
1224 case 2: dec = ConnectionDrive::ReadAngle(is); break;
1225 }
1226
1227 if (is.fail())
1228 break;
1229 }
1230
1231 if (i==3)
1232 {
1233 // FIXME: Check double names
1234 fSources[name] = make_pair(ra, dec);
1235 }
1236 }
1237
1238 if (conf.Has("source-database"))
1239 ReadDatabase(conf.Get<string>("source-database"));
1240
1241 return -1;
1242 }
1243};
1244
1245// ------------------------------------------------------------------------
1246
1247#include "Main.h"
1248
1249
1250template<class T, class S, class R>
1251int RunShell(Configuration &conf)
1252{
1253 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1254}
1255
1256void SetupConfiguration(Configuration &conf)
1257{
1258 po::options_description control("Drive control options");
1259 control.add_options()
1260 ("no-dim,d", po_switch(), "Disable dim services")
1261 ("addr,a", var<string>("localhost:7404"), "Network address of Cosy")
1262 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1263 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
1264 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
1265 ;
1266
1267 conf.AddOptions(control);
1268}
1269
1270/*
1271 Extract usage clause(s) [if any] for SYNOPSIS.
1272 Translators: "Usage" and "or" here are patterns (regular expressions) which
1273 are used to match the usage synopsis in program output. An example from cp
1274 (GNU coreutils) which contains both strings:
1275 Usage: cp [OPTION]... [-T] SOURCE DEST
1276 or: cp [OPTION]... SOURCE... DIRECTORY
1277 or: cp [OPTION]... -t DIRECTORY SOURCE...
1278 */
1279void PrintUsage()
1280{
1281 cout <<
1282 "The drivectrl is an interface to cosy.\n"
1283 "\n"
1284 "The default is that the program is started without user intercation. "
1285 "All actions are supposed to arrive as DimCommands. Using the -c "
1286 "option, a local shell can be initialized. With h or help a short "
1287 "help message about the usuage can be brought to the screen.\n"
1288 "\n"
1289 "Usage: drivectrl [-c type] [OPTIONS]\n"
1290 " or: drivectrl [OPTIONS]\n";
1291 cout << endl;
1292}
1293
1294void PrintHelp()
1295{
1296 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
1297
1298 /* Additional help text which is printed after the configuration
1299 options goes here */
1300
1301 /*
1302 cout << "bla bla bla" << endl << endl;
1303 cout << endl;
1304 cout << "Environment:" << endl;
1305 cout << "environment" << endl;
1306 cout << endl;
1307 cout << "Examples:" << endl;
1308 cout << "test exam" << endl;
1309 cout << endl;
1310 cout << "Files:" << endl;
1311 cout << "files" << endl;
1312 cout << endl;
1313 */
1314}
1315
1316int main(int argc, const char* argv[])
1317{
1318 Configuration conf(argv[0]);
1319 conf.SetPrintUsage(PrintUsage);
1320 Main::SetupConfiguration(conf);
1321 SetupConfiguration(conf);
1322
1323 if (!conf.DoParse(argc, argv, PrintHelp))
1324 return -1;
1325
1326 //try
1327 {
1328 // No console access at all
1329 if (!conf.Has("console"))
1330 {
1331 if (conf.Get<bool>("no-dim"))
1332 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1333 else
1334 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1335 }
1336 // Cosole access w/ and w/o Dim
1337 if (conf.Get<bool>("no-dim"))
1338 {
1339 if (conf.Get<int>("console")==0)
1340 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1341 else
1342 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1343 }
1344 else
1345 {
1346 if (conf.Get<int>("console")==0)
1347 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1348 else
1349 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1350 }
1351 }
1352 /*catch (std::exception& e)
1353 {
1354 cerr << "Exception: " << e.what() << endl;
1355 return -1;
1356 }*/
1357
1358 return 0;
1359}
Note: See TracBrowser for help on using the repository browser.