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

Last change on this file since 19098 was 19064, checked in by tbretz, 6 years ago
Some minor changes to make it compatible with newer sets of headers.
File size: 114.3 KB
Line 
1#include <numeric> // std::accumulate
2
3#include <boost/regex.hpp>
4#include <boost/algorithm/string.hpp>
5
6#ifdef HAVE_SQL
7#include "Database.h"
8#endif
9
10#include "FACT.h"
11#include "Dim.h"
12#include "Event.h"
13#include "Shell.h"
14#include "StateMachineDim.h"
15#include "StateMachineAsio.h"
16#include "Connection.h"
17#include "LocalControl.h"
18#include "Configuration.h"
19#include "Timers.h"
20#include "Console.h"
21
22#include "HeadersDrive.h"
23
24#include "pal.h"
25#include "nova.h"
26
27namespace ba = boost::asio;
28namespace bs = boost::system;
29
30using namespace std;
31using namespace Drive;
32
33// ------------------------------------------------------------------------
34
35// The Nova classes are in degree. This is to be used in rad
36struct RaDec
37{
38 double ra; // [rad]
39 double dec; // [rad]
40 RaDec() : ra(0), dec(0) { }
41 RaDec(double _ra, double _dec) : ra(_ra), dec(_dec) { }
42};
43
44struct RaDecHa : RaDec
45{
46 double ha; // [rad]
47 RaDecHa() : ha(0) { }
48 RaDecHa(double _ra, double _dec, double _ha) : RaDec(_ra, _dec), ha(_ha) { }
49};
50
51struct Local
52{
53 double zd;
54 double az;
55
56 Local(double _zd=0, double _az=0) : zd(_zd), az(_az) { }
57};
58
59struct Velocity : Local
60{
61 Velocity(double _zd=0, double _az=0) : Local(_zd, _az) { }
62 Velocity operator/(double f) const { return Velocity(zd/f, az/f); }
63 Velocity operator*(double f) const { return Velocity(zd*f, az*f); }
64};
65
66struct Encoder : Local // [units: revolutions]
67{
68 Encoder(double _zd=0, double _az=0) : Local(_zd, _az) { }
69
70 Encoder &operator*=(double f) { zd*=f; az*=f; return *this; }
71 Encoder &operator-=(const Encoder &enc) { zd-=enc.zd; az-=enc.az; return *this; }
72 Encoder operator*(double f) const { return Encoder(zd*f, az*f); }
73 Velocity operator/(double t) const { return Velocity(zd/t, az/t); }
74 Encoder Abs() const { return Encoder(fabs(zd), fabs(az)); }
75};
76
77struct ZdAz : Local // [units: rad]
78{
79 ZdAz(double _zd=0, double _az=0) : Local(_zd, _az) { }
80 ZdAz operator*(const double &f) const { return ZdAz(zd*f, az*f); }
81};
82
83struct Acceleration : Local
84{
85 Acceleration(double _zd=0, double _az=0) : Local(_zd, _az) { }
86 bool operator>(const Acceleration &a) const
87 {
88 return zd>a.zd || az>a.az;
89 }
90};
91
92Encoder operator-(const Encoder &a, const Encoder &b)
93{
94 return Encoder(a.zd-b.zd, a.az-b.az);
95}
96Velocity operator-(const Encoder &a, const Velocity &b)
97{
98 return Velocity(a.zd-b.zd, a.az-b.az);
99}
100Velocity operator-(const Velocity &a, const Velocity &b)
101{
102 return Velocity(a.zd-b.zd, a.az-b.az);
103}
104Encoder operator/(const Encoder &a, const Encoder &b)
105{
106 return Encoder(a.zd/b.zd, a.az/b.az);
107}
108
109struct Weather
110{
111 float hum;
112 float temp;
113 float press;
114 Time time;
115};
116
117struct Source
118{
119 Source() : ra(0), dec(0), mag(0), offset(0)
120 {
121 angles[0] = -90;
122 angles[1] = 90;
123 }
124
125 string name;
126 double ra; // [h]
127 double dec; // [deg]
128 double mag;
129
130 double offset;
131 array<double, 2> angles;
132};
133
134enum Planets_t
135{
136 kENone = -1,
137 kESun = 0,
138 kEMercury = 1,
139 kEVenus = 2,
140 kEMoon = 3, // earth moon barycentre
141 kEMars = 4,
142 kEJupiter = 5,
143 kESaturn = 6,
144 kEUranus = 7,
145 kENeptune = 8,
146 kEPluto = 9,
147};
148
149// ------------------------------------------------------------------------
150
151struct PointingSetup
152{
153 Source source; // Informations about source to track [h/deg]
154 Planets_t planet; // Id of the planet if tracking a planet
155 double start; // Starting time of wobble observation [mjd]
156 double orbit_period; // Time for one revolution (0:off) [day]
157 double wobble_offset; // Distance of wobble position [rad]
158 double wobble_angle; // Starting phi angle of wobble observation [rad]
159
160 PointingSetup(Planets_t p=kENone) : planet(p), start(Time::none), orbit_period(0) { }
161};
162
163struct PointingData
164{
165 // Pointing direction of the opticl axis of the telescope
166 RaDec source; // Informations about source to track [rad/rad]
167 RaDec pointing; // Catalog coordinates (J2000, FK5) [rad/rad] pointing position
168 RaDecHa apparent; // Apparent position on the sky [rad/rad]
169 ZdAz sky; // Apparent position on the sky [rad/rad]
170 Encoder mount; // Encoder position corresponding to 'sky' [deg/deg]
171 double mjd;
172};
173
174class PointingModel
175{
176private:
177 double fIe; // [rad] Index Error in Elevation
178 double fIa; // [rad] Index Error in Azimuth
179 double fFlop; // [rad] Vertical Sag
180 double fNpae; // [rad] Az-El Nonperpendicularity
181 double fCa; // [rad] Left-Right Collimation Error
182 double fAn; // [rad] Azimuth Axis Misalignment (N-S, 1st order)
183 double fAw; // [rad] Azimuth Axis Misalignment (E-W, 1st order)
184 double fAn2; // [rad] Azimuth Axis Misalignment (N-S, 2nd order)
185 double fAw2; // [rad] Azimuth Axis Misalignment (E-W, 2nd order)
186 double fTf; // [rad] Tube fluxture (sin)
187 double fTx; // [rad] Tube fluxture (tan)
188 double fNrx; // [rad] Nasmyth rotator displacement, horizontal
189 double fNry; // [rad] Nasmyth rotator displacement, vertical
190 double fCrx; // [rad] Alt/Az Coude Displacement (N-S)
191 double fCry; // [rad] Alt/Az Coude Displacement (E-W)
192 double fEces; // [rad] Elevation Centering Error (sin)
193 double fAces; // [rad] Azimuth Centering Error (sin)
194 double fEcec; // [rad] Elevation Centering Error (cos)
195 double fAcec; // [rad] Azimuth Centering Error (cos)
196
197public:
198
199 void Load(const string &name)
200 {
201 /*
202 ! MMT 1987 July 8
203 ! T 36 7.3622 41.448 -0.0481
204 ! IA -37.5465 20.80602
205 ! IE -13.9180 1.25217
206 ! NPAE +7.0751 26.44763
207 ! CA -6.9149 32.05358
208 ! AN +0.5053 1.40956
209 ! AW -2.2016 1.37480
210 ! END
211 */
212
213 ifstream fin(name);
214 if (!fin)
215 throw runtime_error("Cannot open file "+name+": "+strerror(errno));
216
217 map<string,double> coeff;
218
219 string buf;
220 while (getline(fin, buf))
221 {
222 buf = Tools::Trim(buf);
223
224 vector<string> vec;
225 boost::split(vec, buf, boost::is_any_of(" "), boost::token_compress_on);
226 if (vec.size()<2)
227 continue;
228
229 coeff[vec[0]] = atof(vec[1].c_str()) * M_PI/180;
230 }
231
232 fIe = coeff["IE"]; // [rad] Index Error in Elevation
233 fIa = coeff["IA"]; // [rad] Index Error in Azimuth
234 fFlop = coeff["FLOP"]; // [rad] Vertical Sag
235 fNpae = coeff["NPAE"]; // [rad] Az-El Nonperpendicularity
236 fCa = coeff["CA"]; // [rad] Left-Right Collimation Error
237 fAn = coeff["AN"]; // [rad] Azimuth Axis Misalignment (N-S, 1st order)
238 fAw = coeff["AW"]; // [rad] Azimuth Axis Misalignment (E-W, 1st order)
239 fAn2 = coeff["AN2"]; // [rad] Azimuth Axis Misalignment (N-S, 2nd order)
240 fAw2 = coeff["AW2"]; // [rad] Azimuth Axis Misalignment (E-W, 2nd order)
241 fTf = coeff["TF"]; // [rad] Tube fluxture (sin)
242 fTx = coeff["TX"]; // [rad] Tube fluxture (tan)
243 fNrx = coeff["NRX"]; // [rad] Nasmyth rotator displacement, horizontal
244 fNry = coeff["NRY"]; // [rad] Nasmyth rotator displacement, vertical
245 fCrx = coeff["CRX"]; // [rad] Alt/Az Coude Displacement (N-S)
246 fCry = coeff["CRY"]; // [rad] Alt/Az Coude Displacement (E-W)
247 fEces = coeff["ECES"]; // [rad] Elevation Centering Error (sin)
248 fAces = coeff["ACES"]; // [rad] Azimuth Centering Error (sin)
249 fEcec = coeff["ECEC"]; // [rad] Elevation Centering Error (cos)
250 fAcec = coeff["ACEC"]; // [rad] Azimuth Centering Error (cos)
251 }
252
253 void print(ostream &out)
254 {
255 out << "IE " << Tools::Form("%10.5f", 180/M_PI*fIe) << "\u00b0 # Index Error in Elevation\n";
256 out << "IA " << Tools::Form("%10.5f", 180/M_PI*fIa) << "\u00b0 # Index Error in Azimuth\n";
257 out << "FLOP " << Tools::Form("%10.5f", 180/M_PI*fFlop) << "\u00b0 # Vertical Sag\n";
258 out << "NPAE " << Tools::Form("%10.5f", 180/M_PI*fNpae) << "\u00b0 # Az-El Nonperpendicularity\n";
259 out << "CA " << Tools::Form("%10.5f", 180/M_PI*fCa) << "\u00b0 # Left-Right Collimation Error\n";
260 out << "AN " << Tools::Form("%10.5f", 180/M_PI*fAn) << "\u00b0 # Azimuth Axis Misalignment (N-S, 1st order)\n";
261 out << "AW " << Tools::Form("%10.5f", 180/M_PI*fAw) << "\u00b0 # Azimuth Axis Misalignment (E-W, 1st order)\n";
262 out << "AN2 " << Tools::Form("%10.5f", 180/M_PI*fAn2) << "\u00b0 # Azimuth Axis Misalignment (N-S, 2nd order)\n";
263 out << "AW2 " << Tools::Form("%10.5f", 180/M_PI*fAw2) << "\u00b0 # Azimuth Axis Misalignment (E-W, 2nd order)\n";
264 out << "TF " << Tools::Form("%10.5f", 180/M_PI*fTf) << "\u00b0 # Tube fluxture (sin)\n";
265 out << "TX " << Tools::Form("%10.5f", 180/M_PI*fTx) << "\u00b0 # Tube fluxture (tan)\n";
266 out << "NRX " << Tools::Form("%10.5f", 180/M_PI*fNrx) << "\u00b0 # Nasmyth rotator displacement, horizontal\n";
267 out << "NRY " << Tools::Form("%10.5f", 180/M_PI*fNry) << "\u00b0 # Nasmyth rotator displacement, vertical\n";
268 out << "CRX " << Tools::Form("%10.5f", 180/M_PI*fCrx) << "\u00b0 # Alt/Az Coude Displacement (N-S)\n";
269 out << "CRY " << Tools::Form("%10.5f", 180/M_PI*fCry) << "\u00b0 # Alt/Az Coude Displacement (E-W)\n";
270 out << "ECES " << Tools::Form("%10.5f", 180/M_PI*fEces) << "\u00b0 # Elevation Centering Error (sin)\n";
271 out << "ACES " << Tools::Form("%10.5f", 180/M_PI*fAces) << "\u00b0 # Azimuth Centering Error (sin)\n";
272 out << "ECEC " << Tools::Form("%10.5f", 180/M_PI*fEcec) << "\u00b0 # Elevation Centering Error (cos)\n";
273 out << "ACEC " << Tools::Form("%10.5f", 180/M_PI*fAcec) << "\u00b0 # Azimuth Centering Error (cos)" << endl;
274 }
275
276 struct AltAz
277 {
278 double alt;
279 double az;
280
281 AltAz(double _alt, double _az) : alt(_alt), az(_az) { }
282 AltAz(const ZdAz &za) : alt(M_PI/2-za.zd), az(za.az) { }
283
284 AltAz &operator+=(const AltAz &aa) { alt += aa.alt; az+=aa.az; return *this; }
285 AltAz &operator-=(const AltAz &aa) { alt -= aa.alt; az-=aa.az; return *this; }
286 };
287
288 double Sign(double val, double alt) const
289 {
290 // Some pointing corrections are defined as Delta ZA, which
291 // is (P. Wallace) defined [0,90]deg while Alt is defined
292 // [0,180]deg
293 return (M_PI/2-alt < 0 ? -val : val);
294 }
295
296 Encoder SkyToMount(AltAz p)
297 {
298 const AltAz CRX(-fCrx*sin(p.az-p.alt), fCrx*cos(p.az-p.alt)/cos(p.alt));
299 const AltAz CRY(-fCry*cos(p.az-p.alt), -fCry*sin(p.az-p.alt)/cos(p.alt));
300 p += CRX;
301 p += CRY;
302
303 const AltAz NRX(fNrx*sin(p.alt), -fNrx);
304 const AltAz NRY(fNry*cos(p.alt), -fNry*tan(p.alt));
305 p += NRX;
306 p += NRY;
307
308 const AltAz CES(-fEces*sin(p.alt), -fAces*sin(p.az));
309 const AltAz CEC(-fEcec*cos(p.alt), -fAcec*cos(p.az));
310 p += CES;
311 p += CEC;
312
313 const AltAz TX(Sign(fTx/tan(p.alt), p.alt), 0);
314 const AltAz TF(Sign(fTf*cos(p.alt), p.alt), 0);
315 //p += TX;
316 p += TF;
317
318 const AltAz CA(0, -fCa/cos(p.alt));
319 p += CA;
320
321 const AltAz NPAE(0, -fNpae*tan(p.alt));
322 p += NPAE;
323
324 const AltAz AW2( fAw2*sin(p.az*2), -fAw2*cos(p.az*2)*tan(p.alt));
325 const AltAz AN2(-fAn2*cos(p.az*2), -fAn2*sin(p.az*2)*tan(p.alt));
326 const AltAz AW1( fAw *sin(p.az), -fAw *cos(p.az) *tan(p.alt));
327 const AltAz AN1(-fAn *cos(p.az), -fAn *sin(p.az) *tan(p.alt));
328 p += AW2;
329 p += AN2;
330 p += AW1;
331 p += AN1;
332
333 const AltAz FLOP(Sign(fFlop, p.alt), 0);
334 p += FLOP;
335
336 const AltAz I(fIe, fIa);
337 p += I;
338
339 return Encoder(90 - p.alt*180/M_PI, p.az *180/M_PI);
340 }
341
342 ZdAz MountToSky(const Encoder &mnt) const
343 {
344 AltAz p(M_PI/2-mnt.zd*M_PI/180, mnt.az*M_PI/180);
345
346 const AltAz I(fIe, fIa);
347 p -= I;
348
349 const AltAz FLOP(Sign(fFlop, p.alt), 0);
350 p -= FLOP;
351
352 const AltAz AW1( fAw *sin(p.az), -fAw *cos(p.az) *tan(p.alt));
353 const AltAz AN1(-fAn *cos(p.az), -fAn *sin(p.az) *tan(p.alt));
354 const AltAz AW2( fAw2*sin(p.az*2), -fAw2*cos(p.az*2)*tan(p.alt));
355 const AltAz AN2(-fAn2*cos(p.az*2), -fAn2*sin(p.az*2)*tan(p.alt));
356 p -= AW1;
357 p -= AN1;
358 p -= AW2;
359 p -= AN2;
360
361 const AltAz NPAE(0, -fNpae*tan(p.alt));
362 p -= NPAE;
363
364 const AltAz CA(0, -fCa/cos(p.alt));
365 p -= CA;
366
367 const AltAz TF(Sign(fTf*cos(p.alt), p.alt), 0);
368 const AltAz TX(Sign(fTx/tan(p.alt), p.alt), 0);
369 p -= TF;
370 //p -= TX;
371
372 const AltAz CEC(-fEcec*cos(p.alt), -fAcec*cos(p.az));
373 const AltAz CES(-fEces*sin(p.alt), -fAces*sin(p.az));
374 p -= CEC;
375 p -= CES;
376
377 const AltAz NRY(fNry*cos(p.alt), -fNry*tan(p.alt));
378 const AltAz NRX(fNrx*sin(p.alt), -fNrx);
379 p -= NRY;
380 p -= NRX;
381
382 const AltAz CRY(-fCry*cos(p.az-p.alt), -fCry*sin(p.az-p.alt)/cos(p.alt));
383 const AltAz CRX(-fCrx*sin(p.az-p.alt), fCrx*cos(p.az-p.alt)/cos(p.alt));
384 p -= CRY;
385 p -= CRX;
386
387 return ZdAz(M_PI/2-p.alt, p.az);
388 }
389
390 PointingData CalcPointingPos(const PointingSetup &setup, double _mjd, const Weather &weather, uint16_t timeout, bool tpoint=false)
391 {
392 PointingData out;
393 out.mjd = _mjd;
394
395 const double elong = Nova::kORM.lng * M_PI/180;
396 const double lat = Nova::kORM.lat * M_PI/180;
397 const double height = 2200;
398
399 const bool valid = weather.time+boost::posix_time::seconds(timeout) > Time();
400
401 const double temp = valid ? weather.temp : 10;
402 const double hum = valid ? weather.hum : 0.25;
403 const double press = valid ? weather.press : 780;
404
405 const double dtt = palDtt(_mjd); // 32.184 + 35
406
407 const double tdb = _mjd + dtt/3600/24;
408 const double dut = 0;
409
410 // prepare calculation: Mean Place to geocentric apperent
411 // (UTC would also do, except for the moon?)
412 double fAmprms[21];
413 palMappa(2000.0, tdb, fAmprms); // Epoche, TDB
414
415 // prepare: Apperent to observed place
416 double fAoprms[14];
417 palAoppa(_mjd, dut, // mjd, Delta UT=UT1-UTC
418 elong, lat, height, // long, lat, height
419 0, 0, // polar motion x, y-coordinate (radians)
420 273.155+temp, press, hum, // temp, pressure, humidity
421 0.40, 0.0065, // wavelength, tropo lapse rate
422 fAoprms);
423
424 out.source.ra = setup.source.ra * M_PI/ 12;
425 out.source.dec = setup.source.dec * M_PI/180;
426
427 if (setup.planet!=kENone)
428 {
429 // coordinates of planet: topocentric, equatorial, J2000
430 // One can use TT instead of TDB for all planets (except the moon?)
431 double ra, dec, diam;
432 palRdplan(tdb, setup.planet, elong, lat, &ra, &dec, &diam);
433
434 // ---- apparent to mean ----
435 palAmpqk(ra, dec, fAmprms, &out.source.ra, &out.source.dec);
436 }
437
438 if (setup.wobble_offset<=0 || tpoint)
439 {
440 out.pointing.dec = out.source.dec;
441 out.pointing.ra = out.source.ra;
442 }
443 else
444 {
445 const double dphi =
446 setup.orbit_period==0 ? 0 : 2*M_PI*(_mjd-setup.start)/setup.orbit_period;
447
448 const double phi = setup.wobble_angle + dphi;
449
450 const double cosdir = cos(phi);
451 const double sindir = sin(phi);
452 const double cosoff = cos(setup.wobble_offset);
453 const double sinoff = sin(setup.wobble_offset);
454 const double cosdec = cos(out.source.dec);
455 const double sindec = sin(out.source.dec);
456
457 const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
458
459 const double costheta = sintheta>1 ? 0 : sqrt(1 - sintheta*sintheta);
460
461 const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
462 const double sindeltara = sindir*sinoff/costheta;
463
464 out.pointing.dec = asin(sintheta);
465 out.pointing.ra = atan2(sindeltara, cosdeltara) + out.source.ra;
466 }
467
468 // ---- Mean to apparent ----
469 double r=0, d=0;
470 palMapqkz(out.pointing.ra, out.pointing.dec, fAmprms, &r, &d);
471
472 //
473 // Doesn't work - don't know why
474 //
475 // slaMapqk (radec.Ra(), radec.Dec(), rdpm.Ra(), rdpm.Dec(),
476 // 0, 0, (double*)fAmprms, &r, &d);
477 //
478
479 // -- apparent to observed --
480 palAopqk(r, d, fAoprms,
481 &out.sky.az, // observed azimuth (radians: N=0,E=90) [-pi, pi]
482 &out.sky.zd, // observed zenith distance (radians) [-pi/2, pi/2]
483 &out.apparent.ha, // observed hour angle (radians)
484 &out.apparent.dec, // observed declination (radians)
485 &out.apparent.ra); // observed right ascension (radians)
486
487 // ----- fix ambiguity -----
488 if (out.sky.zd<0)
489 {
490 out.sky.zd = -out.sky.zd;
491 out.sky.az += out.sky.az<0 ? M_PI : -M_PI;
492 }
493
494 // Star culminating behind zenith and Az between ~90 and ~180deg
495 if (out.source.dec<lat && out.sky.az>0)
496 out.sky.az -= 2*M_PI;
497
498 out.mount = SkyToMount(out.sky);
499
500 return out;
501 }
502};
503
504// ------------------------------------------------------------------------
505
506
507class ConnectionDrive : public Connection
508{
509 uint16_t fVerbosity;
510
511public:
512 virtual void UpdatePointing(const Time &, const array<double, 2> &)
513 {
514 }
515
516 virtual void UpdateTracking(const Time &, const array<double, 12> &)
517 {
518 }
519
520 virtual void UpdateStatus(const Time &, const array<uint8_t, 3> &)
521 {
522 }
523
524 virtual void UpdateTPoint(const Time &, const DimTPoint &, const string &)
525 {
526 }
527
528 virtual void UpdateSource(const Time &, const string &, bool)
529 {
530 }
531 virtual void UpdateSource(const Time &,const array<double, 5> &, const string& = "")
532 {
533 }
534
535private:
536 enum NodeId_t
537 {
538 kNodeAz = 1,
539 kNodeZd = 3
540 };
541
542 enum
543 {
544 kRxNodeguard = 0xe,
545 kRxPdo1 = 3,
546 kRxPdo2 = 5,
547 kRxPdo3 = 7,
548 kRxPdo4 = 9,
549 kRxSdo = 0xb,
550 kRxSdo4 = 0x40|0x3,
551 kRxSdo2 = 0x40|0xb,
552 kRxSdo1 = 0x40|0xf,
553 kRxSdoOk = 0x60,
554 kRxSdoErr = 0x80,
555
556 kTxSdo = 0x40,
557 kTxSdo4 = 0x20|0x3,
558 kTxSdo2 = 0x20|0xb,
559 kTxSdo1 = 0x20|0xf,
560 };
561
562 void SendCanFrame(uint16_t cobid,
563 uint8_t m0=0, uint8_t m1=0, uint8_t m2=0, uint8_t m3=0,
564 uint8_t m4=0, uint8_t m5=0, uint8_t m6=0, uint8_t m7=0)
565 {
566 const uint16_t desc = (cobid<<5) | 8;
567
568 vector<uint8_t> data(11);
569 data[0] = 10;
570 data[1] = desc>>8;
571 data[2] = desc&0xff;
572
573 const uint8_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
574 memcpy(data.data()+3, msg, 8);
575
576 PostMessage(data);
577 }
578
579 enum Index_t
580 {
581 kReqArmed = 0x1000,
582 kReqPDO = 0x1001,
583 kReqErrStat = 0x1003,
584 kReqSoftVer = 0x100a,
585 kReqKeepAlive = 0x100b,
586 kReqVel = 0x2002,
587 kReqVelRes = 0x6002,
588 kReqVelMax = 0x6003,
589 kReqPos = 0x6004,
590 kReqPosRes = 0x6501,
591
592 kSetArmed = 0x1000,
593 kSetPointVel = 0x2002,
594 kSetAcc = 0x2003,
595 kSetRpmMode = 0x3006,
596 kSetTrackVel = 0x3007,
597 kSetLedVoltage = 0x4000,
598 kSetPosition = 0x6004,
599 };
600
601 static uint32_t String(uint8_t b0=0, uint8_t b1=0, uint8_t b2=0, uint8_t b3=0)
602 {
603 return uint32_t(b0)<<24 | uint32_t(b1)<<16 | uint32_t(b2)<<8 | uint32_t(b3);
604 }
605
606 uint32_t fVelRes[2];
607 uint32_t fVelMax[2];
608 uint32_t fPosRes[2];
609
610 uint32_t fErrCode[2];
611
612 void HandleSdo(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx,
613 const uint32_t &val, const Time &tv)
614 {
615 if (fVerbosity>0)
616 {
617 ostringstream out;
618 out << hex;
619 out << "SDO[" << int(node) << "] " << idx << "/" << int(subidx) << ": " << val << dec;
620 Out() << out.str() << endl;
621 }
622
623 switch (idx)
624 {
625 case kReqArmed:
626 //fArmed = val==1;
627 return;
628
629 case kReqErrStat:
630 {
631 fErrCode[node/2] = (val>>8);
632 LogErrorCode(node);
633 }
634 return;
635
636 case kReqSoftVer:
637 //fSoftVersion = val;
638 return;
639
640 case kReqKeepAlive:
641 // Do not display, this is used for CheckConnection
642 fIsInitialized[node/2] = true;
643 return;
644
645 case kReqVel:
646 //fVel = val;
647 return;
648
649 case kReqPos:
650 switch (subidx)
651 {
652 case 0:
653 fPdoPos1[node/2] = val;
654 fPdoTime1[node/2] = tv;
655 fHasChangedPos1[node/2] = true;
656 return;
657 case 1:
658 fPdoPos2[node/2] = val;
659 fPdoTime2[node/2] = tv;
660 fHasChangedPos2[node/2] = true;
661 return;
662 }
663 break;
664
665 case kReqVelRes:
666 fVelRes[node/2] = val;
667 return;
668
669 case kReqVelMax:
670 fVelMax[node/2] = val;
671 return;
672
673 case kReqPosRes:
674 fPosRes[node/2] = val;
675 return;
676 }
677
678 ostringstream str;
679 str << "HandleSDO: Idx=0x"<< hex << idx << "/" << (int)subidx;
680 str << ", val=0x" << val;
681 Warn(str);
682 }
683
684 void HandleSdoOk(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx,
685 const Time &)
686 {
687 ostringstream out;
688 out << hex;
689 out << "SDO-OK[" << int(node) << "] " << idx << "/" << int(subidx) << dec << " ";
690
691 switch (idx)
692 {
693 case kSetArmed:
694 out << "(Armed state set)";
695 break;
696 /*
697 case 0x1001:
698 Out() << inf2 << "- " << GetNodeName() << ": PDOs requested." << endl;
699 return;
700 */
701 case kSetPointVel:
702 out << "(Pointing velocity set)";
703 break;
704
705 case kSetAcc:
706 out << "(Acceleration set)";
707 break;
708
709 case kSetRpmMode:
710 out << "(RPM mode set)";
711 break;
712
713 case kSetLedVoltage:
714 out << "(LED Voltage set)";
715 Info(out);
716 return;
717 /*
718 case 0x3007:
719 //Out() << inf2 << "- Velocity set (" << GetNodeName() << ")" << endl;
720 return;
721
722 case 0x4000:
723 HandleNodeguard(tv);
724 return;
725
726 case 0x6000:
727 Out() << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
728 return;
729
730 case 0x6002:
731 Out() << inf2 << "- " << GetNodeName() << ": Velocity resolution set." << endl;
732 return;
733 */
734 case kSetPosition:
735 out << "(Absolute positioning started)";
736 break;
737 /*
738 case 0x6005:
739 Out() << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
740 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
741 return;*/
742 }
743 /*
744 Out() << warn << setfill('0') << "WARNING - Nodedrv::HandleSDOOK: ";
745 Out() << "Node #" << dec << (int)fId << ": Sdo=" << hex << idx << "/" << (int)subidx << " set.";
746 Out() << endl;
747 */
748
749 if (fVerbosity>1)
750 Out() << out.str() << endl;
751 }
752
753 void HandleSdoError(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx,
754 const Time &)
755 {
756 ostringstream out;
757 out << hex;
758 out << "SDO-ERR[" << int(node) << "] " << idx << "/" << int(subidx) << dec;
759 Out() << out.str() << endl;
760 }
761
762
763 int32_t fPdoPos1[2];
764 int32_t fPdoPos2[2];
765
766 Time fPdoTime1[2];
767public:
768 Time fPdoTime2[2];
769private:
770 bool fHasChangedPos1[2];
771 bool fHasChangedPos2[2];
772
773 void HandlePdo1(const uint8_t &node, const uint8_t *data, const Time &tv)
774 {
775 const uint32_t pos1 = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0];
776 const uint32_t pos2 = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4];
777
778 if (fVerbosity>2)
779 Out() << Time().GetAsStr("%M:%S.%f") << " PDO1[" << (int)node << "] " << 360.*int32_t(pos1)/fPosRes[node/2] << " " << 360.*int32_t(pos2)/fPosRes[node/2] << endl;
780
781 // Once every few milliseconds!
782
783 fPdoPos1[node/2] = pos1;
784 fPdoTime1[node/2] = tv;
785 fHasChangedPos1[node/2] = true;
786
787 fPdoPos2[node/2] = pos2;
788 fPdoTime2[node/2] = tv;
789 fHasChangedPos2[node/2] = true;
790 }
791
792 uint8_t fStatusAxis[2];
793 uint8_t fStatusSys;
794
795 enum {
796 kUpsAlarm = 0x01, // UPS Alarm (FACT only)
797 kUpsBattery = 0x02, // UPS on battery (FACT only)
798 kUpsCharging = 0x04, // UPS charging (FACT only)
799 kEmergencyOk = 0x10, // Emergency button released
800 kOvervoltOk = 0x20, // Overvoltage protection ok
801 kManualMode = 0x40, // Manual mode button pressed
802
803 kAxisBb = 0x01, // IndraDrive reports Bb (Regler betriebsbereit)
804 kAxisMoving = 0x02, // SPS reports
805 kAxisRpmMode = 0x04, // SPS reports
806 kAxisRf = 0x20, // IndraDrive reports Rf (Regler freigegeben)
807 kAxisHasPower = 0x80 // IndraDrive reports axis power on
808 };
809
810 //std::function<void(const Time &, const array<uint8_t, 3>&)> fUpdateStatus;
811
812 void HandlePdo3(const uint8_t &node, const uint8_t *data, const Time &tv)
813 {
814 /*
815 TX1M_STATUS.0 := 1;
816 TX1M_STATUS.1 := ((NOT X_in_Standstill OR NOT X_in_AntriebHalt) AND (NOT X_PC_VStart AND NOT X_in_Pos)) OR X_PC_AnnounceStartMovement;
817 TX1M_STATUS.2 := X_PC_VStart;
818 TX1M_STATUS.6 := NOT X_ist_freigegeben;
819
820 TX3M_STATUS.0 := X_ist_betriebsbereit;
821 TX3M_STATUS.1 := 1;
822 TX3M_STATUS.2 := Not_Aus_IO;
823 TX3M_STATUS.3 := UeberspannungsSchutz_OK;
824 TX3M_STATUS.4 := FB_soll_drehen_links OR FB_soll_drehen_rechts OR FB_soll_schwenk_auf OR FB_soll_schwenk_ab;
825 TX3M_STATUS.5 := X_ist_freigegeben;
826 TX3M_STATUS.6 := 1;
827 TX3M_STATUS.7 := LeistungEinAz;
828
829 TX3M_STATUS.8 := NOT UPS_ALARM;
830 TX3M_STATUS.9 := UPS_BattMode;
831 TX3M_STATUS.10 := UPS_Charging;
832 */
833
834 const uint8_t sys = ((data[0] & 0x1c)<<2) | (data[1]);
835 if (fStatusSys!=sys)
836 {
837 fStatusSys = sys;
838
839 const bool alarm = sys&kUpsAlarm; // 01 TX3M.8 100
840 const bool batt = sys&kUpsBattery; // 02 TX3M.9 200
841 const bool charge = sys&kUpsCharging; // 04 TX3M.10 400
842 const bool emcy = sys&kEmergencyOk; // 10 TX3M.2 04
843 const bool vltg = sys&kOvervoltOk; // 20 TX3M.3 08
844 const bool mode = sys&kManualMode; // 40 TX3M.4 10
845
846 ostringstream out;
847 if (alarm) out << " UPS-PowerLoss";
848 if (batt) out << " UPS-OnBattery";
849 if (charge) out << " UPS-Charging";
850 if (emcy) out << " EmcyOk";
851 if (vltg) out << " OvervoltOk";
852 if (mode) out << " ManualMove";
853
854 Info("New system status["+string(node==kNodeAz?"Az":"Zd")+"]:"+out.str());
855 if (fVerbosity>1)
856 Out() << "PDO3[" << (int)node << "] StatusSys=" << hex << (int)fStatusSys << dec << endl;
857 }
858
859 const uint8_t axis = (data[0]&0xa1) | (data[3]&0x06);
860 if (fStatusAxis[node/2]!=axis)
861 {
862 fStatusAxis[node/2] = axis;
863
864 const bool ready = axis&kAxisBb; // 01
865 const bool move = axis&kAxisMoving; // 02
866 const bool rpm = axis&kAxisRpmMode; // 04
867 const bool rf = axis&kAxisRf; // 20
868 const bool power = axis&kAxisHasPower; // 80
869
870 ostringstream out;
871 if (ready) out << " DKC-Ready";
872 if (move) out << " Moving";
873 if (rpm) out << " RpmMode";
874 if (rf) out << " RF";
875 if (power) out << " PowerOn";
876
877 Info("New axis status["+string(node==kNodeAz?"Az":"Zd")+"]:"+out.str());
878 if (fVerbosity>1)
879 Out() << "PDO3[" << (int)node << "] StatusAxis=" << hex << (int)fStatusAxis[node/2] << dec << endl;
880 }
881
882 array<uint8_t, 3> arr = {{ fStatusAxis[0], fStatusAxis[1], fStatusSys }};
883 UpdateStatus(tv, arr);
884 }
885
886 string ErrCodeToString(uint32_t code) const
887 {
888 switch (code)
889 {
890 case 0: return "offline";
891 case 0xa000: case 0xa0000:
892 case 0xa001: case 0xa0001:
893 case 0xa002: case 0xa0002:
894 case 0xa003: case 0xa0003: return "Communication phase "+to_string(code&0xf);
895 case 0xa010: case 0xa0010: return "Drive HALT";
896 case 0xa012: case 0xa0012: return "Control and power section ready for operation";
897 case 0xa013: case 0xa0013: return "Ready for power on";
898 case 0xa100: case 0xa0100: return "Drive in Torque mode";
899 case 0xa101: case 0xa0101: return "Drive in Velocity mode";
900 case 0xa102: case 0xa0102: return "Position control mode with encoder 1";
901 case 0xa103: case 0xa0103: return "Position control mode with encoder 2";
902 case 0xa104: case 0xa0104: return "Position control mode with encoder 1, lagless";
903 case 0xa105: case 0xa0105: return "Position control mode with encoder 2, lagless";
904 case 0xa106: case 0xa0106: return "Drive controlled interpolated positioning with encoder 1";
905 case 0xa107: case 0xa0107: return "Drive controlled interpolated positioning with encoder 2";
906 case 0xa108: case 0xa0108: return "Drive controlled interpolated positioning with encoder 1, lagless";
907 case 0xa109: case 0xa0109: return "Drive controlled interpolated positioning with encoder 2, lagless";
908 //case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1";
909 //case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2";
910 //case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1";
911 //case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2";
912 case 0xa150: case 0xa0150: return "Drive controlled positioning with encoder 1";
913 case 0xa151: case 0xa0151: return "Drive controlled positioning with encoder 1, lagless";
914 case 0xa152: case 0xa0152: return "Drive controlled positioning with encoder 2";
915 case 0xa153: case 0xa0153: return "Drive controlled positioning with encoder 2, lagless";
916 case 0xa208: return "Jog mode positive";
917 case 0xa218: return "Jog mode negative";
918 case 0xa400: case 0xa4000: return "Automatic drive check and adjustment";
919 case 0xa401: case 0xa4001: return "Drive decelerating to standstill";
920 case 0xa800: case 0xa0800: return "Unknown operation mode";
921 case 0xc217: return "Motor encoder reading error";
922 case 0xc218: return "Shaft encoder reading error";
923 case 0xc220: return "Motor encoder initialization error";
924 case 0xc221: return "Shaft encoder initialization error";
925 case 0xc300: return "Command: set absolute measure";
926 case 0xc400: case 0xc0400: return "Switching to parameter mode";
927 case 0xc401: case 0xc0401: return "Drive active, switching mode not allowed";
928 case 0xc500: case 0xc0500: return "Error reset";
929 case 0xc600: case 0xc0600: return "Drive controlled homing procedure";
930 case 0xe225: return "Motor overload";
931 case 0xe249: case 0xe2049: return "Positioning command velocity exceeds limit bipolar";
932 case 0xe250: return "Drive overtemp warning";
933 case 0xe251: return "Motor overtemp warning";
934 case 0xe252: return "Bleeder overtemp warning";
935 case 0xe257: return "Continous current limit active";
936 case 0xe2819: return "Main power failure";
937 case 0xe259: return "Command velocity limit active";
938 case 0xe8260: return "Torque limit active";
939 case 0xe264: return "Target position out of numerical range";
940 case 0xe829: case 0xe8029: return "Positive position limit exceeded";
941 case 0xe830: case 0xe8030: return "Negative position limit exceeded";
942 case 0xe831: return "Position limit reached during jog";
943 case 0xe834: return "Emergency-Stop";
944 case 0xe842: return "Both end-switches activated";
945 case 0xe843: return "Positive end-switch activated";
946 case 0xe844: return "Negative end-switch activated";
947 case 0xf218: case 0xf2018: return "Amplifier overtemp shutdown";
948 case 0xf219: case 0xf2019: return "Motor overtemp shutdown";
949 case 0xf220: return "Bleeder overload shutdown";
950 case 0xf221: case 0xf2021: return "Motor temperature surveillance defective";
951 case 0xf2022: return "Unit temperature surveillance defective";
952 case 0xf224: return "Maximum breaking time exceeded";
953 case 0xf2025: return "Drive not ready for power on";
954 case 0xf228: case 0xf2028: return "Excessive control deviation";
955 case 0xf250: return "Overflow of target position preset memory";
956 case 0xf257: case 0xf2057: return "Command position out of range";
957 case 0xf269: return "Error during release of the motor holding brake";
958 case 0xf276: return "Absolute encoder moved out of monitoring window";
959 case 0xf2074: return "Absolute encoder 1 moved out of monitoring window";
960 case 0xf2075: return "Absolute encoder 2 moved out of monitoring window";
961 case 0xf2174: return "Lost reference of motor encoder";
962 case 0xf409: case 0xf4009: return "Bus error on Profibus interface";
963 case 0xf434: return "Emergency-Stop";
964 case 0xf629: return "Positive position limit exceeded";
965 case 0xf630: return "Negative position limit exceeded";
966 case 0xf634: return "Emergency-Stop";
967 case 0xf643: return "Positive end-switch activated";
968 case 0xf644: return "Negative end-switch activated";
969 case 0xf8069: return "15V DC error";
970 case 0xf870: case 0xf8070: return "24V DC error";
971 case 0xf878: case 0xf8078: return "Velocity loop error";
972 case 0xf8079: return "Velocity limit exceeded";
973 case 0xf2026: return "Undervoltage in power section";
974 }
975 return "unknown";
976 }
977
978 void LogErrorCode(uint32_t node)
979 {
980 const uint8_t typ = fErrCode[node/2]>>16;
981
982 ostringstream out;
983 out << "IndraDrive ";
984 out << (node==kNodeAz?"Az":"Zd");
985 out << " [" << hex << fErrCode[node/2];
986 out << "]: ";
987 out << ErrCodeToString(fErrCode[node/2]);
988 out << (typ==0xf || typ==0xe ? "!" : ".");
989
990 switch (typ)
991 {
992 case 0xf: Error(out); break;
993 case 0xe: Warn(out); break;
994 case 0xa: Info(out); break;
995 case 0x0:
996 case 0xc:
997 case 0xd: Message(out); break;
998 default: Fatal(out); break;
999 }
1000 }
1001
1002 void HandlePdo2(const uint8_t &node, const uint8_t *data, const Time &)
1003 {
1004 fErrCode[node/2] = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
1005
1006 if (fVerbosity>0)
1007 Out() << "PDO2[" << int(node) << "] err=" << hex << fErrCode[node/2] << endl;
1008
1009 LogErrorCode(node);
1010 }
1011
1012 struct SDO
1013 {
1014 uint8_t node;
1015 uint8_t req;
1016 uint16_t idx;
1017 uint8_t subidx;
1018 uint32_t val;
1019
1020 SDO(uint8_t n, uint8_t r, uint16_t i, uint8_t s, uint32_t v=0)
1021 : node(n), req(r&0xf), idx(i), subidx(s), val(v) { }
1022
1023 bool operator==(const SDO &s) const
1024 {
1025 return node==s.node && idx==s.idx && subidx==s.subidx;
1026 }
1027 };
1028
1029 struct Timeout_t : SDO, ba::deadline_timer
1030 {
1031
1032 Timeout_t(ba::io_service& ioservice,
1033 uint8_t n, uint8_t r, uint16_t i, uint8_t s, uint32_t v, uint16_t millisec) : SDO(n, r, i, s, v),
1034 ba::deadline_timer(ioservice)
1035 {
1036 expires_from_now(boost::posix_time::milliseconds(millisec));
1037 }
1038 // get_io_service()
1039 };
1040
1041 std::list<Timeout_t> fTimeouts;
1042
1043 vector<uint8_t> fData;
1044
1045 void HandleReceivedData(const boost::system::error_code& err, size_t bytes_received, int)
1046 {
1047 // Do not schedule a new read if the connection failed.
1048 if (bytes_received!=11 || fData[0]!=10 || err)
1049 {
1050 if (err==ba::error::eof)
1051 Warn("Connection closed by remote host (cosy).");
1052
1053 // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
1054 // 125: Operation canceled
1055 if (err && err!=ba::error::eof && // Connection closed by remote host
1056 err!=ba::error::basic_errors::not_connected && // Connection closed by remote host
1057 err!=ba::error::basic_errors::operation_aborted) // Connection closed by us
1058 {
1059 ostringstream str;
1060 str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
1061 Error(str);
1062 }
1063 PostClose(err!=ba::error::basic_errors::operation_aborted);
1064 return;
1065 }
1066
1067 Time now;
1068
1069 const uint16_t desc = fData[1]<<8 | fData[2];
1070 const uint16_t cobid = desc>>5;
1071
1072 const uint8_t *data = fData.data()+3;
1073
1074 const uint16_t fcode = cobid >> 7;
1075 const uint8_t node = cobid & 0x1f;
1076
1077 switch (fcode)
1078 {
1079 case kRxNodeguard:
1080 Out() << "Received nodeguard" << endl;
1081 //HandleNodeguard(node, now);
1082 break;
1083
1084 case kRxSdo:
1085 {
1086 const uint8_t cmd = data[0];
1087 const uint16_t idx = data[1] | (data[2]<<8);
1088 const uint8_t subidx = data[3];
1089 const uint32_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
1090
1091 const auto it = find(fTimeouts.begin(), fTimeouts.end(), SDO(node, cmd, idx, subidx));
1092 if (it!=fTimeouts.end())
1093 {
1094 // This will call the handler and in turn remove the object from the list
1095 it->cancel();
1096 }
1097 else
1098 {
1099 ostringstream str;
1100 str << hex;
1101 str << "Unexpected SDO (";
1102 str << uint32_t(node) << ": ";
1103 str << ((cmd&0xf)==kTxSdo?"RX ":"TX ");
1104 str << idx << "/" << uint32_t(subidx) << ")";
1105
1106 Warn(str);
1107 }
1108
1109 switch (cmd)
1110 {
1111 case kRxSdo4: // answer to 0x40 with 4 bytes of data
1112 HandleSdo(node, idx, subidx, dat, now);
1113 break;
1114
1115 case kRxSdo2: // answer to 0x40 with 2 bytes of data
1116 HandleSdo(node, idx, subidx, dat&0xffff, now);
1117 break;
1118
1119 case kRxSdo1: // answer to 0x40 with 1 byte of data
1120 HandleSdo(node, idx, subidx, dat&0xff, now);
1121 break;
1122
1123 case kRxSdoOk: // answer to a SDO_TX message
1124 HandleSdoOk(node, idx, subidx, now);
1125 break;
1126
1127 case kRxSdoErr: // error message
1128 HandleSdoError(node, idx, subidx, now);
1129 break;
1130
1131 default:
1132 {
1133 ostringstream out;
1134 out << "Invalid SDO command code " << hex << cmd << " received.";
1135 Error(out);
1136 PostClose(false);
1137 return;
1138 }
1139 }
1140 }
1141 break;
1142
1143 case kRxPdo1:
1144 HandlePdo1(node, data, now);
1145 break;
1146
1147 case kRxPdo2:
1148 HandlePdo2(node, data, now);
1149 break;
1150
1151 case kRxPdo3:
1152 HandlePdo3(node, data, now);
1153 break;
1154
1155 default:
1156 {
1157 ostringstream out;
1158 out << "Invalid function code " << hex << fcode << " received.";
1159 Error(out);
1160 PostClose(false);
1161 return;
1162 }
1163 }
1164
1165 StartReadReport();
1166 }
1167
1168 void StartReadReport()
1169 {
1170 ba::async_read(*this, ba::buffer(fData),
1171 boost::bind(&ConnectionDrive::HandleReceivedData, this,
1172 ba::placeholders::error, ba::placeholders::bytes_transferred, 0));
1173
1174 //AsyncWait(fInTimeout, 35000, &Connection::HandleReadTimeout); // 30s
1175 }
1176
1177 bool fIsInitialized[2];
1178
1179 // This is called when a connection was established
1180 void ConnectionEstablished()
1181 {
1182 //Info("Connection to PLC established.");
1183
1184 fIsInitialized[0] = false;
1185 fIsInitialized[1] = false;
1186
1187 SendSdo(kNodeZd, kSetArmed, 1);
1188 SendSdo(kNodeAz, kSetArmed, 1);
1189
1190 RequestSdo(kNodeZd, kReqErrStat);
1191 RequestSdo(kNodeAz, kReqErrStat);
1192
1193 SetRpmMode(false);
1194
1195 RequestSdo(kNodeZd, kReqPosRes);
1196 RequestSdo(kNodeAz, kReqPosRes);
1197
1198 RequestSdo(kNodeZd, kReqVelRes);
1199 RequestSdo(kNodeAz, kReqVelRes);
1200
1201 RequestSdo(kNodeZd, kReqVelMax);
1202 RequestSdo(kNodeAz, kReqVelMax);
1203
1204 RequestSdo(kNodeZd, kReqPos, 0);
1205 RequestSdo(kNodeAz, kReqPos, 0);
1206 RequestSdo(kNodeZd, kReqPos, 1);
1207 RequestSdo(kNodeAz, kReqPos, 1);
1208
1209 RequestSdo(kNodeZd, kReqKeepAlive);
1210 RequestSdo(kNodeAz, kReqKeepAlive);
1211
1212 StartReadReport();
1213 }
1214
1215 void HandleTimeoutImp(const std::list<Timeout_t>::iterator &ref, const bs::error_code &error)
1216 {
1217 if (error==ba::error::basic_errors::operation_aborted)
1218 return;
1219
1220 if (error)
1221 {
1222 ostringstream str;
1223 str << "SDO timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
1224 Error(str);
1225
1226 //PostClose();
1227 return;
1228 }
1229
1230 if (!is_open())
1231 {
1232 // For example: Here we could schedule a new accept if we
1233 // would not want to allow two connections at the same time.
1234 return;
1235 }
1236
1237 // Check whether the deadline has passed. We compare the deadline
1238 // against the current time since a new asynchronous operation
1239 // may have moved the deadline before this actor had a chance
1240 // to run.
1241 if (ref->expires_at() > ba::deadline_timer::traits_type::now())
1242 return;
1243
1244 ostringstream str;
1245 str << hex;
1246 str << "SDO timeout (";
1247 str << uint32_t(ref->node) << ": ";
1248 str << (ref->req==kTxSdo?"RX ":"TX ");
1249 str << ref->idx << "/" << uint32_t(ref->subidx) << " [" << ref->val << "] ";
1250 str << to_simple_string(ref->expires_from_now());
1251 str << ")";
1252
1253 Warn(str);
1254
1255 //PostClose();
1256 }
1257
1258 void HandleTimeout(const std::list<Timeout_t>::iterator &ref, const bs::error_code &error)
1259 {
1260 HandleTimeoutImp(ref, error);
1261 fTimeouts.erase(ref);
1262 }
1263
1264 void SendSdoRequest(uint8_t node, uint8_t req,
1265 uint16_t idx, uint8_t subidx, uint32_t val=0)
1266 {
1267 if (fVerbosity>1)
1268 Out() << "SDO-" << (req==kTxSdo?"REQ":"SET") << "[" << int(node) << "] " << idx << "/" << int(subidx) << " = " << val << endl;
1269
1270
1271 SendCanFrame(0x600|(node&0x1f), req, idx&0xff, idx>>8, subidx,
1272 val&0xff, (val>>8)&0xff, (val>>16)&0xff, (val>>24)&0xff);
1273
1274 // - The boost::asio::basic_deadline_timer::expires_from_now()
1275 // function cancels any pending asynchronous waits, and returns
1276 // the number of asynchronous waits that were cancelled. If it
1277 // returns 0 then you were too late and the wait handler has
1278 // already been executed, or will soon be executed. If it
1279 // returns 1 then the wait handler was successfully cancelled.
1280 // - If a wait handler is cancelled, the bs::error_code passed to
1281 // it contains the value bs::error::operation_aborted.
1282
1283 const uint32_t milliseconds = 3000;
1284 fTimeouts.emplace_front(get_io_service(), node, req, idx, subidx, val, milliseconds);
1285
1286 const std::list<Timeout_t>::iterator &timeout = fTimeouts.begin();
1287
1288 timeout->async_wait(boost::bind(&ConnectionDrive::HandleTimeout, this, timeout, ba::placeholders::error));
1289 }
1290
1291public:
1292 ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
1293 fVerbosity(0), fData(11)
1294 {
1295 SetLogStream(&imp);
1296 }
1297
1298 void SetVerbosity(const uint16_t &v)
1299 {
1300 fVerbosity = v;
1301 }
1302
1303 uint16_t GetVerbosity() const
1304 {
1305 return fVerbosity;
1306 }
1307
1308 void RequestSdo(uint8_t node, uint16_t idx, uint8_t subidx=0)
1309 {
1310 SendSdoRequest(node, kTxSdo, idx, subidx);
1311 }
1312 void SendSdo(uint8_t node, uint16_t idx, uint8_t subidx, uint32_t val)
1313 {
1314 SendSdoRequest(node, kTxSdo4, idx, subidx, val);
1315 }
1316
1317 void SendSdo(uint8_t node, uint16_t idx, uint32_t val)
1318 {
1319 SendSdo(node, idx, 0, val);
1320 }
1321
1322 bool IsMoving() const
1323 {
1324 return (fStatusAxis[0]&kAxisMoving) || (fStatusAxis[1]&kAxisMoving)
1325 || (fStatusAxis[0]&kAxisRpmMode) || (fStatusAxis[1]&kAxisRpmMode);
1326 }
1327
1328 bool IsInitialized() const
1329 {
1330 // All important information has been successfully requested from the
1331 // SPS and the power control units are in RF (Regler freigegeben)
1332 return fIsInitialized[0] && fIsInitialized[1];
1333 }
1334
1335 bool HasWarning() const
1336 {
1337 const uint8_t typ0 = fErrCode[0]>>16;
1338 const uint8_t typ1 = fErrCode[1]>>16;
1339 return typ0==0xe || typ1==0xe;
1340 }
1341
1342 bool HasError() const
1343 {
1344 const uint8_t typ0 = fErrCode[0]>>16;
1345 const uint8_t typ1 = fErrCode[1]>>16;
1346 return typ0==0xf || typ1==0xf;
1347 }
1348
1349 bool IsOnline() const
1350 {
1351 return fErrCode[0]!=0 && fErrCode[1]!=0;
1352 }
1353
1354 bool IsReady() const
1355 {
1356 return fStatusAxis[0]&kAxisRf && fStatusAxis[1]&kAxisRf;
1357 }
1358
1359 bool IsBlocked() const
1360 {
1361 return (fStatusSys&kEmergencyOk)==0 || (fStatusSys&kManualMode);
1362 }
1363
1364 Encoder GetSePos() const // [rev]
1365 {
1366 return Encoder(double(fPdoPos2[1])/fPosRes[1], double(fPdoPos2[0])/fPosRes[0]);
1367 }
1368
1369 double GetSeTime() const // [rev]
1370 {
1371 // The maximum difference here should not be larger than 100ms.
1372 // So th error we make on both axes should not exceed 50ms;
1373 return (Time(fPdoTime2[0]).Mjd()+Time(fPdoTime2[1]).Mjd())/2;
1374 }
1375
1376 Encoder GetVelUnit() const
1377 {
1378 return Encoder(fVelMax[1], fVelMax[0]);
1379 }
1380
1381 void SetRpmMode(bool mode)
1382 {
1383 const uint32_t val = mode ? String('s','t','r','t') : String('s','t','o','p');
1384 SendSdo(kNodeAz, kSetRpmMode, val);
1385 SendSdo(kNodeZd, kSetRpmMode, val);
1386 }
1387
1388 void SetAcceleration(const Acceleration &acc)
1389 {
1390 SendSdo(kNodeAz, kSetAcc, lrint(acc.az*1000000000+0.5));
1391 SendSdo(kNodeZd, kSetAcc, lrint(acc.zd*1000000000+0.5));
1392 }
1393
1394 void SetPointingVelocity(const Velocity &vel, double scale=1)
1395 {
1396 SendSdo(kNodeAz, kSetPointVel, lrint(vel.az*fVelMax[0]*scale));
1397 SendSdo(kNodeZd, kSetPointVel, lrint(vel.zd*fVelMax[1]*scale));
1398 }
1399 void SetTrackingVelocity(const Velocity &vel)
1400 {
1401 SendSdo(kNodeAz, kSetTrackVel, lrint(vel.az*fVelRes[0]));
1402 SendSdo(kNodeZd, kSetTrackVel, lrint(vel.zd*fVelRes[1]));
1403 }
1404
1405 void StartAbsolutePositioning(const Encoder &enc, bool zd, bool az)
1406 {
1407 if (az) SendSdo(kNodeAz, kSetPosition, lrint(enc.az*fPosRes[0]));
1408 if (zd) SendSdo(kNodeZd, kSetPosition, lrint(enc.zd*fPosRes[1]));
1409
1410 // Make sure that the status is set correctly already before the first PDO
1411 if (az) fStatusAxis[0] |= 0x02;
1412 if (zd) fStatusAxis[1] |= 0x02;
1413
1414 // FIXME: UpdateDim?
1415 }
1416
1417 void SetLedVoltage(const uint32_t &v1, const uint32_t &v2)
1418 {
1419 SendSdo(kNodeAz, 0x4000, v1);
1420 SendSdo(kNodeZd, 0x4000, v2);
1421 }
1422};
1423
1424
1425// ------------------------------------------------------------------------
1426
1427#include "DimDescriptionService.h"
1428
1429class ConnectionDimDrive : public ConnectionDrive
1430{
1431private:
1432 DimDescribedService fDimPointing;
1433 DimDescribedService fDimTracking;
1434 DimDescribedService fDimSource;
1435 DimDescribedService fDimTPoint;
1436 DimDescribedService fDimStatus;
1437
1438 // Update dim from a different thread to ensure that these
1439 // updates cannot block the main eventloop which eventually
1440 // also checks the timeouts
1441 Queue<pair<Time,array<double, 2>>> fQueuePointing;
1442 Queue<pair<Time,array<double, 12>>> fQueueTracking;
1443 Queue<tuple<Time,vector<char>,bool>> fQueueSource;
1444 Queue<pair<Time,vector<char>>> fQueueTPoint;
1445 Queue<pair<Time,array<uint8_t, 3>>> fQueueStatus;
1446
1447 bool SendPointing(const pair<Time,array<double,2>> &p)
1448 {
1449 fDimPointing.setData(p.second);
1450 fDimPointing.Update(p.first);
1451 return true;
1452 }
1453
1454 bool SendTracking(const pair<Time,array<double, 12>> &p)
1455 {
1456 fDimTracking.setData(p.second);
1457 fDimTracking.Update(p.first);
1458 return true;
1459 }
1460
1461 bool SendSource(const tuple<Time,vector<char>,bool> &t)
1462 {
1463 const Time &time = get<0>(t);
1464 const vector<char> &data = get<1>(t);
1465 const bool &tracking = get<2>(t);
1466
1467 fDimSource.setQuality(tracking);
1468 fDimSource.setData(data);
1469 fDimSource.Update(time);
1470 return true;
1471 }
1472
1473 bool SendStatus(const pair<Time,array<uint8_t, 3>> &p)
1474 {
1475 fDimStatus.setData(p.second);
1476 fDimStatus.Update(p.first);
1477 return true;
1478 }
1479
1480 bool SendTPoint(const pair<Time,vector<char>> &p)
1481 {
1482 fDimTPoint.setData(p.second);
1483 fDimTPoint.Update(p.first);
1484 return true;
1485 }
1486
1487public:
1488 void UpdatePointing(const Time &t, const array<double, 2> &arr)
1489 {
1490 fQueuePointing.emplace(t, arr);
1491 }
1492
1493 void UpdateTracking(const Time &t,const array<double, 12> &arr)
1494 {
1495 fQueueTracking.emplace(t, arr);
1496 }
1497
1498 void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
1499 {
1500 fQueueStatus.emplace(t, arr);
1501 }
1502
1503 void UpdateTPoint(const Time &t, const DimTPoint &data,
1504 const string &name)
1505 {
1506 vector<char> dim(sizeof(data)+name.length()+1);
1507 memcpy(dim.data(), &data, sizeof(data));
1508 memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1);
1509
1510 fQueueTPoint.emplace(t, dim);
1511 }
1512
1513 void UpdateSource(const Time &t, const string &name, bool tracking)
1514 {
1515 vector<char> dat(5*sizeof(double)+31, 0);
1516 strncpy(dat.data()+5*sizeof(double), name.c_str(), 30);
1517
1518 fQueueSource.emplace(t, dat, tracking);
1519 }
1520
1521 void UpdateSource(const Time &t, const array<double, 5> &arr, const string &name="")
1522 {
1523 vector<char> dat(5*sizeof(double)+31, 0);
1524 memcpy(dat.data(), arr.data(), 5*sizeof(double));
1525 strncpy(dat.data()+5*sizeof(double), name.c_str(), 30);
1526
1527 fQueueSource.emplace(t, dat, true);
1528 }
1529
1530public:
1531 ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
1532 ConnectionDrive(ioservice, imp),
1533 fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1",
1534 "|Zd[deg]:Zenith distance (derived from encoder readout)"
1535 "|Az[deg]:Azimuth angle (derived from encoder readout)"),
1536 fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1",
1537 "|Ra[h]:Command right ascension pointing direction (J2000)"
1538 "|Dec[deg]:Command declination pointing direction (J2000)"
1539 "|Ha[h]:Hour angle pointing direction"
1540 "|SrcRa[h]:Right ascension source (J2000)"
1541 "|SrcDec[deg]:Declination source (J2000)"
1542 "|SrcHa[h]:Hour angle source"
1543 "|Zd[deg]:Nominal zenith distance"
1544 "|Az[deg]:Nominal azimuth angle"
1545 "|dZd[deg]:Control deviation Zd"
1546 "|dAz[deg]:Control deviation Az"
1547 "|dev[arcsec]:Absolute control deviation"
1548 "|avgdev[arcsec]:Average control deviation used to define OnTrack"),
1549 fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;C:31",
1550 "|Ra_src[h]:Source right ascension"
1551 "|Dec_src[deg]:Source declination"
1552 "|Offset[deg]:Wobble offset"
1553 "|Angle[deg]:Wobble angle"
1554 "|Period[min]:Time for one orbit"
1555 "|Name[string]:Source name if available"),
1556 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;D:1;D:1;D:1;C",
1557 "|Ra[h]:Command right ascension"
1558 "|Dec[deg]:Command declination"
1559 "|Zd_nom[deg]:Nominal zenith distance"
1560 "|Az_nom[deg]:Nominal azimuth angle"
1561 "|Zd_cur[deg]:Current zenith distance (calculated from image)"
1562 "|Az_cur[deg]:Current azimuth angle (calculated from image)"
1563 "|Zd_enc[deg]:Feedback zenith axis (from encoder)"
1564 "|Az_enc[deg]:Feedback azimuth angle (from encoder)"
1565 "|N_leds[cnt]:Number of detected LEDs"
1566 "|N_rings[cnt]:Number of rings used to calculate the camera center"
1567 "|Xc[pix]:X position of center in CCD camera frame"
1568 "|Yc[pix]:Y position of center in CCD camera frame"
1569 "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)"
1570 "|Xs[pix]:X position of start in CCD camera frame"
1571 "|Ys[pix]:Y position of star in CCD camera frame"
1572 "|Ms[mag]:Artifical magnitude of star (calculated from image)"
1573 "|Phi[deg]:Rotation angle of image derived from detected LEDs"
1574 "|Mc[mag]:Catalog magnitude of star"
1575 "|Dx[arcsec]:De-rotated dx"
1576 "|Dy[arcsec]:De-rotated dy"
1577 "|Name[string]:Name of star"),
1578 fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", ""),
1579 fQueuePointing(std::bind(&ConnectionDimDrive::SendPointing, this, placeholders::_1)),
1580 fQueueTracking(std::bind(&ConnectionDimDrive::SendTracking, this, placeholders::_1)),
1581 fQueueSource( std::bind(&ConnectionDimDrive::SendSource, this, placeholders::_1)),
1582 fQueueTPoint( std::bind(&ConnectionDimDrive::SendTPoint, this, placeholders::_1)),
1583 fQueueStatus( std::bind(&ConnectionDimDrive::SendStatus, this, placeholders::_1))
1584 {
1585 }
1586
1587 // 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
1588};
1589
1590// ------------------------------------------------------------------------
1591
1592template <class T, class S>
1593class StateMachineDrive : public StateMachineAsio<T>
1594{
1595private:
1596 S fDrive;
1597
1598 ba::deadline_timer fTrackingLoop;
1599
1600 string fDatabase;
1601
1602 typedef map<string, Source> sources;
1603 sources fSources;
1604
1605 Weather fWeather;
1606 uint16_t fWeatherTimeout;
1607
1608 ZdAz fParkingPos;
1609
1610 PointingModel fPointingModel;
1611 PointingSetup fPointingSetup;
1612 Encoder fMovementTarget;
1613
1614 Time fSunRise;
1615
1616 Encoder fPointingMin;
1617 Encoder fPointingMax;
1618
1619 uint16_t fDeviationLimit;
1620 uint16_t fDeviationCounter;
1621 uint16_t fDeviationMax;
1622
1623 vector<double> fDevBuffer;
1624 uint64_t fDevCount;
1625
1626 uint64_t fTrackingCounter;
1627
1628
1629 // --------------------- DIM Sending ------------------
1630
1631 bool CheckEventSize(size_t has, const char *name, size_t size)
1632 {
1633 if (has==size)
1634 return true;
1635
1636 ostringstream msg;
1637 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
1638 T::Fatal(msg);
1639 return false;
1640 }
1641
1642 // --------------------- DIM Receiving ------------------
1643
1644 int HandleWeatherData(const EventImp &evt)
1645 {
1646 if (evt.GetSize()==0)
1647 {
1648 T::Warn("MAGIC_WEATHER disconnected... using default weather values");
1649 fWeather.time = Time(Time::none);
1650 return T::GetCurrentState();
1651 }
1652
1653 if (!CheckEventSize(evt.GetSize(), "HandleWeatherData", 7*4+2))
1654 {
1655 fWeather.time = Time(Time::none);
1656 return T::GetCurrentState();
1657 }
1658
1659 const float *ptr = evt.Ptr<float>(2);
1660
1661 fWeather.temp = ptr[0];
1662 fWeather.hum = ptr[2];
1663 fWeather.press = ptr[3];
1664 fWeather.time = evt.GetTime();
1665
1666 return T::GetCurrentState();
1667 }
1668
1669 int HandleTPoint(const EventImp &evt)
1670 {
1671 // Skip disconnect events
1672 if (evt.GetSize()==0)
1673 return T::GetCurrentState();
1674
1675 // skip invalid events
1676 if (!CheckEventSize(evt.GetSize(), "HandleTPoint", 11*8))
1677 return T::GetCurrentState();
1678
1679 // skip event which are older than one minute
1680 if (Time().UnixTime()-evt.GetTime().UnixTime()>60)
1681 return T::GetCurrentState();
1682
1683 // Original code in slaTps2c:
1684 //
1685 // From the tangent plane coordinates of a star of known RA,Dec,
1686 // determine the RA,Dec of the tangent point.
1687
1688 const double *ptr = evt.Ptr<double>();
1689
1690 // Tangent plane rectangular coordinates
1691 const double dx = ptr[0] * M_PI/648000; // [arcsec -> rad]
1692 const double dy = ptr[1] * M_PI/648000; // [arcsec -> rad]
1693
1694 const PointingData data = fPointingModel.CalcPointingPos(fPointingSetup, evt.GetTime().Mjd(), fWeather, fWeatherTimeout, true);
1695
1696 const double x2 = dx*dx;
1697 const double y2 = 1 + dy*dy;
1698
1699 const double sd = cos(data.sky.zd);//sin(M_PI/2-sky.zd);
1700 const double cd = sin(data.sky.zd);//cos(M_PI/2-sky.zd);
1701 const double sdf = sd*sqrt(x2+y2);
1702 const double r2 = cd*cd*y2 - sd*sd*x2;
1703
1704 // Case of no solution ("at the pole") or
1705 // two solutions ("over the pole solution")
1706 if (r2<0 || fabs(sdf)>=1)
1707 {
1708 T::Warn("Could not determine pointing direction from TPoint.");
1709 return T::GetCurrentState();
1710 }
1711
1712 const double r = sqrt(r2);
1713 const double s = sdf - dy * r;
1714 const double c = sdf * dy + r;
1715 const double phi = atan2(dx, r);
1716
1717 // Spherical coordinates of tangent point
1718 const double az = fmod(data.sky.az-phi + 2*M_PI, 2*M_PI);
1719 const double zd = M_PI/2 - atan2(s, c);
1720
1721 const Encoder dev = fDrive.GetSePos()*360 - data.mount;
1722
1723 // --- Output TPoint ---
1724
1725 const string fname = "tpoints-"+to_string(evt.GetTime().NightAsInt())+".txt";
1726 //time.GetAsStr("/%Y/%m/%d");
1727
1728 const bool exist = boost::filesystem::exists(fname);
1729
1730 ofstream fout(fname, ios::app);
1731 if (!exist)
1732 {
1733 fout << "FACT Model TPOINT data file" << endl;
1734 fout << ": ALTAZ" << endl;
1735 fout << "49 48 0 ";
1736 fout << evt.GetTime() << endl;
1737 }
1738 fout << setprecision(7);
1739 fout << fmod(az*180/M_PI+360, 360) << " ";
1740 fout << 90-zd*180/M_PI << " ";
1741 fout << fmod(data.mount.az+360, 360) << " ";
1742 fout << 90-data.mount.zd << " ";
1743 fout << dev.az << " "; // delta az
1744 fout << -dev.zd << " "; // delta el
1745 fout << 90-data.sky.zd * 180/M_PI << " ";
1746 fout << data.sky.az * 180/M_PI << " ";
1747 fout << setprecision(10);
1748 fout << data.mjd << " ";
1749 fout << setprecision(7);
1750 fout << ptr[6] << " "; // center.mag
1751 fout << ptr[9] << " "; // star.mag
1752 fout << ptr[4] << " "; // center.x
1753 fout << ptr[5] << " "; // center.y
1754 fout << ptr[7] << " "; // star.x
1755 fout << ptr[8] << " "; // star.y
1756 fout << ptr[2] << " "; // num leds
1757 fout << ptr[3] << " "; // num rings
1758 fout << ptr[0] << " "; // dx (de-rotated)
1759 fout << ptr[1] << " "; // dy (de-rotated)
1760 fout << ptr[10] << " "; // rotation angle
1761 fout << fPointingSetup.source.mag << " ";
1762 fout << fPointingSetup.source.name;
1763 fout << endl;
1764
1765 DimTPoint dim;
1766 dim.fRa = data.pointing.ra * 12/M_PI;
1767 dim.fDec = data.pointing.dec * 180/M_PI;
1768 dim.fNominalZd = data.sky.zd * 180/M_PI;
1769 dim.fNominalAz = data.sky.az * 180/M_PI;
1770 dim.fPointingZd = zd * 180/M_PI;
1771 dim.fPointingAz = az * 180/M_PI;
1772 dim.fFeedbackZd = data.mount.zd;
1773 dim.fFeedbackAz = data.mount.az;
1774 dim.fNumLeds = uint16_t(ptr[2]);
1775 dim.fNumRings = uint16_t(ptr[3]);
1776 dim.fCenterX = ptr[4];
1777 dim.fCenterY = ptr[5];
1778 dim.fCenterMag = ptr[6];
1779 dim.fStarX = ptr[7];
1780 dim.fStarY = ptr[8];
1781 dim.fStarMag = ptr[9];
1782 dim.fRotation = ptr[10];
1783 dim.fDx = ptr[0];
1784 dim.fDy = ptr[1];
1785 dim.fRealMag = fPointingSetup.source.mag;
1786
1787 fDrive.UpdateTPoint(evt.GetTime(), dim, fPointingSetup.source.name);
1788
1789 ostringstream txt;
1790 txt << "TPoint recorded [" << zd*180/M_PI << "/" << az*180/M_PI << " | "
1791 << data.sky.zd*180/M_PI << "/" << data.sky.az*180/M_PI << " | "
1792 << data.mount.zd << "/" << data.mount.az << " | "
1793 << dx*180/M_PI << "/" << dy*180/M_PI << "]";
1794 T::Info(txt);
1795
1796 return T::GetCurrentState();
1797 }
1798
1799 // -------------------------- Helpers -----------------------------------
1800
1801 double GetDevAbs(double nomzd, double meszd, double devaz)
1802 {
1803 nomzd *= M_PI/180;
1804 meszd *= M_PI/180;
1805 devaz *= M_PI/180;
1806
1807 const double x = sin(meszd) * sin(nomzd) * cos(devaz);
1808 const double y = cos(meszd) * cos(nomzd);
1809
1810 return acos(x + y) * 180/M_PI;
1811 }
1812
1813 double ReadAngle(istream &in)
1814 {
1815 char sgn;
1816 uint16_t d, m;
1817 float s;
1818
1819 in >> sgn >> d >> m >> s;
1820
1821 const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
1822 return sgn=='-' ? -ret : ret;
1823 }
1824
1825 bool CheckRange(ZdAz pos)
1826 {
1827 if (pos.zd<fPointingMin.zd)
1828 {
1829 T::Error("Zenith distance "+to_string(pos.zd)+" below limit "+to_string(fPointingMin.zd));
1830 return false;
1831 }
1832
1833 if (pos.zd>fPointingMax.zd)
1834 {
1835 T::Error("Zenith distance "+to_string(pos.zd)+" exceeds limit "+to_string(fPointingMax.zd));
1836 return false;
1837 }
1838
1839 if (pos.az<fPointingMin.az)
1840 {
1841 T::Error("Azimuth angle "+to_string(pos.az)+" below limit "+to_string(fPointingMin.az));
1842 return false;
1843 }
1844
1845 if (pos.az>fPointingMax.az)
1846 {
1847 T::Error("Azimuth angle "+to_string(pos.az)+" exceeds limit "+to_string(fPointingMax.az));
1848 return false;
1849 }
1850
1851 return true;
1852 }
1853
1854 PointingData CalcPointingPos(double mjd)
1855 {
1856 return fPointingModel.CalcPointingPos(fPointingSetup, mjd, fWeather, fWeatherTimeout);
1857 }
1858
1859 // ----------------------------- SDO Commands ------------------------------
1860
1861 int RequestSdo(const EventImp &evt)
1862 {
1863 // FIXME: STop telescope
1864 if (!CheckEventSize(evt.GetSize(), "RequestSdo", 6))
1865 return T::kSM_FatalError;
1866
1867 const uint16_t node = evt.Get<uint16_t>();
1868 const uint16_t index = evt.Get<uint16_t>(2);
1869 const uint16_t subidx = evt.Get<uint16_t>(4);
1870
1871 if (node!=1 && node !=3)
1872 {
1873 T::Error("Node id must be 1 (az) or 3 (zd).");
1874 return T::GetCurrentState();
1875 }
1876
1877 if (subidx>0xff)
1878 {
1879 T::Error("Subindex must not be larger than 255.");
1880 return T::GetCurrentState();
1881 }
1882
1883 fDrive.RequestSdo(node, index, subidx);
1884
1885 return T::GetCurrentState();
1886 }
1887
1888 int SendSdo(const EventImp &evt)
1889 {
1890 if (!CheckEventSize(evt.GetSize(), "SendSdo", 6+8))
1891 return T::kSM_FatalError;
1892
1893 const uint16_t node = evt.Get<uint16_t>();
1894 const uint16_t index = evt.Get<uint16_t>(2);
1895 const uint16_t subidx = evt.Get<uint16_t>(4);
1896 const uint64_t value = evt.Get<uint64_t>(6);
1897
1898 if (node!=1 && node!=3)
1899 {
1900 T::Error("Node id must be 1 (az) or 3 (zd).");
1901 return T::GetCurrentState();
1902 }
1903
1904 if (subidx>0xff)
1905 {
1906 T::Error("Subindex must not be larger than 255.");
1907 return T::GetCurrentState();
1908 }
1909
1910 fDrive.SendSdo(node, index, subidx, value);
1911
1912 return T::GetCurrentState();
1913 }
1914
1915 // --------------------- Moving and tracking ---------------------
1916
1917 uint16_t fStep;
1918 bool fIsTracking;
1919 Acceleration fAccPointing;
1920 Acceleration fAccTracking;
1921 Acceleration fAccMax;
1922 double fMaxPointingResidual;
1923 double fMaxParkingResidual;
1924 double fPointingVelocity;
1925
1926 int InitMovement(const ZdAz &sky, bool tracking=false, const string &name="")
1927 {
1928 fMovementTarget = fPointingModel.SkyToMount(sky);
1929
1930 // Check whether bending is valid!
1931 if (!CheckRange(sky*(180/M_PI)))
1932 return StopMovement();
1933
1934 fStep = 0;
1935 fIsTracking = tracking;
1936
1937 fDrive.SetRpmMode(false); // *NEW* (Stop a previous tracking to avoid the pointing command to be ignored)
1938 fDrive.SetAcceleration(fAccPointing);
1939
1940 if (!tracking)
1941 fDrive.UpdateSource(Time(), name, false);
1942 else
1943 {
1944 const array<double, 5> dim =
1945 {{
1946 fPointingSetup.source.ra,
1947 fPointingSetup.source.dec,
1948 fPointingSetup.wobble_offset * 180/M_PI,
1949 fPointingSetup.wobble_angle * 180/M_PI,
1950 fPointingSetup.orbit_period * 24*60
1951 }};
1952 fDrive.UpdateSource(fPointingSetup.start, dim, fPointingSetup.source.name);
1953 }
1954
1955 return State::kMoving;
1956 }
1957
1958 int MoveTo(const EventImp &evt)
1959 {
1960 if (!CheckEventSize(evt.GetSize(), "MoveTo", 16))
1961 return T::kSM_FatalError;
1962
1963 const double *dat = evt.Ptr<double>();
1964
1965 ostringstream out;
1966 out << "Pointing telescope to Zd=" << dat[0] << "deg Az=" << dat[1] << "deg";
1967 T::Message(out);
1968
1969 return InitMovement(ZdAz(dat[0]*M_PI/180, dat[1]*M_PI/180));
1970 }
1971
1972 int InitTracking()
1973 {
1974 fPointingSetup.start = Time().Mjd();
1975
1976 const PointingData data = CalcPointingPos(fPointingSetup.start);
1977
1978 ostringstream out;
1979 out << "Tracking position now at Zd=" << data.sky.zd*180/M_PI << "deg Az=" << data.sky.az*180/M_PI << "deg";
1980 T::Info(out);
1981
1982 return InitMovement(data.sky, true);
1983 }
1984
1985 int StartTracking(const Source &src, double offset, double angle, double period=0)
1986 {
1987 if (src.ra<0 || src.ra>=24)
1988 {
1989 ostringstream out;
1990 out << "Right ascension out of range [0;24[: Ra=" << src.ra << "h Dec=" << src.dec << "deg";
1991 if (!src.name.empty())
1992 out << " [" << src.name << "]";
1993 T::Error(out);
1994 return State::kInvalidCoordinates;
1995 }
1996 if (src.dec<-90 || src.dec>90)
1997 {
1998 ostringstream out;
1999 out << "Declination out of range [-90;90]: Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2000 if (!src.name.empty())
2001 out << " [" << src.name << "]";
2002 T::Error(out);
2003 return State::kInvalidCoordinates;
2004 }
2005
2006 ostringstream out;
2007 out << "Tracking Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2008 if (!src.name.empty())
2009 out << " [" << src.name << "]";
2010 T::Info(out);
2011
2012 fPointingSetup.planet = kENone;
2013 fPointingSetup.source = src;
2014 fPointingSetup.orbit_period = period / 1440; // [min->day]
2015 fPointingSetup.wobble_angle = angle * M_PI/180; // [deg->rad]
2016 fPointingSetup.wobble_offset = offset * M_PI/180; // [deg->rad]
2017
2018 return InitTracking();
2019 }
2020
2021 int TrackCelest(const Planets_t &p)
2022 {
2023 switch (p)
2024 {
2025 case kEMoon: fPointingSetup.source.name = "Moon"; break;
2026 case kEVenus: fPointingSetup.source.name = "Venus"; break;
2027 case kEMars: fPointingSetup.source.name = "Mars"; break;
2028 case kEJupiter: fPointingSetup.source.name = "Jupiter"; break;
2029 case kESaturn: fPointingSetup.source.name = "Saturn"; break;
2030 default:
2031 T::Error("TrackCelest - Celestial object "+to_string(p)+" not yet supported.");
2032 return T::GetCurrentState();
2033 }
2034
2035 fPointingSetup.planet = p;
2036 fPointingSetup.wobble_offset = 0;
2037
2038 fDrive.UpdateSource(Time(), fPointingSetup.source.name, true);
2039
2040 return InitTracking();
2041 }
2042
2043 int Park()
2044 {
2045 ostringstream out;
2046 out << "Parking telescope at Zd=" << fParkingPos.zd << "deg Az=" << fParkingPos.az << "deg";
2047 T::Message(out);
2048
2049 const int rc = InitMovement(ZdAz(fParkingPos.zd*M_PI/180, fParkingPos.az*M_PI/180), false, "Park");
2050 return rc==State::kMoving ? State::kParking : rc;
2051 }
2052
2053 int Wobble(const EventImp &evt)
2054 {
2055 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
2056 return T::kSM_FatalError;
2057
2058 const double *dat = evt.Ptr<double>();
2059
2060 Source src;
2061 src.ra = dat[0];
2062 src.dec = dat[1];
2063 return StartTracking(src, dat[2], dat[3]);
2064 }
2065
2066 int Orbit(const EventImp &evt)
2067 {
2068 if (!CheckEventSize(evt.GetSize(), "Orbit", 40))
2069 return T::kSM_FatalError;
2070
2071 const double *dat = evt.Ptr<double>();
2072
2073 Source src;
2074 src.ra = dat[0];
2075 src.dec = dat[1];
2076 return StartTracking(src, dat[2], dat[3], dat[4]);
2077 }
2078
2079 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
2080 {
2081 if (find(ptr, last, '\0')==last)
2082 {
2083 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
2084 throw uint32_t(T::kSM_FatalError);
2085 }
2086
2087 const string name(ptr);
2088
2089 const sources::const_iterator it = fSources.find(name);
2090 if (it==fSources.end())
2091 {
2092 T::Error("Source '"+name+"' not found in list.");
2093 throw uint32_t(T::GetCurrentState());
2094 }
2095
2096 return it;
2097 }
2098
2099 int TrackWobble(const EventImp &evt)
2100 {
2101 if (evt.GetSize()<2)
2102 {
2103 ostringstream msg;
2104 msg << "TrackWobble - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
2105 T::Fatal(msg);
2106 return T::kSM_FatalError;
2107 }
2108
2109 if (evt.GetSize()==2)
2110 {
2111 ostringstream msg;
2112 msg << "TrackWobble - Source name missing.";
2113 T::Error(msg);
2114 return T::GetCurrentState();
2115 }
2116
2117 const uint16_t wobble = evt.GetUShort();
2118 if (wobble!=1 && wobble!=2)
2119 {
2120 ostringstream msg;
2121 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
2122 T::Error(msg);
2123 return T::GetCurrentState();
2124 }
2125
2126 const char *ptr = evt.Ptr<char>(2);
2127 const char *last = ptr+evt.GetSize()-2;
2128
2129 try
2130 {
2131 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2132
2133 const Source &src = it->second;
2134 return StartTracking(src, src.offset, src.angles[wobble-1]);
2135 }
2136 catch (const uint32_t &e)
2137 {
2138 return e;
2139 }
2140 }
2141
2142 int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0, double time=0)
2143 {
2144 const char *last = ptr+size;
2145
2146 try
2147 {
2148 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2149
2150 const Source &src = it->second;
2151 return StartTracking(src, offset<0?0.6/*src.offset*/:offset, angle, time);
2152 }
2153 catch (const uint32_t &e)
2154 {
2155 return e;
2156 }
2157 }
2158
2159 int Track(const EventImp &evt)
2160 {
2161 if (!CheckEventSize(evt.GetSize(), "Track", 16))
2162 return T::kSM_FatalError;
2163
2164 Source src;
2165
2166 src.name = "";
2167 src.ra = evt.Get<double>(0);
2168 src.dec = evt.Get<double>(8);
2169
2170 return StartTracking(src, 0, 0);
2171 }
2172
2173 int TrackSource(const EventImp &evt)
2174 {
2175 if (evt.GetSize()<16)
2176 {
2177 ostringstream msg;
2178 msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2179 T::Fatal(msg);
2180 return T::kSM_FatalError;
2181 }
2182
2183 if (evt.GetSize()==16)
2184 {
2185 ostringstream msg;
2186 msg << "TrackOn - Source name missing.";
2187 T::Error(msg);
2188 return T::GetCurrentState();
2189 }
2190
2191 const double offset = evt.Get<double>(0);
2192 const double angle = evt.Get<double>(8);
2193
2194 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, offset, angle);
2195 }
2196
2197 int TrackOn(const EventImp &evt)
2198 {
2199 if (evt.GetSize()==0)
2200 {
2201 ostringstream msg;
2202 msg << "TrackOn - Source name missing.";
2203 T::Error(msg);
2204 return T::GetCurrentState();
2205 }
2206
2207 return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
2208 }
2209
2210 int TrackOrbit(const EventImp &evt)
2211 {
2212 if (evt.GetSize()<16)
2213 {
2214 ostringstream msg;
2215 msg << "TrackOrbit - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2216 T::Fatal(msg);
2217 return T::kSM_FatalError;
2218 }
2219 if (evt.GetSize()==16)
2220 {
2221 ostringstream msg;
2222 msg << "TrackOrbit - Source name missing.";
2223 T::Error(msg);
2224 return T::GetCurrentState();
2225 }
2226
2227 const double angle = evt.Get<double>(0);
2228 const double time = evt.Get<double>(8);
2229
2230 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, -1, angle, time);
2231 }
2232
2233 int StopMovement()
2234 {
2235 fDrive.SetAcceleration(fAccMax);
2236 fDrive.SetRpmMode(false);
2237
2238 fTrackingLoop.cancel();
2239
2240 fDrive.UpdateSource(Time(), "", false);
2241
2242 return State::kStopping;
2243 }
2244
2245 int ResetError()
2246 {
2247 const int rc = CheckState();
2248 return rc>0 ? rc : State::kInitialized;
2249 }
2250
2251 // --------------------- Others ---------------------
2252
2253 int TPoint()
2254 {
2255 T::Info("TPoint initiated.");
2256 Dim::SendCommandNB("TPOINT/EXECUTE");
2257 return T::GetCurrentState();
2258 }
2259
2260 int Screenshot(const EventImp &evt)
2261 {
2262 if (evt.GetSize()<2)
2263 {
2264 ostringstream msg;
2265 msg << "Screenshot - Received event has " << evt.GetSize() << " bytes, but expected at least 2.";
2266 T::Fatal(msg);
2267 return T::kSM_FatalError;
2268 }
2269
2270 if (evt.GetSize()==2)
2271 {
2272 ostringstream msg;
2273 msg << "Screenshot - Filename missing.";
2274 T::Error(msg);
2275 return T::GetCurrentState();
2276 }
2277
2278 T::Info("Screenshot initiated.");
2279 Dim::SendCommandNB("TPOINT/SCREENSHOT", evt.GetData(), evt.GetSize());
2280 return T::GetCurrentState();
2281 }
2282
2283 int SetLedBrightness(const EventImp &evt)
2284 {
2285 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
2286 return T::kSM_FatalError;
2287
2288 const uint32_t *led = evt.Ptr<uint32_t>();
2289
2290 fDrive.SetLedVoltage(led[0], led[1]);
2291
2292 return T::GetCurrentState();
2293 }
2294
2295 int SetLedsOff()
2296 {
2297 fDrive.SetLedVoltage(0, 0);
2298 return T::GetCurrentState();
2299 }
2300
2301 // --------------------- Internal ---------------------
2302
2303 int SetVerbosity(const EventImp &evt)
2304 {
2305 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 2))
2306 return T::kSM_FatalError;
2307
2308 fDrive.SetVerbosity(evt.GetUShort());
2309
2310 return T::GetCurrentState();
2311 }
2312
2313 int Print()
2314 {
2315 for (auto it=fSources.begin(); it!=fSources.end(); it++)
2316 {
2317 const string &name = it->first;
2318 const Source &src = it->second;
2319
2320 T::Out() << name << ",";
2321 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
2322 T::Out() << src.angles[0] << "," << src.angles[1] << endl;
2323 }
2324 return T::GetCurrentState();
2325 }
2326
2327 int PrintPointingModel()
2328 {
2329 fPointingModel.print(T::Out());
2330 return T::GetCurrentState();
2331 }
2332
2333 int Unlock()
2334 {
2335 const int rc = CheckState();
2336 return rc<0 ? State::kInitialized : rc;
2337 }
2338
2339 int ReloadSources()
2340 {
2341 try
2342 {
2343 ReadDatabase();
2344 }
2345 catch (const exception &e)
2346 {
2347 T::Error("Reading sources from databse failed: "+string(e.what()));
2348 }
2349 return T::GetCurrentState();
2350 }
2351
2352 int Disconnect()
2353 {
2354 // Close all connections
2355 fDrive.PostClose(false);
2356
2357 /*
2358 // Now wait until all connection have been closed and
2359 // all pending handlers have been processed
2360 poll();
2361 */
2362
2363 return T::GetCurrentState();
2364 }
2365
2366 int Reconnect(const EventImp &evt)
2367 {
2368 // Close all connections to supress the warning in SetEndpoint
2369 fDrive.PostClose(false);
2370
2371 // Now wait until all connection have been closed and
2372 // all pending handlers have been processed
2373 ba::io_service::poll();
2374
2375 if (evt.GetBool())
2376 fDrive.SetEndpoint(evt.GetString());
2377
2378 // Now we can reopen the connection
2379 fDrive.PostClose(true);
2380
2381 return T::GetCurrentState();
2382 }
2383
2384 // ========================= Tracking code =============================
2385
2386 int UpdateTrackingPosition()
2387 {
2388 // First calculate deviation between
2389 // command position and nominal position
2390 //fPointing.mount = sepos; // [deg] ref pos for alignment
2391 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2392
2393 // Get current position and calculate deviation
2394 const Encoder sepos = fDrive.GetSePos()*360; // [deg]
2395 const Encoder dev = sepos - data.mount;
2396
2397 // Calculate absolut deviation on the sky
2398 const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az)*3600;
2399
2400 // Smoothing
2401 fDevBuffer[fDevCount++%5] = absdev;
2402
2403 // Calculate average
2404 const uint8_t cnt = fDevCount<5 ? fDevCount : 5;
2405 const double avgdev = accumulate(fDevBuffer.begin(), fDevBuffer.begin()+cnt, 0.)/cnt;
2406
2407 // Count the consecutive number of avgdev below fDeviationLimit
2408 if (avgdev<fDeviationLimit)
2409 fTrackingCounter++;
2410 else
2411 fTrackingCounter = 0;
2412
2413 const double ha = fmod(fDrive.GetSeTime(),1)*24 - Nova::kORM.lng/15;
2414
2415 array<double, 12> dim;
2416 dim[0] = data.pointing.ra * 12/M_PI; // Ra [h] optical axis
2417 dim[1] = data.pointing.dec * 180/M_PI; // Dec [deg] optical axis
2418 dim[2] = ha - data.pointing.ra; // Ha [h] optical axis
2419 dim[3] = data.source.ra * 12/M_PI; // SrcRa [h] source position
2420 dim[4] = data.source.dec * 180/M_PI; // SrcDec [deg] source position
2421 dim[5] = ha - data.source.ra; // SrcHa [h] source position
2422 dim[6] = data.sky.zd * 180/M_PI; // Zd [deg] optical axis
2423 dim[7] = data.sky.az * 180/M_PI; // Az [deg] optical axis
2424 dim[8] = dev.zd; // dZd [deg] control deviation
2425 dim[9] = dev.az; // dAz [deg] control deviation
2426 dim[10] = absdev; // dev [arcsec] absolute control deviation
2427 dim[11] = avgdev; // dev [arcsec] average control deviation
2428
2429 fDrive.UpdateTracking(fDrive.GetSeTime(), dim);
2430
2431 if (fDrive.GetVerbosity())
2432 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Deviation [deg] " << absdev << "\"|" << avgdev << "\"|" << fDevCount<< " dZd=" << dev.zd*3600 << "\" dAz=" << dev.az*3600 << "\"" << endl;
2433
2434 // Maximum deviation execeeded -> fall back to Tracking state
2435 if (T::GetCurrentState()==State::kOnTrack && avgdev>fDeviationMax)
2436 return State::kTracking;
2437
2438 // Condition for OnTrack state achieved -> enhance to OnTrack state
2439 if (T::GetCurrentState()==State::kTracking && fTrackingCounter>=fDeviationCounter)
2440 return State::kOnTrack;
2441
2442 // No state change
2443 return T::GetCurrentState();
2444 }
2445
2446 void UpdatePointingPosition()
2447 {
2448 const Encoder sepos = fDrive.GetSePos()*360; // [deg] ref pos for alignment
2449
2450 const ZdAz pos = fPointingModel.MountToSky(sepos);
2451
2452 array<double, 2> data;
2453 data[0] = pos.zd*180/M_PI; // Zd [deg]
2454 data[1] = pos.az*180/M_PI; // Az [deg]
2455 fDrive.UpdatePointing(fDrive.GetSeTime(), data);
2456
2457 if (fDrive.GetVerbosity())
2458 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Position [deg] " << pos.zd*180/M_PI << " " << pos.az*180/M_PI << endl;
2459 }
2460
2461 void TrackingLoop(const boost::system::error_code &error=boost::system::error_code())
2462 {
2463 if (error==ba::error::basic_errors::operation_aborted)
2464 return;
2465
2466 if (error)
2467 {
2468 ostringstream str;
2469 str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl;
2470 T::Error(str);
2471 return;
2472 }
2473
2474 if (T::GetCurrentState()!=State::kTracking &&
2475 T::GetCurrentState()!=State::kOnTrack)
2476 return;
2477
2478 //
2479 // Update speed as often as possible.
2480 // make sure, that dt is around 10 times larger than the
2481 // update time
2482 //
2483 // The loop should not be executed faster than the ramp of
2484 // a change in the velocity can be followed.
2485 //
2486 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(250));
2487
2488 const double mjd = Time().Mjd();
2489
2490 // I assume that it takes about 50ms for the value to be
2491 // transmitted and the drive needs time to follow as well (maybe
2492 // more than 50ms), therefore the calculated speec is calculated
2493 // for a moment 50ms in the future
2494 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2495 const PointingData data0 = CalcPointingPos(mjd-0.45/24/3600);
2496 const PointingData data1 = CalcPointingPos(mjd+0.55/24/3600);
2497
2498 const Encoder dest = data.mount *(1./360); // [rev]
2499 const Encoder dest0 = data0.mount*(1./360); // [rev]
2500 const Encoder dest1 = data1.mount*(1./360); // [rev]
2501
2502 if (!CheckRange(data1.sky))
2503 {
2504 StopMovement();
2505 T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop");
2506 return;
2507 }
2508
2509 // Current position
2510 const Encoder sepos = fDrive.GetSePos(); // [rev]
2511
2512 // Now calculate the current velocity
2513 const Encoder dist = dest1 - dest0; // [rev] Distance between t-1s and t+1s
2514 const Velocity vel = dist/(1./60); // [rev/min] Actual velocity of the pointing position
2515
2516 const Encoder dev = sepos - dest; // [rev] Current control deviation
2517 const Velocity vt = vel - dev/(1./60); // [rev/min] Correct velocity by recent control deviation
2518 // correct control deviation with 5s
2519 if (fDrive.GetVerbosity()>1)
2520 {
2521 T::Out() << "Ideal position [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2522 T::Out() << "Encoder pos. [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2523 T::Out() << "Deviation [arcmin] " << dev.zd *360*60 << " " << dev.az *360*60 << endl;
2524 T::Out() << "Distance 1s [arcmin] " << dist.zd *360*60 << " " << dist.az *360*60 << endl;
2525 T::Out() << "Velocity 1s [rpm] " << vt.zd << " " << vt.az << endl;
2526 T::Out() << "Delta T (enc) [ms] " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl;
2527 T::Out() << "Delta T (now) [ms] " << (Time().Mjd()-mjd)*24*3600*1000 << endl;
2528 }
2529
2530 // Tracking loop every 250ms
2531 // Vorsteuerung 2s
2532 // Delta T (enc) 5ms, every 5th, 25ms
2533 // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s)
2534
2535 //
2536 // FIXME: check if the drive is fast enough to follow the star
2537 //
2538 // Velocity units (would be 100 for %)
2539
2540 fDrive.SetTrackingVelocity(vt);
2541
2542 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2543 this, ba::placeholders::error));
2544 }
2545
2546 // =====================================================================
2547
2548 int CheckState()
2549 {
2550 if (!fDrive.IsConnected())
2551 return State::kDisconnected;
2552
2553 if (!fDrive.IsOnline())
2554 return State::kUnavailable;
2555
2556 // FIXME: This can prevent parking in case e.g.
2557 // of e8029 Position limit exceeded
2558 if (fDrive.HasWarning() || fDrive.HasError())
2559 {
2560 if (T::GetCurrentState()==State::kOnTrack ||
2561 T::GetCurrentState()==State::kTracking ||
2562 T::GetCurrentState()==State::kMoving ||
2563 T::GetCurrentState()==State::kParking)
2564 return StopMovement();
2565
2566 if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving())
2567 return State::kStopping;
2568
2569 if (fDrive.HasError())
2570 return State::kHardwareError;
2571
2572 if (fDrive.HasWarning())
2573 return State::kHardwareWarning;
2574
2575 return StateMachineImp::kSM_Error;
2576 }
2577
2578 // This can happen if one of the drives is not in RF.
2579 // Usually this only happens when the drive is not yet in RF
2580 // or an error was just cleared. Usually there is no way that
2581 // a drive goes below the RF state during operation without
2582 // a warning or error message.
2583 if (fDrive.IsOnline() && fDrive.IsBlocked())
2584 return State::kBlocked;
2585
2586 if (fDrive.IsOnline() && !fDrive.IsReady())
2587 return State::kAvailable;
2588
2589 // This is the case as soon as the init commands were send
2590 // after a connection to the SPS was established
2591 if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized())
2592 return State::kArmed;
2593
2594 return -1;
2595 }
2596
2597 int Execute()
2598 {
2599 const Time now;
2600 if (now>fSunRise && T::GetCurrentState()!=State::kParking)
2601 {
2602 fSunRise = now.GetNextSunRise();
2603
2604 ostringstream msg;
2605 msg << "Next sun-rise will be at " << fSunRise;
2606 T::Info(msg);
2607
2608 if (T::GetCurrentState()>State::kArmed && T::GetCurrentState()!=StateMachineImp::kError)
2609 return Park();
2610 }
2611
2612 if (T::GetCurrentState()==State::kLocked)
2613 return State::kLocked;
2614
2615 // FIXME: Send STOP if IsPositioning or RpmActive but no
2616 // Moving or Tracking state
2617
2618 const int rc = CheckState();
2619 if (rc>0)
2620 return rc;
2621
2622 // Once every second
2623 static time_t lastTime = 0;
2624 const time_t tm = time(NULL);
2625 if (lastTime!=tm && fDrive.IsInitialized())
2626 {
2627 lastTime=tm;
2628
2629 UpdatePointingPosition();
2630
2631 if (T::GetCurrentState()==State::kTracking || T::GetCurrentState()==State::kOnTrack)
2632 return UpdateTrackingPosition();
2633 }
2634
2635 if (T::GetCurrentState()==State::kStopping && !fDrive.IsMoving())
2636 return State::kArmed;
2637
2638 if ((T::GetCurrentState()==State::kMoving ||
2639 T::GetCurrentState()==State::kParking) && !fDrive.IsMoving())
2640 {
2641 if (fIsTracking && fStep==1)
2642 {
2643 // Init tracking
2644 fDrive.SetAcceleration(fAccTracking);
2645 fDrive.SetRpmMode(true);
2646
2647 fDevCount = 0;
2648 fTrackingCounter = 0;
2649
2650 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(1));
2651 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2652 this, ba::placeholders::error));
2653
2654 fPointingSetup.start = Time().Mjd();
2655
2656 const PointingData data = CalcPointingPos(fPointingSetup.start);
2657
2658 ostringstream out;
2659 out << "Start tracking at Ra=" << data.pointing.ra*12/M_PI << "h Dec=" << data.pointing.dec*180/M_PI << "deg";
2660 T::Info(out);
2661
2662 return State::kTracking;
2663 }
2664
2665 // Get feedback 2
2666 const Encoder dest = fMovementTarget*(1./360); // [rev]
2667 const Encoder sepos = fDrive.GetSePos(); // [rev]
2668
2669 // Calculate residual to move deviation
2670 const Encoder dist = dest - sepos; // [rev]
2671
2672 // Check which axis should still be moved
2673 Encoder cd = dist; // [rev]
2674 cd *= T::GetCurrentState()==State::kParking ? 1./fMaxParkingResidual : 1./fMaxPointingResidual; // Scale to units of the maximum residual
2675 cd = cd.Abs();
2676
2677 // Check if there is a control deviation on the axis
2678 const bool cdzd = cd.zd>1;
2679 const bool cdaz = cd.az>1;
2680
2681 if (!fIsTracking)
2682 {
2683 // check if we reached the correct position already
2684 if (!cdzd && !cdaz)
2685 {
2686 T::Info("Target position reached in "+to_string(fStep)+" steps.");
2687 return T::GetCurrentState()==State::kParking ? State::kLocked : State::kArmed;
2688 }
2689
2690 if (fStep==10)
2691 {
2692 T::Error("Target position not reached in "+to_string(fStep)+" steps.");
2693 return State::kPositioningFailed;
2694 }
2695 }
2696
2697 const Encoder t = dist.Abs()/fDrive.GetVelUnit();
2698
2699 const Velocity vel =
2700 t.zd > t.az ?
2701 Velocity(1, t.zd==0?0:t.az/t.zd) :
2702 Velocity(t.az==0?0:t.zd/t.az, 1);
2703
2704 if (fDrive.GetVerbosity())
2705 {
2706 T::Out() << "Moving step " << fStep << endl;
2707 T::Out() << "Encoder [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2708 T::Out() << "Destination [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2709 T::Out() << "Residual [deg] " << dist.zd *360 << " " << dist.az *360 << endl;
2710 T::Out() << "Residual/max [1] " << cd.zd << " " << cd.az << endl;
2711 T::Out() << "Rel. time [1] " << t.zd << " " << t.az << endl;
2712 T::Out() << "Rel. velocity [1] " << vel.zd << " " << vel.az << endl;
2713 }
2714
2715 fDrive.SetPointingVelocity(vel, fPointingVelocity);
2716 fDrive.StartAbsolutePositioning(dest, cdzd, cdaz);
2717
2718 ostringstream out;
2719 if (fStep==0)
2720 out << "Moving to encoder Zd=" << dest.zd*360 << "deg Az=" << dest.az*360 << "deg";
2721 else
2722 out << "Moving residual of dZd=" << dist.zd*360*60 << "' dAz=" << dist.az*360*60 << "'";
2723 T::Info(out);
2724
2725 fStep++;
2726 }
2727
2728 return T::GetCurrentState()>=State::kInitialized ?
2729 T::GetCurrentState() : State::kInitialized;
2730 }
2731
2732public:
2733 StateMachineDrive(ostream &out=cout) :
2734 StateMachineAsio<T>(out, "DRIVE_CONTROL"), fDrive(*this, *this),
2735 fTrackingLoop(*this), fSunRise(Time().GetNextSunRise()), fDevBuffer(5)
2736 {
2737
2738 T::Subscribe("MAGIC_WEATHER/DATA")
2739 (bind(&StateMachineDrive::HandleWeatherData, this, placeholders::_1));
2740
2741 T::Subscribe("TPOINT/DATA")
2742 (bind(&StateMachineDrive::HandleTPoint, this, placeholders::_1));
2743
2744 // State names
2745 T::AddStateName(State::kDisconnected, "Disconnected",
2746 "No connection to SPS");
2747 T::AddStateName(State::kConnected, "Connected",
2748 "Connection to SPS, no information received yet");
2749
2750 T::AddStateName(State::kLocked, "Locked",
2751 "Drive system is locked (will not accept commands)");
2752
2753 T::AddStateName(State::kUnavailable, "Unavailable",
2754 "Connected to SPS, no connection to at least one IndraDrives");
2755 T::AddStateName(State::kAvailable, "Available",
2756 "Connected to SPS and to IndraDrives, but at least one drive not in RF");
2757 T::AddStateName(State::kBlocked, "Blocked",
2758 "Drive system is blocked by manual operation or a pressed emergeny button");
2759 T::AddStateName(State::kArmed, "Armed",
2760 "Connected to SPS and IndraDrives in RF, but not yet initialized");
2761 T::AddStateName(State::kInitialized, "Initialized",
2762 "Connected to SPS and IndraDrives in RF and initialized");
2763
2764 T::AddStateName(State::kStopping, "Stopping",
2765 "Stop command sent, waiting for telescope to be still");
2766 T::AddStateName(State::kParking, "Parking",
2767 "Telescope in parking operation, waiting for telescope to be still");
2768 T::AddStateName(State::kMoving, "Moving",
2769 "Telescope moving");
2770 T::AddStateName(State::kTracking, "Tracking",
2771 "Telescope in tracking mode");
2772 T::AddStateName(State::kOnTrack, "OnTrack",
2773 "Telescope tracking stable");
2774
2775 T::AddStateName(State::kPositioningFailed, "PositioningFailed",
2776 "Target position was not reached within ten steps");
2777 T::AddStateName(State::kAllowedRangeExceeded, "OutOfRange",
2778 "Telecope went out of range during tracking");
2779 T::AddStateName(State::kInvalidCoordinates, "InvalidCoordinates",
2780 "Tracking coordinates out of range");
2781
2782 T::AddStateName(State::kHardwareWarning, "HardwareWarning",
2783 "At least one IndraDrive in a warning condition... check carefully!");
2784 T::AddStateName(State::kHardwareError, "HardwareError",
2785 "At least one IndraDrive in an error condition... this is a serious incident!");
2786
2787
2788 T::AddEvent("REQUEST_SDO", "S:3", State::kArmed)
2789 (bind(&StateMachineDrive::RequestSdo, this, placeholders::_1))
2790 ("Request an SDO from the drive"
2791 "|node[uint32]:Node identifier (1:az, 3:zd)"
2792 "|index[uint32]:SDO index"
2793 "|subindex[uint32]:SDO subindex");
2794
2795 T::AddEvent("SET_SDO", "S:3;X:1", State::kArmed)
2796 (bind(&StateMachineDrive::SendSdo, this, placeholders::_1))
2797 ("Request an SDO from the drive"
2798 "|node[uint32]:Node identifier (1:az, 3:zd)"
2799 "|index[uint32]:SDO index"
2800 "|subindex[uint32]:SDO subindex"
2801 "|value[uint64]:Value");
2802
2803 // Drive Commands
2804 T::AddEvent("MOVE_TO", "D:2", State::kInitialized) // ->ZDAZ
2805 (bind(&StateMachineDrive::MoveTo, this, placeholders::_1))
2806 ("Move the telescope to the given local sky coordinates"
2807 "|Zd[deg]:Zenith distance"
2808 "|Az[deg]:Azimuth");
2809
2810 T::AddEvent("TRACK", "D:2", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2811 (bind(&StateMachineDrive::Track, this, placeholders::_1))
2812 ("Move the telescope to the given sky coordinates and start tracking them"
2813 "|Ra[h]:Right ascension"
2814 "|Dec[deg]:Declination");
2815
2816 T::AddEvent("WOBBLE", "D:4", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2817 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
2818 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
2819 "|Ra[h]:Right ascension"
2820 "|Dec[deg]:Declination"
2821 "|Offset[deg]:Wobble offset"
2822 "|Angle[deg]:Wobble angle");
2823
2824 T::AddEvent("ORBIT", "D:5", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2825 (bind(&StateMachineDrive::Orbit, this, placeholders::_1))
2826 ("Move the telescope in a circle around the source"
2827 "|Ra[h]:Right ascension"
2828 "|Dec[deg]:Declination"
2829 "|Offset[deg]:Wobble offset"
2830 "|Angle[deg]:Starting angle"
2831 "|Period[min]:Time for one orbit");
2832
2833 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2834 (bind(&StateMachineDrive::TrackSource, this, placeholders::_1))
2835 ("Move the telescope to the given wobble position around the given source and start tracking"
2836 "|Offset[deg]:Wobble offset"
2837 "|Angle[deg]:Wobble angle"
2838 "|Name[string]:Source name");
2839
2840 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2841 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
2842 ("Move the telescope to the given wobble position around the given source and start tracking"
2843 "|Id:Wobble angle id (1 or 2)"
2844 "|Name[string]:Source name");
2845
2846 T::AddEvent("TRACK_ORBIT", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2847 (bind(&StateMachineDrive::TrackOrbit, this, placeholders::_1))
2848 ("Move the telescope in a circle around the source"
2849 "|Angle[deg]:Starting angle"
2850 "|Period[min]:Time for one orbit"
2851 "|Name[string]:Source name");
2852
2853 T::AddEvent("TRACK_ON", "C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2854 (bind(&StateMachineDrive::TrackOn, this, placeholders::_1))
2855 ("Move the telescope to the given position and start tracking"
2856 "|Name[string]:Source name");
2857
2858 T::AddEvent("MOON", State::kInitialized, State::kTracking, State::kOnTrack)
2859 (bind(&StateMachineDrive::TrackCelest, this, kEMoon))
2860 ("Start tracking the moon");
2861 T::AddEvent("VENUS", State::kInitialized, State::kTracking, State::kOnTrack)
2862 (bind(&StateMachineDrive::TrackCelest, this, kEVenus))
2863 ("Start tracking Venus");
2864 T::AddEvent("MARS", State::kInitialized, State::kTracking, State::kOnTrack)
2865 (bind(&StateMachineDrive::TrackCelest, this, kEMars))
2866 ("Start tracking Mars");
2867 T::AddEvent("JUPITER", State::kInitialized, State::kTracking, State::kOnTrack)
2868 (bind(&StateMachineDrive::TrackCelest, this, kEJupiter))
2869 ("Start tracking Jupiter");
2870 T::AddEvent("SATURN", State::kInitialized, State::kTracking, State::kOnTrack)
2871 (bind(&StateMachineDrive::TrackCelest, this, kESaturn))
2872 ("Start tracking Saturn");
2873
2874 // FIXME: What to do in error state?
2875 T::AddEvent("PARK", State::kInitialized, State::kMoving, State::kTracking, State::kOnTrack, State::kHardwareWarning)
2876 (bind(&StateMachineDrive::Park, this))
2877 ("Park the telescope");
2878
2879 T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kTracking)(State::kOnTrack)
2880 (bind(&StateMachineDrive::StopMovement, this))
2881 ("Stop any kind of movement.");
2882
2883 T::AddEvent("RESET", State::kPositioningFailed, State::kAllowedRangeExceeded, State::kInvalidCoordinates, State::kHardwareWarning)
2884 (bind(&StateMachineDrive::ResetError, this))
2885 ("Acknowledge an internal drivectrl error (PositioningFailed, AllowedRangeExceeded, InvalidCoordinates)");
2886
2887 T::AddEvent("TPOINT", State::kOnTrack)
2888 (bind(&StateMachineDrive::TPoint, this))
2889 ("Take a TPoint");
2890
2891 T::AddEvent("SCREENSHOT", "B:1;C")
2892 (bind(&StateMachineDrive::Screenshot, this, placeholders::_1))
2893 ("Take a screenshot"
2894 "|color[bool]:False if just the gray image should be saved."
2895 "|name[string]:Filename");
2896
2897 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
2898 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
2899 ("Set the LED brightness of the top and bottom leds"
2900 "|top[au]:Allowed range 0-32767 for top LEDs"
2901 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
2902
2903 T::AddEvent("LEDS_OFF")
2904 (bind(&StateMachineDrive::SetLedsOff, this))
2905 ("Switch off TPoint LEDs");
2906
2907 T::AddEvent("UNLOCK", Drive::State::kLocked)
2908 (bind(&StateMachineDrive::Unlock, this))
2909 ("Unlock locked state.");
2910
2911 // Verbosity commands
2912 T::AddEvent("SET_VERBOSITY", "S:1")
2913 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
2914 ("Set verbosity state"
2915 "|verbosity[uint16]:disable or enable verbosity for received data (yes/no), except dynamic data");
2916
2917 // Conenction commands
2918 T::AddEvent("DISCONNECT", State::kConnected)
2919 (bind(&StateMachineDrive::Disconnect, this))
2920 ("disconnect from ethernet");
2921
2922 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected)
2923 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
2924 ("(Re)connect Ethernet connection to SPS, a new address can be given"
2925 "|[host][string]:new ethernet address in the form <host:port>");
2926
2927
2928 T::AddEvent("PRINT_POINTING_MODEL")
2929 (bind(&StateMachineDrive::PrintPointingModel, this))
2930 ("Print the ponting model.");
2931
2932
2933 T::AddEvent("PRINT")
2934 (bind(&StateMachineDrive::Print, this))
2935 ("Print source list.");
2936
2937 T::AddEvent("RELOAD_SOURCES", State::kDisconnected, State::kConnected, State::kArmed, State::kInitialized, State::kLocked)
2938 (bind(&StateMachineDrive::ReloadSources, this))
2939 ("Reload sources from database after database has changed..");
2940
2941
2942 //fDrive.SetUpdateStatus(std::bind(&StateMachineDrive::UpdateStatus, this, placeholders::_1, placeholders::_2));
2943 fDrive.StartConnect();
2944 }
2945
2946 void SetEndpoint(const string &url)
2947 {
2948 fDrive.SetEndpoint(url);
2949 }
2950
2951 bool AddSource(const string &name, const Source &src)
2952 {
2953 const auto it = fSources.find(name);
2954 if (it!=fSources.end())
2955 T::Warn("Source '"+name+"' already in list... overwriting.");
2956
2957 fSources[name] = src;
2958 return it==fSources.end();
2959 }
2960
2961 void ReadDatabase(bool print=true)
2962 {
2963#ifdef HAVE_SQL
2964 Database db(fDatabase);
2965
2966 T::Message("Connected to '"+db.uri()+"'");
2967
2968 const mysqlpp::StoreQueryResult res =
2969 db.query("SELECT fSourceName, fRightAscension, fDeclination, fWobbleOffset, fWobbleAngle0, fWobbleAngle1, fMagnitude FROM Source").store();
2970
2971 fSources.clear();
2972 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
2973 {
2974 const string name = (*v)[0].c_str();
2975
2976 Source src;
2977 src.name = name;
2978 src.ra = (*v)[1];
2979 src.dec = (*v)[2];
2980 src.offset = (*v)[3];
2981 src.angles[0] = (*v)[4];
2982 src.angles[1] = (*v)[5];
2983 src.mag = (*v)[6] ? double((*v)[6]) : 0;
2984 AddSource(name, src);
2985
2986 if (!print)
2987 continue;
2988
2989 ostringstream msg;
2990 msg << " " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2991 msg << " Wobble=[" << src.offset << "," << src.angles[0] << "," << src.angles[1] << "] Mag=" << src.mag;
2992 T::Message(msg);
2993 }
2994#else
2995 T::Warn("MySQL support not compiled into the program.");
2996#endif
2997 }
2998
2999 int EvalOptions(Configuration &conf)
3000 {
3001 if (!fSunRise)
3002 return 1;
3003
3004 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
3005
3006 fMaxPointingResidual = conf.Get<double>("pointing.max.residual");
3007 fPointingVelocity = conf.Get<double>("pointing.velocity");
3008
3009 fPointingMin = Encoder(conf.Get<double>("pointing.min.zd"),
3010 conf.Get<double>("pointing.min.az"));
3011 fPointingMax = Encoder(conf.Get<double>("pointing.max.zd"),
3012 conf.Get<double>("pointing.max.az"));
3013
3014 fParkingPos.zd = conf.Has("parking-pos.zd") ? conf.Get<double>("parking-pos.zd") : 90;
3015 fParkingPos.az = conf.Has("parking-pos.az") ? conf.Get<double>("parking-pos.az") : 0;
3016 fMaxParkingResidual = conf.Get<double>("parking-pos.residual");
3017
3018 if (!CheckRange(fParkingPos))
3019 return 2;
3020
3021 fAccPointing = Acceleration(conf.Get<double>("pointing.acceleration.zd"),
3022 conf.Get<double>("pointing.acceleration.az"));
3023 fAccTracking = Acceleration(conf.Get<double>("tracking.acceleration.zd"),
3024 conf.Get<double>("tracking.acceleration.az"));
3025 fAccMax = Acceleration(conf.Get<double>("acceleration.max.zd"),
3026 conf.Get<double>("acceleration.max.az"));
3027
3028 fWeatherTimeout = conf.Get<uint16_t>("weather-timeout");
3029
3030 if (fAccPointing>fAccMax)
3031 {
3032 T::Error("Pointing acceleration exceeds maximum acceleration.");
3033 return 3;
3034 }
3035
3036 if (fAccTracking>fAccMax)
3037 {
3038 T::Error("Tracking acceleration exceeds maximum acceleration.");
3039 return 4;
3040 }
3041
3042 fDeviationLimit = conf.Get<uint16_t>("deviation-limit");
3043 fDeviationCounter = conf.Get<uint16_t>("deviation-count");
3044 fDeviationMax = conf.Get<uint16_t>("deviation-max");
3045
3046 const string fname = conf.Get<string>("pointing.model-file");
3047
3048 try
3049 {
3050 fPointingModel.Load(fname);
3051 }
3052 catch (const exception &e)
3053 {
3054 T::Error(e.what());
3055 return 5;
3056 }
3057
3058 const vector<string> &vec = conf.Vec<string>("source");
3059
3060 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
3061 {
3062 istringstream stream(*it);
3063
3064 string name;
3065
3066 int i=0;
3067
3068 Source src;
3069
3070 string buffer;
3071 while (getline(stream, buffer, ','))
3072 {
3073 istringstream is(buffer);
3074
3075 switch (i++)
3076 {
3077 case 0: name = buffer; break;
3078 case 1: src.ra = ReadAngle(is); break;
3079 case 2: src.dec = ReadAngle(is); break;
3080 case 3: is >> src.offset; break;
3081 case 4: is >> src.angles[0]; break;
3082 case 5: is >> src.angles[1]; break;
3083 }
3084
3085 if (is.fail())
3086 break;
3087 }
3088
3089 if (i==3 || i==6)
3090 {
3091 AddSource(name, src);
3092 continue;
3093 }
3094
3095 T::Warn("Resource 'source' not correctly formatted: '"+*it+"'");
3096 }
3097
3098 //fAutoResume = conf.Get<bool>("auto-resume");
3099
3100 if (conf.Has("source-database"))
3101 {
3102 fDatabase = conf.Get<string>("source-database");
3103 ReadDatabase();
3104 }
3105
3106 if (fSunRise.IsValid())
3107 {
3108 ostringstream msg;
3109 msg << "Next sun-rise will be at " << fSunRise;
3110 T::Message(msg);
3111 }
3112
3113 // The possibility to connect should be last, so that
3114 // everything else is already initialized.
3115 SetEndpoint(conf.Get<string>("addr"));
3116
3117 return -1;
3118 }
3119};
3120
3121// ------------------------------------------------------------------------
3122
3123#include "Main.h"
3124
3125
3126template<class T, class S, class R>
3127int RunShell(Configuration &conf)
3128{
3129 return Main::execute<T, StateMachineDrive<S, R>>(conf);
3130}
3131
3132void SetupConfiguration(Configuration &conf)
3133{
3134 po::options_description control("Drive control options");
3135 control.add_options()
3136 ("quiet,q", po_bool(), "Disable debug messages")
3137 ("no-dim,d", po_switch(), "Disable dim services")
3138 ("addr,a", var<string>("sps:5357"), "Network address of cosy")
3139 ("verbosity,v", var<uint16_t>(0), "Vervosity level (0=off; 1=major updates; 2=most updates; 3=frequent updates)")
3140 ("pointing.model-file", var<string>()->required(), "Name of the file with the pointing model in use")
3141 ("pointing.max.zd", var<double>( 104.9), "Maximum allowed zenith angle in sky pointing coordinates [deg]")
3142 ("pointing.max.az", var<double>( 85.0), "Maximum allowed azimuth angle in sky pointing coordinates [deg]")
3143 ("pointing.min.zd", var<double>(-104.9), "Minimum allowed zenith angle in sky pointing coordinates [deg]")
3144 ("pointing.min.az", var<double>(-295.0), "Minimum allowed azimuth angle in sky pointing coordinates [deg]")
3145 ("pointing.max.residual", var<double>(1./32768), "Maximum residual for a pointing operation [revolutions]")
3146 ("pointing.velocity", var<double>(0.3), "Moving velocity when pointing [% max]")
3147 ("pointing.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis for pointing operations")
3148 ("pointing.acceleration.zd", var<double>(0.03), "Acceleration for zenith axis for pointing operations")
3149 ("tracking.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis during tracking operations")
3150 ("tracking.acceleration.zd", var<double>(0.01), "Acceleration for zenith axis during tracking operations")
3151 ("parking-pos.zd", var<double>(101), "Parking position zenith angle in sky pointing coordinates [deg]")
3152 ("parking-pos.az", var<double>(0), "Parking position azimuth angle in sky pointing coordinates [deg]")
3153 ("parking-pos.residual", var<double>(0.5/360), "Maximum residual for a parking position [revolutions]")
3154 ("acceleration.max.az", var<double>(0.03), "Maximum allowed acceleration value for azimuth axis")
3155 ("acceleration.max.zd", var<double>(0.09), "Maximum allowed acceleration value for zenith axis")
3156 ("weather-timeout", var<uint16_t>(300), "Timeout [sec] for weather data (after timeout default values are used)")
3157 ("deviation-limit", var<uint16_t>(90), "Deviation limit in arcsec to get 'OnTrack'")
3158 ("deviation-count", var<uint16_t>(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
3159 ("deviation-max", var<uint16_t>(180), "Maximum deviation in arcsec allowed to keep status 'OnTrack'")
3160 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
3161 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
3162 ;
3163
3164 conf.AddOptions(control);
3165}
3166
3167/*
3168 Extract usage clause(s) [if any] for SYNOPSIS.
3169 Translators: "Usage" and "or" here are patterns (regular expressions) which
3170 are used to match the usage synopsis in program output. An example from cp
3171 (GNU coreutils) which contains both strings:
3172 Usage: cp [OPTION]... [-T] SOURCE DEST
3173 or: cp [OPTION]... SOURCE... DIRECTORY
3174 or: cp [OPTION]... -t DIRECTORY SOURCE...
3175 */
3176void PrintUsage()
3177{
3178 cout <<
3179 "The drivectrl is an interface to the drive PLC.\n"
3180 "\n"
3181 "The default is that the program is started without user intercation. "
3182 "All actions are supposed to arrive as DimCommands. Using the -c "
3183 "option, a local shell can be initialized. With h or help a short "
3184 "help message about the usuage can be brought to the screen.\n"
3185 "\n"
3186 "Usage: drivectrl [-c type] [OPTIONS]\n"
3187 " or: drivectrl [OPTIONS]\n";
3188 cout << endl;
3189}
3190
3191void PrintHelp()
3192{
3193 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
3194
3195 /* Additional help text which is printed after the configuration
3196 options goes here */
3197
3198 /*
3199 cout << "bla bla bla" << endl << endl;
3200 cout << endl;
3201 cout << "Environment:" << endl;
3202 cout << "environment" << endl;
3203 cout << endl;
3204 cout << "Examples:" << endl;
3205 cout << "test exam" << endl;
3206 cout << endl;
3207 cout << "Files:" << endl;
3208 cout << "files" << endl;
3209 cout << endl;
3210 */
3211}
3212
3213int main(int argc, const char* argv[])
3214{
3215 Configuration conf(argv[0]);
3216 conf.SetPrintUsage(PrintUsage);
3217 Main::SetupConfiguration(conf);
3218 SetupConfiguration(conf);
3219
3220 if (!conf.DoParse(argc, argv, PrintHelp))
3221 return 127;
3222
3223 //try
3224 {
3225 // No console access at all
3226 if (!conf.Has("console"))
3227 {
3228 if (conf.Get<bool>("no-dim"))
3229 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
3230 else
3231 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
3232 }
3233 // Cosole access w/ and w/o Dim
3234 if (conf.Get<bool>("no-dim"))
3235 {
3236 if (conf.Get<int>("console")==0)
3237 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
3238 else
3239 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
3240 }
3241 else
3242 {
3243 if (conf.Get<int>("console")==0)
3244 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
3245 else
3246 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
3247 }
3248 }
3249 /*catch (std::exception& e)
3250 {
3251 cerr << "Exception: " << e.what() << endl;
3252 return -1;
3253 }*/
3254
3255 return 0;
3256}
Note: See TracBrowser for help on using the repository browser.