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

Last change on this file since 14498 was 14496, checked in by tbretz, 12 years ago
Initialize fAutoResume.
File size: 44.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#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 = StateMachineImp::kSM_Error - Drive::State::kNotReady;
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 string fLastCommand; // Last tracking (RADEC) command
676 int fAutoResume; // 0: disabled, 1: enables, 2: resuming
677
678 // Status 0: Error
679 // Status 1: Unlocked
680 // Status 2: Locked
681 // Status 3: Stopping || Moving
682 // Status 4: Tracking
683
684 bool CheckEventSize(size_t has, const char *name, size_t size)
685 {
686 if (has==size)
687 return true;
688
689 ostringstream msg;
690 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
691 T::Fatal(msg);
692 return false;
693 }
694
695 enum Coordinates
696 {
697 kPoint,
698 kTrackSlow,
699 kTrackFast
700 };
701
702 string AngleToStr(double angle)
703 {
704 /* Handle sign */
705 const char sgn = angle<0?'-':'+';
706
707 /* Round interval and express in smallest units required */
708 double a = round(3600. * fabs(angle)); // deg to seconds
709
710 /* Separate into fields */
711 const double ad = trunc(a/3600.);
712 a -= ad * 3600.;
713 const double am = trunc(a/60.);
714 a -= am * 60.;
715 const double as = trunc(a);
716
717 /* Return results */
718 ostringstream str;
719 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
720 return str.str();
721 }
722
723 int SendCommand(const string &str, bool upd=true)
724 {
725 // This happens if fLastCommand should be send,
726 // but the last command was not a tracking command
727 if (str.empty())
728 {
729 T::Info("Last command was not a tracking command. RESUME ignored.");
730 return T::GetCurrentState();
731 }
732
733 fLastCommand = str.compare(0, 6, "RADEC ")==0 ? str : "";
734
735 fDrive.PostMessage(str);
736 T::Message("Sending: "+str);
737
738 if (upd)
739 fDrive.UpdateSource();
740
741 return T::GetCurrentState();
742 }
743
744 int Resume()
745 {
746 if (fLastCommand.empty())
747 {
748 T::Info("Last command was not a tracking command. RESUME ignored.");
749 return T::GetCurrentState();
750 }
751
752 return SendCommand(fLastCommand, false);
753 }
754
755 int SendCoordinates(const EventImp &evt, const Coordinates type)
756 {
757 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
758 return T::kSM_FatalError;
759
760 const double *dat = evt.Ptr<double>();
761
762 string command;
763
764 switch (type)
765 {
766 case kPoint: command += "ZDAZ "; break;
767 case kTrackSlow: command += "RADEC "; break;
768 case kTrackFast: command += "GRB "; break;
769 }
770
771 if (type!=kPoint)
772 {
773 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
774 fDrive.UpdateSource(dim);
775 }
776
777 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
778 return SendCommand(command, type==kPoint);
779 }
780
781 int StartWobble(const double &srcra, const double &srcdec,
782 const double &woboff, const double &wobang,
783 const string name="")
784 {
785 const double ra = srcra *M_PI/12;
786 const double dec = srcdec*M_PI/180;
787 const double off = woboff*M_PI/180;
788 const double dir = wobang*M_PI/180;
789
790 const double cosdir = cos(dir);
791 const double sindir = sin(dir);
792 const double cosoff = cos(off);
793 const double sinoff = sin(off);
794 const double cosdec = cos(dec);
795 const double sindec = sin(dec);
796
797 if (off==0)
798 {
799 const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
800 fDrive.UpdateSource(dim, name);
801
802 string command = "RADEC ";
803 command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
804 return SendCommand(command, false);
805 }
806
807 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
808 if (sintheta >= 1)
809 {
810 T::Error("cos(Zd) > 1");
811 return T::GetCurrentState();
812 }
813
814 const double costheta = sqrt(1 - sintheta*sintheta);
815
816 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
817 const double sindeltara = sindir*sinoff/costheta;
818
819 const double ndec = asin(sintheta)*180/M_PI;
820 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
821
822 const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
823 fDrive.UpdateSource(dim, name);
824
825 string command = "RADEC ";
826 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
827 return SendCommand(command, false);
828 }
829
830 int Wobble(const EventImp &evt)
831 {
832 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
833 return T::kSM_FatalError;
834
835 const double *dat = evt.Ptr<double>();
836
837 return StartWobble(dat[0], dat[1], dat[2], dat[3]);
838 }
839
840 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
841 {
842 if (find(ptr, last, '\0')==last)
843 {
844 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
845 throw uint32_t(T::kSM_FatalError);
846 }
847
848 const string name(Tools::TrimQuotes(ptr));
849
850 const sources::const_iterator it = fSources.find(name);
851 if (it==fSources.end())
852 {
853 T::Error("Source '"+name+"' not found in list.");
854 throw uint32_t(T::GetCurrentState());
855 }
856
857 return it;
858 }
859
860 int TrackWobble(const EventImp &evt)
861 {
862 if (evt.GetSize()<=2)
863 {
864 ostringstream msg;
865 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
866 T::Fatal(msg);
867 return T::kSM_FatalError;
868 }
869
870 const uint16_t wobble = evt.GetUShort();
871 if (wobble!=1 && wobble!=2)
872 {
873 ostringstream msg;
874 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
875 T::Error(msg);
876 return T::GetCurrentState();
877 }
878
879 const char *ptr = evt.Ptr<char>(2);
880 const char *last = ptr+evt.GetSize()-2;
881
882 try
883 {
884 const sources::const_iterator it = GetSourceFromDB(ptr, last);
885
886 const string &name = it->first;
887 const Source &src = it->second;
888
889 return StartWobble(src.ra, src.dec, src.offset, src.angle[wobble-1], name);
890 }
891 catch (const uint32_t &e)
892 {
893 return e;
894 }
895 }
896
897 int Track(const EventImp &evt)
898 {
899 if (evt.GetSize()<=16)
900 {
901 ostringstream msg;
902 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
903 T::Fatal(msg);
904 return T::kSM_FatalError;
905 }
906
907 const double *dat = evt.Ptr<double>();
908 const char *ptr = evt.Ptr<char>(16);
909 const char *last = ptr+evt.GetSize()-16;
910
911 try
912 {
913 const sources::const_iterator it = GetSourceFromDB(ptr, last);
914
915 const string &name = it->first;
916 const Source &src = it->second;
917
918 return StartWobble(src.ra, src.dec, dat[0], dat[1], name);
919 }
920 catch (const uint32_t &e)
921 {
922 return e;
923 }
924 }
925
926 int SetLedBrightness(const EventImp &evt)
927 {
928 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
929 return T::kSM_FatalError;
930
931 const uint32_t *led = evt.Ptr<uint32_t>();
932
933 ostringstream cmd;
934 cmd << "LEDS " << led[0] << " " << led[1];
935 return SendCommand(cmd.str(), false);
936 }
937
938
939 int SetVerbosity(const EventImp &evt)
940 {
941 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
942 return T::kSM_FatalError;
943
944 fDrive.SetVerbose(evt.GetBool());
945
946 return T::GetCurrentState();
947 }
948
949 int SetAutoResume(const EventImp &evt)
950 {
951 if (!CheckEventSize(evt.GetSize(), "SetAutoResume", 1))
952 return T::kSM_FatalError;
953
954 fAutoResume = evt.GetBool();
955
956 return T::GetCurrentState();
957 }
958
959 int Unlock()
960 {
961 return Drive::State::kNotReady;
962 }
963
964 int Print()
965 {
966 for (auto it=fSources.begin(); it!=fSources.end(); it++)
967 {
968 const string &name = it->first;
969 const Source &src = it->second;
970
971 T::Out() << name << ",";
972 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
973 T::Out() << src.angle[0] << "," << src.angle[1] << endl;
974 }
975 return T::GetCurrentState();
976 }
977
978 int ReloadSources()
979 {
980 try
981 {
982 ReadDatabase();
983 }
984 catch (const exception &e)
985 {
986 T::Error("Reading sources from databse failed: "+string(e.what()));
987 }
988 return T::GetCurrentState();
989 }
990
991 int Disconnect()
992 {
993 // Close all connections
994 fDrive.PostClose(false);
995
996 /*
997 // Now wait until all connection have been closed and
998 // all pending handlers have been processed
999 poll();
1000 */
1001
1002 return T::GetCurrentState();
1003 }
1004
1005 int Reconnect(const EventImp &evt)
1006 {
1007 // Close all connections to supress the warning in SetEndpoint
1008 fDrive.PostClose(false);
1009
1010 // Now wait until all connection have been closed and
1011 // all pending handlers have been processed
1012 poll();
1013
1014 if (evt.GetBool())
1015 fDrive.SetEndpoint(evt.GetString());
1016
1017 // Now we can reopen the connection
1018 fDrive.PostClose(true);
1019
1020 return T::GetCurrentState();
1021 }
1022
1023 Time fSunRise;
1024
1025 Time GetSunRise(const Time &time=Time())
1026 {
1027#ifdef HAVE_LIBNOVA
1028 const double lon = -(17.+53./60+26.525/3600);
1029 const double lat = 28.+45./60+42.462/3600;
1030
1031 ln_lnlat_posn observer;
1032 observer.lng = lon;
1033 observer.lat = lat;
1034
1035 // This caluclates the sun-rise of the next day after 12:00 noon
1036 ln_rst_time sun_day;
1037 if (ln_get_solar_rst(time.JD(), &observer, &sun_day)==1)
1038 {
1039 T::Fatal("GetSunRise reported the sun to be circumpolar!");
1040 return Time(Time::none);
1041 }
1042
1043 if (Time(sun_day.rise)>=time)
1044 return Time(sun_day.rise);
1045
1046 if (ln_get_solar_rst(time.JD()+0.5, &observer, &sun_day)==1)
1047 {
1048 T::Fatal("GetSunRise reported the sun to be circumpolar!");
1049 return Time(Time::none);
1050 }
1051
1052 return Time(sun_day.rise);
1053#else
1054 return Time(boost::date_time::pos_infin);
1055#endif
1056 }
1057
1058 int Execute()
1059 {
1060 // Dispatch (execute) at most one handler from the queue. In contrary
1061 // to run_one(), it doesn't wait until a handler is available
1062 // which can be dispatched, so poll_one() might return with 0
1063 // handlers dispatched. The handlers are always dispatched/executed
1064 // synchronously, i.e. within the call to poll_one()
1065 poll_one();
1066
1067 if (T::GetCurrentState()==Drive::State::kLocked)
1068 return Drive::State::kLocked;
1069
1070
1071 if (T::GetCurrentState()>Drive::State::kLocked)
1072 {
1073 Time now;
1074 if (now>fSunRise)
1075 {
1076 SendCommand("PREPS Park", false);
1077
1078 fSunRise = GetSunRise(now);
1079 if (!fSunRise)
1080 return T::kSM_FatalError;
1081
1082 ostringstream msg;
1083 msg << "Next sun-rise will be at " << fSunRise;
1084 T::Info(msg);
1085
1086 return Drive::State::kLocked;
1087 }
1088 }
1089
1090 const int state = fDrive.GetState();
1091
1092 if (!fLastCommand.empty())
1093 {
1094 // If auto resume is enabled and the drive is in error,
1095 // resume tracking
1096 if (fAutoResume==1 && state==StateMachineImp::kSM_Error)
1097 {
1098 Resume();
1099 fAutoResume = 2;
1100 }
1101
1102 // If drive got out of the error state,
1103 // enable auto resume again
1104 if (fAutoResume==2 && state!=StateMachineImp::kSM_Error)
1105 fAutoResume = 1;
1106 }
1107
1108 return state;
1109 }
1110
1111
1112public:
1113 StateMachineDrive(ostream &out=cout) :
1114 T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
1115 fDrive(*this, *this), fAutoResume(false), fSunRise(GetSunRise())
1116 {
1117 // State names
1118 T::AddStateName(State::kDisconnected, "Disconnected",
1119 "No connection to cosy");
1120
1121 T::AddStateName(State::kConnected, "Connected",
1122 "Cosy connected, drive stopped");
1123
1124 T::AddStateName(State::kNotReady, "NotReady",
1125 "Drive system not ready for movement");
1126
1127 T::AddStateName(State::kLocked, "Locked",
1128 "Drive system is locked (will not accept commands)");
1129
1130 T::AddStateName(State::kReady, "Ready",
1131 "Drive system ready for movement");
1132
1133 T::AddStateName(State::kArmed, "Armed",
1134 "Cosy armed, drive stopped");
1135
1136 T::AddStateName(State::kMoving, "Moving",
1137 "Telescope moving");
1138
1139 T::AddStateName(State::kTracking, "Tracking",
1140 "Telescope tracking");
1141
1142 // State::kIdle
1143 // State::kArmed
1144 // State::kMoving
1145 // State::kTracking
1146
1147 // Init
1148 // -----------
1149 // "ARM lock"
1150 // "STGMD off"
1151
1152 /*
1153 [ ] WAIT -> WM_WAIT
1154 [x] STOP! -> WM_STOP
1155 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
1156 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
1157 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
1158 [ ] CELEST id offset angle
1159 [ ] MOON wobble offset
1160 [ ] PREPS string
1161 [ ] TPOIN star mag
1162 [ ] ARM lock/unlock
1163 [ ] STGMD on/off
1164 */
1165
1166 // Drive Commands
1167 T::AddEvent("MOVE_TO", "D:2", State::kArmed) // ->ZDAZ
1168 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
1169 ("Move the telescope to the given local coordinates"
1170 "|Zd[deg]:Zenith distance"
1171 "|Az[deg]:Azimuth");
1172
1173 T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking) // ->RADEC/GRB
1174 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
1175 ("Move the telescope to the given sky coordinates and start tracking them"
1176 "|Ra[h]:Right ascension"
1177 "|Dec[deg]:Declination");
1178
1179 T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking) // ->RADEC/GRB
1180 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
1181 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
1182 "|Ra[h]:Right ascension"
1183 "|Dec[deg]:Declination"
1184 "|Offset[deg]:Wobble offset"
1185 "|Angle[deg]:Wobble angle");
1186
1187 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking) // ->RADEC/GRB
1188 (bind(&StateMachineDrive::Track, this, placeholders::_1))
1189 ("Move the telescope to the given wobble position around the given source and start tracking"
1190 "|Offset[deg]:Wobble offset"
1191 "|Angle[deg]:Wobble angle"
1192 "|Name[string]:Source name");
1193
1194 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking) // ->RADEC/GRB
1195 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
1196 ("Move the telescope to the given wobble position around the given source and start tracking"
1197 "|id:Wobble angle id (1 or 2)"
1198 "|Name[string]:Source name");
1199
1200 T::AddEvent("RESUME", StateMachineImp::kSM_Error)
1201 (bind(&StateMachineDrive::Resume, this))
1202 ("If drive is in Error state, this can b used to resume the last tracking command, if the last command sent to cosy was a tracking command.");
1203
1204 T::AddEvent("MOON", State::kArmed, State::kTracking)
1205 (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
1206 ("Start tracking the moon");
1207 T::AddEvent("VENUS", State::kArmed, State::kTracking)
1208 (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
1209 ("Start tracking Venus");
1210 T::AddEvent("MARS", State::kArmed, State::kTracking)
1211 (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
1212 ("Start tracking Mars");
1213 T::AddEvent("JUPITER", State::kArmed, State::kTracking)
1214 (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
1215 ("Start tracking Jupiter");
1216 T::AddEvent("SATURN", State::kArmed, State::kTracking)
1217 (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
1218 ("Start tracking Saturn");
1219
1220 T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, 0x100)
1221 (bind(&StateMachineDrive::SendCommand, this, "PREPS Park", false))
1222 ("Park the telescope");
1223
1224 T::AddEvent("TAKE_TPOINT")
1225 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
1226 ("Take a TPoint");
1227
1228 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1229 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1230 ("Set the LED brightness of the top and bottom leds"
1231 "|top[au]:Allowed range 0-32767 for top LEDs"
1232 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1233
1234 T::AddEvent("LEDS_OFF")
1235 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
1236 ("Switch off TPoint LEDs");
1237
1238 T::AddEvent("STOP")
1239 (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
1240 ("Stop any kind of movement.");
1241
1242// T::AddEvent("ARM", State::kConnected)
1243// (bind(&StateMachineSendCommand, this, "ARM lock"))
1244// ("");
1245
1246 T::AddEvent("UNLOCK", Drive::State::kLocked)
1247 (bind(&StateMachineDrive::Unlock, this))
1248 ("Stop any kind of movement.");
1249
1250 T::AddEvent("SET_AUTORESUME", "B")
1251 (bind(&StateMachineDrive::SetAutoResume, this, placeholders::_1))
1252 ("Enable/disable auto resume"
1253 "|resume[bool]:if enabled, drive is tracking and goes to error state, the last tracking command is repeated automatically.");
1254
1255 // Verbosity commands
1256 T::AddEvent("SET_VERBOSE", "B")
1257 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1258 ("Set verbosity state"
1259 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1260
1261 // Conenction commands
1262 T::AddEvent("DISCONNECT", State::kConnected, State::kArmed)
1263 (bind(&StateMachineDrive::Disconnect, this))
1264 ("disconnect from ethernet");
1265
1266 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected, State::kArmed)
1267 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1268 ("(Re)connect ethernet connection to FTM, a new address can be given"
1269 "|[host][string]:new ethernet address in the form <host:port>");
1270
1271
1272 T::AddEvent("PRINT")
1273 (bind(&StateMachineDrive::Print, this))
1274 ("Print source list.");
1275
1276 T::AddEvent("RELOAD_SOURCES")
1277 (bind(&StateMachineDrive::ReloadSources, this))
1278 ("Reload sources from database.");
1279
1280 fDrive.StartConnect();
1281 }
1282
1283 void SetEndpoint(const string &url)
1284 {
1285 fDrive.SetEndpoint(url);
1286 }
1287
1288 void ReadDatabase(bool print=true)
1289 {
1290#ifdef HAVE_SQL
1291 Database db(fDatabase);
1292
1293 T::Message("Connected to '"+db.uri()+"'");
1294
1295 const mysqlpp::StoreQueryResult res =
1296 db.query("SELECT fSourceName, fRightAscension, fDeclination FROM source").store();
1297
1298 fSources.clear();
1299 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
1300 {
1301 Source src;
1302 src.ra = (*v)[1];
1303 src.dec = (*v)[2];
1304 const string name = (*v)[0].c_str();
1305
1306 // FIXME: Check double names
1307 fSources[name] = src;
1308
1309 if (print)
1310 {
1311 ostringstream msg;
1312 msg << " " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
1313 T::Message(msg);
1314 }
1315 }
1316#else
1317 T::Warn("MySQL support not compiled into the program.");
1318#endif
1319 }
1320
1321 int EvalOptions(Configuration &conf)
1322 {
1323 if (!fSunRise)
1324 return 1;
1325
1326 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1327
1328 const vector<string> &vec = conf.Vec<string>("source");
1329
1330 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
1331 {
1332 istringstream stream(*it);
1333
1334 string name;
1335 double ra=0;
1336 double dec=0;
1337
1338 int i=0;
1339
1340 string buffer;
1341 while (getline(stream, buffer, ','))
1342 {
1343 istringstream is(buffer);
1344
1345 switch (i++)
1346 {
1347 case 0: name = buffer; break;
1348 case 1: ra = ConnectionDrive::ReadAngle(is); break;
1349 case 2: dec = ConnectionDrive::ReadAngle(is); break;
1350 }
1351
1352 if (is.fail())
1353 break;
1354 }
1355
1356 if (i==3)
1357 {
1358 Source src;
1359 src.ra = ra;
1360 src.dec = dec;
1361
1362 // FIXME: Check double names
1363 fSources[name] = src;
1364 }
1365 }
1366
1367 fAutoResume = conf.Get<bool>("auto-resume");
1368
1369 if (conf.Has("source-database"))
1370 {
1371 fDatabase = conf.Get<string>("source-database");
1372 ReadDatabase();
1373 }
1374
1375 if (fSunRise.IsValid())
1376 {
1377 ostringstream msg;
1378 msg << "Next sun-rise will be at " << fSunRise;
1379 T::Message(msg);
1380 }
1381
1382 // The possibility to connect should be last, so that
1383 // everything else is already initialized.
1384 SetEndpoint(conf.Get<string>("addr"));
1385
1386 return -1;
1387 }
1388};
1389
1390// ------------------------------------------------------------------------
1391
1392#include "Main.h"
1393
1394
1395template<class T, class S, class R>
1396int RunShell(Configuration &conf)
1397{
1398 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1399}
1400
1401void SetupConfiguration(Configuration &conf)
1402{
1403 po::options_description control("Drive control options");
1404 control.add_options()
1405 ("no-dim,d", po_switch(), "Disable dim services")
1406 ("addr,a", var<string>("localhost:7404"), "Network address of Cosy")
1407 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1408 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
1409 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
1410 ("auto-resume", po_bool(false), "Enable auto result during tracking")
1411 ;
1412
1413 conf.AddOptions(control);
1414}
1415
1416/*
1417 Extract usage clause(s) [if any] for SYNOPSIS.
1418 Translators: "Usage" and "or" here are patterns (regular expressions) which
1419 are used to match the usage synopsis in program output. An example from cp
1420 (GNU coreutils) which contains both strings:
1421 Usage: cp [OPTION]... [-T] SOURCE DEST
1422 or: cp [OPTION]... SOURCE... DIRECTORY
1423 or: cp [OPTION]... -t DIRECTORY SOURCE...
1424 */
1425void PrintUsage()
1426{
1427 cout <<
1428 "The drivectrl is an interface to cosy.\n"
1429 "\n"
1430 "The default is that the program is started without user intercation. "
1431 "All actions are supposed to arrive as DimCommands. Using the -c "
1432 "option, a local shell can be initialized. With h or help a short "
1433 "help message about the usuage can be brought to the screen.\n"
1434 "\n"
1435 "Usage: drivectrl [-c type] [OPTIONS]\n"
1436 " or: drivectrl [OPTIONS]\n";
1437 cout << endl;
1438}
1439
1440void PrintHelp()
1441{
1442 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
1443
1444 /* Additional help text which is printed after the configuration
1445 options goes here */
1446
1447 /*
1448 cout << "bla bla bla" << endl << endl;
1449 cout << endl;
1450 cout << "Environment:" << endl;
1451 cout << "environment" << endl;
1452 cout << endl;
1453 cout << "Examples:" << endl;
1454 cout << "test exam" << endl;
1455 cout << endl;
1456 cout << "Files:" << endl;
1457 cout << "files" << endl;
1458 cout << endl;
1459 */
1460}
1461
1462int main(int argc, const char* argv[])
1463{
1464 Configuration conf(argv[0]);
1465 conf.SetPrintUsage(PrintUsage);
1466 Main::SetupConfiguration(conf);
1467 SetupConfiguration(conf);
1468
1469 if (!conf.DoParse(argc, argv, PrintHelp))
1470 return 127;
1471
1472 //try
1473 {
1474 // No console access at all
1475 if (!conf.Has("console"))
1476 {
1477 if (conf.Get<bool>("no-dim"))
1478 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1479 else
1480 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1481 }
1482 // Cosole access w/ and w/o Dim
1483 if (conf.Get<bool>("no-dim"))
1484 {
1485 if (conf.Get<int>("console")==0)
1486 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1487 else
1488 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1489 }
1490 else
1491 {
1492 if (conf.Get<int>("console")==0)
1493 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1494 else
1495 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1496 }
1497 }
1498 /*catch (std::exception& e)
1499 {
1500 cerr << "Exception: " << e.what() << endl;
1501 return -1;
1502 }*/
1503
1504 return 0;
1505}
Note: See TracBrowser for help on using the repository browser.