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

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