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

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