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

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