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

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