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

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