1 | #include <boost/bind.hpp>
|
---|
2 | #include <boost/array.hpp>
|
---|
3 | #if BOOST_VERSION < 104400
|
---|
4 | #if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 4))
|
---|
5 | #undef BOOST_HAS_RVALUE_REFS
|
---|
6 | #endif
|
---|
7 | #endif
|
---|
8 | #include <boost/thread.hpp>
|
---|
9 | #include <boost/asio/error.hpp>
|
---|
10 | #include <boost/asio/deadline_timer.hpp>
|
---|
11 |
|
---|
12 | #include "FACT.h"
|
---|
13 | #include "Dim.h"
|
---|
14 | #include "Event.h"
|
---|
15 | #include "Shell.h"
|
---|
16 | #include "StateMachineDim.h"
|
---|
17 | #include "Connection.h"
|
---|
18 | #include "Configuration.h"
|
---|
19 | #include "Timers.h"
|
---|
20 | #include "Console.h"
|
---|
21 | #include "Converter.h"
|
---|
22 |
|
---|
23 | #include "tools.h"
|
---|
24 |
|
---|
25 | #include "LocalControl.h"
|
---|
26 |
|
---|
27 |
|
---|
28 | namespace ba = boost::asio;
|
---|
29 | namespace bs = boost::system;
|
---|
30 | namespace dummy = ba::placeholders;
|
---|
31 |
|
---|
32 | using namespace std;
|
---|
33 |
|
---|
34 | // ------------------------------------------------------------------------
|
---|
35 |
|
---|
36 | namespace Drive
|
---|
37 | {
|
---|
38 | struct DimPointing
|
---|
39 | {
|
---|
40 | } __attribute__((__packed__));
|
---|
41 |
|
---|
42 | struct DimTracking
|
---|
43 | {
|
---|
44 | } __attribute__((__packed__));
|
---|
45 |
|
---|
46 | struct DimStarguider
|
---|
47 | {
|
---|
48 | double fMissZd;
|
---|
49 | double fMissAz;
|
---|
50 |
|
---|
51 | double fNominalZd;
|
---|
52 | double fNominalAz;
|
---|
53 |
|
---|
54 | double fCenterX;
|
---|
55 | double fCenterY;
|
---|
56 |
|
---|
57 | double fBrightness;
|
---|
58 |
|
---|
59 | uint16_t fNumCorrelated;
|
---|
60 | uint16_t fNumLeds;
|
---|
61 | uint16_t fNumRings;
|
---|
62 | uint16_t fNumStars;
|
---|
63 |
|
---|
64 | } __attribute__((__packed__));
|
---|
65 |
|
---|
66 | struct DimTPoint
|
---|
67 | {
|
---|
68 | double fNominalAlt;
|
---|
69 | double fNominalAz;
|
---|
70 |
|
---|
71 | double fCurrentAlt;
|
---|
72 | double fCurrentAz;
|
---|
73 |
|
---|
74 | double fDevZd;
|
---|
75 | double fDevAz;
|
---|
76 |
|
---|
77 | double fRa;
|
---|
78 | double fDec;
|
---|
79 |
|
---|
80 | double fCenterX;
|
---|
81 | double fCenterY;
|
---|
82 | double fCenterMag;
|
---|
83 |
|
---|
84 | double fStarX;
|
---|
85 | double fStarY;
|
---|
86 | double fStarMag;
|
---|
87 |
|
---|
88 | double fBrightness;
|
---|
89 | double fRealMag;
|
---|
90 |
|
---|
91 | uint16_t fNumLeds;
|
---|
92 | uint16_t fNumRings;
|
---|
93 | uint16_t fNumStars;
|
---|
94 | uint16_t fNumCorrelated;
|
---|
95 |
|
---|
96 | } __attribute__((__packed__));
|
---|
97 | };
|
---|
98 |
|
---|
99 |
|
---|
100 |
|
---|
101 | // ------------------------------------------------------------------------
|
---|
102 |
|
---|
103 | class ConnectionDrive : public Connection
|
---|
104 | {
|
---|
105 | int fState;
|
---|
106 |
|
---|
107 | bool fIsVerbose;
|
---|
108 |
|
---|
109 | // --verbose
|
---|
110 | // --hex-out
|
---|
111 | // --dynamic-out
|
---|
112 | // --load-file
|
---|
113 | // --leds
|
---|
114 | // --trigger-interval
|
---|
115 | // --physcis-coincidence
|
---|
116 | // --calib-coincidence
|
---|
117 | // --physcis-window
|
---|
118 | // --physcis-window
|
---|
119 | // --trigger-delay
|
---|
120 | // --time-marker-delay
|
---|
121 | // --dead-time
|
---|
122 | // --clock-conditioner-r0
|
---|
123 | // --clock-conditioner-r1
|
---|
124 | // --clock-conditioner-r8
|
---|
125 | // --clock-conditioner-r9
|
---|
126 | // --clock-conditioner-r11
|
---|
127 | // --clock-conditioner-r13
|
---|
128 | // --clock-conditioner-r14
|
---|
129 | // --clock-conditioner-r15
|
---|
130 | // ...
|
---|
131 |
|
---|
132 | virtual void UpdatePointing(const Time &, const boost::array<double, 2> &)
|
---|
133 | {
|
---|
134 | }
|
---|
135 |
|
---|
136 | virtual void UpdateTracking(const Time &, const boost::array<double, 7> &)
|
---|
137 | {
|
---|
138 | }
|
---|
139 |
|
---|
140 | virtual void UpdateStarguider(const Time &, const Drive::DimStarguider &)
|
---|
141 | {
|
---|
142 | }
|
---|
143 |
|
---|
144 | virtual void UpdateTPoint(const Time &, const Drive::DimTPoint &)
|
---|
145 | {
|
---|
146 | }
|
---|
147 |
|
---|
148 | protected:
|
---|
149 | map<uint16_t, int> fCounter;
|
---|
150 |
|
---|
151 | ba::streambuf fBuffer;
|
---|
152 |
|
---|
153 | Time ReadTime(istream &in)
|
---|
154 | {
|
---|
155 | uint16_t y, m, d, hh, mm, ss, ms;
|
---|
156 | in >> y >> m >> d >> hh >> mm >> ss >> ms;
|
---|
157 |
|
---|
158 | return Time(y, m, d, hh, mm, ss, ms*1000);
|
---|
159 | }
|
---|
160 |
|
---|
161 | double ReadAngle(istream &in)
|
---|
162 | {
|
---|
163 | char sgn;
|
---|
164 | uint16_t d, m;
|
---|
165 | float s;
|
---|
166 |
|
---|
167 | in >> sgn >> d >> m >> s;
|
---|
168 |
|
---|
169 | const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
|
---|
170 | return sgn=='-' ? -ret : ret;
|
---|
171 | }
|
---|
172 |
|
---|
173 | void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received)
|
---|
174 | {
|
---|
175 | // Do not schedule a new read if the connection failed.
|
---|
176 | if (bytes_received==0 || err)
|
---|
177 | {
|
---|
178 | if (err==ba::error::eof)
|
---|
179 | Warn("Connection closed by remote host (FTM).");
|
---|
180 |
|
---|
181 | // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
|
---|
182 | // 125: Operation canceled
|
---|
183 | if (err && err!=ba::error::eof && // Connection closed by remote host
|
---|
184 | err!=ba::error::basic_errors::not_connected && // Connection closed by remote host
|
---|
185 | err!=ba::error::basic_errors::operation_aborted) // Connection closed by us
|
---|
186 | {
|
---|
187 | stringstream str;
|
---|
188 | str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
|
---|
189 | Error(str);
|
---|
190 | }
|
---|
191 | PostClose(err!=ba::error::basic_errors::operation_aborted);
|
---|
192 | return;
|
---|
193 | }
|
---|
194 |
|
---|
195 | istream is(&fBuffer);
|
---|
196 |
|
---|
197 | string line;
|
---|
198 | getline(is, line);
|
---|
199 |
|
---|
200 | if (fIsVerbose)
|
---|
201 | Out() << line << endl;
|
---|
202 |
|
---|
203 | StartReadReport();
|
---|
204 |
|
---|
205 | if (line.substr(0, 13)=="STARG-REPORT ")
|
---|
206 | {
|
---|
207 | istringstream stream(line.substr(16));
|
---|
208 |
|
---|
209 | // 0: Error
|
---|
210 | // 1: Standby
|
---|
211 | // 2: Monitoring
|
---|
212 | uint16_t status1;
|
---|
213 | stream >> status1;
|
---|
214 | const Time t1 = ReadTime(stream);
|
---|
215 |
|
---|
216 | uint16_t status2;
|
---|
217 | stream >> status2;
|
---|
218 | const Time t2 = ReadTime(stream);
|
---|
219 |
|
---|
220 | double misszd, missaz;
|
---|
221 | stream >> misszd >> missaz;
|
---|
222 |
|
---|
223 | const double zd = ReadAngle(stream);
|
---|
224 | const double az = ReadAngle(stream);
|
---|
225 |
|
---|
226 | double cx, cy;
|
---|
227 | stream >> cx >> cy;
|
---|
228 |
|
---|
229 | int ncor;
|
---|
230 | stream >> ncor;
|
---|
231 |
|
---|
232 | double bright, mjd;
|
---|
233 | stream >> bright >> mjd;
|
---|
234 |
|
---|
235 | int nled, nring, nstars;
|
---|
236 | stream >> nled >> nring >> nstars;
|
---|
237 |
|
---|
238 | if (stream.fail())
|
---|
239 | return;
|
---|
240 |
|
---|
241 | Drive::DimStarguider data;
|
---|
242 |
|
---|
243 | data.fMissZd = misszd;
|
---|
244 | data.fMissAz = missaz;
|
---|
245 | data.fNominalZd = zd;
|
---|
246 | data.fNominalAz = az;
|
---|
247 | data.fCenterX = cx;
|
---|
248 | data.fCenterY = cy;
|
---|
249 | data.fNumCorrelated = ncor;
|
---|
250 | data.fBrightness = bright;
|
---|
251 | data.fNumLeds = nled;
|
---|
252 | data.fNumRings = nring;
|
---|
253 | data.fNumStars = nstars;
|
---|
254 |
|
---|
255 | UpdateStarguider(Time(mjd), data);
|
---|
256 |
|
---|
257 | return;
|
---|
258 |
|
---|
259 | }
|
---|
260 |
|
---|
261 | if (line.substr(0, 14)=="TPOINT-REPORT ")
|
---|
262 | {
|
---|
263 | istringstream stream(line.substr(17));
|
---|
264 |
|
---|
265 | uint16_t status1;
|
---|
266 | stream >> status1;
|
---|
267 | const Time t1 = ReadTime(stream);
|
---|
268 |
|
---|
269 | uint16_t status2;
|
---|
270 | stream >> status2;
|
---|
271 | const Time t2 = ReadTime(stream);
|
---|
272 |
|
---|
273 | double az1, alt1, az2, alt2, ra, dec, dzd, daz;
|
---|
274 | stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
|
---|
275 |
|
---|
276 | // c: center, s:start
|
---|
277 | double mjd, cmag, smag, cx, cy, sx, sy;
|
---|
278 | stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
|
---|
279 |
|
---|
280 | int nled, nring, nstar, ncor;
|
---|
281 | stream >> nled >> nring >> nstar >> ncor;
|
---|
282 |
|
---|
283 | double bright, mag;
|
---|
284 | stream >> bright >> mag;
|
---|
285 |
|
---|
286 | string name;
|
---|
287 | stream >> name;
|
---|
288 |
|
---|
289 | if (stream.fail())
|
---|
290 | return;
|
---|
291 |
|
---|
292 | Drive::DimTPoint tpoint;
|
---|
293 |
|
---|
294 | tpoint.fNominalAz = az1;
|
---|
295 | tpoint.fNominalAlt = alt1;
|
---|
296 | tpoint.fCurrentAz = az2;
|
---|
297 | tpoint.fCurrentAlt = alt2;
|
---|
298 | tpoint.fDevAz = daz;
|
---|
299 | tpoint.fDevZd = dzd;
|
---|
300 | tpoint.fRa = ra;
|
---|
301 | tpoint.fDec = dec;
|
---|
302 |
|
---|
303 | tpoint.fCenterX = cx;
|
---|
304 | tpoint.fCenterY = cy;
|
---|
305 | tpoint.fCenterMag = cmag;
|
---|
306 |
|
---|
307 | tpoint.fStarX = sx;
|
---|
308 | tpoint.fStarY = sy;
|
---|
309 | tpoint.fStarMag = smag;
|
---|
310 |
|
---|
311 | tpoint.fBrightness = bright;
|
---|
312 |
|
---|
313 | tpoint.fNumCorrelated = ncor;
|
---|
314 | tpoint.fNumLeds = nled;
|
---|
315 | tpoint.fNumRings = nring;
|
---|
316 | tpoint.fNumStars = nstar;
|
---|
317 |
|
---|
318 | tpoint.fRealMag = mag;
|
---|
319 |
|
---|
320 | return;
|
---|
321 | }
|
---|
322 |
|
---|
323 | if (line.substr(0, 13)=="DRIVE-REPORT ")
|
---|
324 | {
|
---|
325 | // DRIVE-REPORT M1
|
---|
326 | // 01 2011 05 14 11 31 19 038
|
---|
327 | // 02 1858 11 17 00 00 00 000
|
---|
328 | // + 000 00 000 + 000 00 000
|
---|
329 | // + 000 00 000
|
---|
330 | // 55695.480081
|
---|
331 | // + 000 00 000 + 000 00 000
|
---|
332 | // + 000 00 000 + 000 00 000
|
---|
333 | // 0000.000 0000.000
|
---|
334 | // 0 2
|
---|
335 |
|
---|
336 | // status
|
---|
337 | // year month day hour minute seconds millisec
|
---|
338 | // year month day hour minute seconds millisec
|
---|
339 | // ra(+ h m s) dec(+ d m s) ha(+ h m s)
|
---|
340 | // mjd
|
---|
341 | // zd(+ d m s) az(+ d m s)
|
---|
342 | // zd(+ d m s) az(+ d m s)
|
---|
343 | // zd_err az_err
|
---|
344 | // armed(0=unlocked, 1=locked)
|
---|
345 | // stgmd(0=none, 1=starguider, 2=starguider off)
|
---|
346 | istringstream stream(line.substr(16));
|
---|
347 |
|
---|
348 | uint16_t status1;
|
---|
349 | stream >> status1;
|
---|
350 | const Time t1 = ReadTime(stream);
|
---|
351 |
|
---|
352 | uint16_t status2;
|
---|
353 | stream >> status2;
|
---|
354 | const Time t2 = ReadTime(stream);
|
---|
355 |
|
---|
356 | const double ra = ReadAngle(stream);
|
---|
357 | const double dec = ReadAngle(stream);
|
---|
358 | const double ha = ReadAngle(stream);
|
---|
359 |
|
---|
360 | double mjd;
|
---|
361 | stream >> mjd;
|
---|
362 |
|
---|
363 | const double zd1 = ReadAngle(stream);
|
---|
364 | const double az1 = ReadAngle(stream);
|
---|
365 | const double zd2 = ReadAngle(stream);
|
---|
366 | const double az2 = ReadAngle(stream);
|
---|
367 |
|
---|
368 | double zd_err, az_err;
|
---|
369 | stream >> zd_err;
|
---|
370 | stream >> az_err;
|
---|
371 |
|
---|
372 | uint16_t armed, stgmd;
|
---|
373 | stream >> armed;
|
---|
374 | stream >> stgmd;
|
---|
375 |
|
---|
376 | if (stream.fail())
|
---|
377 | return;
|
---|
378 |
|
---|
379 | // Status 0: Error
|
---|
380 | // Status 1: Stopped
|
---|
381 | // Status 3: Stopping || Moving
|
---|
382 | // Status 4: Tracking
|
---|
383 | if (status1==0)
|
---|
384 | status1 = 99;
|
---|
385 | fState = status1==1 ? armed+1 : status1;
|
---|
386 |
|
---|
387 | const boost::array<double, 2> point = {{ zd2, az2 }};
|
---|
388 | UpdatePointing(t1, point);
|
---|
389 |
|
---|
390 | const boost::array<double, 7> track =
|
---|
391 | {{
|
---|
392 | ra, dec, ha,
|
---|
393 | zd1, az1,
|
---|
394 | zd_err, az_err
|
---|
395 | }};
|
---|
396 | UpdateTracking(Time(mjd), track);
|
---|
397 |
|
---|
398 | // ---- DIM ----> t1 as event time
|
---|
399 | // status1
|
---|
400 | // mjd
|
---|
401 | // ra/dec/ha
|
---|
402 | // zd/az (nominal)
|
---|
403 | // zd/az (current)
|
---|
404 | // err(zd/az)
|
---|
405 | // [armed] [stgmd]
|
---|
406 |
|
---|
407 | // Maybe:
|
---|
408 | // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
|
---|
409 | //
|
---|
410 | // if (mjd>0)
|
---|
411 | // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
|
---|
412 | // ra/dec, ha(not well defined),
|
---|
413 | // [Nominal + Error == Current]
|
---|
414 |
|
---|
415 | // MJD is the time which corresponds to the nominal position
|
---|
416 | // t1 is the time which corresponds to the current position/HA
|
---|
417 |
|
---|
418 | return;
|
---|
419 | }
|
---|
420 | }
|
---|
421 |
|
---|
422 | void StartReadReport()
|
---|
423 | {
|
---|
424 | boost::asio::async_read_until(*this, fBuffer, '\n',
|
---|
425 | boost::bind(&ConnectionDrive::HandleReceivedReport, this,
|
---|
426 | dummy::error, dummy::bytes_transferred));
|
---|
427 | }
|
---|
428 |
|
---|
429 | boost::asio::deadline_timer fKeepAlive;
|
---|
430 |
|
---|
431 | void KeepAlive()
|
---|
432 | {
|
---|
433 | PostMessage(string("KEEP_ALIVE"));
|
---|
434 |
|
---|
435 | fKeepAlive.expires_from_now(boost::posix_time::seconds(10));
|
---|
436 | fKeepAlive.async_wait(boost::bind(&ConnectionDrive::HandleKeepAlive,
|
---|
437 | this, dummy::error));
|
---|
438 | }
|
---|
439 |
|
---|
440 | void HandleKeepAlive(const bs::error_code &error)
|
---|
441 | {
|
---|
442 | // 125: Operation canceled (bs::error_code(125, bs::system_category))
|
---|
443 | if (error && error!=ba::error::basic_errors::operation_aborted)
|
---|
444 | {
|
---|
445 | stringstream str;
|
---|
446 | str << "Write timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
|
---|
447 | Error(str);
|
---|
448 |
|
---|
449 | PostClose(false);
|
---|
450 | return;
|
---|
451 | }
|
---|
452 |
|
---|
453 | if (!is_open())
|
---|
454 | {
|
---|
455 | // For example: Here we could schedule a new accept if we
|
---|
456 | // would not want to allow two connections at the same time.
|
---|
457 | return;
|
---|
458 | }
|
---|
459 |
|
---|
460 | // Check whether the deadline has passed. We compare the deadline
|
---|
461 | // against the current time since a new asynchronous operation
|
---|
462 | // may have moved the deadline before this actor had a chance
|
---|
463 | // to run.
|
---|
464 | if (fKeepAlive.expires_at() > ba::deadline_timer::traits_type::now())
|
---|
465 | return;
|
---|
466 |
|
---|
467 | KeepAlive();
|
---|
468 | }
|
---|
469 |
|
---|
470 |
|
---|
471 | private:
|
---|
472 | // This is called when a connection was established
|
---|
473 | void ConnectionEstablished()
|
---|
474 | {
|
---|
475 | StartReadReport();
|
---|
476 | KeepAlive();
|
---|
477 | }
|
---|
478 |
|
---|
479 | /*
|
---|
480 | void HandleReadTimeout(const bs::error_code &error)
|
---|
481 | {
|
---|
482 | if (error && error!=ba::error::basic_errors::operation_aborted)
|
---|
483 | {
|
---|
484 | stringstream str;
|
---|
485 | str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
|
---|
486 | Error(str);
|
---|
487 |
|
---|
488 | PostClose();
|
---|
489 | return;
|
---|
490 |
|
---|
491 | }
|
---|
492 |
|
---|
493 | if (!is_open())
|
---|
494 | {
|
---|
495 | // For example: Here we could schedule a new accept if we
|
---|
496 | // would not want to allow two connections at the same time.
|
---|
497 | return;
|
---|
498 | }
|
---|
499 |
|
---|
500 | // Check whether the deadline has passed. We compare the deadline
|
---|
501 | // against the current time since a new asynchronous operation
|
---|
502 | // may have moved the deadline before this actor had a chance
|
---|
503 | // to run.
|
---|
504 | if (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
|
---|
505 | return;
|
---|
506 |
|
---|
507 | Error("Timeout reading data from "+URL());
|
---|
508 |
|
---|
509 | PostClose();
|
---|
510 | }*/
|
---|
511 |
|
---|
512 |
|
---|
513 | public:
|
---|
514 |
|
---|
515 | static const uint16_t kMaxAddr;
|
---|
516 |
|
---|
517 | public:
|
---|
518 | ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
|
---|
519 | fState(0), fIsVerbose(true), fKeepAlive(ioservice)
|
---|
520 | {
|
---|
521 | SetLogStream(&imp);
|
---|
522 | }
|
---|
523 |
|
---|
524 | void SetVerbose(bool b)
|
---|
525 | {
|
---|
526 | fIsVerbose = b;
|
---|
527 | }
|
---|
528 |
|
---|
529 | int GetState() const { return IsConnected() ? fState+1 : 1; }
|
---|
530 | };
|
---|
531 |
|
---|
532 | const uint16_t ConnectionDrive::kMaxAddr = 0xfff;
|
---|
533 |
|
---|
534 | // ------------------------------------------------------------------------
|
---|
535 |
|
---|
536 | #include "DimDescriptionService.h"
|
---|
537 |
|
---|
538 | class ConnectionDimDrive : public ConnectionDrive
|
---|
539 | {
|
---|
540 | private:
|
---|
541 |
|
---|
542 | DimDescribedService fDimPointing;
|
---|
543 | DimDescribedService fDimTracking;
|
---|
544 |
|
---|
545 | template<size_t N>
|
---|
546 | void Update(DimDescribedService &svc, const Time &t, const boost::array<double, N> &arr) const
|
---|
547 | {
|
---|
548 | svc.setTimestamp(int(t.UnixTime()), t.ms());
|
---|
549 | svc.setData(const_cast<double*>(arr.data()), arr.size()*sizeof(double));
|
---|
550 | svc.updateService();
|
---|
551 | }
|
---|
552 |
|
---|
553 | virtual void UpdatePointing(const Time &t,
|
---|
554 | const boost::array<double, 2> &arr)
|
---|
555 | {
|
---|
556 | Update(fDimPointing, t, arr);
|
---|
557 | }
|
---|
558 |
|
---|
559 | virtual void UpdateTracking(const Time &t,
|
---|
560 | const boost::array<double, 7> &arr)
|
---|
561 | {
|
---|
562 | Update(fDimTracking, t, arr);
|
---|
563 | }
|
---|
564 |
|
---|
565 | public:
|
---|
566 | ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
|
---|
567 | ConnectionDrive(ioservice, imp),
|
---|
568 | fDimPointing("FTM_CONTROL/POINTING_POSITION", "D:2", ""),
|
---|
569 | fDimTracking("FTM_CONTROL/TRACKING_POSITION", "D:7", "")
|
---|
570 | {
|
---|
571 | }
|
---|
572 |
|
---|
573 | // 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
|
---|
574 | };
|
---|
575 |
|
---|
576 | // ------------------------------------------------------------------------
|
---|
577 |
|
---|
578 | template <class T, class S>
|
---|
579 | class StateMachineDrive : public T, public ba::io_service, public ba::io_service::work
|
---|
580 | {
|
---|
581 | int Wrap(boost::function<void()> f)
|
---|
582 | {
|
---|
583 | f();
|
---|
584 | return T::GetCurrentState();
|
---|
585 | }
|
---|
586 |
|
---|
587 | boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
|
---|
588 | {
|
---|
589 | return boost::bind(&StateMachineDrive::Wrap, this, func);
|
---|
590 | }
|
---|
591 |
|
---|
592 | private:
|
---|
593 | S fDrive;
|
---|
594 |
|
---|
595 | enum states_t
|
---|
596 | {
|
---|
597 | kStateDisconnected = 1,
|
---|
598 | kStateConnected,
|
---|
599 | kStateArmed,
|
---|
600 | kStateMoving,
|
---|
601 | kStateTracking,
|
---|
602 | };
|
---|
603 |
|
---|
604 | // Status 0: Error
|
---|
605 | // Status 1: Unlocked
|
---|
606 | // Status 2: Locked
|
---|
607 | // Status 3: Stopping || Moving
|
---|
608 | // Status 4: Tracking
|
---|
609 |
|
---|
610 | bool CheckEventSize(size_t has, const char *name, size_t size)
|
---|
611 | {
|
---|
612 | if (has==size)
|
---|
613 | return true;
|
---|
614 |
|
---|
615 | stringstream msg;
|
---|
616 | msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
|
---|
617 | T::Fatal(msg);
|
---|
618 | return false;
|
---|
619 | }
|
---|
620 |
|
---|
621 | enum Coordinates
|
---|
622 | {
|
---|
623 | kPoint,
|
---|
624 | kTrackSlow,
|
---|
625 | kTrackFast
|
---|
626 | };
|
---|
627 |
|
---|
628 | string AngleToStr(double angle)
|
---|
629 | {
|
---|
630 | /* Handle sign */
|
---|
631 | const char sgn = angle<0?'-':'+';
|
---|
632 |
|
---|
633 | /* Round interval and express in smallest units required */
|
---|
634 | double a = round(3600. * fabs(angle)); // deg to seconds
|
---|
635 |
|
---|
636 | /* Separate into fields */
|
---|
637 | const double ad = trunc(a/3600.);
|
---|
638 | a -= ad * 3600.;
|
---|
639 | const double am = trunc(a/60.);
|
---|
640 | a -= am * 60.;
|
---|
641 | const double as = trunc(a);
|
---|
642 |
|
---|
643 | /* Return results */
|
---|
644 | ostringstream str;
|
---|
645 | str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
|
---|
646 | return str.str();
|
---|
647 | }
|
---|
648 |
|
---|
649 | int SendCommand(const string &str)
|
---|
650 | {
|
---|
651 | fDrive.PostMessage(str);
|
---|
652 | return T::GetCurrentState();
|
---|
653 | }
|
---|
654 |
|
---|
655 | int SendCoordinates(const EventImp &evt, const Coordinates type)
|
---|
656 | {
|
---|
657 | if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
|
---|
658 | return T::kSM_FatalError;
|
---|
659 |
|
---|
660 | const double *dat = evt.Ptr<double>();
|
---|
661 |
|
---|
662 | string command;
|
---|
663 |
|
---|
664 | switch (type)
|
---|
665 | {
|
---|
666 | case kPoint: command += "ZDAZ "; break;
|
---|
667 | case kTrackSlow: command += "RADEC "; break;
|
---|
668 | case kTrackFast: command += "GRB "; break;
|
---|
669 | }
|
---|
670 |
|
---|
671 | command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
|
---|
672 |
|
---|
673 | return SendCommand(command);
|
---|
674 | }
|
---|
675 |
|
---|
676 | int SetVerbosity(const EventImp &evt)
|
---|
677 | {
|
---|
678 | if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
|
---|
679 | return T::kSM_FatalError;
|
---|
680 |
|
---|
681 | fDrive.SetVerbose(evt.GetBool());
|
---|
682 |
|
---|
683 | return T::GetCurrentState();
|
---|
684 | }
|
---|
685 |
|
---|
686 | int Disconnect()
|
---|
687 | {
|
---|
688 | // Close all connections
|
---|
689 | fDrive.PostClose(false);
|
---|
690 |
|
---|
691 | /*
|
---|
692 | // Now wait until all connection have been closed and
|
---|
693 | // all pending handlers have been processed
|
---|
694 | poll();
|
---|
695 | */
|
---|
696 |
|
---|
697 | return T::GetCurrentState();
|
---|
698 | }
|
---|
699 |
|
---|
700 | int Reconnect(const EventImp &evt)
|
---|
701 | {
|
---|
702 | // Close all connections to supress the warning in SetEndpoint
|
---|
703 | fDrive.PostClose(false);
|
---|
704 |
|
---|
705 | // Now wait until all connection have been closed and
|
---|
706 | // all pending handlers have been processed
|
---|
707 | poll();
|
---|
708 |
|
---|
709 | if (evt.GetBool())
|
---|
710 | fDrive.SetEndpoint(evt.GetString());
|
---|
711 |
|
---|
712 | // Now we can reopen the connection
|
---|
713 | fDrive.PostClose(true);
|
---|
714 |
|
---|
715 | return T::GetCurrentState();
|
---|
716 | }
|
---|
717 |
|
---|
718 | int Execute()
|
---|
719 | {
|
---|
720 | // Dispatch (execute) at most one handler from the queue. In contrary
|
---|
721 | // to run_one(), it doesn't wait until a handler is available
|
---|
722 | // which can be dispatched, so poll_one() might return with 0
|
---|
723 | // handlers dispatched. The handlers are always dispatched/executed
|
---|
724 | // synchronously, i.e. within the call to poll_one()
|
---|
725 | poll_one();
|
---|
726 |
|
---|
727 | return fDrive.GetState();
|
---|
728 | }
|
---|
729 |
|
---|
730 |
|
---|
731 | public:
|
---|
732 | StateMachineDrive(ostream &out=cout) :
|
---|
733 | T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
|
---|
734 | fDrive(*this, *this)
|
---|
735 | {
|
---|
736 | // ba::io_service::work is a kind of keep_alive for the loop.
|
---|
737 | // It prevents the io_service to go to stopped state, which
|
---|
738 | // would prevent any consecutive calls to run()
|
---|
739 | // or poll() to do nothing. reset() could also revoke to the
|
---|
740 | // previous state but this might introduce some overhead of
|
---|
741 | // deletion and creation of threads and more.
|
---|
742 |
|
---|
743 | // State names
|
---|
744 | AddStateName(kStateDisconnected, "Disconnected",
|
---|
745 | "");
|
---|
746 |
|
---|
747 | AddStateName(kStateConnected, "Connected",
|
---|
748 | "");
|
---|
749 |
|
---|
750 | AddStateName(kStateArmed, "Armed",
|
---|
751 | "");
|
---|
752 |
|
---|
753 | AddStateName(kStateMoving, "Moving",
|
---|
754 | "");
|
---|
755 |
|
---|
756 | AddStateName(kStateTracking, "Tracking",
|
---|
757 | "");
|
---|
758 |
|
---|
759 | // kStateIdle
|
---|
760 | // kStateArmed
|
---|
761 | // kStateMoving
|
---|
762 | // kStateTracking
|
---|
763 |
|
---|
764 | // Init
|
---|
765 | // -----------
|
---|
766 | // "ARM lock"
|
---|
767 | // "STGMD off"
|
---|
768 |
|
---|
769 | /*
|
---|
770 | [ ] WAIT -> WM_WAIT
|
---|
771 | [x] STOP! -> WM_STOP
|
---|
772 | [x] RADEC ra(+ d m s.f) dec(+ d m s.f)
|
---|
773 | [x] GRB ra(+ d m s.f) dec(+ d m s.f)
|
---|
774 | [x] ZDAZ zd(+ d m s.f) az (+ d m s.f)
|
---|
775 | [ ] CELEST id offset angle
|
---|
776 | [ ] MOON wobble offset
|
---|
777 | [ ] PREPS string
|
---|
778 | [ ] TPOIN star mag
|
---|
779 | [ ] ARM lock/unlock
|
---|
780 | [ ] STGMD on/off
|
---|
781 | */
|
---|
782 |
|
---|
783 | // Drive Commands
|
---|
784 | T::AddEvent("MOVE_TO", "D:2", kStateArmed) // ->ZDAZ
|
---|
785 | (boost::bind(&StateMachineDrive::SendCoordinates, this, _1, kPoint))
|
---|
786 | (""
|
---|
787 | "|zd[deg]:"
|
---|
788 | "|az[deg]:");
|
---|
789 |
|
---|
790 | T::AddEvent("TRACK", "D:2", kStateArmed) // ->RADEC/GRB
|
---|
791 | (boost::bind(&StateMachineDrive::SendCoordinates, this, _1, kTrackSlow))
|
---|
792 | (""
|
---|
793 | "|ra[h]:"
|
---|
794 | "|dec[deg]:");
|
---|
795 |
|
---|
796 | T::AddEvent("MOON", kStateArmed)
|
---|
797 | (boost::bind(&StateMachineDrive::SendCommand, this, "MOON 0 0"))
|
---|
798 | ("");
|
---|
799 | T::AddEvent("VENUS", kStateArmed)
|
---|
800 | (boost::bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0"))
|
---|
801 | ("");
|
---|
802 | T::AddEvent("MARS", kStateArmed)
|
---|
803 | (boost::bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0"))
|
---|
804 | ("");
|
---|
805 | T::AddEvent("JUPITER", kStateArmed)
|
---|
806 | (boost::bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0"))
|
---|
807 | ("");
|
---|
808 | T::AddEvent("SATURN", kStateArmed)
|
---|
809 | (boost::bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0"))
|
---|
810 | ("");
|
---|
811 |
|
---|
812 | T::AddEvent("TPOINT")
|
---|
813 | (boost::bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0"))
|
---|
814 | ("");
|
---|
815 |
|
---|
816 | T::AddEvent("STOP")
|
---|
817 | (boost::bind(&StateMachineDrive::SendCommand, this, "STOP!"))
|
---|
818 | ("");
|
---|
819 |
|
---|
820 | T::AddEvent("ARM", kStateConnected)
|
---|
821 | (boost::bind(&StateMachineDrive::SendCommand, this, "ARM lock"))
|
---|
822 | ("");
|
---|
823 |
|
---|
824 |
|
---|
825 | // Verbosity commands
|
---|
826 | T::AddEvent("SET_VERBOSE", "B")
|
---|
827 | (boost::bind(&StateMachineDrive::SetVerbosity, this, _1))
|
---|
828 | ("set verbosity state"
|
---|
829 | "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
|
---|
830 |
|
---|
831 | // Conenction commands
|
---|
832 | AddEvent("DISCONNECT", kStateConnected, kStateArmed)
|
---|
833 | (boost::bind(&StateMachineDrive::Disconnect, this))
|
---|
834 | ("disconnect from ethernet");
|
---|
835 |
|
---|
836 | AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected, kStateArmed)
|
---|
837 | (boost::bind(&StateMachineDrive::Reconnect, this, _1))
|
---|
838 | ("(Re)connect ethernet connection to FTM, a new address can be given"
|
---|
839 | "|[host][string]:new ethernet address in the form <host:port>");
|
---|
840 |
|
---|
841 | fDrive.StartConnect();
|
---|
842 | }
|
---|
843 |
|
---|
844 | void SetEndpoint(const string &url)
|
---|
845 | {
|
---|
846 | fDrive.SetEndpoint(url);
|
---|
847 | }
|
---|
848 |
|
---|
849 | bool SetConfiguration(const Configuration &conf)
|
---|
850 | {
|
---|
851 | SetEndpoint(conf.Get<string>("addr"));
|
---|
852 |
|
---|
853 | fDrive.SetVerbose(!conf.Get<bool>("quiet"));
|
---|
854 |
|
---|
855 | return true;
|
---|
856 | }
|
---|
857 | };
|
---|
858 |
|
---|
859 | // ------------------------------------------------------------------------
|
---|
860 |
|
---|
861 | void RunThread(StateMachineImp *io_service)
|
---|
862 | {
|
---|
863 | // This is necessary so that the StateMachien Thread can signal the
|
---|
864 | // Readline to exit
|
---|
865 | io_service->Run();
|
---|
866 | Readline::Stop();
|
---|
867 | }
|
---|
868 |
|
---|
869 | /*
|
---|
870 | template<class S, class T>
|
---|
871 | int RunDim(Configuration &conf)
|
---|
872 | {
|
---|
873 | WindowLog wout;
|
---|
874 |
|
---|
875 | ReadlineColor::PrintBootMsg(wout, conf.GetName(), false);
|
---|
876 |
|
---|
877 | if (conf.Has("log"))
|
---|
878 | if (!wout.OpenLogFile(conf.Get<string>("log")))
|
---|
879 | wout << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;
|
---|
880 |
|
---|
881 | // Start io_service.Run to use the StateMachineImp::Run() loop
|
---|
882 | // Start io_service.run to only use the commandHandler command detaching
|
---|
883 | StateMachineDrive<S, T> io_service(wout);
|
---|
884 | if (!io_service.SetConfiguration(conf))
|
---|
885 | return -1;
|
---|
886 |
|
---|
887 | io_service.Run();
|
---|
888 |
|
---|
889 | return 0;
|
---|
890 | }
|
---|
891 | */
|
---|
892 |
|
---|
893 | template<class T, class S, class R>
|
---|
894 | int RunShell(Configuration &conf)
|
---|
895 | {
|
---|
896 | static T shell(conf.GetName().c_str(), conf.Get<int>("console")!=1);
|
---|
897 |
|
---|
898 | WindowLog &win = shell.GetStreamIn();
|
---|
899 | WindowLog &wout = shell.GetStreamOut();
|
---|
900 |
|
---|
901 | if (conf.Has("log"))
|
---|
902 | if (!wout.OpenLogFile(conf.Get<string>("log")))
|
---|
903 | win << kRed << "ERROR - Couldn't open log-file " << conf.Get<string>("log") << ": " << strerror(errno) << endl;
|
---|
904 |
|
---|
905 | StateMachineDrive<S, R> io_service(wout);
|
---|
906 | if (!io_service.SetConfiguration(conf))
|
---|
907 | return -1;
|
---|
908 |
|
---|
909 | shell.SetReceiver(io_service);
|
---|
910 |
|
---|
911 | boost::thread t(boost::bind(RunThread, &io_service));
|
---|
912 | // boost::thread t(boost::bind(&StateMachineDrive<S>::Run, &io_service));
|
---|
913 |
|
---|
914 | if (conf.Has("exec"))
|
---|
915 | {
|
---|
916 | const vector<string> v = conf.Get<vector<string>>("exec");
|
---|
917 | for (vector<string>::const_iterator it=v.begin(); it!=v.end(); it++)
|
---|
918 | shell.Execute(*it);
|
---|
919 | }
|
---|
920 |
|
---|
921 | shell.Run(); // Run the shell
|
---|
922 | io_service.Stop(); // Signal Loop-thread to stop
|
---|
923 | // io_service.Close(); // Obsolete, done by the destructor
|
---|
924 |
|
---|
925 | // Wait until the StateMachine has finished its thread
|
---|
926 | // before returning and destroying the dim objects which might
|
---|
927 | // still be in use.
|
---|
928 | t.join();
|
---|
929 |
|
---|
930 | return 0;
|
---|
931 | }
|
---|
932 |
|
---|
933 | void SetupConfiguration(Configuration &conf)
|
---|
934 | {
|
---|
935 | const string n = conf.GetName()+".log";
|
---|
936 |
|
---|
937 | po::options_description config("Program options");
|
---|
938 | config.add_options()
|
---|
939 | ("dns", var<string>("localhost"), "Dim nameserver host name (Overwites DIM_DNS_NODE environment variable)")
|
---|
940 | ("log,l", var<string>(n), "Write log-file")
|
---|
941 | ("no-dim,d", po_switch(), "Disable dim services")
|
---|
942 | ("console,c", var<int>(), "Use console (0=shell, 1=simple buffered, X=simple unbuffered)")
|
---|
943 | ("exec,e", vars<strings>(),"Execute one or more scrips at startup")
|
---|
944 | ;
|
---|
945 |
|
---|
946 | po::options_description control("FTM control options");
|
---|
947 | control.add_options()
|
---|
948 | ("addr,a", var<string>("localhost:7404"), "Network address of FTM")
|
---|
949 | ("quiet,q", po_bool(), "Disable printing contents of all received messages (except dynamic data) in clear text.")
|
---|
950 | ;
|
---|
951 |
|
---|
952 | conf.AddEnv("dns", "DIM_DNS_NODE");
|
---|
953 |
|
---|
954 | conf.AddOptions(config);
|
---|
955 | conf.AddOptions(control);
|
---|
956 | }
|
---|
957 |
|
---|
958 | /*
|
---|
959 | Extract usage clause(s) [if any] for SYNOPSIS.
|
---|
960 | Translators: "Usage" and "or" here are patterns (regular expressions) which
|
---|
961 | are used to match the usage synopsis in program output. An example from cp
|
---|
962 | (GNU coreutils) which contains both strings:
|
---|
963 | Usage: cp [OPTION]... [-T] SOURCE DEST
|
---|
964 | or: cp [OPTION]... SOURCE... DIRECTORY
|
---|
965 | or: cp [OPTION]... -t DIRECTORY SOURCE...
|
---|
966 | */
|
---|
967 | void PrintUsage()
|
---|
968 | {
|
---|
969 | cout <<
|
---|
970 | "The drivectrl is an interface to cosy.\n"
|
---|
971 | "\n"
|
---|
972 | "The default is that the program is started without user intercation. "
|
---|
973 | "All actions are supposed to arrive as DimCommands. Using the -c "
|
---|
974 | "option, a local shell can be initialized. With h or help a short "
|
---|
975 | "help message about the usuage can be brought to the screen.\n"
|
---|
976 | "\n"
|
---|
977 | "Usage: drivectrl [-c type] [OPTIONS]\n"
|
---|
978 | " or: drivectrl [OPTIONS]\n";
|
---|
979 | cout << endl;
|
---|
980 | }
|
---|
981 |
|
---|
982 | void PrintHelp()
|
---|
983 | {
|
---|
984 | /* Additional help text which is printed after the configuration
|
---|
985 | options goes here */
|
---|
986 |
|
---|
987 | /*
|
---|
988 | cout << "bla bla bla" << endl << endl;
|
---|
989 | cout << endl;
|
---|
990 | cout << "Environment:" << endl;
|
---|
991 | cout << "environment" << endl;
|
---|
992 | cout << endl;
|
---|
993 | cout << "Examples:" << endl;
|
---|
994 | cout << "test exam" << endl;
|
---|
995 | cout << endl;
|
---|
996 | cout << "Files:" << endl;
|
---|
997 | cout << "files" << endl;
|
---|
998 | cout << endl;
|
---|
999 | */
|
---|
1000 | }
|
---|
1001 |
|
---|
1002 | int main(int argc, const char* argv[])
|
---|
1003 | {
|
---|
1004 | Configuration conf(argv[0]);
|
---|
1005 | conf.SetPrintUsage(PrintUsage);
|
---|
1006 | SetupConfiguration(conf);
|
---|
1007 |
|
---|
1008 | po::variables_map vm;
|
---|
1009 | try
|
---|
1010 | {
|
---|
1011 | vm = conf.Parse(argc, argv);
|
---|
1012 | }
|
---|
1013 | #if BOOST_VERSION > 104000
|
---|
1014 | catch (po::multiple_occurrences &e)
|
---|
1015 | {
|
---|
1016 | cerr << "Program options invalid due to: " << e.what() << " of '" << e.get_option_name() << "'." << endl;
|
---|
1017 | return -1;
|
---|
1018 | }
|
---|
1019 | #endif
|
---|
1020 | catch (exception& e)
|
---|
1021 | {
|
---|
1022 | cerr << "Program options invalid due to: " << e.what() << endl;
|
---|
1023 | return -1;
|
---|
1024 | }
|
---|
1025 |
|
---|
1026 | if (conf.HasVersion() || conf.HasPrint())
|
---|
1027 | return -1;
|
---|
1028 |
|
---|
1029 | if (conf.HasHelp())
|
---|
1030 | {
|
---|
1031 | PrintHelp();
|
---|
1032 | return -1;
|
---|
1033 | }
|
---|
1034 |
|
---|
1035 | Dim::Setup(conf.Get<string>("dns"));
|
---|
1036 |
|
---|
1037 | //try
|
---|
1038 | {
|
---|
1039 | // No console access at all
|
---|
1040 | if (!conf.Has("console"))
|
---|
1041 | {
|
---|
1042 | if (conf.Get<bool>("no-dim"))
|
---|
1043 | return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
|
---|
1044 | else
|
---|
1045 | return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
|
---|
1046 | }
|
---|
1047 | // Cosole access w/ and w/o Dim
|
---|
1048 | if (conf.Get<bool>("no-dim"))
|
---|
1049 | {
|
---|
1050 | if (conf.Get<int>("console")==0)
|
---|
1051 | return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
|
---|
1052 | else
|
---|
1053 | return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
|
---|
1054 | }
|
---|
1055 | else
|
---|
1056 | {
|
---|
1057 | if (conf.Get<int>("console")==0)
|
---|
1058 | return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
|
---|
1059 | else
|
---|
1060 | return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
|
---|
1061 | }
|
---|
1062 | }
|
---|
1063 | /*catch (std::exception& e)
|
---|
1064 | {
|
---|
1065 | cerr << "Exception: " << e.what() << endl;
|
---|
1066 | return -1;
|
---|
1067 | }*/
|
---|
1068 |
|
---|
1069 | return 0;
|
---|
1070 | }
|
---|