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

Last change on this file since 17959 was 17417, checked in by tbretz, 11 years ago
Fixed a bug when calculating the azimuth from the tpoint data; fixed the format string for the TPOINT service; renamed the TPOINT service because a command with the same name already exists; fixed a problem with the padding of the 'C' formated data; replaces 'B' as format by the more accurate 'B:1'
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 = 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_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
783struct Source
784{
785 Source() : ra(0), dec(0), offset(0)
786 {
787 angle[0] = -90;
788 angle[1] = 90;
789 }
790
791 double ra;
792 double dec;
793 double offset;
794 array<double, 2> angle;
795};
796
797
798template <class T, class S>
799class StateMachineDrive : public StateMachineAsio<T>
800{
801private:
802 S fDrive;
803
804 string fDatabase;
805
806 typedef map<string, Source> sources;
807 sources fSources;
808
809 string fLastCommand; // Last tracking (RADEC) command
810 int fAutoResume; // 0: disabled, 1: enables, 2: resuming
811 Time fAutoResumeTime;
812
813 // Status 0: Error
814 // Status 1: Unlocked
815 // Status 2: Locked
816 // Status 3: Stopping || Moving
817 // Status 4: Tracking
818
819 bool CheckEventSize(size_t has, const char *name, size_t size)
820 {
821 if (has==size)
822 return true;
823
824 ostringstream msg;
825 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
826 T::Fatal(msg);
827 return false;
828 }
829
830 enum Coordinates
831 {
832 kPoint,
833 kTrackSlow,
834 kTrackFast
835 };
836
837 string AngleToStr(double angle)
838 {
839 /* Handle sign */
840 const char sgn = angle<0?'-':'+';
841
842 /* Round interval and express in smallest units required */
843 double a = round(3600. * fabs(angle)); // deg to seconds
844
845 /* Separate into fields */
846 const double ad = trunc(a/3600.);
847 a -= ad * 3600.;
848 const double am = trunc(a/60.);
849 a -= am * 60.;
850 const double as = trunc(a);
851
852 /* Return results */
853 ostringstream str;
854 str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
855 return str.str();
856 }
857
858 int SendCommand(const string &str)
859 {
860 // This happens if fLastCommand should be send,
861 // but the last command was not a tracking command
862 if (str.empty())
863 {
864 T::Info("Last command was not a tracking command. RESUME ignored.");
865 return T::GetCurrentState();
866 }
867
868 fLastCommand = str.compare(0, 6, "RADEC ")==0 ? str : "";
869
870 fDrive.PostMessage(str);
871 T::Message("Sending: "+str);
872
873 return T::GetCurrentState();
874 }
875
876 int TrackCelest(const string &cmd, const string &source)
877 {
878 SendCommand(cmd);
879
880 fDrive.UpdateSource(source, true);
881
882 return T::GetCurrentState();
883 }
884
885 int Park()
886 {
887 SendCommand("PREPS Park");
888 fDrive.UpdateSource("Park");
889
890 // FIXME: Go to locked state only when park position properly reached
891 return Drive::State::kLocked;
892 }
893
894 int SendStop()
895 {
896 SendCommand("STOP!");
897 fDrive.UpdateSource();
898
899 return T::GetCurrentState();
900 }
901
902 int Resume()
903 {
904 if (fLastCommand.empty())
905 {
906 T::Info("Last command was not a tracking command. RESUME ignored.");
907 return T::GetCurrentState();
908 }
909
910
911 T::Info("Resume: "+fLastCommand);
912 return SendCommand(fLastCommand);
913 }
914
915 int SendCoordinates(const EventImp &evt, const Coordinates type)
916 {
917 if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
918 return T::kSM_FatalError;
919
920 const double *dat = evt.Ptr<double>();
921
922 string command;
923
924 switch (type)
925 {
926 case kPoint: command += "ZDAZ "; break;
927 case kTrackSlow: command += "RADEC "; break;
928 case kTrackFast: command += "GRB "; break;
929 }
930
931 if (type!=kPoint)
932 {
933 const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
934 fDrive.UpdateSource(dim);
935 }
936 else
937 fDrive.UpdateSource("", false);
938
939
940 command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
941 return SendCommand(command);
942 }
943
944 int StartWobble(const double &srcra, const double &srcdec,
945 const double &woboff, const double &wobang,
946 const string name="")
947 {
948 const double ra = srcra *M_PI/12;
949 const double dec = srcdec*M_PI/180;
950 const double off = woboff*M_PI/180;
951 const double dir = wobang*M_PI/180;
952
953 const double cosdir = cos(dir);
954 const double sindir = sin(dir);
955 const double cosoff = cos(off);
956 const double sinoff = sin(off);
957 const double cosdec = cos(dec);
958 const double sindec = sin(dec);
959
960 if (off==0)
961 {
962 const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
963 fDrive.UpdateSource(dim, name);
964
965 string command = "RADEC ";
966 command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
967 return SendCommand(command);
968 }
969
970 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
971 if (sintheta >= 1)
972 {
973 T::Error("cos(Zd) > 1");
974 return T::GetCurrentState();
975 }
976
977 const double costheta = sqrt(1 - sintheta*sintheta);
978
979 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
980 const double sindeltara = sindir*sinoff/costheta;
981
982 const double ndec = asin(sintheta)*180/M_PI;
983 const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
984
985 const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
986 fDrive.UpdateSource(dim, name);
987
988 string command = "RADEC ";
989 command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
990 return SendCommand(command);
991 }
992
993 int Wobble(const EventImp &evt)
994 {
995 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
996 return T::kSM_FatalError;
997
998 const double *dat = evt.Ptr<double>();
999
1000 return StartWobble(dat[0], dat[1], dat[2], dat[3]);
1001 }
1002
1003 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
1004 {
1005 if (find(ptr, last, '\0')==last)
1006 {
1007 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
1008 throw uint32_t(T::kSM_FatalError);
1009 }
1010
1011 const string name(ptr);
1012
1013 const sources::const_iterator it = fSources.find(name);
1014 if (it==fSources.end())
1015 {
1016 T::Error("Source '"+name+"' not found in list.");
1017 throw uint32_t(T::GetCurrentState());
1018 }
1019
1020 return it;
1021 }
1022
1023 int TrackWobble(const EventImp &evt)
1024 {
1025 if (evt.GetSize()<=2)
1026 {
1027 ostringstream msg;
1028 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
1029 T::Fatal(msg);
1030 return T::kSM_FatalError;
1031 }
1032
1033 const uint16_t wobble = evt.GetUShort();
1034 if (wobble!=1 && wobble!=2)
1035 {
1036 ostringstream msg;
1037 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
1038 T::Error(msg);
1039 return T::GetCurrentState();
1040 }
1041
1042 const char *ptr = evt.Ptr<char>(2);
1043 const char *last = ptr+evt.GetSize()-2;
1044
1045 try
1046 {
1047 const sources::const_iterator it = GetSourceFromDB(ptr, last);
1048
1049 const string &name = it->first;
1050 const Source &src = it->second;
1051
1052 return StartWobble(src.ra, src.dec, src.offset, src.angle[wobble-1], name);
1053 }
1054 catch (const uint32_t &e)
1055 {
1056 return e;
1057 }
1058 }
1059
1060 int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0)
1061 {
1062 const char *last = ptr+size;
1063
1064 try
1065 {
1066 const sources::const_iterator it = GetSourceFromDB(ptr, last);
1067
1068 const string &name = it->first;
1069 const Source &src = it->second;
1070
1071 return StartWobble(src.ra, src.dec, offset, angle, name);
1072 }
1073 catch (const uint32_t &e)
1074 {
1075 return e;
1076 }
1077
1078 }
1079
1080 int Track(const EventImp &evt)
1081 {
1082 if (evt.GetSize()<=16)
1083 {
1084 ostringstream msg;
1085 msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
1086 T::Fatal(msg);
1087 return T::kSM_FatalError;
1088 }
1089
1090 const double *dat = evt.Ptr<double>();
1091 const char *ptr = evt.Ptr<char>(16);
1092 const size_t size = evt.GetSize()-16;
1093
1094 return StartTrackWobble(ptr, size, dat[0], dat[1]);
1095 }
1096
1097 int TrackOn(const EventImp &evt)
1098 {
1099 if (evt.GetSize()==0)
1100 {
1101 ostringstream msg;
1102 msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 1.";
1103 T::Fatal(msg);
1104 return T::kSM_FatalError;
1105 }
1106
1107 return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
1108 }
1109
1110
1111 int TakeTPoint(const EventImp &evt)
1112 {
1113 if (evt.GetSize()<=4)
1114 {
1115 ostringstream msg;
1116 msg << "TakePoint - Received event has " << evt.GetSize() << " bytes, but expected at least 5.";
1117 T::Fatal(msg);
1118 return T::kSM_FatalError;
1119 }
1120
1121 const float mag = evt.Get<float>();
1122 const char *ptr = evt.Ptr<char>(4);
1123
1124 string src(ptr);
1125
1126 while (src.find_first_of(' ')!=string::npos)
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:1")
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:1")
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.