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

Last change on this file since 19615 was 19577, checked in by tbretz, 5 years ago
Some more beauty to source table loading.
File size: 117.4 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(ba::io_service& ioservice,
1049 uint8_t n, uint8_t r, uint16_t i, uint8_t s, uint32_t v, uint16_t millisec) : SDO(n, r, i, s, v),
1050 ba::deadline_timer(ioservice)
1051 {
1052 expires_from_now(boost::posix_time::milliseconds(millisec));
1053 }
1054 // get_io_service()
1055 };
1056
1057 std::list<Timeout_t> fTimeouts;
1058
1059 vector<uint8_t> fData;
1060
1061 void HandleReceivedData(const boost::system::error_code& err, size_t bytes_received, int)
1062 {
1063 // Do not schedule a new read if the connection failed.
1064 if (bytes_received!=11 || fData[0]!=10 || err)
1065 {
1066 if (err==ba::error::eof)
1067 Warn("Connection closed by remote host (cosy).");
1068
1069 // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
1070 // 125: Operation canceled
1071 if (err && err!=ba::error::eof && // Connection closed by remote host
1072 err!=ba::error::basic_errors::not_connected && // Connection closed by remote host
1073 err!=ba::error::basic_errors::operation_aborted) // Connection closed by us
1074 {
1075 ostringstream str;
1076 str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
1077 Error(str);
1078 }
1079 PostClose(err!=ba::error::basic_errors::operation_aborted);
1080 return;
1081 }
1082
1083 Time now;
1084
1085 const uint16_t desc = fData[1]<<8 | fData[2];
1086 const uint16_t cobid = desc>>5;
1087
1088 const uint8_t *data = fData.data()+3;
1089
1090 const uint16_t fcode = cobid >> 7;
1091 const uint8_t node = cobid & 0x1f;
1092
1093 switch (fcode)
1094 {
1095 case kRxNodeguard:
1096 Out() << "Received nodeguard" << endl;
1097 //HandleNodeguard(node, now);
1098 break;
1099
1100 case kRxSdo:
1101 {
1102 const uint8_t cmd = data[0];
1103 const uint16_t idx = data[1] | (data[2]<<8);
1104 const uint8_t subidx = data[3];
1105 const uint32_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
1106
1107 const auto it = find(fTimeouts.begin(), fTimeouts.end(), SDO(node, cmd, idx, subidx));
1108 if (it!=fTimeouts.end())
1109 {
1110 // This will call the handler and in turn remove the object from the list
1111 it->cancel();
1112 }
1113 else
1114 {
1115 ostringstream str;
1116 str << hex;
1117 str << "Unexpected SDO (";
1118 str << uint32_t(node) << ": ";
1119 str << ((cmd&0xf)==kTxSdo?"RX ":"TX ");
1120 str << idx << "/" << uint32_t(subidx) << ")";
1121
1122 Warn(str);
1123 }
1124
1125 switch (cmd)
1126 {
1127 case kRxSdo4: // answer to 0x40 with 4 bytes of data
1128 HandleSdo(node, idx, subidx, dat, now);
1129 break;
1130
1131 case kRxSdo2: // answer to 0x40 with 2 bytes of data
1132 HandleSdo(node, idx, subidx, dat&0xffff, now);
1133 break;
1134
1135 case kRxSdo1: // answer to 0x40 with 1 byte of data
1136 HandleSdo(node, idx, subidx, dat&0xff, now);
1137 break;
1138
1139 case kRxSdoOk: // answer to a SDO_TX message
1140 HandleSdoOk(node, idx, subidx, now);
1141 break;
1142
1143 case kRxSdoErr: // error message
1144 HandleSdoError(node, idx, subidx, now);
1145 break;
1146
1147 default:
1148 {
1149 ostringstream out;
1150 out << "Invalid SDO command code " << hex << cmd << " received.";
1151 Error(out);
1152 PostClose(false);
1153 return;
1154 }
1155 }
1156 }
1157 break;
1158
1159 case kRxPdo1:
1160 HandlePdo1(node, data, now);
1161 break;
1162
1163 case kRxPdo2:
1164 HandlePdo2(node, data, now);
1165 break;
1166
1167 case kRxPdo3:
1168 HandlePdo3(node, data, now);
1169 break;
1170
1171 default:
1172 {
1173 ostringstream out;
1174 out << "Invalid function code " << hex << fcode << " received.";
1175 Error(out);
1176 PostClose(false);
1177 return;
1178 }
1179 }
1180
1181 StartReadReport();
1182 }
1183
1184 void StartReadReport()
1185 {
1186 ba::async_read(*this, ba::buffer(fData),
1187 boost::bind(&ConnectionDrive::HandleReceivedData, this,
1188 ba::placeholders::error, ba::placeholders::bytes_transferred, 0));
1189
1190 //AsyncWait(fInTimeout, 35000, &Connection::HandleReadTimeout); // 30s
1191 }
1192
1193 bool fIsInitialized[2];
1194
1195 // This is called when a connection was established
1196 void ConnectionEstablished()
1197 {
1198 //Info("Connection to PLC established.");
1199
1200 fIsInitialized[0] = false;
1201 fIsInitialized[1] = false;
1202
1203 SendSdo(kNodeZd, kSetArmed, 1);
1204 SendSdo(kNodeAz, kSetArmed, 1);
1205
1206 RequestSdo(kNodeZd, kReqErrStat);
1207 RequestSdo(kNodeAz, kReqErrStat);
1208
1209 SetRpmMode(false);
1210
1211 RequestSdo(kNodeZd, kReqPosRes);
1212 RequestSdo(kNodeAz, kReqPosRes);
1213
1214 RequestSdo(kNodeZd, kReqVelRes);
1215 RequestSdo(kNodeAz, kReqVelRes);
1216
1217 RequestSdo(kNodeZd, kReqVelMax);
1218 RequestSdo(kNodeAz, kReqVelMax);
1219
1220 RequestSdo(kNodeZd, kReqPos, 0);
1221 RequestSdo(kNodeAz, kReqPos, 0);
1222 RequestSdo(kNodeZd, kReqPos, 1);
1223 RequestSdo(kNodeAz, kReqPos, 1);
1224
1225 RequestSdo(kNodeZd, kReqKeepAlive);
1226 RequestSdo(kNodeAz, kReqKeepAlive);
1227
1228 StartReadReport();
1229 }
1230
1231 void HandleTimeoutImp(const std::list<Timeout_t>::iterator &ref, const bs::error_code &error)
1232 {
1233 if (error==ba::error::basic_errors::operation_aborted)
1234 return;
1235
1236 if (error)
1237 {
1238 ostringstream str;
1239 str << "SDO timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
1240 Error(str);
1241
1242 //PostClose();
1243 return;
1244 }
1245
1246 if (!is_open())
1247 {
1248 // For example: Here we could schedule a new accept if we
1249 // would not want to allow two connections at the same time.
1250 return;
1251 }
1252
1253 // Check whether the deadline has passed. We compare the deadline
1254 // against the current time since a new asynchronous operation
1255 // may have moved the deadline before this actor had a chance
1256 // to run.
1257 if (ref->expires_at() > ba::deadline_timer::traits_type::now())
1258 return;
1259
1260 ostringstream str;
1261 str << hex;
1262 str << "SDO timeout (";
1263 str << uint32_t(ref->node) << ": ";
1264 str << (ref->req==kTxSdo?"RX ":"TX ");
1265 str << ref->idx << "/" << uint32_t(ref->subidx) << " [" << ref->val << "] ";
1266 str << to_simple_string(ref->expires_from_now());
1267 str << ")";
1268
1269 Warn(str);
1270
1271 //PostClose();
1272 }
1273
1274 void HandleTimeout(const std::list<Timeout_t>::iterator &ref, const bs::error_code &error)
1275 {
1276 HandleTimeoutImp(ref, error);
1277 fTimeouts.erase(ref);
1278 }
1279
1280 void SendSdoRequest(uint8_t node, uint8_t req,
1281 uint16_t idx, uint8_t subidx, uint32_t val=0)
1282 {
1283 if (fVerbosity>1)
1284 Out() << "SDO-" << (req==kTxSdo?"REQ":"SET") << "[" << int(node) << "] " << idx << "/" << int(subidx) << " = " << val << endl;
1285
1286
1287 SendCanFrame(0x600|(node&0x1f), req, idx&0xff, idx>>8, subidx,
1288 val&0xff, (val>>8)&0xff, (val>>16)&0xff, (val>>24)&0xff);
1289
1290 // - The boost::asio::basic_deadline_timer::expires_from_now()
1291 // function cancels any pending asynchronous waits, and returns
1292 // the number of asynchronous waits that were cancelled. If it
1293 // returns 0 then you were too late and the wait handler has
1294 // already been executed, or will soon be executed. If it
1295 // returns 1 then the wait handler was successfully cancelled.
1296 // - If a wait handler is cancelled, the bs::error_code passed to
1297 // it contains the value bs::error::operation_aborted.
1298
1299 const uint32_t milliseconds = 3000;
1300 fTimeouts.emplace_front(get_io_service(), node, req, idx, subidx, val, milliseconds);
1301
1302 const std::list<Timeout_t>::iterator &timeout = fTimeouts.begin();
1303
1304 timeout->async_wait(boost::bind(&ConnectionDrive::HandleTimeout, this, timeout, ba::placeholders::error));
1305 }
1306
1307public:
1308 ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
1309 fVerbosity(0), fData(11)
1310 {
1311 SetLogStream(&imp);
1312 }
1313
1314 void SetVerbosity(const uint16_t &v)
1315 {
1316 fVerbosity = v;
1317 }
1318
1319 uint16_t GetVerbosity() const
1320 {
1321 return fVerbosity;
1322 }
1323
1324 void RequestSdo(uint8_t node, uint16_t idx, uint8_t subidx=0)
1325 {
1326 SendSdoRequest(node, kTxSdo, idx, subidx);
1327 }
1328 void SendSdo(uint8_t node, uint16_t idx, uint8_t subidx, uint32_t val)
1329 {
1330 SendSdoRequest(node, kTxSdo4, idx, subidx, val);
1331 }
1332
1333 void SendSdo(uint8_t node, uint16_t idx, uint32_t val)
1334 {
1335 SendSdo(node, idx, 0, val);
1336 }
1337
1338 bool IsMoving() const
1339 {
1340 return (fStatusAxis[0]&kAxisMoving) || (fStatusAxis[1]&kAxisMoving)
1341 || (fStatusAxis[0]&kAxisRpmMode) || (fStatusAxis[1]&kAxisRpmMode);
1342 }
1343
1344 bool IsInitialized() const
1345 {
1346 // All important information has been successfully requested from the
1347 // SPS and the power control units are in RF (Regler freigegeben)
1348 return fIsInitialized[0] && fIsInitialized[1];
1349 }
1350
1351 bool HasWarning() const
1352 {
1353 const uint8_t typ0 = fErrCode[0]>>16;
1354 const uint8_t typ1 = fErrCode[1]>>16;
1355 return typ0==0xe || typ1==0xe;
1356 }
1357
1358 bool HasError() const
1359 {
1360 const uint8_t typ0 = fErrCode[0]>>16;
1361 const uint8_t typ1 = fErrCode[1]>>16;
1362 return typ0==0xf || typ1==0xf;
1363 }
1364
1365 bool IsOnline() const
1366 {
1367 return fErrCode[0]!=0 && fErrCode[1]!=0;
1368 }
1369
1370 bool IsReady() const
1371 {
1372 return fStatusAxis[0]&kAxisRf && fStatusAxis[1]&kAxisRf;
1373 }
1374
1375 bool IsBlocked() const
1376 {
1377 return (fStatusSys&kEmergencyOk)==0 || (fStatusSys&kManualMode);
1378 }
1379
1380 Encoder GetSePos() const // [rev]
1381 {
1382 return Encoder(double(fPdoPos2[1])/fPosRes[1], double(fPdoPos2[0])/fPosRes[0]);
1383 }
1384
1385 double GetSeTime() const // [rev]
1386 {
1387 // The maximum difference here should not be larger than 100ms.
1388 // So th error we make on both axes should not exceed 50ms;
1389 return (Time(fPdoTime2[0]).Mjd()+Time(fPdoTime2[1]).Mjd())/2;
1390 }
1391
1392 Encoder GetVelUnit() const
1393 {
1394 return Encoder(fVelMax[1], fVelMax[0]);
1395 }
1396
1397 void SetRpmMode(bool mode)
1398 {
1399 const uint32_t val = mode ? String('s','t','r','t') : String('s','t','o','p');
1400 SendSdo(kNodeAz, kSetRpmMode, val);
1401 SendSdo(kNodeZd, kSetRpmMode, val);
1402 }
1403
1404 void SetAcceleration(const Acceleration &acc)
1405 {
1406 SendSdo(kNodeAz, kSetAcc, lrint(acc.az*1000000000+0.5));
1407 SendSdo(kNodeZd, kSetAcc, lrint(acc.zd*1000000000+0.5));
1408 }
1409
1410 void SetPointingVelocity(const Velocity &vel, double scale=1)
1411 {
1412 SendSdo(kNodeAz, kSetPointVel, lrint(vel.az*fVelMax[0]*scale));
1413 SendSdo(kNodeZd, kSetPointVel, lrint(vel.zd*fVelMax[1]*scale));
1414 }
1415 void SetTrackingVelocity(const Velocity &vel)
1416 {
1417 SendSdo(kNodeAz, kSetTrackVel, lrint(vel.az*fVelRes[0]));
1418 SendSdo(kNodeZd, kSetTrackVel, lrint(vel.zd*fVelRes[1]));
1419 }
1420
1421 void StartAbsolutePositioning(const Encoder &enc, bool zd, bool az)
1422 {
1423 if (az) SendSdo(kNodeAz, kSetPosition, lrint(enc.az*fPosRes[0]));
1424 if (zd) SendSdo(kNodeZd, kSetPosition, lrint(enc.zd*fPosRes[1]));
1425
1426 // Make sure that the status is set correctly already before the first PDO
1427 if (az) fStatusAxis[0] |= 0x02;
1428 if (zd) fStatusAxis[1] |= 0x02;
1429
1430 // FIXME: UpdateDim?
1431 }
1432
1433 void SetLedVoltage(const uint32_t &v1, const uint32_t &v2)
1434 {
1435 SendSdo(kNodeAz, 0x4000, v1);
1436 SendSdo(kNodeZd, 0x4000, v2);
1437 }
1438};
1439
1440
1441// ------------------------------------------------------------------------
1442
1443#include "DimDescriptionService.h"
1444
1445class ConnectionDimDrive : public ConnectionDrive
1446{
1447private:
1448 DimDescribedService fDimPointing;
1449 DimDescribedService fDimTracking;
1450 DimDescribedService fDimSource;
1451 DimDescribedService fDimTPoint;
1452 DimDescribedService fDimStatus;
1453
1454 // Update dim from a different thread to ensure that these
1455 // updates cannot block the main eventloop which eventually
1456 // also checks the timeouts
1457 Queue<pair<Time,array<double, 2>>> fQueuePointing;
1458 Queue<pair<Time,array<double, 12>>> fQueueTracking;
1459 Queue<tuple<Time,vector<char>,bool>> fQueueSource;
1460 Queue<pair<Time,vector<char>>> fQueueTPoint;
1461 Queue<pair<Time,array<uint8_t, 3>>> fQueueStatus;
1462
1463 bool SendPointing(const pair<Time,array<double,2>> &p)
1464 {
1465 fDimPointing.setData(p.second);
1466 fDimPointing.Update(p.first);
1467 return true;
1468 }
1469
1470 bool SendTracking(const pair<Time,array<double, 12>> &p)
1471 {
1472 fDimTracking.setData(p.second);
1473 fDimTracking.Update(p.first);
1474 return true;
1475 }
1476
1477 bool SendSource(const tuple<Time,vector<char>,bool> &t)
1478 {
1479 const Time &time = get<0>(t);
1480 const vector<char> &data = get<1>(t);
1481 const bool &tracking = get<2>(t);
1482
1483 fDimSource.setQuality(tracking);
1484 fDimSource.setData(data);
1485 fDimSource.Update(time);
1486 return true;
1487 }
1488
1489 bool SendStatus(const pair<Time,array<uint8_t, 3>> &p)
1490 {
1491 fDimStatus.setData(p.second);
1492 fDimStatus.Update(p.first);
1493 return true;
1494 }
1495
1496 bool SendTPoint(const pair<Time,vector<char>> &p)
1497 {
1498 fDimTPoint.setData(p.second);
1499 fDimTPoint.Update(p.first);
1500 return true;
1501 }
1502
1503public:
1504 void UpdatePointing(const Time &t, const array<double, 2> &arr)
1505 {
1506 fQueuePointing.emplace(t, arr);
1507 }
1508
1509 void UpdateTracking(const Time &t,const array<double, 12> &arr)
1510 {
1511 fQueueTracking.emplace(t, arr);
1512 }
1513
1514 void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
1515 {
1516 fQueueStatus.emplace(t, arr);
1517 }
1518
1519 void UpdateTPoint(const Time &t, const DimTPoint &data,
1520 const string &name)
1521 {
1522 vector<char> dim(sizeof(data)+name.length()+1);
1523 memcpy(dim.data(), &data, sizeof(data));
1524 memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1);
1525
1526 fQueueTPoint.emplace(t, dim);
1527 }
1528
1529 void UpdateSource(const Time &t, const string &name, bool tracking)
1530 {
1531 vector<char> dat(5*sizeof(double)+31, 0);
1532 strncpy(dat.data()+5*sizeof(double), name.c_str(), 30);
1533
1534 fQueueSource.emplace(t, dat, tracking);
1535 }
1536
1537 void UpdateSource(const Time &t, const array<double, 5> &arr, const string &name="")
1538 {
1539 vector<char> dat(5*sizeof(double)+31, 0);
1540 memcpy(dat.data(), arr.data(), 5*sizeof(double));
1541 strncpy(dat.data()+5*sizeof(double), name.c_str(), 30);
1542
1543 fQueueSource.emplace(t, dat, true);
1544 }
1545
1546public:
1547 ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
1548 ConnectionDrive(ioservice, imp),
1549 fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1",
1550 "|Zd[deg]:Zenith distance (derived from encoder readout)"
1551 "|Az[deg]:Azimuth angle (derived from encoder readout)"),
1552 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",
1553 "|Ra[h]:Command right ascension pointing direction (J2000)"
1554 "|Dec[deg]:Command declination pointing direction (J2000)"
1555 "|Ha[h]:Hour angle pointing direction"
1556 "|SrcRa[h]:Right ascension source (J2000)"
1557 "|SrcDec[deg]:Declination source (J2000)"
1558 "|SrcHa[h]:Hour angle source"
1559 "|Zd[deg]:Nominal zenith distance"
1560 "|Az[deg]:Nominal azimuth angle"
1561 "|dZd[deg]:Control deviation Zd"
1562 "|dAz[deg]:Control deviation Az"
1563 "|dev[arcsec]:Absolute control deviation"
1564 "|avgdev[arcsec]:Average control deviation used to define OnTrack"),
1565 fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;C:31",
1566 "|Ra_src[h]:Source right ascension"
1567 "|Dec_src[deg]:Source declination"
1568 "|Offset[deg]:Wobble offset"
1569 "|Angle[deg]:Wobble angle"
1570 "|Period[min]:Time for one orbit"
1571 "|Name[string]:Source name if available"),
1572 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",
1573 "|Ra[h]:Command right ascension"
1574 "|Dec[deg]:Command declination"
1575 "|Zd_nom[deg]:Nominal zenith distance"
1576 "|Az_nom[deg]:Nominal azimuth angle"
1577 "|Zd_cur[deg]:Current zenith distance (calculated from image)"
1578 "|Az_cur[deg]:Current azimuth angle (calculated from image)"
1579 "|Zd_enc[deg]:Feedback zenith axis (from encoder)"
1580 "|Az_enc[deg]:Feedback azimuth angle (from encoder)"
1581 "|N_leds[cnt]:Number of detected LEDs"
1582 "|N_rings[cnt]:Number of rings used to calculate the camera center"
1583 "|Xc[pix]:X position of center in CCD camera frame"
1584 "|Yc[pix]:Y position of center in CCD camera frame"
1585 "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)"
1586 "|Xs[pix]:X position of start in CCD camera frame"
1587 "|Ys[pix]:Y position of star in CCD camera frame"
1588 "|Ms[mag]:Artifical magnitude of star (calculated from image)"
1589 "|Phi[deg]:Rotation angle of image derived from detected LEDs"
1590 "|Mc[mag]:Catalog magnitude of star"
1591 "|Dx[arcsec]:De-rotated dx"
1592 "|Dy[arcsec]:De-rotated dy"
1593 "|Name[string]:Name of star"),
1594 fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", ""),
1595 fQueuePointing(std::bind(&ConnectionDimDrive::SendPointing, this, placeholders::_1)),
1596 fQueueTracking(std::bind(&ConnectionDimDrive::SendTracking, this, placeholders::_1)),
1597 fQueueSource( std::bind(&ConnectionDimDrive::SendSource, this, placeholders::_1)),
1598 fQueueTPoint( std::bind(&ConnectionDimDrive::SendTPoint, this, placeholders::_1)),
1599 fQueueStatus( std::bind(&ConnectionDimDrive::SendStatus, this, placeholders::_1))
1600 {
1601 }
1602
1603 // 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
1604};
1605
1606// ------------------------------------------------------------------------
1607
1608template <class T, class S>
1609class StateMachineDrive : public StateMachineAsio<T>
1610{
1611private:
1612 S fDrive;
1613
1614 ba::deadline_timer fTrackingLoop;
1615
1616 string fDatabase;
1617
1618 typedef map<string, Source> sources;
1619 sources fSources;
1620
1621 Weather fWeather;
1622 uint16_t fWeatherTimeout;
1623
1624 ZdAz fParkingPos;
1625
1626 PointingModel fPointingModel;
1627 PointingSetup fPointingSetup;
1628 Encoder fMovementTarget;
1629
1630 Time fSunRise;
1631
1632 Encoder fPointingMin;
1633 Encoder fPointingMax;
1634
1635 uint16_t fDeviationLimit;
1636 uint16_t fDeviationCounter;
1637 uint16_t fDeviationMax;
1638
1639 float fApproachingLimit;
1640
1641 vector<double> fDevBuffer;
1642 uint64_t fDevCount;
1643
1644 uint64_t fTrackingCounter;
1645
1646
1647 // --------------------- DIM Sending ------------------
1648
1649 bool CheckEventSize(size_t has, const char *name, size_t size)
1650 {
1651 if (has==size)
1652 return true;
1653
1654 ostringstream msg;
1655 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
1656 T::Fatal(msg);
1657 return false;
1658 }
1659
1660 // --------------------- DIM Receiving ------------------
1661
1662 int HandleWeatherData(const EventImp &evt)
1663 {
1664 if (evt.GetSize()==0)
1665 {
1666 T::Warn("MAGIC_WEATHER disconnected... using default weather values");
1667 fWeather.time = Time(Time::none);
1668 return T::GetCurrentState();
1669 }
1670
1671 if (!CheckEventSize(evt.GetSize(), "HandleWeatherData", 7*4+2))
1672 {
1673 fWeather.time = Time(Time::none);
1674 return T::GetCurrentState();
1675 }
1676
1677 const float *ptr = evt.Ptr<float>(2);
1678
1679 fWeather.temp = ptr[0];
1680 fWeather.hum = ptr[2];
1681 fWeather.press = ptr[3];
1682 fWeather.time = evt.GetTime();
1683
1684 return T::GetCurrentState();
1685 }
1686
1687 int HandleTPoint(const EventImp &evt)
1688 {
1689 // Skip disconnect events
1690 if (evt.GetSize()==0)
1691 return T::GetCurrentState();
1692
1693 // skip invalid events
1694 if (!CheckEventSize(evt.GetSize(), "HandleTPoint", 11*8))
1695 return T::GetCurrentState();
1696
1697 // skip event which are older than one minute
1698 if (Time().UnixTime()-evt.GetTime().UnixTime()>60)
1699 return T::GetCurrentState();
1700
1701 // Original code in slaTps2c:
1702 //
1703 // From the tangent plane coordinates of a star of known RA,Dec,
1704 // determine the RA,Dec of the tangent point.
1705
1706 const double *ptr = evt.Ptr<double>();
1707
1708 // Tangent plane rectangular coordinates
1709 const double dx = ptr[0] * M_PI/648000; // [arcsec -> rad]
1710 const double dy = ptr[1] * M_PI/648000; // [arcsec -> rad]
1711
1712 const PointingData data = fPointingModel.CalcPointingPos(fPointingSetup, evt.GetTime().Mjd(), fWeather, fWeatherTimeout, true);
1713
1714 const double x2 = dx*dx;
1715 const double y2 = 1 + dy*dy;
1716
1717 const double sd = cos(data.sky.zd);//sin(M_PI/2-sky.zd);
1718 const double cd = sin(data.sky.zd);//cos(M_PI/2-sky.zd);
1719 const double sdf = sd*sqrt(x2+y2);
1720 const double r2 = cd*cd*y2 - sd*sd*x2;
1721
1722 // Case of no solution ("at the pole") or
1723 // two solutions ("over the pole solution")
1724 if (r2<0 || fabs(sdf)>=1)
1725 {
1726 T::Warn("Could not determine pointing direction from TPoint.");
1727 return T::GetCurrentState();
1728 }
1729
1730 const double r = sqrt(r2);
1731 const double s = sdf - dy * r;
1732 const double c = sdf * dy + r;
1733 const double phi = atan2(dx, r);
1734
1735 // Spherical coordinates of tangent point
1736 const double az = fmod(data.sky.az-phi + 2*M_PI, 2*M_PI);
1737 const double zd = M_PI/2 - atan2(s, c);
1738
1739 const Encoder dev = fDrive.GetSePos()*360 - data.mount;
1740
1741 // --- Output TPoint ---
1742
1743 const string fname = "tpoints-"+to_string(evt.GetTime().NightAsInt())+".txt";
1744 //time.GetAsStr("/%Y/%m/%d");
1745
1746 const bool exist = boost::filesystem::exists(fname);
1747
1748 ofstream fout(fname, ios::app);
1749 if (!exist)
1750 {
1751 fout << "FACT Model TPOINT data file" << endl;
1752 fout << ": ALTAZ" << endl;
1753 fout << "49 48 0 ";
1754 fout << evt.GetTime() << endl;
1755 }
1756 fout << setprecision(7);
1757 fout << fmod(az*180/M_PI+360, 360) << " ";
1758 fout << 90-zd*180/M_PI << " ";
1759 fout << fmod(data.mount.az+360, 360) << " ";
1760 fout << 90-data.mount.zd << " ";
1761 fout << dev.az << " "; // delta az
1762 fout << -dev.zd << " "; // delta el
1763 fout << 90-data.sky.zd * 180/M_PI << " ";
1764 fout << data.sky.az * 180/M_PI << " ";
1765 fout << setprecision(10);
1766 fout << data.mjd << " ";
1767 fout << setprecision(7);
1768 fout << ptr[6] << " "; // center.mag
1769 fout << ptr[9] << " "; // star.mag
1770 fout << ptr[4] << " "; // center.x
1771 fout << ptr[5] << " "; // center.y
1772 fout << ptr[7] << " "; // star.x
1773 fout << ptr[8] << " "; // star.y
1774 fout << ptr[2] << " "; // num leds
1775 fout << ptr[3] << " "; // num rings
1776 fout << ptr[0] << " "; // dx (de-rotated)
1777 fout << ptr[1] << " "; // dy (de-rotated)
1778 fout << ptr[10] << " "; // rotation angle
1779 fout << fPointingSetup.source.mag << " ";
1780 fout << fPointingSetup.source.name;
1781 fout << endl;
1782
1783 DimTPoint dim;
1784 dim.fRa = data.pointing.ra * 12/M_PI;
1785 dim.fDec = data.pointing.dec * 180/M_PI;
1786 dim.fNominalZd = data.sky.zd * 180/M_PI;
1787 dim.fNominalAz = data.sky.az * 180/M_PI;
1788 dim.fPointingZd = zd * 180/M_PI;
1789 dim.fPointingAz = az * 180/M_PI;
1790 dim.fFeedbackZd = data.mount.zd;
1791 dim.fFeedbackAz = data.mount.az;
1792 dim.fNumLeds = uint16_t(ptr[2]);
1793 dim.fNumRings = uint16_t(ptr[3]);
1794 dim.fCenterX = ptr[4];
1795 dim.fCenterY = ptr[5];
1796 dim.fCenterMag = ptr[6];
1797 dim.fStarX = ptr[7];
1798 dim.fStarY = ptr[8];
1799 dim.fStarMag = ptr[9];
1800 dim.fRotation = ptr[10];
1801 dim.fDx = ptr[0];
1802 dim.fDy = ptr[1];
1803 dim.fRealMag = fPointingSetup.source.mag;
1804
1805 fDrive.UpdateTPoint(evt.GetTime(), dim, fPointingSetup.source.name);
1806
1807 ostringstream txt;
1808 txt << "TPoint recorded [" << zd*180/M_PI << "/" << az*180/M_PI << " | "
1809 << data.sky.zd*180/M_PI << "/" << data.sky.az*180/M_PI << " | "
1810 << data.mount.zd << "/" << data.mount.az << " | "
1811 << dx*180/M_PI << "/" << dy*180/M_PI << "]";
1812 T::Info(txt);
1813
1814 return T::GetCurrentState();
1815 }
1816
1817 // -------------------------- Helpers -----------------------------------
1818
1819 double GetDevAbs(double nomzd, double meszd, double devaz)
1820 {
1821 nomzd *= M_PI/180;
1822 meszd *= M_PI/180;
1823 devaz *= M_PI/180;
1824
1825 const double x = sin(meszd) * sin(nomzd) * cos(devaz);
1826 const double y = cos(meszd) * cos(nomzd);
1827
1828 return acos(x + y) * 180/M_PI;
1829 }
1830
1831 double ReadAngle(istream &in)
1832 {
1833 char sgn;
1834 uint16_t d, m;
1835 float s;
1836
1837 in >> sgn >> d >> m >> s;
1838
1839 const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
1840 return sgn=='-' ? -ret : ret;
1841 }
1842
1843 bool CheckRange(ZdAz pos)
1844 {
1845 if (pos.zd<fPointingMin.zd)
1846 {
1847 T::Error("Zenith distance "+to_string(pos.zd)+" below limit "+to_string(fPointingMin.zd));
1848 return false;
1849 }
1850
1851 if (pos.zd>fPointingMax.zd)
1852 {
1853 T::Error("Zenith distance "+to_string(pos.zd)+" exceeds limit "+to_string(fPointingMax.zd));
1854 return false;
1855 }
1856
1857 if (pos.az<fPointingMin.az)
1858 {
1859 T::Error("Azimuth angle "+to_string(pos.az)+" below limit "+to_string(fPointingMin.az));
1860 return false;
1861 }
1862
1863 if (pos.az>fPointingMax.az)
1864 {
1865 T::Error("Azimuth angle "+to_string(pos.az)+" exceeds limit "+to_string(fPointingMax.az));
1866 return false;
1867 }
1868
1869 return true;
1870 }
1871
1872 PointingData CalcPointingPos(double mjd)
1873 {
1874 return fPointingModel.CalcPointingPos(fPointingSetup, mjd, fWeather, fWeatherTimeout);
1875 }
1876
1877 // ----------------------------- SDO Commands ------------------------------
1878
1879 int RequestSdo(const EventImp &evt)
1880 {
1881 // FIXME: STop telescope
1882 if (!CheckEventSize(evt.GetSize(), "RequestSdo", 6))
1883 return T::kSM_FatalError;
1884
1885 const uint16_t node = evt.Get<uint16_t>();
1886 const uint16_t index = evt.Get<uint16_t>(2);
1887 const uint16_t subidx = evt.Get<uint16_t>(4);
1888
1889 if (node!=1 && node !=3)
1890 {
1891 T::Error("Node id must be 1 (az) or 3 (zd).");
1892 return T::GetCurrentState();
1893 }
1894
1895 if (subidx>0xff)
1896 {
1897 T::Error("Subindex must not be larger than 255.");
1898 return T::GetCurrentState();
1899 }
1900
1901 fDrive.RequestSdo(node, index, subidx);
1902
1903 return T::GetCurrentState();
1904 }
1905
1906 int SendSdo(const EventImp &evt)
1907 {
1908 if (!CheckEventSize(evt.GetSize(), "SendSdo", 6+8))
1909 return T::kSM_FatalError;
1910
1911 const uint16_t node = evt.Get<uint16_t>();
1912 const uint16_t index = evt.Get<uint16_t>(2);
1913 const uint16_t subidx = evt.Get<uint16_t>(4);
1914 const uint64_t value = evt.Get<uint64_t>(6);
1915
1916 if (node!=1 && node!=3)
1917 {
1918 T::Error("Node id must be 1 (az) or 3 (zd).");
1919 return T::GetCurrentState();
1920 }
1921
1922 if (subidx>0xff)
1923 {
1924 T::Error("Subindex must not be larger than 255.");
1925 return T::GetCurrentState();
1926 }
1927
1928 fDrive.SendSdo(node, index, subidx, value);
1929
1930 return T::GetCurrentState();
1931 }
1932
1933 // --------------------- Moving and tracking ---------------------
1934
1935 uint16_t fStep;
1936 bool fIsTracking;
1937 Acceleration fAccPointing;
1938 Acceleration fAccTracking;
1939 Acceleration fAccMax;
1940 double fMaxPointingResidual;
1941 double fMaxParkingResidual;
1942 double fPointingVelocity;
1943
1944 int InitMovement(const ZdAz &sky, bool tracking=false, const string &name="")
1945 {
1946 fMovementTarget = fPointingModel.SkyToMount(sky);
1947
1948 // Check whether bending is valid!
1949 if (!CheckRange(sky*(180/M_PI)))
1950 return StopMovement();
1951
1952 fStep = 0;
1953 fIsTracking = tracking;
1954
1955 fDrive.SetRpmMode(false); // *NEW* (Stop a previous tracking to avoid the pointing command to be ignored)
1956 fDrive.SetAcceleration(fAccPointing);
1957
1958 if (!tracking)
1959 fDrive.UpdateSource(Time(), name, false);
1960 else
1961 {
1962 const array<double, 5> dim =
1963 {{
1964 fPointingSetup.source.ra,
1965 fPointingSetup.source.dec,
1966 fPointingSetup.wobble_offset * 180/M_PI,
1967 fPointingSetup.wobble_angle * 180/M_PI,
1968 fPointingSetup.orbit_period * 24*60
1969 }};
1970 fDrive.UpdateSource(fPointingSetup.start, dim, fPointingSetup.source.name);
1971 }
1972
1973 return State::kMoving;
1974 }
1975
1976 int MoveTo(const EventImp &evt)
1977 {
1978 if (!CheckEventSize(evt.GetSize(), "MoveTo", 16))
1979 return T::kSM_FatalError;
1980
1981 const double *dat = evt.Ptr<double>();
1982
1983 ostringstream out;
1984 out << "Pointing telescope to Zd=" << dat[0] << "deg Az=" << dat[1] << "deg";
1985 T::Message(out);
1986
1987 return InitMovement(ZdAz(dat[0]*M_PI/180, dat[1]*M_PI/180));
1988 }
1989
1990 int InitTracking()
1991 {
1992 fPointingSetup.start = Time().Mjd();
1993
1994 const PointingData data = CalcPointingPos(fPointingSetup.start);
1995
1996 ostringstream out;
1997 out << "Tracking position now at Zd=" << data.sky.zd*180/M_PI << "deg Az=" << data.sky.az*180/M_PI << "deg";
1998 T::Info(out);
1999
2000 return InitMovement(data.sky, true);
2001 }
2002
2003 int StartTracking(const Source &src, double offset, double angle, double period=0)
2004 {
2005 if (src.ra<0 || src.ra>=24)
2006 {
2007 ostringstream out;
2008 out << "Right ascension out of range [0;24[: Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2009 if (!src.name.empty())
2010 out << " [" << src.name << "]";
2011 T::Error(out);
2012 return State::kInvalidCoordinates;
2013 }
2014 if (src.dec<-90 || src.dec>90)
2015 {
2016 ostringstream out;
2017 out << "Declination out of range [-90;90]: 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
2024 ostringstream out;
2025 out << "Tracking Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2026 if (!src.name.empty())
2027 out << " [" << src.name << "]";
2028 T::Info(out);
2029
2030 fPointingSetup.planet = kENone;
2031 fPointingSetup.source = src;
2032 fPointingSetup.orbit_period = period / 1440; // [min->day]
2033 fPointingSetup.wobble_angle = angle * M_PI/180; // [deg->rad]
2034 fPointingSetup.wobble_offset = offset * M_PI/180; // [deg->rad]
2035
2036 return InitTracking();
2037 }
2038
2039 int TrackCelest(const Planets_t &p)
2040 {
2041 switch (p)
2042 {
2043 case kEMoon: fPointingSetup.source.name = "Moon"; break;
2044 case kEVenus: fPointingSetup.source.name = "Venus"; break;
2045 case kEMars: fPointingSetup.source.name = "Mars"; break;
2046 case kEJupiter: fPointingSetup.source.name = "Jupiter"; break;
2047 case kESaturn: fPointingSetup.source.name = "Saturn"; break;
2048 default:
2049 T::Error("TrackCelest - Celestial object "+to_string(p)+" not yet supported.");
2050 return T::GetCurrentState();
2051 }
2052
2053 fPointingSetup.planet = p;
2054 fPointingSetup.wobble_offset = 0;
2055
2056 fDrive.UpdateSource(Time(), fPointingSetup.source.name, true);
2057
2058 return InitTracking();
2059 }
2060
2061 int Park()
2062 {
2063 ostringstream out;
2064 out << "Parking telescope at Zd=" << fParkingPos.zd << "deg Az=" << fParkingPos.az << "deg";
2065 T::Message(out);
2066
2067 const int rc = InitMovement(ZdAz(fParkingPos.zd*M_PI/180, fParkingPos.az*M_PI/180), false, "Park");
2068 return rc==State::kMoving ? State::kParking : rc;
2069 }
2070
2071 int Wobble(const EventImp &evt)
2072 {
2073 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
2074 return T::kSM_FatalError;
2075
2076 const double *dat = evt.Ptr<double>();
2077
2078 Source src;
2079 src.ra = dat[0];
2080 src.dec = dat[1];
2081 return StartTracking(src, dat[2], dat[3]);
2082 }
2083
2084 int Orbit(const EventImp &evt)
2085 {
2086 if (!CheckEventSize(evt.GetSize(), "Orbit", 40))
2087 return T::kSM_FatalError;
2088
2089 const double *dat = evt.Ptr<double>();
2090
2091 Source src;
2092 src.ra = dat[0];
2093 src.dec = dat[1];
2094 return StartTracking(src, dat[2], dat[3], dat[4]);
2095 }
2096
2097 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
2098 {
2099 if (find(ptr, last, '\0')==last)
2100 {
2101 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
2102 throw uint32_t(T::kSM_FatalError);
2103 }
2104
2105 const string name(ptr);
2106
2107 sources::const_iterator it = fSources.find(name);
2108 if (it!=fSources.end())
2109 return it;
2110
2111 T::Warn("Source '"+name+"' not found in list... reloading database.");
2112
2113 ReloadSources();
2114
2115 it = fSources.find(name);
2116 if (it!=fSources.end())
2117 return it;
2118
2119 T::Error("Source '"+name+"' still not found in list.");
2120 throw uint32_t(T::GetCurrentState());
2121 }
2122
2123 int TrackWobble(const EventImp &evt)
2124 {
2125 if (evt.GetSize()<2)
2126 {
2127 ostringstream msg;
2128 msg << "TrackWobble - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
2129 T::Fatal(msg);
2130 return T::kSM_FatalError;
2131 }
2132
2133 if (evt.GetSize()==2)
2134 {
2135 ostringstream msg;
2136 msg << "TrackWobble - Source name missing.";
2137 T::Error(msg);
2138 return T::GetCurrentState();
2139 }
2140
2141 const uint16_t wobble = evt.GetUShort();
2142 if (wobble!=1 && wobble!=2)
2143 {
2144 ostringstream msg;
2145 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
2146 T::Error(msg);
2147 return T::GetCurrentState();
2148 }
2149
2150 const char *ptr = evt.Ptr<char>(2);
2151 const char *last = ptr+evt.GetSize()-2;
2152
2153 try
2154 {
2155 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2156
2157 const Source &src = it->second;
2158 return StartTracking(src, src.offset, src.angles[wobble-1]);
2159 }
2160 catch (const uint32_t &e)
2161 {
2162 return e;
2163 }
2164 }
2165
2166 int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0, double time=0)
2167 {
2168 const char *last = ptr+size;
2169
2170 try
2171 {
2172 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2173
2174 const Source &src = it->second;
2175 return StartTracking(src, offset<0?0.6/*src.offset*/:offset, angle, time);
2176 }
2177 catch (const uint32_t &e)
2178 {
2179 return e;
2180 }
2181 }
2182
2183 int Track(const EventImp &evt)
2184 {
2185 if (!CheckEventSize(evt.GetSize(), "Track", 16))
2186 return T::kSM_FatalError;
2187
2188 Source src;
2189
2190 src.name = "";
2191 src.ra = evt.Get<double>(0);
2192 src.dec = evt.Get<double>(8);
2193
2194 return StartTracking(src, 0, 0);
2195 }
2196
2197 int TrackSource(const EventImp &evt)
2198 {
2199 if (evt.GetSize()<16)
2200 {
2201 ostringstream msg;
2202 msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2203 T::Fatal(msg);
2204 return T::kSM_FatalError;
2205 }
2206
2207 if (evt.GetSize()==16)
2208 {
2209 ostringstream msg;
2210 msg << "TrackOn - Source name missing.";
2211 T::Error(msg);
2212 return T::GetCurrentState();
2213 }
2214
2215 const double offset = evt.Get<double>(0);
2216 const double angle = evt.Get<double>(8);
2217
2218 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, offset, angle);
2219 }
2220
2221 int TrackOn(const EventImp &evt)
2222 {
2223 if (evt.GetSize()==0)
2224 {
2225 ostringstream msg;
2226 msg << "TrackOn - Source name missing.";
2227 T::Error(msg);
2228 return T::GetCurrentState();
2229 }
2230
2231 return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
2232 }
2233
2234 int TrackOrbit(const EventImp &evt)
2235 {
2236 if (evt.GetSize()<16)
2237 {
2238 ostringstream msg;
2239 msg << "TrackOrbit - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2240 T::Fatal(msg);
2241 return T::kSM_FatalError;
2242 }
2243 if (evt.GetSize()==16)
2244 {
2245 ostringstream msg;
2246 msg << "TrackOrbit - Source name missing.";
2247 T::Error(msg);
2248 return T::GetCurrentState();
2249 }
2250
2251 const double angle = evt.Get<double>(0);
2252 const double time = evt.Get<double>(8);
2253
2254 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, -1, angle, time);
2255 }
2256
2257 int StopMovement(bool reload=false)
2258 {
2259 fDrive.SetAcceleration(fAccMax);
2260 fDrive.SetRpmMode(false);
2261
2262 fTrackingLoop.cancel();
2263
2264 fDrive.UpdateSource(Time(), "", false);
2265
2266 if (reload)
2267 ReloadSources();
2268
2269 return State::kStopping;
2270 }
2271
2272 int ResetError()
2273 {
2274 const int rc = CheckState();
2275 return rc>0 ? rc : State::kInitialized;
2276 }
2277
2278 // --------------------- Others ---------------------
2279
2280 int TPoint()
2281 {
2282 T::Info("TPoint initiated.");
2283 Dim::SendCommandNB("TPOINT/EXECUTE");
2284 return T::GetCurrentState();
2285 }
2286
2287 int Screenshot(const EventImp &evt)
2288 {
2289 if (evt.GetSize()<2)
2290 {
2291 ostringstream msg;
2292 msg << "Screenshot - Received event has " << evt.GetSize() << " bytes, but expected at least 2.";
2293 T::Fatal(msg);
2294 return T::kSM_FatalError;
2295 }
2296
2297 if (evt.GetSize()==2)
2298 {
2299 ostringstream msg;
2300 msg << "Screenshot - Filename missing.";
2301 T::Error(msg);
2302 return T::GetCurrentState();
2303 }
2304
2305 T::Info("Screenshot initiated.");
2306 Dim::SendCommandNB("TPOINT/SCREENSHOT", evt.GetData(), evt.GetSize());
2307 return T::GetCurrentState();
2308 }
2309
2310 int SetLedBrightness(const EventImp &evt)
2311 {
2312 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
2313 return T::kSM_FatalError;
2314
2315 const uint32_t *led = evt.Ptr<uint32_t>();
2316
2317 fDrive.SetLedVoltage(led[0], led[1]);
2318
2319 return T::GetCurrentState();
2320 }
2321
2322 int SetLedsOff()
2323 {
2324 fDrive.SetLedVoltage(0, 0);
2325 return T::GetCurrentState();
2326 }
2327
2328 // --------------------- Internal ---------------------
2329
2330 int SetVerbosity(const EventImp &evt)
2331 {
2332 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 2))
2333 return T::kSM_FatalError;
2334
2335 fDrive.SetVerbosity(evt.GetUShort());
2336
2337 return T::GetCurrentState();
2338 }
2339
2340 int Print()
2341 {
2342 for (auto it=fSources.begin(); it!=fSources.end(); it++)
2343 {
2344 const string &name = it->first;
2345 const Source &src = it->second;
2346
2347 T::Out() << name << ",";
2348 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
2349 T::Out() << src.angles[0] << "," << src.angles[1] << endl;
2350 }
2351 return T::GetCurrentState();
2352 }
2353
2354 int PrintPointingModel()
2355 {
2356 fPointingModel.print(T::Out());
2357 return T::GetCurrentState();
2358 }
2359
2360 int Unlock()
2361 {
2362 const int rc = CheckState();
2363 return rc<0 ? State::kInitialized : rc;
2364 }
2365
2366 int ReloadSources()
2367 {
2368 try
2369 {
2370 ReadDatabase(false);
2371 }
2372 catch (const exception &e)
2373 {
2374 T::Error("Reading sources from databse failed: "+string(e.what()));
2375 }
2376 return T::GetCurrentState();
2377 }
2378
2379 int Disconnect()
2380 {
2381 // Close all connections
2382 fDrive.PostClose(false);
2383
2384 /*
2385 // Now wait until all connection have been closed and
2386 // all pending handlers have been processed
2387 poll();
2388 */
2389
2390 return T::GetCurrentState();
2391 }
2392
2393 int Reconnect(const EventImp &evt)
2394 {
2395 // Close all connections to supress the warning in SetEndpoint
2396 fDrive.PostClose(false);
2397
2398 // Now wait until all connection have been closed and
2399 // all pending handlers have been processed
2400 ba::io_service::poll();
2401
2402 if (evt.GetBool())
2403 fDrive.SetEndpoint(evt.GetString());
2404
2405 // Now we can reopen the connection
2406 fDrive.PostClose(true);
2407
2408 return T::GetCurrentState();
2409 }
2410
2411 // ========================= Tracking code =============================
2412
2413 int UpdateTrackingPosition()
2414 {
2415 // First calculate deviation between
2416 // command position and nominal position
2417 //fPointing.mount = sepos; // [deg] ref pos for alignment
2418 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2419
2420 // Get current position and calculate deviation
2421 const Encoder sepos = fDrive.GetSePos()*360; // [deg]
2422 const Encoder dev = sepos - data.mount;
2423
2424 // Calculate absolut deviation on the sky
2425 const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az)*3600;
2426
2427 // Smoothing
2428 fDevBuffer[fDevCount++%5] = absdev;
2429
2430 // Calculate average
2431 const uint8_t cnt = fDevCount<5 ? fDevCount : 5;
2432 const double avgdev = accumulate(fDevBuffer.begin(), fDevBuffer.begin()+cnt, 0.)/cnt;
2433
2434 // Count the consecutive number of avgdev below fDeviationLimit
2435 if (avgdev<fDeviationLimit)
2436 fTrackingCounter++;
2437 else
2438 fTrackingCounter = 0;
2439
2440 const double ha = fmod(fDrive.GetSeTime(),1)*24 - Nova::kORM.lng/15;
2441
2442 array<double, 12> dim;
2443 dim[0] = data.pointing.ra * 12/M_PI; // Ra [h] optical axis
2444 dim[1] = data.pointing.dec * 180/M_PI; // Dec [deg] optical axis
2445 dim[2] = ha - data.pointing.ra; // Ha [h] optical axis
2446 dim[3] = data.source.ra * 12/M_PI; // SrcRa [h] source position
2447 dim[4] = data.source.dec * 180/M_PI; // SrcDec [deg] source position
2448 dim[5] = ha - data.source.ra; // SrcHa [h] source position
2449 dim[6] = data.sky.zd * 180/M_PI; // Zd [deg] optical axis
2450 dim[7] = data.sky.az * 180/M_PI; // Az [deg] optical axis
2451 dim[8] = dev.zd; // dZd [deg] control deviation
2452 dim[9] = dev.az; // dAz [deg] control deviation
2453 dim[10] = absdev; // dev [arcsec] absolute control deviation
2454 dim[11] = avgdev; // dev [arcsec] average control deviation
2455
2456 fDrive.UpdateTracking(fDrive.GetSeTime(), dim);
2457
2458 if (fDrive.GetVerbosity())
2459 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Deviation [deg] " << absdev << "\"|" << avgdev << "\"|" << fDevCount<< " dZd=" << dev.zd*3600 << "\" dAz=" << dev.az*3600 << "\"" << endl;
2460
2461 // Maximum deviation execeeded -> fall back to Tracking state
2462 if (T::GetCurrentState()==State::kOnTrack && avgdev>fDeviationMax)
2463 return State::kTracking;
2464
2465 // Condition for OnTrack state achieved -> enhance to OnTrack state
2466 if (T::GetCurrentState()==State::kTracking && fTrackingCounter>=fDeviationCounter)
2467 return State::kOnTrack;
2468
2469 // No state change
2470 return T::GetCurrentState();
2471 }
2472
2473 void UpdatePointingPosition()
2474 {
2475 const Encoder sepos = fDrive.GetSePos()*360; // [deg] ref pos for alignment
2476
2477 const ZdAz pos = fPointingModel.MountToSky(sepos);
2478
2479 array<double, 2> data;
2480 data[0] = pos.zd*180/M_PI; // Zd [deg]
2481 data[1] = pos.az*180/M_PI; // Az [deg]
2482 fDrive.UpdatePointing(fDrive.GetSeTime(), data);
2483
2484 if (fDrive.GetVerbosity())
2485 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Position [deg] " << pos.zd*180/M_PI << " " << pos.az*180/M_PI << endl;
2486 }
2487
2488 void TrackingLoop(const boost::system::error_code &error=boost::system::error_code())
2489 {
2490 if (error==ba::error::basic_errors::operation_aborted)
2491 return;
2492
2493 if (error)
2494 {
2495 ostringstream str;
2496 str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl;
2497 T::Error(str);
2498 return;
2499 }
2500
2501 if (T::GetCurrentState()!=State::kTracking &&
2502 T::GetCurrentState()!=State::kOnTrack)
2503 return;
2504
2505 //
2506 // Update speed as often as possible.
2507 // make sure, that dt is around 10 times larger than the
2508 // update time
2509 //
2510 // The loop should not be executed faster than the ramp of
2511 // a change in the velocity can be followed.
2512 //
2513 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(250));
2514
2515 const double mjd = Time().Mjd();
2516
2517 // I assume that it takes about 50ms for the value to be
2518 // transmitted and the drive needs time to follow as well (maybe
2519 // more than 50ms), therefore the calculated speec is calculated
2520 // for a moment 50ms in the future
2521 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2522 const PointingData data0 = CalcPointingPos(mjd-0.45/24/3600);
2523 const PointingData data1 = CalcPointingPos(mjd+0.55/24/3600);
2524
2525 const Encoder dest = data.mount *(1./360); // [rev]
2526 const Encoder dest0 = data0.mount*(1./360); // [rev]
2527 const Encoder dest1 = data1.mount*(1./360); // [rev]
2528
2529 if (!CheckRange(data1.sky))
2530 {
2531 StopMovement();
2532 T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop");
2533 return;
2534 }
2535
2536 // Current position
2537 const Encoder sepos = fDrive.GetSePos(); // [rev]
2538
2539 // Now calculate the current velocity
2540 const Encoder dist = dest1 - dest0; // [rev] Distance between t-1s and t+1s
2541 const Velocity vel = dist/(1./60); // [rev/min] Actual velocity of the pointing position
2542
2543 const Encoder dev = sepos - dest; // [rev] Current control deviation
2544 const Velocity vt = vel - dev/(1./60); // [rev/min] Correct velocity by recent control deviation
2545 // correct control deviation with 5s
2546 if (fDrive.GetVerbosity()>1)
2547 {
2548 T::Out() << "Ideal position [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2549 T::Out() << "Encoder pos. [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2550 T::Out() << "Deviation [arcmin] " << dev.zd *360*60 << " " << dev.az *360*60 << endl;
2551 T::Out() << "Distance 1s [arcmin] " << dist.zd *360*60 << " " << dist.az *360*60 << endl;
2552 T::Out() << "Velocity 1s [rpm] " << vt.zd << " " << vt.az << endl;
2553 T::Out() << "Delta T (enc) [ms] " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl;
2554 T::Out() << "Delta T (now) [ms] " << (Time().Mjd()-mjd)*24*3600*1000 << endl;
2555 }
2556
2557 // Tracking loop every 250ms
2558 // Vorsteuerung 2s
2559 // Delta T (enc) 5ms, every 5th, 25ms
2560 // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s)
2561
2562 //
2563 // FIXME: check if the drive is fast enough to follow the star
2564 //
2565 // Velocity units (would be 100 for %)
2566
2567 fDrive.SetTrackingVelocity(vt);
2568
2569 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2570 this, ba::placeholders::error));
2571 }
2572
2573 // =====================================================================
2574
2575 int CheckState()
2576 {
2577 if (!fDrive.IsConnected())
2578 return State::kDisconnected;
2579
2580 if (!fDrive.IsOnline())
2581 return State::kUnavailable;
2582
2583 // FIXME: This can prevent parking in case e.g.
2584 // of e8029 Position limit exceeded
2585 if (fDrive.HasWarning() || fDrive.HasError())
2586 {
2587 if (T::GetCurrentState()==State::kOnTrack ||
2588 T::GetCurrentState()==State::kTracking ||
2589 T::GetCurrentState()==State::kMoving ||
2590 T::GetCurrentState()==State::kApproaching ||
2591 T::GetCurrentState()==State::kParking)
2592 return StopMovement();
2593
2594 if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving())
2595 return State::kStopping;
2596
2597 if (fDrive.HasError())
2598 return State::kHardwareError;
2599
2600 if (fDrive.HasWarning())
2601 return State::kHardwareWarning;
2602
2603 return StateMachineImp::kSM_Error;
2604 }
2605
2606 // This can happen if one of the drives is not in RF.
2607 // Usually this only happens when the drive is not yet in RF
2608 // or an error was just cleared. Usually there is no way that
2609 // a drive goes below the RF state during operation without
2610 // a warning or error message.
2611 if (fDrive.IsOnline() && fDrive.IsBlocked())
2612 return State::kBlocked;
2613
2614 if (fDrive.IsOnline() && !fDrive.IsReady())
2615 return State::kAvailable;
2616
2617 // This is the case as soon as the init commands were send
2618 // after a connection to the SPS was established
2619 if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized())
2620 return State::kArmed;
2621
2622 return -1;
2623 }
2624
2625 int Execute()
2626 {
2627 const Time now;
2628 if (now>fSunRise && T::GetCurrentState()!=State::kParking)
2629 {
2630 fSunRise = now.GetNextSunRise();
2631
2632 ostringstream msg;
2633 msg << "Next sun-rise will be at " << fSunRise;
2634 T::Info(msg);
2635
2636 if (T::GetCurrentState()>State::kArmed && T::GetCurrentState()!=StateMachineImp::kError)
2637 return Park();
2638 }
2639
2640 if (T::GetCurrentState()==State::kLocked)
2641 return State::kLocked;
2642
2643 // FIXME: Send STOP if IsPositioning or RpmActive but no
2644 // Moving or Tracking state
2645
2646 const int rc = CheckState();
2647 if (rc>0)
2648 return rc;
2649
2650 // Once every second
2651 static time_t lastTime = 0;
2652 const time_t tm = time(NULL);
2653 if (lastTime!=tm && fDrive.IsInitialized())
2654 {
2655 lastTime=tm;
2656
2657 UpdatePointingPosition();
2658
2659 if (T::GetCurrentState()==State::kTracking || T::GetCurrentState()==State::kOnTrack)
2660 return UpdateTrackingPosition();
2661 }
2662
2663 if (T::GetCurrentState()==State::kStopping && !fDrive.IsMoving())
2664 return State::kArmed;
2665
2666 if ((T::GetCurrentState()==State::kMoving || T::GetCurrentState()==State::kApproaching ||
2667 T::GetCurrentState()==State::kParking) && !fDrive.IsMoving())
2668 {
2669 if (fIsTracking && fStep==1)
2670 {
2671 // Init tracking
2672 fDrive.SetAcceleration(fAccTracking);
2673 fDrive.SetRpmMode(true);
2674
2675 fDevCount = 0;
2676 fTrackingCounter = 0;
2677
2678 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(1));
2679 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2680 this, ba::placeholders::error));
2681
2682 fPointingSetup.start = Time().Mjd();
2683
2684 const PointingData data = CalcPointingPos(fPointingSetup.start);
2685
2686 ostringstream out;
2687 out << "Start tracking at Ra=" << data.pointing.ra*12/M_PI << "h Dec=" << data.pointing.dec*180/M_PI << "deg";
2688 T::Info(out);
2689
2690 return State::kTracking;
2691 }
2692
2693 // Get feedback 2
2694 const Encoder dest = fMovementTarget*(1./360); // [rev]
2695 const Encoder sepos = fDrive.GetSePos(); // [rev]
2696
2697 // Calculate residual to move deviation
2698 const Encoder dist = dest - sepos; // [rev]
2699
2700 // Check which axis should still be moved
2701 Encoder cd = dist; // [rev]
2702 cd *= T::GetCurrentState()==State::kParking ? 1./fMaxParkingResidual : 1./fMaxPointingResidual; // Scale to units of the maximum residual
2703 cd = cd.Abs();
2704
2705 // Check if there is a control deviation on the axis
2706 const bool cdzd = cd.zd>1;
2707 const bool cdaz = cd.az>1;
2708
2709 if (!fIsTracking)
2710 {
2711 // check if we reached the correct position already
2712 if (!cdzd && !cdaz)
2713 {
2714 T::Info("Target position reached in "+to_string(fStep)+" steps.");
2715 return T::GetCurrentState()==State::kParking ? State::kLocked : State::kArmed;
2716 }
2717
2718 if (fStep==10)
2719 {
2720 T::Error("Target position not reached in "+to_string(fStep)+" steps.");
2721 return State::kPositioningFailed;
2722 }
2723 }
2724
2725 const Encoder t = dist.Abs()/fDrive.GetVelUnit();
2726
2727 const Velocity vel =
2728 t.zd > t.az ?
2729 Velocity(1, t.zd==0?0:t.az/t.zd) :
2730 Velocity(t.az==0?0:t.zd/t.az, 1);
2731
2732 if (fDrive.GetVerbosity())
2733 {
2734 T::Out() << "Moving step " << fStep << endl;
2735 T::Out() << "Encoder [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2736 T::Out() << "Destination [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2737 T::Out() << "Residual [deg] " << dist.zd *360 << " " << dist.az *360 << endl;
2738 T::Out() << "Residual/max [1] " << cd.zd << " " << cd.az << endl;
2739 T::Out() << "Rel. time [1] " << t.zd << " " << t.az << endl;
2740 T::Out() << "Rel. velocity [1] " << vel.zd << " " << vel.az << endl;
2741 }
2742
2743 fDrive.SetPointingVelocity(vel, fPointingVelocity);
2744 fDrive.StartAbsolutePositioning(dest, cdzd, cdaz);
2745
2746 ostringstream out;
2747 if (fStep==0)
2748 out << "Moving to encoder Zd=" << dest.zd*360 << "deg Az=" << dest.az*360 << "deg";
2749 else
2750 out << "Moving residual of dZd=" << dist.zd*360*60 << "' dAz=" << dist.az*360*60 << "'";
2751 T::Info(out);
2752
2753 fStep++;
2754 }
2755
2756 if (T::GetCurrentState()==State::kMoving && fDrive.IsMoving() && fIsTracking)
2757 {
2758 // First calculate deviation between
2759 // command position and nominal position
2760 //fPointing.mount = sepos; // [deg] ref pos for alignment
2761 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2762
2763 // Get current position and calculate deviation
2764 const Encoder sepos = fDrive.GetSePos()*360; // [deg]
2765 const Encoder dev = sepos - data.mount;
2766
2767 // Calculate absolut deviation on the sky
2768 const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az);
2769
2770 if (absdev<fApproachingLimit)
2771 return State::kApproaching;
2772 }
2773
2774 return T::GetCurrentState()>=State::kInitialized ?
2775 T::GetCurrentState() : State::kInitialized;
2776 }
2777
2778public:
2779 StateMachineDrive(ostream &out=cout) :
2780 StateMachineAsio<T>(out, "DRIVE_CONTROL"), fDrive(*this, *this),
2781 fTrackingLoop(*this), fSunRise(Time().GetNextSunRise()), fDevBuffer(5)
2782 {
2783
2784 T::Subscribe("MAGIC_WEATHER/DATA")
2785 (bind(&StateMachineDrive::HandleWeatherData, this, placeholders::_1));
2786
2787 T::Subscribe("TPOINT/DATA")
2788 (bind(&StateMachineDrive::HandleTPoint, this, placeholders::_1));
2789
2790 // State names
2791 T::AddStateName(State::kDisconnected, "Disconnected",
2792 "No connection to SPS");
2793 T::AddStateName(State::kConnected, "Connected",
2794 "Connection to SPS, no information received yet");
2795
2796 T::AddStateName(State::kLocked, "Locked",
2797 "Drive system is locked (will not accept commands)");
2798
2799 T::AddStateName(State::kUnavailable, "Unavailable",
2800 "Connected to SPS, no connection to at least one IndraDrives");
2801 T::AddStateName(State::kAvailable, "Available",
2802 "Connected to SPS and to IndraDrives, but at least one drive not in RF");
2803 T::AddStateName(State::kBlocked, "Blocked",
2804 "Drive system is blocked by manual operation or a pressed emergeny button");
2805 T::AddStateName(State::kArmed, "Armed",
2806 "Connected to SPS and IndraDrives in RF, but not yet initialized");
2807 T::AddStateName(State::kInitialized, "Initialized",
2808 "Connected to SPS and IndraDrives in RF and initialized");
2809
2810 T::AddStateName(State::kStopping, "Stopping",
2811 "Stop command sent, waiting for telescope to be still");
2812 T::AddStateName(State::kParking, "Parking",
2813 "Telescope in parking operation, waiting for telescope to be still");
2814 T::AddStateName(State::kMoving, "Moving",
2815 "Telescope moving");
2816 T::AddStateName(State::kApproaching, "Approaching",
2817 "Telescope approaching destination");
2818 T::AddStateName(State::kTracking, "Tracking",
2819 "Telescope in tracking mode");
2820 T::AddStateName(State::kOnTrack, "OnTrack",
2821 "Telescope tracking stable");
2822
2823 T::AddStateName(State::kPositioningFailed, "PositioningFailed",
2824 "Target position was not reached within ten steps");
2825 T::AddStateName(State::kAllowedRangeExceeded, "OutOfRange",
2826 "Telecope went out of range during tracking");
2827 T::AddStateName(State::kInvalidCoordinates, "InvalidCoordinates",
2828 "Tracking coordinates out of range");
2829
2830 T::AddStateName(State::kHardwareWarning, "HardwareWarning",
2831 "At least one IndraDrive in a warning condition... check carefully!");
2832 T::AddStateName(State::kHardwareError, "HardwareError",
2833 "At least one IndraDrive in an error condition... this is a serious incident!");
2834
2835
2836 T::AddEvent("REQUEST_SDO", "S:3", State::kArmed)
2837 (bind(&StateMachineDrive::RequestSdo, this, placeholders::_1))
2838 ("Request an SDO from the drive"
2839 "|node[uint32]:Node identifier (1:az, 3:zd)"
2840 "|index[uint32]:SDO index"
2841 "|subindex[uint32]:SDO subindex");
2842
2843 T::AddEvent("SET_SDO", "S:3;X:1", State::kArmed)
2844 (bind(&StateMachineDrive::SendSdo, this, placeholders::_1))
2845 ("Request an SDO from the drive"
2846 "|node[uint32]:Node identifier (1:az, 3:zd)"
2847 "|index[uint32]:SDO index"
2848 "|subindex[uint32]:SDO subindex"
2849 "|value[uint64]:Value");
2850
2851 // Drive Commands
2852 T::AddEvent("MOVE_TO", "D:2", State::kInitialized) // ->ZDAZ
2853 (bind(&StateMachineDrive::MoveTo, this, placeholders::_1))
2854 ("Move the telescope to the given local sky coordinates"
2855 "|Zd[deg]:Zenith distance"
2856 "|Az[deg]:Azimuth");
2857
2858 T::AddEvent("TRACK", "D:2", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2859 (bind(&StateMachineDrive::Track, this, placeholders::_1))
2860 ("Move the telescope to the given sky coordinates and start tracking them"
2861 "|Ra[h]:Right ascension"
2862 "|Dec[deg]:Declination");
2863
2864 T::AddEvent("WOBBLE", "D:4", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2865 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
2866 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
2867 "|Ra[h]:Right ascension"
2868 "|Dec[deg]:Declination"
2869 "|Offset[deg]:Wobble offset"
2870 "|Angle[deg]:Wobble angle");
2871
2872 T::AddEvent("ORBIT", "D:5", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2873 (bind(&StateMachineDrive::Orbit, this, placeholders::_1))
2874 ("Move the telescope in a circle around the source"
2875 "|Ra[h]:Right ascension"
2876 "|Dec[deg]:Declination"
2877 "|Offset[deg]:Wobble offset"
2878 "|Angle[deg]:Starting angle"
2879 "|Period[min]:Time for one orbit");
2880
2881 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2882 (bind(&StateMachineDrive::TrackSource, this, placeholders::_1))
2883 ("Move the telescope to the given wobble position around the given source and start tracking"
2884 "|Offset[deg]:Wobble offset"
2885 "|Angle[deg]:Wobble angle"
2886 "|Name[string]:Source name");
2887
2888 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2889 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
2890 ("Move the telescope to the given wobble position around the given source and start tracking"
2891 "|Id:Wobble angle id (1 or 2)"
2892 "|Name[string]:Source name");
2893
2894 T::AddEvent("TRACK_ORBIT", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2895 (bind(&StateMachineDrive::TrackOrbit, this, placeholders::_1))
2896 ("Move the telescope in a circle around the source"
2897 "|Angle[deg]:Starting angle"
2898 "|Period[min]:Time for one orbit"
2899 "|Name[string]:Source name");
2900
2901 T::AddEvent("TRACK_ON", "C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2902 (bind(&StateMachineDrive::TrackOn, this, placeholders::_1))
2903 ("Move the telescope to the given position and start tracking"
2904 "|Name[string]:Source name");
2905
2906 T::AddEvent("MOON", State::kInitialized, State::kTracking, State::kOnTrack)
2907 (bind(&StateMachineDrive::TrackCelest, this, kEMoon))
2908 ("Start tracking the moon");
2909 T::AddEvent("VENUS", State::kInitialized, State::kTracking, State::kOnTrack)
2910 (bind(&StateMachineDrive::TrackCelest, this, kEVenus))
2911 ("Start tracking Venus");
2912 T::AddEvent("MARS", State::kInitialized, State::kTracking, State::kOnTrack)
2913 (bind(&StateMachineDrive::TrackCelest, this, kEMars))
2914 ("Start tracking Mars");
2915 T::AddEvent("JUPITER", State::kInitialized, State::kTracking, State::kOnTrack)
2916 (bind(&StateMachineDrive::TrackCelest, this, kEJupiter))
2917 ("Start tracking Jupiter");
2918 T::AddEvent("SATURN", State::kInitialized, State::kTracking, State::kOnTrack)
2919 (bind(&StateMachineDrive::TrackCelest, this, kESaturn))
2920 ("Start tracking Saturn");
2921
2922 // FIXME: What to do in error state?
2923 T::AddEvent("PARK")(State::kInitialized)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)(State::kHardwareWarning)
2924 (bind(&StateMachineDrive::Park, this))
2925 ("Park the telescope");
2926
2927 T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)
2928 (bind(&StateMachineDrive::StopMovement, this, false))
2929 ("Stop any kind of movement.");
2930
2931 T::AddEvent("PREPARE_GRB")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)
2932 (bind(&StateMachineDrive::StopMovement, this, true))
2933 ("Combines STOP and RELOAD_SOURCES");
2934
2935 T::AddEvent("RESET", State::kPositioningFailed, State::kAllowedRangeExceeded, State::kInvalidCoordinates, State::kHardwareWarning)
2936 (bind(&StateMachineDrive::ResetError, this))
2937 ("Acknowledge an internal drivectrl error (PositioningFailed, AllowedRangeExceeded, InvalidCoordinates)");
2938
2939 T::AddEvent("TPOINT", State::kOnTrack)
2940 (bind(&StateMachineDrive::TPoint, this))
2941 ("Take a TPoint");
2942
2943 T::AddEvent("SCREENSHOT", "B:1;C")
2944 (bind(&StateMachineDrive::Screenshot, this, placeholders::_1))
2945 ("Take a screenshot"
2946 "|color[bool]:False if just the gray image should be saved."
2947 "|name[string]:Filename");
2948
2949 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
2950 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
2951 ("Set the LED brightness of the top and bottom leds"
2952 "|top[au]:Allowed range 0-32767 for top LEDs"
2953 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
2954
2955 T::AddEvent("LEDS_OFF")
2956 (bind(&StateMachineDrive::SetLedsOff, this))
2957 ("Switch off TPoint LEDs");
2958
2959 T::AddEvent("UNLOCK", Drive::State::kLocked)
2960 (bind(&StateMachineDrive::Unlock, this))
2961 ("Unlock locked state.");
2962
2963 // Verbosity commands
2964 T::AddEvent("SET_VERBOSITY", "S:1")
2965 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
2966 ("Set verbosity state"
2967 "|verbosity[uint16]:disable or enable verbosity for received data (yes/no), except dynamic data");
2968
2969 // Conenction commands
2970 T::AddEvent("DISCONNECT", State::kConnected, State::kUnavailable)
2971 (bind(&StateMachineDrive::Disconnect, this))
2972 ("disconnect from ethernet");
2973
2974 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected, State::kUnavailable)
2975 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
2976 ("(Re)connect Ethernet connection to SPS, a new address can be given"
2977 "|[host][string]:new ethernet address in the form <host:port>");
2978
2979
2980 T::AddEvent("PRINT_POINTING_MODEL")
2981 (bind(&StateMachineDrive::PrintPointingModel, this))
2982 ("Print the ponting model.");
2983
2984
2985 T::AddEvent("PRINT")
2986 (bind(&StateMachineDrive::Print, this))
2987 ("Print source list.");
2988
2989 T::AddEvent("RELOAD_SOURCES", State::kDisconnected, State::kConnected, State::kArmed, State::kInitialized, State::kLocked)
2990 (bind(&StateMachineDrive::ReloadSources, this))
2991 ("Reload sources from database after database has changed..");
2992
2993
2994 //fDrive.SetUpdateStatus(std::bind(&StateMachineDrive::UpdateStatus, this, placeholders::_1, placeholders::_2));
2995 fDrive.StartConnect();
2996 }
2997
2998 void SetEndpoint(const string &url)
2999 {
3000 fDrive.SetEndpoint(url);
3001 }
3002
3003 bool AddSource(const string &name, const Source &src)
3004 {
3005 const auto it = fSources.find(name);
3006 if (it!=fSources.end())
3007 T::Warn("Source '"+name+"' already in list... overwriting.");
3008
3009 fSources[name] = src;
3010 return it==fSources.end();
3011 }
3012
3013 void ReadDatabase(bool print=true)
3014 {
3015#ifdef HAVE_SQL
3016 Database db(fDatabase);
3017
3018 T::Message("Loading sources from '"+db.uri()+"'");
3019
3020 const mysqlpp::StoreQueryResult res =
3021 db.query("SELECT fSourceName, fRightAscension, fDeclination, fWobbleOffset, fWobbleAngle0, fWobbleAngle1, fMagnitude FROM Source").store();
3022
3023 auto old = fSources;
3024
3025 fSources.clear();
3026 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
3027 {
3028 const string name = (*v)[0].c_str();
3029
3030 Source src;
3031 src.name = name;
3032 src.ra = (*v)[1];
3033 src.dec = (*v)[2];
3034 src.offset = (*v)[3];
3035 src.angles[0] = (*v)[4];
3036 src.angles[1] = (*v)[5];
3037 src.mag = (*v)[6] ? double((*v)[6]) : 0;
3038 AddSource(name, src);
3039
3040 ostringstream msg;
3041 msg << "> " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
3042 msg << " Wobble=[" << src.offset << "," << src.angles[0] << "," << src.angles[1] << "] Mag=" << src.mag;
3043
3044 const auto it = old.find(name);
3045 if (it==old.end())
3046 T::Message(" <a"+msg.str());
3047 else
3048 {
3049 if (print || it->second!=src)
3050 T::Message(" <c"+msg.str());
3051 old.erase(it);
3052 }
3053 }
3054
3055 for (auto it=old.begin(); it!=old.end(); it++)
3056 {
3057 const auto &src = it->second;
3058
3059 ostringstream msg;
3060 msg << " <r> " << it->first << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
3061 msg << " Wobble=[" << src.offset << "," << src.angles[0] << "," << src.angles[1] << "] Mag=" << src.mag;
3062 T::Message(msg.str());
3063 }
3064
3065 T::Message("Loaded "+to_string(fSources.size())+" sources from '"+db.uri()+"'");
3066
3067#else
3068 T::Warn("MySQL support not compiled into the program.");
3069#endif
3070 }
3071
3072 int EvalOptions(Configuration &conf)
3073 {
3074 if (!fSunRise)
3075 return 1;
3076
3077 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
3078
3079 fMaxPointingResidual = conf.Get<double>("pointing.max.residual");
3080 fPointingVelocity = conf.Get<double>("pointing.velocity");
3081
3082 fPointingMin = Encoder(conf.Get<double>("pointing.min.zd"),
3083 conf.Get<double>("pointing.min.az"));
3084 fPointingMax = Encoder(conf.Get<double>("pointing.max.zd"),
3085 conf.Get<double>("pointing.max.az"));
3086
3087 fParkingPos.zd = conf.Has("parking-pos.zd") ? conf.Get<double>("parking-pos.zd") : 90;
3088 fParkingPos.az = conf.Has("parking-pos.az") ? conf.Get<double>("parking-pos.az") : 0;
3089 fMaxParkingResidual = conf.Get<double>("parking-pos.residual");
3090
3091 if (!CheckRange(fParkingPos))
3092 return 2;
3093
3094 fAccPointing = Acceleration(conf.Get<double>("pointing.acceleration.zd"),
3095 conf.Get<double>("pointing.acceleration.az"));
3096 fAccTracking = Acceleration(conf.Get<double>("tracking.acceleration.zd"),
3097 conf.Get<double>("tracking.acceleration.az"));
3098 fAccMax = Acceleration(conf.Get<double>("acceleration.max.zd"),
3099 conf.Get<double>("acceleration.max.az"));
3100
3101 fWeatherTimeout = conf.Get<uint16_t>("weather-timeout");
3102
3103 if (fAccPointing>fAccMax)
3104 {
3105 T::Error("Pointing acceleration exceeds maximum acceleration.");
3106 return 3;
3107 }
3108
3109 if (fAccTracking>fAccMax)
3110 {
3111 T::Error("Tracking acceleration exceeds maximum acceleration.");
3112 return 4;
3113 }
3114
3115 fApproachingLimit = conf.Get<float>("approaching-limit");
3116
3117 fDeviationLimit = conf.Get<uint16_t>("deviation-limit");
3118 fDeviationCounter = conf.Get<uint16_t>("deviation-count");
3119 fDeviationMax = conf.Get<uint16_t>("deviation-max");
3120
3121 const string fname = conf.GetPrefixedString("pointing.model-file");
3122
3123 try
3124 {
3125 fPointingModel.Load(fname);
3126 }
3127 catch (const exception &e)
3128 {
3129 T::Error(e.what());
3130 return 5;
3131 }
3132
3133 const vector<string> &vec = conf.Vec<string>("source");
3134
3135 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
3136 {
3137 istringstream stream(*it);
3138
3139 string name;
3140
3141 int i=0;
3142
3143 Source src;
3144
3145 string buffer;
3146 while (getline(stream, buffer, ','))
3147 {
3148 istringstream is(buffer);
3149
3150 switch (i++)
3151 {
3152 case 0: name = buffer; break;
3153 case 1: src.ra = ReadAngle(is); break;
3154 case 2: src.dec = ReadAngle(is); break;
3155 case 3: is >> src.offset; break;
3156 case 4: is >> src.angles[0]; break;
3157 case 5: is >> src.angles[1]; break;
3158 }
3159
3160 if (is.fail())
3161 break;
3162 }
3163
3164 if (i==3 || i==6)
3165 {
3166 AddSource(name, src);
3167 continue;
3168 }
3169
3170 T::Warn("Resource 'source' not correctly formatted: '"+*it+"'");
3171 }
3172
3173 //fAutoResume = conf.Get<bool>("auto-resume");
3174
3175 if (conf.Has("source-database"))
3176 {
3177 fDatabase = conf.Get<string>("source-database");
3178 ReadDatabase();
3179 }
3180
3181 if (fSunRise.IsValid())
3182 {
3183 ostringstream msg;
3184 msg << "Next sun-rise will be at " << fSunRise;
3185 T::Message(msg);
3186 }
3187
3188 // The possibility to connect should be last, so that
3189 // everything else is already initialized.
3190 SetEndpoint(conf.Get<string>("addr"));
3191
3192 return -1;
3193 }
3194};
3195
3196// ------------------------------------------------------------------------
3197
3198#include "Main.h"
3199
3200
3201template<class T, class S, class R>
3202int RunShell(Configuration &conf)
3203{
3204 return Main::execute<T, StateMachineDrive<S, R>>(conf);
3205}
3206
3207void SetupConfiguration(Configuration &conf)
3208{
3209 po::options_description control("Drive control options");
3210 control.add_options()
3211 ("quiet,q", po_bool(), "Disable debug messages")
3212 ("no-dim,d", po_switch(), "Disable dim services")
3213 ("addr,a", var<string>("sps:5357"), "Network address of cosy")
3214 ("verbosity,v", var<uint16_t>(0), "Vervosity level (0=off; 1=major updates; 2=most updates; 3=frequent updates)")
3215 ("pointing.model-file", var<string>()->required(), "Name of the file with the pointing model in use")
3216 ("pointing.max.zd", var<double>( 104.9), "Maximum allowed zenith angle in sky pointing coordinates [deg]")
3217 ("pointing.max.az", var<double>( 85.0), "Maximum allowed azimuth angle in sky pointing coordinates [deg]")
3218 ("pointing.min.zd", var<double>(-104.9), "Minimum allowed zenith angle in sky pointing coordinates [deg]")
3219 ("pointing.min.az", var<double>(-295.0), "Minimum allowed azimuth angle in sky pointing coordinates [deg]")
3220 ("pointing.max.residual", var<double>(1./32768), "Maximum residual for a pointing operation [revolutions]")
3221 ("pointing.velocity", var<double>(0.3), "Moving velocity when pointing [% max]")
3222 ("pointing.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis for pointing operations")
3223 ("pointing.acceleration.zd", var<double>(0.03), "Acceleration for zenith axis for pointing operations")
3224 ("tracking.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis during tracking operations")
3225 ("tracking.acceleration.zd", var<double>(0.01), "Acceleration for zenith axis during tracking operations")
3226 ("parking-pos.zd", var<double>(101), "Parking position zenith angle in sky pointing coordinates [deg]")
3227 ("parking-pos.az", var<double>(0), "Parking position azimuth angle in sky pointing coordinates [deg]")
3228 ("parking-pos.residual", var<double>(0.5/360), "Maximum residual for a parking position [revolutions]")
3229 ("acceleration.max.az", var<double>(0.03), "Maximum allowed acceleration value for azimuth axis")
3230 ("acceleration.max.zd", var<double>(0.09), "Maximum allowed acceleration value for zenith axis")
3231 ("weather-timeout", var<uint16_t>(300), "Timeout [sec] for weather data (after timeout default values are used)")
3232 ("approaching-limit", var<float>(2.25), "Limit to get 'Approaching' state")
3233 ("deviation-limit", var<uint16_t>(90), "Deviation limit in arcsec to get 'OnTrack'")
3234 ("deviation-count", var<uint16_t>(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
3235 ("deviation-max", var<uint16_t>(180), "Maximum deviation in arcsec allowed to keep status 'OnTrack'")
3236 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database[?compress=0|1].")
3237 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
3238 ;
3239
3240 conf.AddOptions(control);
3241}
3242
3243/*
3244 Extract usage clause(s) [if any] for SYNOPSIS.
3245 Translators: "Usage" and "or" here are patterns (regular expressions) which
3246 are used to match the usage synopsis in program output. An example from cp
3247 (GNU coreutils) which contains both strings:
3248 Usage: cp [OPTION]... [-T] SOURCE DEST
3249 or: cp [OPTION]... SOURCE... DIRECTORY
3250 or: cp [OPTION]... -t DIRECTORY SOURCE...
3251 */
3252void PrintUsage()
3253{
3254 cout <<
3255 "The drivectrl is an interface to the drive PLC.\n"
3256 "\n"
3257 "The default is that the program is started without user intercation. "
3258 "All actions are supposed to arrive as DimCommands. Using the -c "
3259 "option, a local shell can be initialized. With h or help a short "
3260 "help message about the usuage can be brought to the screen.\n"
3261 "\n"
3262 "Usage: drivectrl [-c type] [OPTIONS]\n"
3263 " or: drivectrl [OPTIONS]\n";
3264 cout << endl;
3265}
3266
3267void PrintHelp()
3268{
3269 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
3270
3271 /* Additional help text which is printed after the configuration
3272 options goes here */
3273
3274 /*
3275 cout << "bla bla bla" << endl << endl;
3276 cout << endl;
3277 cout << "Environment:" << endl;
3278 cout << "environment" << endl;
3279 cout << endl;
3280 cout << "Examples:" << endl;
3281 cout << "test exam" << endl;
3282 cout << endl;
3283 cout << "Files:" << endl;
3284 cout << "files" << endl;
3285 cout << endl;
3286 */
3287}
3288
3289int main(int argc, const char* argv[])
3290{
3291 Configuration conf(argv[0]);
3292 conf.SetPrintUsage(PrintUsage);
3293 Main::SetupConfiguration(conf);
3294 SetupConfiguration(conf);
3295
3296 if (!conf.DoParse(argc, argv, PrintHelp))
3297 return 127;
3298
3299 //try
3300 {
3301 // No console access at all
3302 if (!conf.Has("console"))
3303 {
3304 if (conf.Get<bool>("no-dim"))
3305 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
3306 else
3307 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
3308 }
3309 // Cosole access w/ and w/o Dim
3310 if (conf.Get<bool>("no-dim"))
3311 {
3312 if (conf.Get<int>("console")==0)
3313 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
3314 else
3315 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
3316 }
3317 else
3318 {
3319 if (conf.Get<int>("console")==0)
3320 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
3321 else
3322 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
3323 }
3324 }
3325 /*catch (std::exception& e)
3326 {
3327 cerr << "Exception: " << e.what() << endl;
3328 return -1;
3329 }*/
3330
3331 return 0;
3332}
Note: See TracBrowser for help on using the repository browser.