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

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