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

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