source: branches/fscctrl_safety_limits/src/drivectrl.cc@ 18558

Last change on this file since 18558 was 18117, checked in by tbretz, 10 years ago
Even if the state was OnTrack it could happen that the fDevCount was reset because not the state but errornously a random double was compared. Removed an obsolete structure.
File size: 52.2 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 = 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 && fState!=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_DATA", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;S: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 "|name[string]:Name of star"),
754 fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", "")
755
756 {
757 }
758
759 void UpdateSource(const string &name="", bool tracking=false)
760 {
761 vector<char> dat(6*sizeof(double)+31, 0);
762 strncpy(dat.data()+6*sizeof(double), name.c_str(), 30);
763
764 fDimSource.setQuality(tracking);
765 fDimSource.Update(dat);
766 }
767
768 void UpdateSource(const array<double, 6> &arr, const string &name="")
769 {
770 vector<char> dat(6*sizeof(double)+31, 0);
771 memcpy(dat.data(), arr.data(), 6*sizeof(double));
772 strncpy(dat.data()+6*sizeof(double), name.c_str(), 30);
773
774 fDimSource.setQuality(1);
775 fDimSource.Update(dat);
776 }
777
778 // 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
779};
780
781// ------------------------------------------------------------------------
782
783template <class T, class S>
784class StateMachineDrive : public StateMachineAsio<T>
785{
786private:
787 S fDrive;
788
789 string fDatabase;
790
791 typedef map<string, Source> sources;
792 sources fSources;
793
794 string fLastCommand; // Last tracking (RADEC) command
795 int fAutoResume; // 0: disabled, 1: enables, 2: resuming
796 Time fAutoResumeTime;
797
798 // Status 0: Error
799 // Status 1: Unlocked
800 // Status 2: Locked
801 // Status 3: Stopping || Moving
802 // Status 4: Tracking
803
804 bool CheckEventSize(size_t has, const char *name, size_t size)
805 {
806 if (has==size)
807 return true;
808
809 ostringstream msg;
810 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
811 T::Fatal(msg);
812 return false;
813 }
814
815 enum Coordinates
816 {
817 kPoint,
818 kTrackSlow,
819 kTrackFast
820 };
821
822 string AngleToStr(double angle)
823 {
824 /* Handle sign */
825 const char sgn = angle<0?'-':'+';
826
827 /* Round interval and express in smallest units required */
828 double a = round(3600. * fabs(angle)); // deg to seconds
829
830 /* Separate into fields */
831 const double ad = trunc(a/3600.);
832 a -= ad * 3600.;
833 const double am = trunc(a/60.);
834 a -= am * 60.;
835 const double as = trunc(a);
836
837 /* Return results */
838 ostringstream str;
839 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
840 return str.str();
841 }
842
843 int SendCommand(const string &str)
844 {
845 // This happens if fLastCommand should be send,
846 // but the last command was not a tracking command
847 if (str.empty())
848 {
849 T::Info("Last command was not a tracking command. RESUME ignored.");
850 return T::GetCurrentState();
851 }
852
853 fLastCommand = str.compare(0, 6, "RADEC ")==0 ? str : "";
854
855 fDrive.PostMessage(str);
856 T::Message("Sending: "+str);
857
858 return T::GetCurrentState();
859 }
860
861 int TrackCelest(const string &cmd, const string &source)
862 {
863 SendCommand(cmd);
864
865 fDrive.UpdateSource(source, true);
866
867 return T::GetCurrentState();
868 }
869
870 int Park()
871 {
872 SendCommand("PREPS Park");
873 fDrive.UpdateSource("Park");
874
875 // FIXME: Go to locked state only when park position properly reached
876 return Drive::State::kLocked;
877 }
878
879 int SendStop()
880 {
881 SendCommand("STOP!");
882 fDrive.UpdateSource();
883
884 return T::GetCurrentState();
885 }
886
887 int Resume()
888 {
889 if (fLastCommand.empty())
890 {
891 T::Info("Last command was not a tracking command. RESUME ignored.");
892 return T::GetCurrentState();
893 }
894
895
896 T::Info("Resume: "+fLastCommand);
897 return SendCommand(fLastCommand);
898 }
899
900 int SendCoordinates(const EventImp &evt, const Coordinates type)
901 {
902 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
903 return T::kSM_FatalError;
904
905 const double *dat = evt.Ptr<double>();
906
907 string command;
908
909 switch (type)
910 {
911 case kPoint: command += "ZDAZ "; break;
912 case kTrackSlow: command += "RADEC "; break;
913 case kTrackFast: command += "GRB "; break;
914 }
915
916 if (type!=kPoint)
917 {
918 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
919 fDrive.UpdateSource(dim);
920 }
921 else
922 fDrive.UpdateSource("", false);
923
924
925 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
926 return SendCommand(command);
927 }
928
929 int StartWobble(const double &srcra, const double &srcdec,
930 const double &woboff, const double &wobang,
931 const string name="")
932 {
933 const double ra = srcra *M_PI/12;
934 const double dec = srcdec*M_PI/180;
935 const double off = woboff*M_PI/180;
936 const double dir = wobang*M_PI/180;
937
938 const double cosdir = cos(dir);
939 const double sindir = sin(dir);
940 const double cosoff = cos(off);
941 const double sinoff = sin(off);
942 const double cosdec = cos(dec);
943 const double sindec = sin(dec);
944
945 if (off==0)
946 {
947 const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
948 fDrive.UpdateSource(dim, name);
949
950 string command = "RADEC ";
951 command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
952 return SendCommand(command);
953 }
954
955 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
956 if (sintheta >= 1)
957 {
958 T::Error("cos(Zd) > 1");
959 return T::GetCurrentState();
960 }
961
962 const double costheta = sqrt(1 - sintheta*sintheta);
963
964 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
965 const double sindeltara = sindir*sinoff/costheta;
966
967 const double ndec = asin(sintheta)*180/M_PI;
968 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
969
970 const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
971 fDrive.UpdateSource(dim, name);
972
973 string command = "RADEC ";
974 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
975 return SendCommand(command);
976 }
977
978 int Wobble(const EventImp &evt)
979 {
980 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
981 return T::kSM_FatalError;
982
983 const double *dat = evt.Ptr<double>();
984
985 return StartWobble(dat[0], dat[1], dat[2], dat[3]);
986 }
987
988 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
989 {
990 if (find(ptr, last, '\0')==last)
991 {
992 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
993 throw uint32_t(T::kSM_FatalError);
994 }
995
996 const string name(ptr);
997
998 const sources::const_iterator it = fSources.find(name);
999 if (it==fSources.end())
1000 {
1001 T::Error("Source '"+name+"' not found in list.");
1002 throw uint32_t(T::GetCurrentState());
1003 }
1004
1005 return it;
1006 }
1007
1008 int TrackWobble(const EventImp &evt)
1009 {
1010 if (evt.GetSize()<=2)
1011 {
1012 ostringstream msg;
1013 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
1014 T::Fatal(msg);
1015 return T::kSM_FatalError;
1016 }
1017
1018 const uint16_t wobble = evt.GetUShort();
1019 if (wobble!=1 && wobble!=2)
1020 {
1021 ostringstream msg;
1022 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
1023 T::Error(msg);
1024 return T::GetCurrentState();
1025 }
1026
1027 const char *ptr = evt.Ptr<char>(2);
1028 const char *last = ptr+evt.GetSize()-2;
1029
1030 try
1031 {
1032 const sources::const_iterator it = GetSourceFromDB(ptr, last);
1033
1034 const string &name = it->first;
1035 const Source &src = it->second;
1036
1037 return StartWobble(src.ra, src.dec, src.offset, src.angle[wobble-1], name);
1038 }
1039 catch (const uint32_t &e)
1040 {
1041 return e;
1042 }
1043 }
1044
1045 int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0)
1046 {
1047 const char *last = ptr+size;
1048
1049 try
1050 {
1051 const sources::const_iterator it = GetSourceFromDB(ptr, last);
1052
1053 const string &name = it->first;
1054 const Source &src = it->second;
1055
1056 return StartWobble(src.ra, src.dec, offset, angle, name);
1057 }
1058 catch (const uint32_t &e)
1059 {
1060 return e;
1061 }
1062
1063 }
1064
1065 int Track(const EventImp &evt)
1066 {
1067 if (evt.GetSize()<=16)
1068 {
1069 ostringstream msg;
1070 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
1071 T::Fatal(msg);
1072 return T::kSM_FatalError;
1073 }
1074
1075 const double *dat = evt.Ptr<double>();
1076 const char *ptr = evt.Ptr<char>(16);
1077 const size_t size = evt.GetSize()-16;
1078
1079 return StartTrackWobble(ptr, size, dat[0], dat[1]);
1080 }
1081
1082 int TrackOn(const EventImp &evt)
1083 {
1084 if (evt.GetSize()==0)
1085 {
1086 ostringstream msg;
1087 msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 1.";
1088 T::Fatal(msg);
1089 return T::kSM_FatalError;
1090 }
1091
1092 return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
1093 }
1094
1095
1096 int TakeTPoint(const EventImp &evt)
1097 {
1098 if (evt.GetSize()<=4)
1099 {
1100 ostringstream msg;
1101 msg << "TakePoint - Received event has " << evt.GetSize() << " bytes, but expected at least 5.";
1102 T::Fatal(msg);
1103 return T::kSM_FatalError;
1104 }
1105
1106 const float mag = evt.Get<float>();
1107 const char *ptr = evt.Ptr<char>(4);
1108
1109 string src(ptr);
1110
1111 while (src.find_first_of(' ')!=string::npos)
1112 src.erase(src.find_first_of(' '), 1);
1113
1114 SendCommand("TPOIN "+src+" "+to_string(mag));;
1115
1116 return T::GetCurrentState();
1117 }
1118
1119 int SetLedBrightness(const EventImp &evt)
1120 {
1121 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
1122 return T::kSM_FatalError;
1123
1124 const uint32_t *led = evt.Ptr<uint32_t>();
1125
1126 return SendCommand("LEDS "+to_string(led[0])+" "+to_string(led[1]));
1127 }
1128
1129
1130 int SetVerbosity(const EventImp &evt)
1131 {
1132 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
1133 return T::kSM_FatalError;
1134
1135 fDrive.SetVerbose(evt.GetBool());
1136
1137 return T::GetCurrentState();
1138 }
1139
1140 int SetAutoResume(const EventImp &evt)
1141 {
1142 if (!CheckEventSize(evt.GetSize(), "SetAutoResume", 1))
1143 return T::kSM_FatalError;
1144
1145 fAutoResume = evt.GetBool();
1146
1147 return T::GetCurrentState();
1148 }
1149
1150 int Unlock()
1151 {
1152 if (fDrive.GetState()==StateMachineImp::kSM_Error)
1153 {
1154 T::Warn("Drive in error - maybe no connection to electronics... trying to send STOP.");
1155 SendStop();
1156 }
1157
1158 return Drive::State::kNotReady;
1159 }
1160
1161 int Print()
1162 {
1163 for (auto it=fSources.begin(); it!=fSources.end(); it++)
1164 {
1165 const string &name = it->first;
1166 const Source &src = it->second;
1167
1168 T::Out() << name << ",";
1169 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
1170 T::Out() << src.angle[0] << "," << src.angle[1] << endl;
1171 }
1172 return T::GetCurrentState();
1173 }
1174
1175 int ReloadSources()
1176 {
1177 try
1178 {
1179 ReadDatabase();
1180 }
1181 catch (const exception &e)
1182 {
1183 T::Error("Reading sources from databse failed: "+string(e.what()));
1184 }
1185 return T::GetCurrentState();
1186 }
1187
1188 int Disconnect()
1189 {
1190 // Close all connections
1191 fDrive.PostClose(false);
1192
1193 /*
1194 // Now wait until all connection have been closed and
1195 // all pending handlers have been processed
1196 poll();
1197 */
1198
1199 return T::GetCurrentState();
1200 }
1201
1202 int Reconnect(const EventImp &evt)
1203 {
1204 // Close all connections to supress the warning in SetEndpoint
1205 fDrive.PostClose(false);
1206
1207 // Now wait until all connection have been closed and
1208 // all pending handlers have been processed
1209 ba::io_service::poll();
1210
1211 if (evt.GetBool())
1212 fDrive.SetEndpoint(evt.GetString());
1213
1214 // Now we can reopen the connection
1215 fDrive.PostClose(true);
1216
1217 return T::GetCurrentState();
1218 }
1219
1220 Time fSunRise;
1221 /*
1222 int ShiftSunRise()
1223 {
1224 const Time sunrise = fSunRise;
1225
1226 fSunRise = Time().GetNextSunRise();
1227
1228 if (sunrise==fSunRise)
1229 return Drive::State::kLocked;
1230
1231 ostringstream msg;
1232 msg << "Next sun-rise will be at " << fSunRise;
1233 T::Info(msg);
1234
1235 return Drive::State::kLocked;
1236 }*/
1237
1238 int Execute()
1239 {
1240 /*
1241 if (T::GetCurrentState()==Drive::State::kLocked)
1242 return ShiftSunRise();
1243
1244 if (T::GetCurrentState()>Drive::State::kLocked)
1245 {
1246 if (Time()>fSunRise)
1247 return Park();
1248 }*/
1249
1250 const Time now;
1251
1252
1253 if (now>fSunRise)
1254 {
1255 if (T::GetCurrentState()>Drive::State::kLocked)
1256 return Park();
1257
1258 if (T::GetCurrentState()==Drive::State::kLocked)
1259 {
1260 fSunRise = now.GetNextSunRise();
1261
1262 ostringstream msg;
1263 msg << "Next sun-rise will be at " << fSunRise;
1264 T::Info(msg);
1265
1266 return Drive::State::kLocked;
1267 }
1268 }
1269
1270 if (T::GetCurrentState()==Drive::State::kLocked)
1271 return Drive::State::kLocked;
1272
1273 const int state = fDrive.GetState();
1274
1275 if (!fLastCommand.empty())
1276 {
1277 // If auto resume is enabled and the drive is in error,
1278 // resume tracking
1279 if (state==StateMachineImp::kSM_Error)
1280 {
1281 if (fAutoResume==1)
1282 {
1283 Resume();
1284 fAutoResume = 2;
1285 fAutoResumeTime = now;
1286 }
1287
1288 if (fAutoResume==2 && fAutoResumeTime+boost::posix_time::seconds(5)<now)
1289 {
1290 Resume();
1291 fAutoResume = 3;
1292 }
1293 }
1294 else
1295 {
1296 // If drive got out of the error state,
1297 // enable auto resume again
1298 if (fAutoResume>1)
1299 fAutoResume = 1;
1300 }
1301 }
1302
1303 return state;
1304 }
1305
1306
1307public:
1308 StateMachineDrive(ostream &out=cout) :
1309 StateMachineAsio<T>(out, "DRIVE_CONTROL"), fDrive(*this, *this),
1310 fAutoResume(false), fSunRise(Time().GetNextSunRise())
1311 {
1312 // State names
1313 T::AddStateName(State::kDisconnected, "Disconnected",
1314 "No connection to cosy");
1315
1316 T::AddStateName(State::kConnected, "Connected",
1317 "Cosy connected, drive stopped");
1318
1319 T::AddStateName(State::kNotReady, "NotReady",
1320 "Drive system not ready for movement");
1321
1322 T::AddStateName(State::kLocked, "Locked",
1323 "Drive system is locked (will not accept commands)");
1324
1325 T::AddStateName(State::kReady, "Ready",
1326 "Drive system ready for movement");
1327
1328 T::AddStateName(State::kArmed, "Armed",
1329 "Cosy armed, drive stopped");
1330
1331 T::AddStateName(State::kMoving, "Moving",
1332 "Telescope moving");
1333
1334 T::AddStateName(State::kTracking, "Tracking",
1335 "Telescope is in tracking mode");
1336
1337 T::AddStateName(State::kOnTrack, "OnTrack",
1338 "Telescope tracking stable");
1339
1340 // State::kIdle
1341 // State::kArmed
1342 // State::kMoving
1343 // State::kTracking
1344
1345 // Init
1346 // -----------
1347 // "ARM lock"
1348 // "STGMD off"
1349
1350 /*
1351 [ ] WAIT -> WM_WAIT
1352 [x] STOP! -> WM_STOP
1353 [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
1354 [x] GRB ra(+ d m s.f) dec(+ d m s.f)
1355 [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
1356 [ ] CELEST id offset angle
1357 [ ] MOON wobble offset
1358 [ ] PREPS string
1359 [ ] TPOIN star mag
1360 [ ] ARM lock/unlock
1361 [ ] STGMD on/off
1362 */
1363
1364 // Drive Commands
1365 T::AddEvent("MOVE_TO", "D:2", State::kArmed) // ->ZDAZ
1366 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
1367 ("Move the telescope to the given local coordinates"
1368 "|Zd[deg]:Zenith distance"
1369 "|Az[deg]:Azimuth");
1370
1371 T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB
1372 (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
1373 ("Move the telescope to the given sky coordinates and start tracking them"
1374 "|Ra[h]:Right ascension"
1375 "|Dec[deg]:Declination");
1376
1377 T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB
1378 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
1379 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
1380 "|Ra[h]:Right ascension"
1381 "|Dec[deg]:Declination"
1382 "|Offset[deg]:Wobble offset"
1383 "|Angle[deg]:Wobble angle");
1384
1385 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB
1386 (bind(&StateMachineDrive::Track, this, placeholders::_1))
1387 ("Move the telescope to the given wobble position around the given source and start tracking"
1388 "|Offset[deg]:Wobble offset"
1389 "|Angle[deg]:Wobble angle"
1390 "|Name[string]:Source name");
1391
1392 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB
1393 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
1394 ("Move the telescope to the given wobble position around the given source and start tracking"
1395 "|id:Wobble angle id (1 or 2)"
1396 "|Name[string]:Source name");
1397
1398 T::AddEvent("TRACK_ON", "C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB
1399 (bind(&StateMachineDrive::TrackOn, this, placeholders::_1))
1400 ("Move the telescope to the given position and start tracking"
1401 "|Name[string]:Source name");
1402
1403 T::AddEvent("RESUME", StateMachineImp::kSM_Error)
1404 (bind(&StateMachineDrive::Resume, this))
1405 ("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.");
1406
1407 T::AddEvent("MOON", State::kArmed, State::kTracking, State::kOnTrack)
1408 (bind(&StateMachineDrive::TrackCelest, this, "MOON 0 0", "Moon"))
1409 ("Start tracking the moon");
1410 T::AddEvent("VENUS", State::kArmed, State::kTracking, State::kOnTrack)
1411 (bind(&StateMachineDrive::TrackCelest, this, "CELEST 2 0 0", "Venus"))
1412 ("Start tracking Venus");
1413 T::AddEvent("MARS", State::kArmed, State::kTracking, State::kOnTrack)
1414 (bind(&StateMachineDrive::TrackCelest, this, "CELEST 4 0 0", "Mars"))
1415 ("Start tracking Mars");
1416 T::AddEvent("JUPITER", State::kArmed, State::kTracking, State::kOnTrack)
1417 (bind(&StateMachineDrive::TrackCelest, this, "CELEST 5 0 0", "Jupiter"))
1418 ("Start tracking Jupiter");
1419 T::AddEvent("SATURN", State::kArmed, State::kTracking, State::kOnTrack)
1420 (bind(&StateMachineDrive::TrackCelest, this, "CELEST 6 0 0", "Saturn"))
1421 ("Start tracking Saturn");
1422
1423 T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, State::kOnTrack, 0x100)
1424 (bind(&StateMachineDrive::Park, this))
1425 ("Park the telescope");
1426
1427 T::AddEvent("TAKE_TPOINT")
1428 (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0"))
1429 ("Take a TPoint");
1430
1431 T::AddEvent("TPOINT", "F:1;C")
1432 (bind(&StateMachineDrive::TakeTPoint, this, placeholders::_1))
1433 ("Take a TPoint (given values will be written to the TPoint files)"
1434 "|mag[float]:Magnitude of the star"
1435 "|name[string]:Name of the star");
1436
1437 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
1438 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
1439 ("Set the LED brightness of the top and bottom leds"
1440 "|top[au]:Allowed range 0-32767 for top LEDs"
1441 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
1442
1443 T::AddEvent("LEDS_OFF")
1444 (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0"))
1445 ("Switch off TPoint LEDs");
1446
1447 T::AddEvent("STOP")
1448 (bind(&StateMachineDrive::SendStop, this))
1449 ("Stop any kind of movement.");
1450
1451// T::AddEvent("ARM", State::kConnected)
1452// (bind(&StateMachineSendCommand, this, "ARM lock"))
1453// ("");
1454
1455 T::AddEvent("UNLOCK", Drive::State::kLocked)
1456 (bind(&StateMachineDrive::Unlock, this))
1457 ("Unlock locked state.");
1458
1459 T::AddEvent("SET_AUTORESUME", "B:1")
1460 (bind(&StateMachineDrive::SetAutoResume, this, placeholders::_1))
1461 ("Enable/disable auto resume"
1462 "|resume[bool]:if enabled, drive is tracking and goes to error state, the last tracking command is repeated automatically.");
1463
1464 // Verbosity commands
1465 T::AddEvent("SET_VERBOSE", "B:1")
1466 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
1467 ("Set verbosity state"
1468 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
1469
1470 // Conenction commands
1471 T::AddEvent("DISCONNECT", State::kConnected, State::kArmed)
1472 (bind(&StateMachineDrive::Disconnect, this))
1473 ("disconnect from ethernet");
1474
1475 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected, State::kArmed)
1476 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
1477 ("(Re)connect Ethernet connection to cosy, a new address can be given"
1478 "|[host][string]:new ethernet address in the form <host:port>");
1479
1480
1481 T::AddEvent("PRINT")
1482 (bind(&StateMachineDrive::Print, this))
1483 ("Print source list.");
1484
1485 T::AddEvent("RELOAD_SOURCES")
1486 (bind(&StateMachineDrive::ReloadSources, this))
1487 ("Reload sources from database after database has changed..");
1488
1489 fDrive.StartConnect();
1490 }
1491
1492 void SetEndpoint(const string &url)
1493 {
1494 fDrive.SetEndpoint(url);
1495 }
1496
1497 bool AddSource(const string &name, const Source &src)
1498 {
1499 const auto it = fSources.find(name);
1500 if (it!=fSources.end())
1501 T::Warn("Source '"+name+"' already in list... overwriting.");
1502
1503 fSources[name] = src;
1504 return it==fSources.end();
1505 }
1506
1507 void ReadDatabase(bool print=true)
1508 {
1509#ifdef HAVE_SQL
1510 Database db(fDatabase);
1511
1512 T::Message("Connected to '"+db.uri()+"'");
1513
1514 const mysqlpp::StoreQueryResult res =
1515 db.query("SELECT fSourceName, fRightAscension, fDeclination, fWobbleOffset, fWobbleAngle0, fWobbleAngle1 FROM Source").store();
1516
1517 fSources.clear();
1518 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
1519 {
1520 const string name = (*v)[0].c_str();
1521
1522 Source src;
1523 src.ra = (*v)[1];
1524 src.dec = (*v)[2];
1525 src.offset = (*v)[3];
1526 src.angle[0] = (*v)[4];
1527 src.angle[1] = (*v)[5];
1528 AddSource(name, src);
1529
1530 if (!print)
1531 continue;
1532
1533 ostringstream msg;
1534 msg << " " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
1535 msg << " Wobble=[" << src.offset << "," << src.angle[0] << "," << src.angle[1] << "]";
1536 T::Message(msg);
1537 }
1538#else
1539 T::Warn("MySQL support not compiled into the program.");
1540#endif
1541 }
1542
1543 int EvalOptions(Configuration &conf)
1544 {
1545 if (!fSunRise)
1546 return 1;
1547
1548 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
1549
1550 const vector<string> &vec = conf.Vec<string>("source");
1551
1552 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
1553 {
1554 istringstream stream(*it);
1555
1556 string name;
1557
1558 int i=0;
1559
1560 Source src;
1561
1562 string buffer;
1563 while (getline(stream, buffer, ','))
1564 {
1565 istringstream is(buffer);
1566
1567 switch (i++)
1568 {
1569 case 0: name = buffer; break;
1570 case 1: src.ra = ConnectionDrive::ReadAngle(is); break;
1571 case 2: src.dec = ConnectionDrive::ReadAngle(is); break;
1572 case 3: is >> src.offset; break;
1573 case 4: is >> src.angle[0]; break;
1574 case 5: is >> src.angle[1]; break;
1575 }
1576
1577 if (is.fail())
1578 break;
1579 }
1580
1581 if (i==3 || i==6)
1582 {
1583 AddSource(name, src);
1584 continue;
1585 }
1586
1587 T::Warn("Resource 'source' not correctly formatted: '"+*it+"'");
1588 }
1589
1590 fDrive.SetDeviationCondition(conf.Get<uint16_t>("deviation-limit"),
1591 conf.Get<uint16_t>("deviation-count"),
1592 conf.Get<uint16_t>("deviation-max"));
1593
1594 fAutoResume = conf.Get<bool>("auto-resume");
1595
1596 if (conf.Has("source-database"))
1597 {
1598 fDatabase = conf.Get<string>("source-database");
1599 ReadDatabase();
1600 }
1601
1602 if (fSunRise.IsValid())
1603 {
1604 ostringstream msg;
1605 msg << "Next sun-rise will be at " << fSunRise;
1606 T::Message(msg);
1607 }
1608
1609 // The possibility to connect should be last, so that
1610 // everything else is already initialized.
1611 SetEndpoint(conf.Get<string>("addr"));
1612
1613 return -1;
1614 }
1615};
1616
1617// ------------------------------------------------------------------------
1618
1619#include "Main.h"
1620
1621
1622template<class T, class S, class R>
1623int RunShell(Configuration &conf)
1624{
1625 return Main::execute<T, StateMachineDrive<S, R>>(conf);
1626}
1627
1628void SetupConfiguration(Configuration &conf)
1629{
1630 const string def = "localhost:7404";
1631
1632 po::options_description control("Drive control options");
1633 control.add_options()
1634 ("no-dim,d", po_switch(), "Disable dim services")
1635 ("addr,a", var<string>(def), "Network address of cosy")
1636 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
1637 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
1638 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
1639 ("deviation-limit", var<uint16_t>(90), "Deviation limit in arcsec to get 'OnTrack'")
1640 ("deviation-count", var<uint16_t>(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
1641 ("deviation-max", var<uint16_t>(180), "Maximum deviation in arcsec allowed to keep status 'OnTrack'")
1642 ("auto-resume", po_bool(false), "Enable auto result during tracking if connection is lost")
1643 ;
1644
1645 conf.AddOptions(control);
1646}
1647
1648/*
1649 Extract usage clause(s) [if any] for SYNOPSIS.
1650 Translators: "Usage" and "or" here are patterns (regular expressions) which
1651 are used to match the usage synopsis in program output. An example from cp
1652 (GNU coreutils) which contains both strings:
1653 Usage: cp [OPTION]... [-T] SOURCE DEST
1654 or: cp [OPTION]... SOURCE... DIRECTORY
1655 or: cp [OPTION]... -t DIRECTORY SOURCE...
1656 */
1657void PrintUsage()
1658{
1659 cout <<
1660 "The drivectrl is an interface to cosy.\n"
1661 "\n"
1662 "The default is that the program is started without user intercation. "
1663 "All actions are supposed to arrive as DimCommands. Using the -c "
1664 "option, a local shell can be initialized. With h or help a short "
1665 "help message about the usuage can be brought to the screen.\n"
1666 "\n"
1667 "Usage: drivectrl [-c type] [OPTIONS]\n"
1668 " or: drivectrl [OPTIONS]\n";
1669 cout << endl;
1670}
1671
1672void PrintHelp()
1673{
1674 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
1675
1676 /* Additional help text which is printed after the configuration
1677 options goes here */
1678
1679 /*
1680 cout << "bla bla bla" << endl << endl;
1681 cout << endl;
1682 cout << "Environment:" << endl;
1683 cout << "environment" << endl;
1684 cout << endl;
1685 cout << "Examples:" << endl;
1686 cout << "test exam" << endl;
1687 cout << endl;
1688 cout << "Files:" << endl;
1689 cout << "files" << endl;
1690 cout << endl;
1691 */
1692}
1693
1694int main(int argc, const char* argv[])
1695{
1696 Configuration conf(argv[0]);
1697 conf.SetPrintUsage(PrintUsage);
1698 Main::SetupConfiguration(conf);
1699 SetupConfiguration(conf);
1700
1701 if (!conf.DoParse(argc, argv, PrintHelp))
1702 return 127;
1703
1704 //try
1705 {
1706 // No console access at all
1707 if (!conf.Has("console"))
1708 {
1709 if (conf.Get<bool>("no-dim"))
1710 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
1711 else
1712 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
1713 }
1714 // Cosole access w/ and w/o Dim
1715 if (conf.Get<bool>("no-dim"))
1716 {
1717 if (conf.Get<int>("console")==0)
1718 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
1719 else
1720 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
1721 }
1722 else
1723 {
1724 if (conf.Get<int>("console")==0)
1725 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
1726 else
1727 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
1728 }
1729 }
1730 /*catch (std::exception& e)
1731 {
1732 cerr << "Exception: " << e.what() << endl;
1733 return -1;
1734 }*/
1735
1736 return 0;
1737}
Note: See TracBrowser for help on using the repository browser.