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

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