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

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