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

Last change on this file since 13985 was 13985, checked in by tbretz, 12 years ago
Added PARK command.
File size: 41.5 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;
346
347 const bool ready = (pdo3&0xef00ef)==0xef00ef;
348 if (!ready)
349 fState = Drive::State::kNotReady;
350 else
351 fState = status1==1 ?
352 Drive::State::kReady+armed :
353 Drive::State::kNotReady+status1;
354
355 // kDisconnected = 1,
356 // kConnected,
357 // kNotReady,
358 // kReady,
359 // kArmed,
360 // kMoving,
361 // kTracking,
362
363 // pdo3:
364 // 1 Ab
365 // 2 1
366 // 4 Emergency
367 // 8 OverVolt
368 // 10 Move (Drehen-soll)
369 // 20 Af
370 // 40 1
371 // 80 Power on Az
372 // ------------------
373 // 100 NOT UPS Alarm
374 // 200 UPS on Battery
375 // 400 UPS charging
376
377 // Power cut: 2ef02ef
378 // charging: 4ef04ef
379
380 const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
381 UpdateStatus(t1, state);
382
383 const array<double, 2> point = {{ zd2, az2 }};
384 UpdatePointing(t1, point);
385
386 const array<double, 7> track =
387 {{
388 ra, dec, ha,
389 zd1, az1,
390 zd_err/3600, az_err/3600
391 }};
392 if (mjd>0)
393 UpdateTracking(Time(mjd), track);
394
395 // ---- DIM ----> t1 as event time
396 // status1
397 // mjd
398 // ra/dec/ha
399 // zd/az (nominal)
400 // zd/az (current)
401 // err(zd/az)
402 // [armed] [stgmd]
403
404 // Maybe:
405 // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
406 //
407 // if (mjd>0)
408 // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
409 // ra/dec, ha(not well defined),
410 // [Nominal + Error == Current]
411
412 // MJD is the time which corresponds to the nominal position
413 // t1 is the time which corresponds to the current position/HA
414
415 return;
416 }
417 }
418
419 void StartReadReport()
420 {
421 boost::asio::async_read_until(*this, fBuffer, '\n',
422 boost::bind(&ConnectionDrive::HandleReceivedReport, this,
423 dummy::error, dummy::bytes_transferred));
424 }
425
426 boost::asio::deadline_timer fKeepAlive;
427
428 void KeepAlive()
429 {
430 PostMessage(string("KEEP_ALIVE"));
431
432 fKeepAlive.expires_from_now(boost::posix_time::seconds(10));
433 fKeepAlive.async_wait(boost::bind(&ConnectionDrive::HandleKeepAlive,
434 this, dummy::error));
435 }
436
437 void HandleKeepAlive(const bs::error_code &error)
438 {
439 // 125: Operation canceled (bs::error_code(125, bs::system_category))
440 if (error && error!=ba::error::basic_errors::operation_aborted)
441 {
442 ostringstream str;
443 str << "Write timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
444 Error(str);
445
446 PostClose(false);
447 return;
448 }
449
450 if (!is_open())
451 {
452 // For example: Here we could schedule a new accept if we
453 // would not want to allow two connections at the same time.
454 return;
455 }
456
457 // Check whether the deadline has passed. We compare the deadline
458 // against the current time since a new asynchronous operation
459 // may have moved the deadline before this actor had a chance
460 // to run.
461 if (fKeepAlive.expires_at() > ba::deadline_timer::traits_type::now())
462 return;
463
464 KeepAlive();
465 }
466
467
468private:
469 // This is called when a connection was established
470 void ConnectionEstablished()
471 {
472 StartReadReport();
473 KeepAlive();
474 }
475
476 /*
477 void HandleReadTimeout(const bs::error_code &error)
478 {
479 if (error && error!=ba::error::basic_errors::operation_aborted)
480 {
481 stringstream str;
482 str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
483 Error(str);
484
485 PostClose();
486 return;
487
488 }
489
490 if (!is_open())
491 {
492 // For example: Here we could schedule a new accept if we
493 // would not want to allow two connections at the same time.
494 return;
495 }
496
497 // Check whether the deadline has passed. We compare the deadline
498 // against the current time since a new asynchronous operation
499 // may have moved the deadline before this actor had a chance
500 // to run.
501 if (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
502 return;
503
504 Error("Timeout reading data from "+URL());
505
506 PostClose();
507 }*/
508
509
510public:
511
512 static const uint16_t kMaxAddr;
513
514public:
515 ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
516 fState(-1), fIsVerbose(true), fKeepAlive(ioservice)
517 {
518 SetLogStream(&imp);
519 }
520
521 void SetVerbose(bool b)
522 {
523 fIsVerbose = b;
524 }
525
526 int GetState() const
527 {
528 if (!IsConnected())
529 return 1;
530 if (IsConnected() && fState<0)
531 return 2;
532 return fState;
533 }
534};
535
536const uint16_t ConnectionkMaxAddr = 0xfff;
537
538// ------------------------------------------------------------------------
539
540#include "DimDescriptionService.h"
541
542class ConnectionDimDrive : public ConnectionDrive
543{
544private:
545 DimDescribedService fDimPointing;
546 DimDescribedService fDimTracking;
547 DimDescribedService fDimSource;
548 DimDescribedService fDimTPoint;
549 DimDescribedService fDimStatus;
550
551 void UpdatePointing(const Time &t, const array<double, 2> &arr)
552 {
553 fDimPointing.setData(arr);
554 fDimPointing.Update(t);
555 }
556
557 void UpdateTracking(const Time &t,const array<double, 7> &arr)
558 {
559 fDimTracking.setData(arr);
560 fDimTracking.Update(t);
561 }
562
563 void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
564 {
565 fDimStatus.setData(arr);
566 fDimStatus.Update(t);
567 }
568
569 void UpdateTPoint(const Time &t, const DimTPoint &data,
570 const string &name)
571 {
572 vector<char> dim(sizeof(data)+name.length()+1);
573 memcpy(dim.data(), &data, sizeof(data));
574 memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1);
575
576 fDimTPoint.setData(dim);
577 fDimTPoint.Update(t);
578 }
579
580public:
581 ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
582 ConnectionDrive(ioservice, imp),
583 fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1",
584 "|Zd[deg]:Zenith distance (encoder readout)"
585 "|Az[deg]:Azimuth angle (encoder readout)"),
586 fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1",
587 "|Ra[h]:Command right ascension"
588 "|Dec[deg]:Command declination"
589 "|Ha[h]:Corresponding hour angle"
590 "|Zd[deg]:Nominal zenith distance"
591 "|Az[deg]:Nominal azimuth angle"
592 "|dZd[deg]:Control deviation Zd"
593 "|dAz[deg]:Control deviation Az"),
594 fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;C:31",
595 "|Ra_src[h]:Source right ascension"
596 "|Dec_src[deg]:Source declination"
597 "|Ra_cmd[h]:Command right ascension"
598 "|Dec_cmd[deg]:Command declination"
599 "|Offset[deg]:Wobble offset"
600 "|Angle[deg]:Wobble angle"
601 "|Name[string]:Source name if available"),
602 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",
603 "|Ra[h]:Command right ascension"
604 "|Dec[deg]:Command declination"
605 "|Zd_nom[deg]:Nominal zenith distance"
606 "|Az_nom[deg]:Nominal azimuth angle"
607 "|Zd_cur[deg]:Current zenith distance (calculated from image)"
608 "|Az_cur[deg]:Current azimuth angle (calculated from image)"
609 "|Zd_enc[deg]:Feedback zenith axis (from encoder)"
610 "|Az_enc[deg]:Feedback azimuth angle (from encoder)"
611 "|N_leds[cnt]:Number of detected LEDs"
612 "|N_rings[cnt]:Number of rings used to calculate the camera center"
613 "|Xc[pix]:X position of center in CCD camera frame"
614 "|Yc[pix]:Y position of center in CCD camera frame"
615 "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)"
616 "|Xs[pix]:X position of start in CCD camera frame"
617 "|Ys[pix]:Y position of star in CCD camera frame"
618 "|Ms[mag]:Artifical magnitude of star (calculated form image))"
619 "|Mc[mag]:Catalog magnitude of star"),
620 fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", "")
621
622 {
623 }
624
625 void UpdateSource()
626 {
627 const vector<char> empty(6*sizeof(double)+31, 0);
628 fDimSource.setQuality(0);
629 fDimSource.Update(empty);
630 }
631
632 void UpdateSource(const array<double, 6> &arr, const string &name="")
633 {
634 vector<char> dat(6*sizeof(double)+31, 0);
635 memcpy(dat.data(), arr.data(), 6*sizeof(double));
636 strncpy(dat.data()+6*sizeof(double), name.c_str(), 30);
637
638 fDimSource.setQuality(1);
639 fDimSource.Update(dat);
640 }
641
642 // 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
643};
644
645// ------------------------------------------------------------------------
646
647struct Source
648{
649 Source() : ra(0), dec(0), offset(0)
650 {
651 angle[0] = angle[1] = 0;
652 }
653
654 double ra;
655 double dec;
656 double offset;
657 array<double, 2> angle;
658};
659
660
661template <class T, class S>
662class StateMachineDrive : public T, public ba::io_service, public ba::io_service::work
663{
664private:
665 S fDrive;
666
667 string fDatabase;
668
669 typedef map<string, Source> sources;
670 sources fSources;
671
672 // Status 0: Error
673 // Status 1: Unlocked
674 // Status 2: Locked
675 // Status 3: Stopping || Moving
676 // Status 4: Tracking
677
678 bool CheckEventSize(size_t has, const char *name, size_t size)
679 {
680 if (has==size)
681 return true;
682
683 ostringstream msg;
684 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
685 T::Fatal(msg);
686 return false;
687 }
688
689 enum Coordinates
690 {
691 kPoint,
692 kTrackSlow,
693 kTrackFast
694 };
695
696 string AngleToStr(double angle)
697 {
698 /* Handle sign */
699 const char sgn = angle<0?'-':'+';
700
701 /* Round interval and express in smallest units required */
702 double a = round(3600. * fabs(angle)); // deg to seconds
703
704 /* Separate into fields */
705 const double ad = trunc(a/3600.);
706 a -= ad * 3600.;
707 const double am = trunc(a/60.);
708 a -= am * 60.;
709 const double as = trunc(a);
710
711 /* Return results */
712 ostringstream str;
713 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
714 return str.str();
715 }
716
717 int SendCommand(const string &str, bool upd=true)
718 {
719 fDrive.PostMessage(str);
720 T::Message("Sending: "+str);
721
722 if (upd)
723 fDrive.UpdateSource();
724
725 return T::GetCurrentState();
726 }
727
728 int SendCoordinates(const EventImp &evt, const Coordinates type)
729 {
730 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
731 return T::kSM_FatalError;
732
733 const double *dat = evt.Ptr<double>();
734
735 string command;
736
737 switch (type)
738 {
739 case kPoint: command += "ZDAZ "; break;
740 case kTrackSlow: command += "RADEC "; break;
741 case kTrackFast: command += "GRB "; break;
742 }
743
744 if (type!=kPoint)
745 {
746 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
747 fDrive.UpdateSource(dim);
748 }
749
750 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
751 return SendCommand(command, type==kPoint);
752 }
753
754 int StartWobble(const double &srcra, const double &srcdec,
755 const double &woboff, const double &wobang,
756 const string name="")
757 {
758 const double ra = srcra *M_PI/12;
759 const double dec = srcdec*M_PI/180;
760 const double off = woboff*M_PI/180;
761 const double dir = wobang*M_PI/180;
762
763 const double cosdir = cos(dir);
764 const double sindir = sin(dir);
765 const double cosoff = cos(off);
766 const double sinoff = sin(off);
767 const double cosdec = cos(dec);
768 const double sindec = sin(dec);
769
770 if (off==0)
771 {
772 const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
773 fDrive.UpdateSource(dim, name);
774
775 string command = "RADEC ";
776 command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
777 return SendCommand(command, false);
778 }
779
780 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
781 if (sintheta >= 1)
782 {
783 T::Error("cos(Zd) > 1");
784 return T::GetCurrentState();
785 }
786
787 const double costheta = sqrt(1 - sintheta*sintheta);
788
789 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
790 const double sindeltara = sindir*sinoff/costheta;
791
792 const double ndec = asin(sintheta)*180/M_PI;
793 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
794
795 const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
796 fDrive.UpdateSource(dim, name);
797
798 string command = "RADEC ";
799 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
800 return SendCommand(command, false);
801 }
802
803 int Wobble(const EventImp &evt)
804 {
805 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
806 return T::kSM_FatalError;
807
808 const double *dat = evt.Ptr<double>();
809
810 return StartWobble(dat[0], dat[1], dat[2], dat[3]);
811 }
812
813 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
814 {
815 if (find(ptr, last, '\0')==last)
816 {
817 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
818 throw uint32_t(T::kSM_FatalError);
819 }
820
821 const string name(Tools::TrimQuotes(ptr));
822
823 const sources::const_iterator it = fSources.find(name);
824 if (it==fSources.end())
825 {
826 T::Error("Source '"+name+"' not found in list.");
827 throw uint32_t(T::GetCurrentState());
828 }
829
830 return it;
831 }
832
833 int TrackWobble(const EventImp &evt)
834 {
835 if (evt.GetSize()<=2)
836 {
837 ostringstream msg;
838 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
839 T::Fatal(msg);
840 return T::kSM_FatalError;
841 }
842
843 const uint16_t wobble = evt.GetUShort();
844 if (wobble!=1 && wobble!=2)
845 {
846 ostringstream msg;
847 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
848 T::Error(msg);
849 return T::GetCurrentState();
850 }
851
852 const char *ptr = evt.Ptr<char>(2);
853 const char *last = ptr+evt.GetSize()-2;
854
855 try
856 {
857 const sources::const_iterator it = GetSourceFromDB(ptr, last);
858
859 const string &name = it->first;
860 const Source &src = it->second;
861
862 return StartWobble(src.ra, src.dec, src.offset, src.angle[wobble-1], name);
863 }
864 catch (const uint32_t &e)
865 {
866 return e;
867 }
868 }
869
870 int Track(const EventImp &evt)
871 {
872 if (evt.GetSize()<=16)
873 {
874 ostringstream msg;
875 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
876 T::Fatal(msg);
877 return T::kSM_FatalError;
878 }
879
880 const double *dat = evt.Ptr<double>();
881 const char *ptr = evt.Ptr<char>(16);
882 const char *last = ptr+evt.GetSize()-16;
883
884 try
885 {
886 const sources::const_iterator it = GetSourceFromDB(ptr, last);
887
888 const string &name = it->first;
889 const Source &src = it->second;
890
891 return StartWobble(src.ra, src.dec, dat[0], dat[1], name);
892 }
893 catch (const uint32_t &e)
894 {
895 return e;
896 }
897 }
898
899 int SetLedBrightness(const EventImp &evt)
900 {
901 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
902 return T::kSM_FatalError;
903
904 const uint32_t *led = evt.Ptr<uint32_t>();
905
906 ostringstream cmd;
907 cmd << "LEDS " << led[0] << " " << led[1];
908 return SendCommand(cmd.str(), false);
909 }
910
911
912 int SetVerbosity(const EventImp &evt)
913 {
914 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
915 return T::kSM_FatalError;
916
917 fDrive.SetVerbose(evt.GetBool());
918
919 return T::GetCurrentState();
920 }
921
922 int Print()
923 {
924 for (auto it=fSources.begin(); it!=fSources.end(); it++)
925 {
926 const string &name = it->first;
927 const Source &src = it->second;
928
929 T::Out() << name << ",";
930 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
931 T::Out() << src.angle[0] << "," << src.angle[1] << endl;
932 }
933 return T::GetCurrentState();
934 }
935
936 int ReloadSources()
937 {
938 try
939 {
940 ReadDatabase();
941 }
942 catch (const exception &e)
943 {
944 T::Error("Reading sources from databse failed: "+string(e.what()));
945 }
946 return T::GetCurrentState();
947 }
948
949 int Disconnect()
950 {
951 // Close all connections
952 fDrive.PostClose(false);
953
954 /*
955 // Now wait until all connection have been closed and
956 // all pending handlers have been processed
957 poll();
958 */
959
960 return T::GetCurrentState();
961 }
962
963 int Reconnect(const EventImp &evt)
964 {
965 // Close all connections to supress the warning in SetEndpoint
966 fDrive.PostClose(false);
967
968 // Now wait until all connection have been closed and
969 // all pending handlers have been processed
970 poll();
971
972 if (evt.GetBool())
973 fDrive.SetEndpoint(evt.GetString());
974
975 // Now we can reopen the connection
976 fDrive.PostClose(true);
977
978 return T::GetCurrentState();
979 }
980
981 int Execute()
982 {
983 // Dispatch (execute) at most one handler from the queue. In contrary
984 // to run_one(), it doesn't wait until a handler is available
985 // which can be dispatched, so poll_one() might return with 0
986 // handlers dispatched. The handlers are always dispatched/executed
987 // synchronously, i.e. within the call to poll_one()
988 poll_one();
989
990 return fDrive.GetState();
991 }
992
993
994public:
995 StateMachineDrive(ostream &out=cout) :
996 T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
997 fDrive(*this, *this)
998 {
999 // State names
1000 T::AddStateName(State::kDisconnected, "Disconnected",
1001 "No connection to cosy");
1002
1003 T::AddStateName(State::kConnected, "Connected",
1004 "Cosy connected, drive stopped");
1005
1006 T::AddStateName(State::kNotReady, "NotReady",
1007 "Drive system not ready for movement");
1008
1009 T::AddStateName(State::kReady, "Ready",
1010 "Drive system ready for movement");
1011
1012 T::AddStateName(State::kArmed, "Armed",
1013 "Cosy armed, drive stopped");
1014
1015 T::AddStateName(State::kMoving, "Moving",
1016 "Telescope moving");
1017
1018 T::AddStateName(State::kTracking, "Tracking",
1019 "Telescope tracking");
1020
1021 // State::kIdle
1022 // State::kArmed
1023 // State::kMoving
1024 // State::kTracking
1025
1026 // Init
1027 // -----------
1028 // "ARM lock"
1029 // "STGMD off"
1030
1031 /*
1032 [ ] WAIT -> WM_WAIT
1033 [x] STOP! -> WM_STOP
1034 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
1035 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
1036 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
1037 [ ] CELEST id offset angle
1038 [ ] MOON wobble offset
1039 [ ] PREPS string
1040 [ ] TPOIN star mag
1041 [ ] ARM lock/unlock
1042 [ ] STGMD on/off
1043 */
1044
1045 // Drive Commands
1046 T::AddEvent("MOVE_TO", "D:2", State::kArmed) // ->ZDAZ
1047 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
1048 ("Move the telescope to the given local coordinates"
1049 "|Zd[deg]:Zenith distance"
1050 "|Az[deg]:Azimuth");
1051
1052 T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking) // ->RADEC/GRB
1053 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
1054 ("Move the telescope to the given sky coordinates and start tracking them"
1055 "|Ra[h]:Right ascension"
1056 "|Dec[deg]:Declination");
1057
1058 T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking) // ->RADEC/GRB
1059 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
1060 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
1061 "|Ra[h]:Right ascension"
1062 "|Dec[deg]:Declination"
1063 "|Offset[deg]:Wobble offset"
1064 "|Angle[deg]:Wobble angle");
1065
1066 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking) // ->RADEC/GRB
1067 (bind(&StateMachineDrive::Track, this, placeholders::_1))
1068 ("Move the telescope to the given wobble position around the given source and start tracking"
1069 "|Offset[deg]:Wobble offset"
1070 "|Angle[deg]:Wobble angle"
1071 "|Name[string]:Source name");
1072
1073 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking) // ->RADEC/GRB
1074 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
1075 ("Move the telescope to the given wobble position around the given source and start tracking"
1076 "|id:Wobble angle id (1 or 2)"
1077 "|Name[string]:Source name");
1078
1079 T::AddEvent("MOON", State::kArmed, State::kTracking)
1080 (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
1081 ("Start tracking the moon");
1082 T::AddEvent("VENUS", State::kArmed, State::kTracking)
1083 (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
1084 ("Start tracking Venus");
1085 T::AddEvent("MARS", State::kArmed, State::kTracking)
1086 (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
1087 ("Start tracking Mars");
1088 T::AddEvent("JUPITER", State::kArmed, State::kTracking)
1089 (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
1090 ("Start tracking Jupiter");
1091 T::AddEvent("SATURN", State::kArmed, State::kTracking)
1092 (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
1093 ("Start tracking Saturn");
1094
1095 T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking)
1096 (bind(&StateMachineDrive::SendCommand, this, "PREPS Park", false))
1097 ("Park the telescope");
1098
1099 T::AddEvent("TAKE_TPOINT")
1100 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
1101 ("Take a TPoint");
1102
1103 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1104 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1105 ("Set the LED brightness of the top and bottom leds"
1106 "|top[au]:Allowed range 0-32767 for top LEDs"
1107 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1108
1109 T::AddEvent("LEDS_OFF")
1110 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
1111 ("Switch off TPoint LEDs");
1112
1113 T::AddEvent("STOP")
1114 (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
1115 ("Stop any kind of movement.");
1116
1117// T::AddEvent("ARM", State::kConnected)
1118// (bind(&StateMachineSendCommand, this, "ARM lock"))
1119// ("");
1120
1121
1122 // Verbosity commands
1123 T::AddEvent("SET_VERBOSE", "B")
1124 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1125 ("set verbosity state"
1126 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1127
1128 // Conenction commands
1129 T::AddEvent("DISCONNECT", State::kConnected, State::kArmed)
1130 (bind(&StateMachineDrive::Disconnect, this))
1131 ("disconnect from ethernet");
1132
1133 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected, State::kArmed)
1134 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1135 ("(Re)connect ethernet connection to FTM, a new address can be given"
1136 "|[host][string]:new ethernet address in the form <host:port>");
1137
1138
1139 T::AddEvent("PRINT")
1140 (bind(&StateMachineDrive::Print, this))
1141 ("Print source list.");
1142
1143 T::AddEvent("RELOAD_SOURCES")
1144 (bind(&StateMachineDrive::ReloadSources, this))
1145 ("Reload sources from database.");
1146
1147 fDrive.StartConnect();
1148 }
1149
1150 void SetEndpoint(const string &url)
1151 {
1152 fDrive.SetEndpoint(url);
1153 }
1154
1155 void ReadDatabase(bool print=true)
1156 {
1157 //static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))?");
1158 static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))");
1159 // 2: user
1160 // 4: pass
1161 // 5: server
1162 // 7: port
1163 // 9: db
1164
1165 boost::smatch what;
1166 if (!boost::regex_match(fDatabase, what, expr, boost::match_extra))
1167 throw runtime_error("Couldn't parse '"+fDatabase+"'.");
1168
1169 if (what.size()!=10)
1170 throw runtime_error("Error parsing '"+fDatabase+"'.");
1171
1172 const string user = what[2];
1173 const string passwd = what[4];
1174 const string server = what[5];
1175 const string db = what[9];
1176 const int port = atoi(string(what[7]).c_str());
1177
1178 ostringstream out;
1179 out << "Connecting to '";
1180 if (!user.empty())
1181 out << user << "@";
1182 out << server;
1183 if (port)
1184 out << ":" << port;
1185 if (!db.empty())
1186 out << "/" << db;
1187
1188 T::Message(out);
1189
1190 mysqlpp::Connection conn(db.c_str(), server.c_str(), user.c_str(), passwd.c_str(), port);
1191 /* throws exceptions
1192 if (!conn.connected())
1193 {
1194 cout << "MySQL connection error: " << conn.error() << endl;
1195 throw;
1196 }*/
1197
1198 const mysqlpp::StoreQueryResult res =
1199 conn.query("SELECT fSourceName, fRightAscension, fDeclination FROM source").store();
1200 /* throws exceptions
1201 if (!res)
1202 {
1203 cout << "MySQL query failed: " << query.error() << endl;
1204 throw;
1205 }*/
1206
1207 fSources.clear();
1208 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
1209 {
1210 Source src;
1211 src.ra = (*v)[1];
1212 src.dec = (*v)[2];
1213 const string name = (*v)[0].c_str();
1214
1215 // FIXME: Check double names
1216 fSources[name] = src;
1217
1218 if (print)
1219 {
1220 ostringstream msg;
1221 msg << " " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
1222 T::Message(msg);
1223 }
1224 }
1225 }
1226
1227 int EvalOptions(Configuration &conf)
1228 {
1229 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1230
1231 const vector<string> &vec = conf.Vec<string>("source");
1232
1233 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
1234 {
1235 istringstream stream(*it);
1236
1237 string name;
1238 double ra=0;
1239 double dec=0;
1240
1241 int i=0;
1242
1243 string buffer;
1244 while (getline(stream, buffer, ','))
1245 {
1246 istringstream is(buffer);
1247
1248 switch (i++)
1249 {
1250 case 0: name = buffer; break;
1251 case 1: ra = ConnectionDrive::ReadAngle(is); break;
1252 case 2: dec = ConnectionDrive::ReadAngle(is); break;
1253 }
1254
1255 if (is.fail())
1256 break;
1257 }
1258
1259 if (i==3)
1260 {
1261 Source src;
1262 src.ra = ra;
1263 src.dec = dec;
1264
1265 // FIXME: Check double names
1266 fSources[name] = src;
1267 }
1268 }
1269
1270 if (conf.Has("source-database"))
1271 {
1272 fDatabase = conf.Get<string>("source-database");
1273 ReadDatabase();
1274 }
1275
1276 // The possibility to connect should be last, so that
1277 // everything else is already initialized.
1278 SetEndpoint(conf.Get<string>("addr"));
1279
1280 return -1;
1281 }
1282};
1283
1284// ------------------------------------------------------------------------
1285
1286#include "Main.h"
1287
1288
1289template<class T, class S, class R>
1290int RunShell(Configuration &conf)
1291{
1292 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1293}
1294
1295void SetupConfiguration(Configuration &conf)
1296{
1297 po::options_description control("Drive control options");
1298 control.add_options()
1299 ("no-dim,d", po_switch(), "Disable dim services")
1300 ("addr,a", var<string>("localhost:7404"), "Network address of Cosy")
1301 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1302 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
1303 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
1304 ;
1305
1306 conf.AddOptions(control);
1307}
1308
1309/*
1310 Extract usage clause(s) [if any] for SYNOPSIS.
1311 Translators: "Usage" and "or" here are patterns (regular expressions) which
1312 are used to match the usage synopsis in program output. An example from cp
1313 (GNU coreutils) which contains both strings:
1314 Usage: cp [OPTION]... [-T] SOURCE DEST
1315 or: cp [OPTION]... SOURCE... DIRECTORY
1316 or: cp [OPTION]... -t DIRECTORY SOURCE...
1317 */
1318void PrintUsage()
1319{
1320 cout <<
1321 "The drivectrl is an interface to cosy.\n"
1322 "\n"
1323 "The default is that the program is started without user intercation. "
1324 "All actions are supposed to arrive as DimCommands. Using the -c "
1325 "option, a local shell can be initialized. With h or help a short "
1326 "help message about the usuage can be brought to the screen.\n"
1327 "\n"
1328 "Usage: drivectrl [-c type] [OPTIONS]\n"
1329 " or: drivectrl [OPTIONS]\n";
1330 cout << endl;
1331}
1332
1333void PrintHelp()
1334{
1335 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
1336
1337 /* Additional help text which is printed after the configuration
1338 options goes here */
1339
1340 /*
1341 cout << "bla bla bla" << endl << endl;
1342 cout << endl;
1343 cout << "Environment:" << endl;
1344 cout << "environment" << endl;
1345 cout << endl;
1346 cout << "Examples:" << endl;
1347 cout << "test exam" << endl;
1348 cout << endl;
1349 cout << "Files:" << endl;
1350 cout << "files" << endl;
1351 cout << endl;
1352 */
1353}
1354
1355int main(int argc, const char* argv[])
1356{
1357 Configuration conf(argv[0]);
1358 conf.SetPrintUsage(PrintUsage);
1359 Main::SetupConfiguration(conf);
1360 SetupConfiguration(conf);
1361
1362 if (!conf.DoParse(argc, argv, PrintHelp))
1363 return -1;
1364
1365 //try
1366 {
1367 // No console access at all
1368 if (!conf.Has("console"))
1369 {
1370 if (conf.Get<bool>("no-dim"))
1371 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1372 else
1373 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1374 }
1375 // Cosole access w/ and w/o Dim
1376 if (conf.Get<bool>("no-dim"))
1377 {
1378 if (conf.Get<int>("console")==0)
1379 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1380 else
1381 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1382 }
1383 else
1384 {
1385 if (conf.Get<int>("console")==0)
1386 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1387 else
1388 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1389 }
1390 }
1391 /*catch (std::exception& e)
1392 {
1393 cerr << "Exception: " << e.what() << endl;
1394 return -1;
1395 }*/
1396
1397 return 0;
1398}
Note: See TracBrowser for help on using the repository browser.