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

Last change on this file since 19435 was 19416, checked in by tbretz, 6 years ago
Allow RECONNECT also if the IndraDrives are unavailable. Allow to propagate an error in PDO2 (the default in our PLCs is 'no error')
File size: 114.6 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 vector<double> fDevBuffer;
1628 uint64_t fDevCount;
1629
1630 uint64_t fTrackingCounter;
1631
1632
1633 // --------------------- DIM Sending ------------------
1634
1635 bool CheckEventSize(size_t has, const char *name, size_t size)
1636 {
1637 if (has==size)
1638 return true;
1639
1640 ostringstream msg;
1641 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
1642 T::Fatal(msg);
1643 return false;
1644 }
1645
1646 // --------------------- DIM Receiving ------------------
1647
1648 int HandleWeatherData(const EventImp &evt)
1649 {
1650 if (evt.GetSize()==0)
1651 {
1652 T::Warn("MAGIC_WEATHER disconnected... using default weather values");
1653 fWeather.time = Time(Time::none);
1654 return T::GetCurrentState();
1655 }
1656
1657 if (!CheckEventSize(evt.GetSize(), "HandleWeatherData", 7*4+2))
1658 {
1659 fWeather.time = Time(Time::none);
1660 return T::GetCurrentState();
1661 }
1662
1663 const float *ptr = evt.Ptr<float>(2);
1664
1665 fWeather.temp = ptr[0];
1666 fWeather.hum = ptr[2];
1667 fWeather.press = ptr[3];
1668 fWeather.time = evt.GetTime();
1669
1670 return T::GetCurrentState();
1671 }
1672
1673 int HandleTPoint(const EventImp &evt)
1674 {
1675 // Skip disconnect events
1676 if (evt.GetSize()==0)
1677 return T::GetCurrentState();
1678
1679 // skip invalid events
1680 if (!CheckEventSize(evt.GetSize(), "HandleTPoint", 11*8))
1681 return T::GetCurrentState();
1682
1683 // skip event which are older than one minute
1684 if (Time().UnixTime()-evt.GetTime().UnixTime()>60)
1685 return T::GetCurrentState();
1686
1687 // Original code in slaTps2c:
1688 //
1689 // From the tangent plane coordinates of a star of known RA,Dec,
1690 // determine the RA,Dec of the tangent point.
1691
1692 const double *ptr = evt.Ptr<double>();
1693
1694 // Tangent plane rectangular coordinates
1695 const double dx = ptr[0] * M_PI/648000; // [arcsec -> rad]
1696 const double dy = ptr[1] * M_PI/648000; // [arcsec -> rad]
1697
1698 const PointingData data = fPointingModel.CalcPointingPos(fPointingSetup, evt.GetTime().Mjd(), fWeather, fWeatherTimeout, true);
1699
1700 const double x2 = dx*dx;
1701 const double y2 = 1 + dy*dy;
1702
1703 const double sd = cos(data.sky.zd);//sin(M_PI/2-sky.zd);
1704 const double cd = sin(data.sky.zd);//cos(M_PI/2-sky.zd);
1705 const double sdf = sd*sqrt(x2+y2);
1706 const double r2 = cd*cd*y2 - sd*sd*x2;
1707
1708 // Case of no solution ("at the pole") or
1709 // two solutions ("over the pole solution")
1710 if (r2<0 || fabs(sdf)>=1)
1711 {
1712 T::Warn("Could not determine pointing direction from TPoint.");
1713 return T::GetCurrentState();
1714 }
1715
1716 const double r = sqrt(r2);
1717 const double s = sdf - dy * r;
1718 const double c = sdf * dy + r;
1719 const double phi = atan2(dx, r);
1720
1721 // Spherical coordinates of tangent point
1722 const double az = fmod(data.sky.az-phi + 2*M_PI, 2*M_PI);
1723 const double zd = M_PI/2 - atan2(s, c);
1724
1725 const Encoder dev = fDrive.GetSePos()*360 - data.mount;
1726
1727 // --- Output TPoint ---
1728
1729 const string fname = "tpoints-"+to_string(evt.GetTime().NightAsInt())+".txt";
1730 //time.GetAsStr("/%Y/%m/%d");
1731
1732 const bool exist = boost::filesystem::exists(fname);
1733
1734 ofstream fout(fname, ios::app);
1735 if (!exist)
1736 {
1737 fout << "FACT Model TPOINT data file" << endl;
1738 fout << ": ALTAZ" << endl;
1739 fout << "49 48 0 ";
1740 fout << evt.GetTime() << endl;
1741 }
1742 fout << setprecision(7);
1743 fout << fmod(az*180/M_PI+360, 360) << " ";
1744 fout << 90-zd*180/M_PI << " ";
1745 fout << fmod(data.mount.az+360, 360) << " ";
1746 fout << 90-data.mount.zd << " ";
1747 fout << dev.az << " "; // delta az
1748 fout << -dev.zd << " "; // delta el
1749 fout << 90-data.sky.zd * 180/M_PI << " ";
1750 fout << data.sky.az * 180/M_PI << " ";
1751 fout << setprecision(10);
1752 fout << data.mjd << " ";
1753 fout << setprecision(7);
1754 fout << ptr[6] << " "; // center.mag
1755 fout << ptr[9] << " "; // star.mag
1756 fout << ptr[4] << " "; // center.x
1757 fout << ptr[5] << " "; // center.y
1758 fout << ptr[7] << " "; // star.x
1759 fout << ptr[8] << " "; // star.y
1760 fout << ptr[2] << " "; // num leds
1761 fout << ptr[3] << " "; // num rings
1762 fout << ptr[0] << " "; // dx (de-rotated)
1763 fout << ptr[1] << " "; // dy (de-rotated)
1764 fout << ptr[10] << " "; // rotation angle
1765 fout << fPointingSetup.source.mag << " ";
1766 fout << fPointingSetup.source.name;
1767 fout << endl;
1768
1769 DimTPoint dim;
1770 dim.fRa = data.pointing.ra * 12/M_PI;
1771 dim.fDec = data.pointing.dec * 180/M_PI;
1772 dim.fNominalZd = data.sky.zd * 180/M_PI;
1773 dim.fNominalAz = data.sky.az * 180/M_PI;
1774 dim.fPointingZd = zd * 180/M_PI;
1775 dim.fPointingAz = az * 180/M_PI;
1776 dim.fFeedbackZd = data.mount.zd;
1777 dim.fFeedbackAz = data.mount.az;
1778 dim.fNumLeds = uint16_t(ptr[2]);
1779 dim.fNumRings = uint16_t(ptr[3]);
1780 dim.fCenterX = ptr[4];
1781 dim.fCenterY = ptr[5];
1782 dim.fCenterMag = ptr[6];
1783 dim.fStarX = ptr[7];
1784 dim.fStarY = ptr[8];
1785 dim.fStarMag = ptr[9];
1786 dim.fRotation = ptr[10];
1787 dim.fDx = ptr[0];
1788 dim.fDy = ptr[1];
1789 dim.fRealMag = fPointingSetup.source.mag;
1790
1791 fDrive.UpdateTPoint(evt.GetTime(), dim, fPointingSetup.source.name);
1792
1793 ostringstream txt;
1794 txt << "TPoint recorded [" << zd*180/M_PI << "/" << az*180/M_PI << " | "
1795 << data.sky.zd*180/M_PI << "/" << data.sky.az*180/M_PI << " | "
1796 << data.mount.zd << "/" << data.mount.az << " | "
1797 << dx*180/M_PI << "/" << dy*180/M_PI << "]";
1798 T::Info(txt);
1799
1800 return T::GetCurrentState();
1801 }
1802
1803 // -------------------------- Helpers -----------------------------------
1804
1805 double GetDevAbs(double nomzd, double meszd, double devaz)
1806 {
1807 nomzd *= M_PI/180;
1808 meszd *= M_PI/180;
1809 devaz *= M_PI/180;
1810
1811 const double x = sin(meszd) * sin(nomzd) * cos(devaz);
1812 const double y = cos(meszd) * cos(nomzd);
1813
1814 return acos(x + y) * 180/M_PI;
1815 }
1816
1817 double ReadAngle(istream &in)
1818 {
1819 char sgn;
1820 uint16_t d, m;
1821 float s;
1822
1823 in >> sgn >> d >> m >> s;
1824
1825 const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
1826 return sgn=='-' ? -ret : ret;
1827 }
1828
1829 bool CheckRange(ZdAz pos)
1830 {
1831 if (pos.zd<fPointingMin.zd)
1832 {
1833 T::Error("Zenith distance "+to_string(pos.zd)+" below limit "+to_string(fPointingMin.zd));
1834 return false;
1835 }
1836
1837 if (pos.zd>fPointingMax.zd)
1838 {
1839 T::Error("Zenith distance "+to_string(pos.zd)+" exceeds limit "+to_string(fPointingMax.zd));
1840 return false;
1841 }
1842
1843 if (pos.az<fPointingMin.az)
1844 {
1845 T::Error("Azimuth angle "+to_string(pos.az)+" below limit "+to_string(fPointingMin.az));
1846 return false;
1847 }
1848
1849 if (pos.az>fPointingMax.az)
1850 {
1851 T::Error("Azimuth angle "+to_string(pos.az)+" exceeds limit "+to_string(fPointingMax.az));
1852 return false;
1853 }
1854
1855 return true;
1856 }
1857
1858 PointingData CalcPointingPos(double mjd)
1859 {
1860 return fPointingModel.CalcPointingPos(fPointingSetup, mjd, fWeather, fWeatherTimeout);
1861 }
1862
1863 // ----------------------------- SDO Commands ------------------------------
1864
1865 int RequestSdo(const EventImp &evt)
1866 {
1867 // FIXME: STop telescope
1868 if (!CheckEventSize(evt.GetSize(), "RequestSdo", 6))
1869 return T::kSM_FatalError;
1870
1871 const uint16_t node = evt.Get<uint16_t>();
1872 const uint16_t index = evt.Get<uint16_t>(2);
1873 const uint16_t subidx = evt.Get<uint16_t>(4);
1874
1875 if (node!=1 && node !=3)
1876 {
1877 T::Error("Node id must be 1 (az) or 3 (zd).");
1878 return T::GetCurrentState();
1879 }
1880
1881 if (subidx>0xff)
1882 {
1883 T::Error("Subindex must not be larger than 255.");
1884 return T::GetCurrentState();
1885 }
1886
1887 fDrive.RequestSdo(node, index, subidx);
1888
1889 return T::GetCurrentState();
1890 }
1891
1892 int SendSdo(const EventImp &evt)
1893 {
1894 if (!CheckEventSize(evt.GetSize(), "SendSdo", 6+8))
1895 return T::kSM_FatalError;
1896
1897 const uint16_t node = evt.Get<uint16_t>();
1898 const uint16_t index = evt.Get<uint16_t>(2);
1899 const uint16_t subidx = evt.Get<uint16_t>(4);
1900 const uint64_t value = evt.Get<uint64_t>(6);
1901
1902 if (node!=1 && node!=3)
1903 {
1904 T::Error("Node id must be 1 (az) or 3 (zd).");
1905 return T::GetCurrentState();
1906 }
1907
1908 if (subidx>0xff)
1909 {
1910 T::Error("Subindex must not be larger than 255.");
1911 return T::GetCurrentState();
1912 }
1913
1914 fDrive.SendSdo(node, index, subidx, value);
1915
1916 return T::GetCurrentState();
1917 }
1918
1919 // --------------------- Moving and tracking ---------------------
1920
1921 uint16_t fStep;
1922 bool fIsTracking;
1923 Acceleration fAccPointing;
1924 Acceleration fAccTracking;
1925 Acceleration fAccMax;
1926 double fMaxPointingResidual;
1927 double fMaxParkingResidual;
1928 double fPointingVelocity;
1929
1930 int InitMovement(const ZdAz &sky, bool tracking=false, const string &name="")
1931 {
1932 fMovementTarget = fPointingModel.SkyToMount(sky);
1933
1934 // Check whether bending is valid!
1935 if (!CheckRange(sky*(180/M_PI)))
1936 return StopMovement();
1937
1938 fStep = 0;
1939 fIsTracking = tracking;
1940
1941 fDrive.SetRpmMode(false); // *NEW* (Stop a previous tracking to avoid the pointing command to be ignored)
1942 fDrive.SetAcceleration(fAccPointing);
1943
1944 if (!tracking)
1945 fDrive.UpdateSource(Time(), name, false);
1946 else
1947 {
1948 const array<double, 5> dim =
1949 {{
1950 fPointingSetup.source.ra,
1951 fPointingSetup.source.dec,
1952 fPointingSetup.wobble_offset * 180/M_PI,
1953 fPointingSetup.wobble_angle * 180/M_PI,
1954 fPointingSetup.orbit_period * 24*60
1955 }};
1956 fDrive.UpdateSource(fPointingSetup.start, dim, fPointingSetup.source.name);
1957 }
1958
1959 return State::kMoving;
1960 }
1961
1962 int MoveTo(const EventImp &evt)
1963 {
1964 if (!CheckEventSize(evt.GetSize(), "MoveTo", 16))
1965 return T::kSM_FatalError;
1966
1967 const double *dat = evt.Ptr<double>();
1968
1969 ostringstream out;
1970 out << "Pointing telescope to Zd=" << dat[0] << "deg Az=" << dat[1] << "deg";
1971 T::Message(out);
1972
1973 return InitMovement(ZdAz(dat[0]*M_PI/180, dat[1]*M_PI/180));
1974 }
1975
1976 int InitTracking()
1977 {
1978 fPointingSetup.start = Time().Mjd();
1979
1980 const PointingData data = CalcPointingPos(fPointingSetup.start);
1981
1982 ostringstream out;
1983 out << "Tracking position now at Zd=" << data.sky.zd*180/M_PI << "deg Az=" << data.sky.az*180/M_PI << "deg";
1984 T::Info(out);
1985
1986 return InitMovement(data.sky, true);
1987 }
1988
1989 int StartTracking(const Source &src, double offset, double angle, double period=0)
1990 {
1991 if (src.ra<0 || src.ra>=24)
1992 {
1993 ostringstream out;
1994 out << "Right ascension out of range [0;24[: Ra=" << src.ra << "h Dec=" << src.dec << "deg";
1995 if (!src.name.empty())
1996 out << " [" << src.name << "]";
1997 T::Error(out);
1998 return State::kInvalidCoordinates;
1999 }
2000 if (src.dec<-90 || src.dec>90)
2001 {
2002 ostringstream out;
2003 out << "Declination out of range [-90;90]: Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2004 if (!src.name.empty())
2005 out << " [" << src.name << "]";
2006 T::Error(out);
2007 return State::kInvalidCoordinates;
2008 }
2009
2010 ostringstream out;
2011 out << "Tracking Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2012 if (!src.name.empty())
2013 out << " [" << src.name << "]";
2014 T::Info(out);
2015
2016 fPointingSetup.planet = kENone;
2017 fPointingSetup.source = src;
2018 fPointingSetup.orbit_period = period / 1440; // [min->day]
2019 fPointingSetup.wobble_angle = angle * M_PI/180; // [deg->rad]
2020 fPointingSetup.wobble_offset = offset * M_PI/180; // [deg->rad]
2021
2022 return InitTracking();
2023 }
2024
2025 int TrackCelest(const Planets_t &p)
2026 {
2027 switch (p)
2028 {
2029 case kEMoon: fPointingSetup.source.name = "Moon"; break;
2030 case kEVenus: fPointingSetup.source.name = "Venus"; break;
2031 case kEMars: fPointingSetup.source.name = "Mars"; break;
2032 case kEJupiter: fPointingSetup.source.name = "Jupiter"; break;
2033 case kESaturn: fPointingSetup.source.name = "Saturn"; break;
2034 default:
2035 T::Error("TrackCelest - Celestial object "+to_string(p)+" not yet supported.");
2036 return T::GetCurrentState();
2037 }
2038
2039 fPointingSetup.planet = p;
2040 fPointingSetup.wobble_offset = 0;
2041
2042 fDrive.UpdateSource(Time(), fPointingSetup.source.name, true);
2043
2044 return InitTracking();
2045 }
2046
2047 int Park()
2048 {
2049 ostringstream out;
2050 out << "Parking telescope at Zd=" << fParkingPos.zd << "deg Az=" << fParkingPos.az << "deg";
2051 T::Message(out);
2052
2053 const int rc = InitMovement(ZdAz(fParkingPos.zd*M_PI/180, fParkingPos.az*M_PI/180), false, "Park");
2054 return rc==State::kMoving ? State::kParking : rc;
2055 }
2056
2057 int Wobble(const EventImp &evt)
2058 {
2059 if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
2060 return T::kSM_FatalError;
2061
2062 const double *dat = evt.Ptr<double>();
2063
2064 Source src;
2065 src.ra = dat[0];
2066 src.dec = dat[1];
2067 return StartTracking(src, dat[2], dat[3]);
2068 }
2069
2070 int Orbit(const EventImp &evt)
2071 {
2072 if (!CheckEventSize(evt.GetSize(), "Orbit", 40))
2073 return T::kSM_FatalError;
2074
2075 const double *dat = evt.Ptr<double>();
2076
2077 Source src;
2078 src.ra = dat[0];
2079 src.dec = dat[1];
2080 return StartTracking(src, dat[2], dat[3], dat[4]);
2081 }
2082
2083 const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last)
2084 {
2085 if (find(ptr, last, '\0')==last)
2086 {
2087 T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated.");
2088 throw uint32_t(T::kSM_FatalError);
2089 }
2090
2091 const string name(ptr);
2092
2093 const sources::const_iterator it = fSources.find(name);
2094 if (it==fSources.end())
2095 {
2096 T::Error("Source '"+name+"' not found in list.");
2097 throw uint32_t(T::GetCurrentState());
2098 }
2099
2100 return it;
2101 }
2102
2103 int TrackWobble(const EventImp &evt)
2104 {
2105 if (evt.GetSize()<2)
2106 {
2107 ostringstream msg;
2108 msg << "TrackWobble - Received event has " << evt.GetSize() << " bytes, but expected at least 3.";
2109 T::Fatal(msg);
2110 return T::kSM_FatalError;
2111 }
2112
2113 if (evt.GetSize()==2)
2114 {
2115 ostringstream msg;
2116 msg << "TrackWobble - Source name missing.";
2117 T::Error(msg);
2118 return T::GetCurrentState();
2119 }
2120
2121 const uint16_t wobble = evt.GetUShort();
2122 if (wobble!=1 && wobble!=2)
2123 {
2124 ostringstream msg;
2125 msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed.";
2126 T::Error(msg);
2127 return T::GetCurrentState();
2128 }
2129
2130 const char *ptr = evt.Ptr<char>(2);
2131 const char *last = ptr+evt.GetSize()-2;
2132
2133 try
2134 {
2135 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2136
2137 const Source &src = it->second;
2138 return StartTracking(src, src.offset, src.angles[wobble-1]);
2139 }
2140 catch (const uint32_t &e)
2141 {
2142 return e;
2143 }
2144 }
2145
2146 int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0, double time=0)
2147 {
2148 const char *last = ptr+size;
2149
2150 try
2151 {
2152 const sources::const_iterator it = GetSourceFromDB(ptr, last);
2153
2154 const Source &src = it->second;
2155 return StartTracking(src, offset<0?0.6/*src.offset*/:offset, angle, time);
2156 }
2157 catch (const uint32_t &e)
2158 {
2159 return e;
2160 }
2161 }
2162
2163 int Track(const EventImp &evt)
2164 {
2165 if (!CheckEventSize(evt.GetSize(), "Track", 16))
2166 return T::kSM_FatalError;
2167
2168 Source src;
2169
2170 src.name = "";
2171 src.ra = evt.Get<double>(0);
2172 src.dec = evt.Get<double>(8);
2173
2174 return StartTracking(src, 0, 0);
2175 }
2176
2177 int TrackSource(const EventImp &evt)
2178 {
2179 if (evt.GetSize()<16)
2180 {
2181 ostringstream msg;
2182 msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2183 T::Fatal(msg);
2184 return T::kSM_FatalError;
2185 }
2186
2187 if (evt.GetSize()==16)
2188 {
2189 ostringstream msg;
2190 msg << "TrackOn - Source name missing.";
2191 T::Error(msg);
2192 return T::GetCurrentState();
2193 }
2194
2195 const double offset = evt.Get<double>(0);
2196 const double angle = evt.Get<double>(8);
2197
2198 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, offset, angle);
2199 }
2200
2201 int TrackOn(const EventImp &evt)
2202 {
2203 if (evt.GetSize()==0)
2204 {
2205 ostringstream msg;
2206 msg << "TrackOn - Source name missing.";
2207 T::Error(msg);
2208 return T::GetCurrentState();
2209 }
2210
2211 return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
2212 }
2213
2214 int TrackOrbit(const EventImp &evt)
2215 {
2216 if (evt.GetSize()<16)
2217 {
2218 ostringstream msg;
2219 msg << "TrackOrbit - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
2220 T::Fatal(msg);
2221 return T::kSM_FatalError;
2222 }
2223 if (evt.GetSize()==16)
2224 {
2225 ostringstream msg;
2226 msg << "TrackOrbit - Source name missing.";
2227 T::Error(msg);
2228 return T::GetCurrentState();
2229 }
2230
2231 const double angle = evt.Get<double>(0);
2232 const double time = evt.Get<double>(8);
2233
2234 return StartTrackWobble(evt.Ptr<char>(16), evt.GetSize()-16, -1, angle, time);
2235 }
2236
2237 int StopMovement()
2238 {
2239 fDrive.SetAcceleration(fAccMax);
2240 fDrive.SetRpmMode(false);
2241
2242 fTrackingLoop.cancel();
2243
2244 fDrive.UpdateSource(Time(), "", false);
2245
2246 return State::kStopping;
2247 }
2248
2249 int ResetError()
2250 {
2251 const int rc = CheckState();
2252 return rc>0 ? rc : State::kInitialized;
2253 }
2254
2255 // --------------------- Others ---------------------
2256
2257 int TPoint()
2258 {
2259 T::Info("TPoint initiated.");
2260 Dim::SendCommandNB("TPOINT/EXECUTE");
2261 return T::GetCurrentState();
2262 }
2263
2264 int Screenshot(const EventImp &evt)
2265 {
2266 if (evt.GetSize()<2)
2267 {
2268 ostringstream msg;
2269 msg << "Screenshot - Received event has " << evt.GetSize() << " bytes, but expected at least 2.";
2270 T::Fatal(msg);
2271 return T::kSM_FatalError;
2272 }
2273
2274 if (evt.GetSize()==2)
2275 {
2276 ostringstream msg;
2277 msg << "Screenshot - Filename missing.";
2278 T::Error(msg);
2279 return T::GetCurrentState();
2280 }
2281
2282 T::Info("Screenshot initiated.");
2283 Dim::SendCommandNB("TPOINT/SCREENSHOT", evt.GetData(), evt.GetSize());
2284 return T::GetCurrentState();
2285 }
2286
2287 int SetLedBrightness(const EventImp &evt)
2288 {
2289 if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
2290 return T::kSM_FatalError;
2291
2292 const uint32_t *led = evt.Ptr<uint32_t>();
2293
2294 fDrive.SetLedVoltage(led[0], led[1]);
2295
2296 return T::GetCurrentState();
2297 }
2298
2299 int SetLedsOff()
2300 {
2301 fDrive.SetLedVoltage(0, 0);
2302 return T::GetCurrentState();
2303 }
2304
2305 // --------------------- Internal ---------------------
2306
2307 int SetVerbosity(const EventImp &evt)
2308 {
2309 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 2))
2310 return T::kSM_FatalError;
2311
2312 fDrive.SetVerbosity(evt.GetUShort());
2313
2314 return T::GetCurrentState();
2315 }
2316
2317 int Print()
2318 {
2319 for (auto it=fSources.begin(); it!=fSources.end(); it++)
2320 {
2321 const string &name = it->first;
2322 const Source &src = it->second;
2323
2324 T::Out() << name << ",";
2325 T::Out() << src.ra << "," << src.dec << "," << src.offset << ",";
2326 T::Out() << src.angles[0] << "," << src.angles[1] << endl;
2327 }
2328 return T::GetCurrentState();
2329 }
2330
2331 int PrintPointingModel()
2332 {
2333 fPointingModel.print(T::Out());
2334 return T::GetCurrentState();
2335 }
2336
2337 int Unlock()
2338 {
2339 const int rc = CheckState();
2340 return rc<0 ? State::kInitialized : rc;
2341 }
2342
2343 int ReloadSources()
2344 {
2345 try
2346 {
2347 ReadDatabase();
2348 }
2349 catch (const exception &e)
2350 {
2351 T::Error("Reading sources from databse failed: "+string(e.what()));
2352 }
2353 return T::GetCurrentState();
2354 }
2355
2356 int Disconnect()
2357 {
2358 // Close all connections
2359 fDrive.PostClose(false);
2360
2361 /*
2362 // Now wait until all connection have been closed and
2363 // all pending handlers have been processed
2364 poll();
2365 */
2366
2367 return T::GetCurrentState();
2368 }
2369
2370 int Reconnect(const EventImp &evt)
2371 {
2372 // Close all connections to supress the warning in SetEndpoint
2373 fDrive.PostClose(false);
2374
2375 // Now wait until all connection have been closed and
2376 // all pending handlers have been processed
2377 ba::io_service::poll();
2378
2379 if (evt.GetBool())
2380 fDrive.SetEndpoint(evt.GetString());
2381
2382 // Now we can reopen the connection
2383 fDrive.PostClose(true);
2384
2385 return T::GetCurrentState();
2386 }
2387
2388 // ========================= Tracking code =============================
2389
2390 int UpdateTrackingPosition()
2391 {
2392 // First calculate deviation between
2393 // command position and nominal position
2394 //fPointing.mount = sepos; // [deg] ref pos for alignment
2395 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2396
2397 // Get current position and calculate deviation
2398 const Encoder sepos = fDrive.GetSePos()*360; // [deg]
2399 const Encoder dev = sepos - data.mount;
2400
2401 // Calculate absolut deviation on the sky
2402 const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az)*3600;
2403
2404 // Smoothing
2405 fDevBuffer[fDevCount++%5] = absdev;
2406
2407 // Calculate average
2408 const uint8_t cnt = fDevCount<5 ? fDevCount : 5;
2409 const double avgdev = accumulate(fDevBuffer.begin(), fDevBuffer.begin()+cnt, 0.)/cnt;
2410
2411 // Count the consecutive number of avgdev below fDeviationLimit
2412 if (avgdev<fDeviationLimit)
2413 fTrackingCounter++;
2414 else
2415 fTrackingCounter = 0;
2416
2417 const double ha = fmod(fDrive.GetSeTime(),1)*24 - Nova::kORM.lng/15;
2418
2419 array<double, 12> dim;
2420 dim[0] = data.pointing.ra * 12/M_PI; // Ra [h] optical axis
2421 dim[1] = data.pointing.dec * 180/M_PI; // Dec [deg] optical axis
2422 dim[2] = ha - data.pointing.ra; // Ha [h] optical axis
2423 dim[3] = data.source.ra * 12/M_PI; // SrcRa [h] source position
2424 dim[4] = data.source.dec * 180/M_PI; // SrcDec [deg] source position
2425 dim[5] = ha - data.source.ra; // SrcHa [h] source position
2426 dim[6] = data.sky.zd * 180/M_PI; // Zd [deg] optical axis
2427 dim[7] = data.sky.az * 180/M_PI; // Az [deg] optical axis
2428 dim[8] = dev.zd; // dZd [deg] control deviation
2429 dim[9] = dev.az; // dAz [deg] control deviation
2430 dim[10] = absdev; // dev [arcsec] absolute control deviation
2431 dim[11] = avgdev; // dev [arcsec] average control deviation
2432
2433 fDrive.UpdateTracking(fDrive.GetSeTime(), dim);
2434
2435 if (fDrive.GetVerbosity())
2436 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Deviation [deg] " << absdev << "\"|" << avgdev << "\"|" << fDevCount<< " dZd=" << dev.zd*3600 << "\" dAz=" << dev.az*3600 << "\"" << endl;
2437
2438 // Maximum deviation execeeded -> fall back to Tracking state
2439 if (T::GetCurrentState()==State::kOnTrack && avgdev>fDeviationMax)
2440 return State::kTracking;
2441
2442 // Condition for OnTrack state achieved -> enhance to OnTrack state
2443 if (T::GetCurrentState()==State::kTracking && fTrackingCounter>=fDeviationCounter)
2444 return State::kOnTrack;
2445
2446 // No state change
2447 return T::GetCurrentState();
2448 }
2449
2450 void UpdatePointingPosition()
2451 {
2452 const Encoder sepos = fDrive.GetSePos()*360; // [deg] ref pos for alignment
2453
2454 const ZdAz pos = fPointingModel.MountToSky(sepos);
2455
2456 array<double, 2> data;
2457 data[0] = pos.zd*180/M_PI; // Zd [deg]
2458 data[1] = pos.az*180/M_PI; // Az [deg]
2459 fDrive.UpdatePointing(fDrive.GetSeTime(), data);
2460
2461 if (fDrive.GetVerbosity())
2462 T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Position [deg] " << pos.zd*180/M_PI << " " << pos.az*180/M_PI << endl;
2463 }
2464
2465 void TrackingLoop(const boost::system::error_code &error=boost::system::error_code())
2466 {
2467 if (error==ba::error::basic_errors::operation_aborted)
2468 return;
2469
2470 if (error)
2471 {
2472 ostringstream str;
2473 str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl;
2474 T::Error(str);
2475 return;
2476 }
2477
2478 if (T::GetCurrentState()!=State::kTracking &&
2479 T::GetCurrentState()!=State::kOnTrack)
2480 return;
2481
2482 //
2483 // Update speed as often as possible.
2484 // make sure, that dt is around 10 times larger than the
2485 // update time
2486 //
2487 // The loop should not be executed faster than the ramp of
2488 // a change in the velocity can be followed.
2489 //
2490 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(250));
2491
2492 const double mjd = Time().Mjd();
2493
2494 // I assume that it takes about 50ms for the value to be
2495 // transmitted and the drive needs time to follow as well (maybe
2496 // more than 50ms), therefore the calculated speec is calculated
2497 // for a moment 50ms in the future
2498 const PointingData data = CalcPointingPos(fDrive.GetSeTime());
2499 const PointingData data0 = CalcPointingPos(mjd-0.45/24/3600);
2500 const PointingData data1 = CalcPointingPos(mjd+0.55/24/3600);
2501
2502 const Encoder dest = data.mount *(1./360); // [rev]
2503 const Encoder dest0 = data0.mount*(1./360); // [rev]
2504 const Encoder dest1 = data1.mount*(1./360); // [rev]
2505
2506 if (!CheckRange(data1.sky))
2507 {
2508 StopMovement();
2509 T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop");
2510 return;
2511 }
2512
2513 // Current position
2514 const Encoder sepos = fDrive.GetSePos(); // [rev]
2515
2516 // Now calculate the current velocity
2517 const Encoder dist = dest1 - dest0; // [rev] Distance between t-1s and t+1s
2518 const Velocity vel = dist/(1./60); // [rev/min] Actual velocity of the pointing position
2519
2520 const Encoder dev = sepos - dest; // [rev] Current control deviation
2521 const Velocity vt = vel - dev/(1./60); // [rev/min] Correct velocity by recent control deviation
2522 // correct control deviation with 5s
2523 if (fDrive.GetVerbosity()>1)
2524 {
2525 T::Out() << "Ideal position [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2526 T::Out() << "Encoder pos. [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2527 T::Out() << "Deviation [arcmin] " << dev.zd *360*60 << " " << dev.az *360*60 << endl;
2528 T::Out() << "Distance 1s [arcmin] " << dist.zd *360*60 << " " << dist.az *360*60 << endl;
2529 T::Out() << "Velocity 1s [rpm] " << vt.zd << " " << vt.az << endl;
2530 T::Out() << "Delta T (enc) [ms] " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl;
2531 T::Out() << "Delta T (now) [ms] " << (Time().Mjd()-mjd)*24*3600*1000 << endl;
2532 }
2533
2534 // Tracking loop every 250ms
2535 // Vorsteuerung 2s
2536 // Delta T (enc) 5ms, every 5th, 25ms
2537 // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s)
2538
2539 //
2540 // FIXME: check if the drive is fast enough to follow the star
2541 //
2542 // Velocity units (would be 100 for %)
2543
2544 fDrive.SetTrackingVelocity(vt);
2545
2546 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2547 this, ba::placeholders::error));
2548 }
2549
2550 // =====================================================================
2551
2552 int CheckState()
2553 {
2554 if (!fDrive.IsConnected())
2555 return State::kDisconnected;
2556
2557 if (!fDrive.IsOnline())
2558 return State::kUnavailable;
2559
2560 // FIXME: This can prevent parking in case e.g.
2561 // of e8029 Position limit exceeded
2562 if (fDrive.HasWarning() || fDrive.HasError())
2563 {
2564 if (T::GetCurrentState()==State::kOnTrack ||
2565 T::GetCurrentState()==State::kTracking ||
2566 T::GetCurrentState()==State::kMoving ||
2567 T::GetCurrentState()==State::kParking)
2568 return StopMovement();
2569
2570 if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving())
2571 return State::kStopping;
2572
2573 if (fDrive.HasError())
2574 return State::kHardwareError;
2575
2576 if (fDrive.HasWarning())
2577 return State::kHardwareWarning;
2578
2579 return StateMachineImp::kSM_Error;
2580 }
2581
2582 // This can happen if one of the drives is not in RF.
2583 // Usually this only happens when the drive is not yet in RF
2584 // or an error was just cleared. Usually there is no way that
2585 // a drive goes below the RF state during operation without
2586 // a warning or error message.
2587 if (fDrive.IsOnline() && fDrive.IsBlocked())
2588 return State::kBlocked;
2589
2590 if (fDrive.IsOnline() && !fDrive.IsReady())
2591 return State::kAvailable;
2592
2593 // This is the case as soon as the init commands were send
2594 // after a connection to the SPS was established
2595 if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized())
2596 return State::kArmed;
2597
2598 return -1;
2599 }
2600
2601 int Execute()
2602 {
2603 const Time now;
2604 if (now>fSunRise && T::GetCurrentState()!=State::kParking)
2605 {
2606 fSunRise = now.GetNextSunRise();
2607
2608 ostringstream msg;
2609 msg << "Next sun-rise will be at " << fSunRise;
2610 T::Info(msg);
2611
2612 if (T::GetCurrentState()>State::kArmed && T::GetCurrentState()!=StateMachineImp::kError)
2613 return Park();
2614 }
2615
2616 if (T::GetCurrentState()==State::kLocked)
2617 return State::kLocked;
2618
2619 // FIXME: Send STOP if IsPositioning or RpmActive but no
2620 // Moving or Tracking state
2621
2622 const int rc = CheckState();
2623 if (rc>0)
2624 return rc;
2625
2626 // Once every second
2627 static time_t lastTime = 0;
2628 const time_t tm = time(NULL);
2629 if (lastTime!=tm && fDrive.IsInitialized())
2630 {
2631 lastTime=tm;
2632
2633 UpdatePointingPosition();
2634
2635 if (T::GetCurrentState()==State::kTracking || T::GetCurrentState()==State::kOnTrack)
2636 return UpdateTrackingPosition();
2637 }
2638
2639 if (T::GetCurrentState()==State::kStopping && !fDrive.IsMoving())
2640 return State::kArmed;
2641
2642 if ((T::GetCurrentState()==State::kMoving ||
2643 T::GetCurrentState()==State::kParking) && !fDrive.IsMoving())
2644 {
2645 if (fIsTracking && fStep==1)
2646 {
2647 // Init tracking
2648 fDrive.SetAcceleration(fAccTracking);
2649 fDrive.SetRpmMode(true);
2650
2651 fDevCount = 0;
2652 fTrackingCounter = 0;
2653
2654 fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(1));
2655 fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
2656 this, ba::placeholders::error));
2657
2658 fPointingSetup.start = Time().Mjd();
2659
2660 const PointingData data = CalcPointingPos(fPointingSetup.start);
2661
2662 ostringstream out;
2663 out << "Start tracking at Ra=" << data.pointing.ra*12/M_PI << "h Dec=" << data.pointing.dec*180/M_PI << "deg";
2664 T::Info(out);
2665
2666 return State::kTracking;
2667 }
2668
2669 // Get feedback 2
2670 const Encoder dest = fMovementTarget*(1./360); // [rev]
2671 const Encoder sepos = fDrive.GetSePos(); // [rev]
2672
2673 // Calculate residual to move deviation
2674 const Encoder dist = dest - sepos; // [rev]
2675
2676 // Check which axis should still be moved
2677 Encoder cd = dist; // [rev]
2678 cd *= T::GetCurrentState()==State::kParking ? 1./fMaxParkingResidual : 1./fMaxPointingResidual; // Scale to units of the maximum residual
2679 cd = cd.Abs();
2680
2681 // Check if there is a control deviation on the axis
2682 const bool cdzd = cd.zd>1;
2683 const bool cdaz = cd.az>1;
2684
2685 if (!fIsTracking)
2686 {
2687 // check if we reached the correct position already
2688 if (!cdzd && !cdaz)
2689 {
2690 T::Info("Target position reached in "+to_string(fStep)+" steps.");
2691 return T::GetCurrentState()==State::kParking ? State::kLocked : State::kArmed;
2692 }
2693
2694 if (fStep==10)
2695 {
2696 T::Error("Target position not reached in "+to_string(fStep)+" steps.");
2697 return State::kPositioningFailed;
2698 }
2699 }
2700
2701 const Encoder t = dist.Abs()/fDrive.GetVelUnit();
2702
2703 const Velocity vel =
2704 t.zd > t.az ?
2705 Velocity(1, t.zd==0?0:t.az/t.zd) :
2706 Velocity(t.az==0?0:t.zd/t.az, 1);
2707
2708 if (fDrive.GetVerbosity())
2709 {
2710 T::Out() << "Moving step " << fStep << endl;
2711 T::Out() << "Encoder [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl;
2712 T::Out() << "Destination [deg] " << dest.zd *360 << " " << dest.az *360 << endl;
2713 T::Out() << "Residual [deg] " << dist.zd *360 << " " << dist.az *360 << endl;
2714 T::Out() << "Residual/max [1] " << cd.zd << " " << cd.az << endl;
2715 T::Out() << "Rel. time [1] " << t.zd << " " << t.az << endl;
2716 T::Out() << "Rel. velocity [1] " << vel.zd << " " << vel.az << endl;
2717 }
2718
2719 fDrive.SetPointingVelocity(vel, fPointingVelocity);
2720 fDrive.StartAbsolutePositioning(dest, cdzd, cdaz);
2721
2722 ostringstream out;
2723 if (fStep==0)
2724 out << "Moving to encoder Zd=" << dest.zd*360 << "deg Az=" << dest.az*360 << "deg";
2725 else
2726 out << "Moving residual of dZd=" << dist.zd*360*60 << "' dAz=" << dist.az*360*60 << "'";
2727 T::Info(out);
2728
2729 fStep++;
2730 }
2731
2732 return T::GetCurrentState()>=State::kInitialized ?
2733 T::GetCurrentState() : State::kInitialized;
2734 }
2735
2736public:
2737 StateMachineDrive(ostream &out=cout) :
2738 StateMachineAsio<T>(out, "DRIVE_CONTROL"), fDrive(*this, *this),
2739 fTrackingLoop(*this), fSunRise(Time().GetNextSunRise()), fDevBuffer(5)
2740 {
2741
2742 T::Subscribe("MAGIC_WEATHER/DATA")
2743 (bind(&StateMachineDrive::HandleWeatherData, this, placeholders::_1));
2744
2745 T::Subscribe("TPOINT/DATA")
2746 (bind(&StateMachineDrive::HandleTPoint, this, placeholders::_1));
2747
2748 // State names
2749 T::AddStateName(State::kDisconnected, "Disconnected",
2750 "No connection to SPS");
2751 T::AddStateName(State::kConnected, "Connected",
2752 "Connection to SPS, no information received yet");
2753
2754 T::AddStateName(State::kLocked, "Locked",
2755 "Drive system is locked (will not accept commands)");
2756
2757 T::AddStateName(State::kUnavailable, "Unavailable",
2758 "Connected to SPS, no connection to at least one IndraDrives");
2759 T::AddStateName(State::kAvailable, "Available",
2760 "Connected to SPS and to IndraDrives, but at least one drive not in RF");
2761 T::AddStateName(State::kBlocked, "Blocked",
2762 "Drive system is blocked by manual operation or a pressed emergeny button");
2763 T::AddStateName(State::kArmed, "Armed",
2764 "Connected to SPS and IndraDrives in RF, but not yet initialized");
2765 T::AddStateName(State::kInitialized, "Initialized",
2766 "Connected to SPS and IndraDrives in RF and initialized");
2767
2768 T::AddStateName(State::kStopping, "Stopping",
2769 "Stop command sent, waiting for telescope to be still");
2770 T::AddStateName(State::kParking, "Parking",
2771 "Telescope in parking operation, waiting for telescope to be still");
2772 T::AddStateName(State::kMoving, "Moving",
2773 "Telescope moving");
2774 T::AddStateName(State::kTracking, "Tracking",
2775 "Telescope in tracking mode");
2776 T::AddStateName(State::kOnTrack, "OnTrack",
2777 "Telescope tracking stable");
2778
2779 T::AddStateName(State::kPositioningFailed, "PositioningFailed",
2780 "Target position was not reached within ten steps");
2781 T::AddStateName(State::kAllowedRangeExceeded, "OutOfRange",
2782 "Telecope went out of range during tracking");
2783 T::AddStateName(State::kInvalidCoordinates, "InvalidCoordinates",
2784 "Tracking coordinates out of range");
2785
2786 T::AddStateName(State::kHardwareWarning, "HardwareWarning",
2787 "At least one IndraDrive in a warning condition... check carefully!");
2788 T::AddStateName(State::kHardwareError, "HardwareError",
2789 "At least one IndraDrive in an error condition... this is a serious incident!");
2790
2791
2792 T::AddEvent("REQUEST_SDO", "S:3", State::kArmed)
2793 (bind(&StateMachineDrive::RequestSdo, this, placeholders::_1))
2794 ("Request an SDO from the drive"
2795 "|node[uint32]:Node identifier (1:az, 3:zd)"
2796 "|index[uint32]:SDO index"
2797 "|subindex[uint32]:SDO subindex");
2798
2799 T::AddEvent("SET_SDO", "S:3;X:1", State::kArmed)
2800 (bind(&StateMachineDrive::SendSdo, this, placeholders::_1))
2801 ("Request an SDO from the drive"
2802 "|node[uint32]:Node identifier (1:az, 3:zd)"
2803 "|index[uint32]:SDO index"
2804 "|subindex[uint32]:SDO subindex"
2805 "|value[uint64]:Value");
2806
2807 // Drive Commands
2808 T::AddEvent("MOVE_TO", "D:2", State::kInitialized) // ->ZDAZ
2809 (bind(&StateMachineDrive::MoveTo, this, placeholders::_1))
2810 ("Move the telescope to the given local sky coordinates"
2811 "|Zd[deg]:Zenith distance"
2812 "|Az[deg]:Azimuth");
2813
2814 T::AddEvent("TRACK", "D:2", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2815 (bind(&StateMachineDrive::Track, this, placeholders::_1))
2816 ("Move the telescope to the given sky coordinates and start tracking them"
2817 "|Ra[h]:Right ascension"
2818 "|Dec[deg]:Declination");
2819
2820 T::AddEvent("WOBBLE", "D:4", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2821 (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
2822 ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
2823 "|Ra[h]:Right ascension"
2824 "|Dec[deg]:Declination"
2825 "|Offset[deg]:Wobble offset"
2826 "|Angle[deg]:Wobble angle");
2827
2828 T::AddEvent("ORBIT", "D:5", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2829 (bind(&StateMachineDrive::Orbit, this, placeholders::_1))
2830 ("Move the telescope in a circle around the source"
2831 "|Ra[h]:Right ascension"
2832 "|Dec[deg]:Declination"
2833 "|Offset[deg]:Wobble offset"
2834 "|Angle[deg]:Starting angle"
2835 "|Period[min]:Time for one orbit");
2836
2837 T::AddEvent("TRACK_SOURCE", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2838 (bind(&StateMachineDrive::TrackSource, this, placeholders::_1))
2839 ("Move the telescope to the given wobble position around the given source and start tracking"
2840 "|Offset[deg]:Wobble offset"
2841 "|Angle[deg]:Wobble angle"
2842 "|Name[string]:Source name");
2843
2844 T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2845 (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
2846 ("Move the telescope to the given wobble position around the given source and start tracking"
2847 "|Id:Wobble angle id (1 or 2)"
2848 "|Name[string]:Source name");
2849
2850 T::AddEvent("TRACK_ORBIT", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2851 (bind(&StateMachineDrive::TrackOrbit, this, placeholders::_1))
2852 ("Move the telescope in a circle around the source"
2853 "|Angle[deg]:Starting angle"
2854 "|Period[min]:Time for one orbit"
2855 "|Name[string]:Source name");
2856
2857 T::AddEvent("TRACK_ON", "C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB
2858 (bind(&StateMachineDrive::TrackOn, this, placeholders::_1))
2859 ("Move the telescope to the given position and start tracking"
2860 "|Name[string]:Source name");
2861
2862 T::AddEvent("MOON", State::kInitialized, State::kTracking, State::kOnTrack)
2863 (bind(&StateMachineDrive::TrackCelest, this, kEMoon))
2864 ("Start tracking the moon");
2865 T::AddEvent("VENUS", State::kInitialized, State::kTracking, State::kOnTrack)
2866 (bind(&StateMachineDrive::TrackCelest, this, kEVenus))
2867 ("Start tracking Venus");
2868 T::AddEvent("MARS", State::kInitialized, State::kTracking, State::kOnTrack)
2869 (bind(&StateMachineDrive::TrackCelest, this, kEMars))
2870 ("Start tracking Mars");
2871 T::AddEvent("JUPITER", State::kInitialized, State::kTracking, State::kOnTrack)
2872 (bind(&StateMachineDrive::TrackCelest, this, kEJupiter))
2873 ("Start tracking Jupiter");
2874 T::AddEvent("SATURN", State::kInitialized, State::kTracking, State::kOnTrack)
2875 (bind(&StateMachineDrive::TrackCelest, this, kESaturn))
2876 ("Start tracking Saturn");
2877
2878 // FIXME: What to do in error state?
2879 T::AddEvent("PARK", State::kInitialized, State::kMoving, State::kTracking, State::kOnTrack, State::kHardwareWarning)
2880 (bind(&StateMachineDrive::Park, this))
2881 ("Park the telescope");
2882
2883 T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kTracking)(State::kOnTrack)
2884 (bind(&StateMachineDrive::StopMovement, this))
2885 ("Stop any kind of movement.");
2886
2887 T::AddEvent("RESET", State::kPositioningFailed, State::kAllowedRangeExceeded, State::kInvalidCoordinates, State::kHardwareWarning)
2888 (bind(&StateMachineDrive::ResetError, this))
2889 ("Acknowledge an internal drivectrl error (PositioningFailed, AllowedRangeExceeded, InvalidCoordinates)");
2890
2891 T::AddEvent("TPOINT", State::kOnTrack)
2892 (bind(&StateMachineDrive::TPoint, this))
2893 ("Take a TPoint");
2894
2895 T::AddEvent("SCREENSHOT", "B:1;C")
2896 (bind(&StateMachineDrive::Screenshot, this, placeholders::_1))
2897 ("Take a screenshot"
2898 "|color[bool]:False if just the gray image should be saved."
2899 "|name[string]:Filename");
2900
2901 T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
2902 (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
2903 ("Set the LED brightness of the top and bottom leds"
2904 "|top[au]:Allowed range 0-32767 for top LEDs"
2905 "|bot[au]:Allowed range 0-32767 for bottom LEDs");
2906
2907 T::AddEvent("LEDS_OFF")
2908 (bind(&StateMachineDrive::SetLedsOff, this))
2909 ("Switch off TPoint LEDs");
2910
2911 T::AddEvent("UNLOCK", Drive::State::kLocked)
2912 (bind(&StateMachineDrive::Unlock, this))
2913 ("Unlock locked state.");
2914
2915 // Verbosity commands
2916 T::AddEvent("SET_VERBOSITY", "S:1")
2917 (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
2918 ("Set verbosity state"
2919 "|verbosity[uint16]:disable or enable verbosity for received data (yes/no), except dynamic data");
2920
2921 // Conenction commands
2922 T::AddEvent("DISCONNECT", State::kConnected, State::kUnavailable)
2923 (bind(&StateMachineDrive::Disconnect, this))
2924 ("disconnect from ethernet");
2925
2926 T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected, State::kUnavailable)
2927 (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
2928 ("(Re)connect Ethernet connection to SPS, a new address can be given"
2929 "|[host][string]:new ethernet address in the form <host:port>");
2930
2931
2932 T::AddEvent("PRINT_POINTING_MODEL")
2933 (bind(&StateMachineDrive::PrintPointingModel, this))
2934 ("Print the ponting model.");
2935
2936
2937 T::AddEvent("PRINT")
2938 (bind(&StateMachineDrive::Print, this))
2939 ("Print source list.");
2940
2941 T::AddEvent("RELOAD_SOURCES", State::kDisconnected, State::kConnected, State::kArmed, State::kInitialized, State::kLocked)
2942 (bind(&StateMachineDrive::ReloadSources, this))
2943 ("Reload sources from database after database has changed..");
2944
2945
2946 //fDrive.SetUpdateStatus(std::bind(&StateMachineDrive::UpdateStatus, this, placeholders::_1, placeholders::_2));
2947 fDrive.StartConnect();
2948 }
2949
2950 void SetEndpoint(const string &url)
2951 {
2952 fDrive.SetEndpoint(url);
2953 }
2954
2955 bool AddSource(const string &name, const Source &src)
2956 {
2957 const auto it = fSources.find(name);
2958 if (it!=fSources.end())
2959 T::Warn("Source '"+name+"' already in list... overwriting.");
2960
2961 fSources[name] = src;
2962 return it==fSources.end();
2963 }
2964
2965 void ReadDatabase(bool print=true)
2966 {
2967#ifdef HAVE_SQL
2968 Database db(fDatabase);
2969
2970 T::Message("Connected to '"+db.uri()+"'");
2971
2972 const mysqlpp::StoreQueryResult res =
2973 db.query("SELECT fSourceName, fRightAscension, fDeclination, fWobbleOffset, fWobbleAngle0, fWobbleAngle1, fMagnitude FROM Source").store();
2974
2975 fSources.clear();
2976 for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
2977 {
2978 const string name = (*v)[0].c_str();
2979
2980 Source src;
2981 src.name = name;
2982 src.ra = (*v)[1];
2983 src.dec = (*v)[2];
2984 src.offset = (*v)[3];
2985 src.angles[0] = (*v)[4];
2986 src.angles[1] = (*v)[5];
2987 src.mag = (*v)[6] ? double((*v)[6]) : 0;
2988 AddSource(name, src);
2989
2990 if (!print)
2991 continue;
2992
2993 ostringstream msg;
2994 msg << " " << name << setprecision(8) << ": Ra=" << src.ra << "h Dec=" << src.dec << "deg";
2995 msg << " Wobble=[" << src.offset << "," << src.angles[0] << "," << src.angles[1] << "] Mag=" << src.mag;
2996 T::Message(msg);
2997 }
2998#else
2999 T::Warn("MySQL support not compiled into the program.");
3000#endif
3001 }
3002
3003 int EvalOptions(Configuration &conf)
3004 {
3005 if (!fSunRise)
3006 return 1;
3007
3008 fDrive.SetVerbose(!conf.Get<bool>("quiet"));
3009
3010 fMaxPointingResidual = conf.Get<double>("pointing.max.residual");
3011 fPointingVelocity = conf.Get<double>("pointing.velocity");
3012
3013 fPointingMin = Encoder(conf.Get<double>("pointing.min.zd"),
3014 conf.Get<double>("pointing.min.az"));
3015 fPointingMax = Encoder(conf.Get<double>("pointing.max.zd"),
3016 conf.Get<double>("pointing.max.az"));
3017
3018 fParkingPos.zd = conf.Has("parking-pos.zd") ? conf.Get<double>("parking-pos.zd") : 90;
3019 fParkingPos.az = conf.Has("parking-pos.az") ? conf.Get<double>("parking-pos.az") : 0;
3020 fMaxParkingResidual = conf.Get<double>("parking-pos.residual");
3021
3022 if (!CheckRange(fParkingPos))
3023 return 2;
3024
3025 fAccPointing = Acceleration(conf.Get<double>("pointing.acceleration.zd"),
3026 conf.Get<double>("pointing.acceleration.az"));
3027 fAccTracking = Acceleration(conf.Get<double>("tracking.acceleration.zd"),
3028 conf.Get<double>("tracking.acceleration.az"));
3029 fAccMax = Acceleration(conf.Get<double>("acceleration.max.zd"),
3030 conf.Get<double>("acceleration.max.az"));
3031
3032 fWeatherTimeout = conf.Get<uint16_t>("weather-timeout");
3033
3034 if (fAccPointing>fAccMax)
3035 {
3036 T::Error("Pointing acceleration exceeds maximum acceleration.");
3037 return 3;
3038 }
3039
3040 if (fAccTracking>fAccMax)
3041 {
3042 T::Error("Tracking acceleration exceeds maximum acceleration.");
3043 return 4;
3044 }
3045
3046 fDeviationLimit = conf.Get<uint16_t>("deviation-limit");
3047 fDeviationCounter = conf.Get<uint16_t>("deviation-count");
3048 fDeviationMax = conf.Get<uint16_t>("deviation-max");
3049
3050 const string fname = conf.GetPrefixedString("pointing.model-file");
3051
3052 try
3053 {
3054 fPointingModel.Load(fname);
3055 }
3056 catch (const exception &e)
3057 {
3058 T::Error(e.what());
3059 return 5;
3060 }
3061
3062 const vector<string> &vec = conf.Vec<string>("source");
3063
3064 for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
3065 {
3066 istringstream stream(*it);
3067
3068 string name;
3069
3070 int i=0;
3071
3072 Source src;
3073
3074 string buffer;
3075 while (getline(stream, buffer, ','))
3076 {
3077 istringstream is(buffer);
3078
3079 switch (i++)
3080 {
3081 case 0: name = buffer; break;
3082 case 1: src.ra = ReadAngle(is); break;
3083 case 2: src.dec = ReadAngle(is); break;
3084 case 3: is >> src.offset; break;
3085 case 4: is >> src.angles[0]; break;
3086 case 5: is >> src.angles[1]; break;
3087 }
3088
3089 if (is.fail())
3090 break;
3091 }
3092
3093 if (i==3 || i==6)
3094 {
3095 AddSource(name, src);
3096 continue;
3097 }
3098
3099 T::Warn("Resource 'source' not correctly formatted: '"+*it+"'");
3100 }
3101
3102 //fAutoResume = conf.Get<bool>("auto-resume");
3103
3104 if (conf.Has("source-database"))
3105 {
3106 fDatabase = conf.Get<string>("source-database");
3107 ReadDatabase();
3108 }
3109
3110 if (fSunRise.IsValid())
3111 {
3112 ostringstream msg;
3113 msg << "Next sun-rise will be at " << fSunRise;
3114 T::Message(msg);
3115 }
3116
3117 // The possibility to connect should be last, so that
3118 // everything else is already initialized.
3119 SetEndpoint(conf.Get<string>("addr"));
3120
3121 return -1;
3122 }
3123};
3124
3125// ------------------------------------------------------------------------
3126
3127#include "Main.h"
3128
3129
3130template<class T, class S, class R>
3131int RunShell(Configuration &conf)
3132{
3133 return Main::execute<T, StateMachineDrive<S, R>>(conf);
3134}
3135
3136void SetupConfiguration(Configuration &conf)
3137{
3138 po::options_description control("Drive control options");
3139 control.add_options()
3140 ("quiet,q", po_bool(), "Disable debug messages")
3141 ("no-dim,d", po_switch(), "Disable dim services")
3142 ("addr,a", var<string>("sps:5357"), "Network address of cosy")
3143 ("verbosity,v", var<uint16_t>(0), "Vervosity level (0=off; 1=major updates; 2=most updates; 3=frequent updates)")
3144 ("pointing.model-file", var<string>()->required(), "Name of the file with the pointing model in use")
3145 ("pointing.max.zd", var<double>( 104.9), "Maximum allowed zenith angle in sky pointing coordinates [deg]")
3146 ("pointing.max.az", var<double>( 85.0), "Maximum allowed azimuth angle in sky pointing coordinates [deg]")
3147 ("pointing.min.zd", var<double>(-104.9), "Minimum allowed zenith angle in sky pointing coordinates [deg]")
3148 ("pointing.min.az", var<double>(-295.0), "Minimum allowed azimuth angle in sky pointing coordinates [deg]")
3149 ("pointing.max.residual", var<double>(1./32768), "Maximum residual for a pointing operation [revolutions]")
3150 ("pointing.velocity", var<double>(0.3), "Moving velocity when pointing [% max]")
3151 ("pointing.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis for pointing operations")
3152 ("pointing.acceleration.zd", var<double>(0.03), "Acceleration for zenith axis for pointing operations")
3153 ("tracking.acceleration.az", var<double>(0.01), "Acceleration for azimuth axis during tracking operations")
3154 ("tracking.acceleration.zd", var<double>(0.01), "Acceleration for zenith axis during tracking operations")
3155 ("parking-pos.zd", var<double>(101), "Parking position zenith angle in sky pointing coordinates [deg]")
3156 ("parking-pos.az", var<double>(0), "Parking position azimuth angle in sky pointing coordinates [deg]")
3157 ("parking-pos.residual", var<double>(0.5/360), "Maximum residual for a parking position [revolutions]")
3158 ("acceleration.max.az", var<double>(0.03), "Maximum allowed acceleration value for azimuth axis")
3159 ("acceleration.max.zd", var<double>(0.09), "Maximum allowed acceleration value for zenith axis")
3160 ("weather-timeout", var<uint16_t>(300), "Timeout [sec] for weather data (after timeout default values are used)")
3161 ("deviation-limit", var<uint16_t>(90), "Deviation limit in arcsec to get 'OnTrack'")
3162 ("deviation-count", var<uint16_t>(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
3163 ("deviation-max", var<uint16_t>(180), "Maximum deviation in arcsec allowed to keep status 'OnTrack'")
3164 ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database[?compress=0|1].")
3165 ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
3166 ;
3167
3168 conf.AddOptions(control);
3169}
3170
3171/*
3172 Extract usage clause(s) [if any] for SYNOPSIS.
3173 Translators: "Usage" and "or" here are patterns (regular expressions) which
3174 are used to match the usage synopsis in program output. An example from cp
3175 (GNU coreutils) which contains both strings:
3176 Usage: cp [OPTION]... [-T] SOURCE DEST
3177 or: cp [OPTION]... SOURCE... DIRECTORY
3178 or: cp [OPTION]... -t DIRECTORY SOURCE...
3179 */
3180void PrintUsage()
3181{
3182 cout <<
3183 "The drivectrl is an interface to the drive PLC.\n"
3184 "\n"
3185 "The default is that the program is started without user intercation. "
3186 "All actions are supposed to arrive as DimCommands. Using the -c "
3187 "option, a local shell can be initialized. With h or help a short "
3188 "help message about the usuage can be brought to the screen.\n"
3189 "\n"
3190 "Usage: drivectrl [-c type] [OPTIONS]\n"
3191 " or: drivectrl [OPTIONS]\n";
3192 cout << endl;
3193}
3194
3195void PrintHelp()
3196{
3197 Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();
3198
3199 /* Additional help text which is printed after the configuration
3200 options goes here */
3201
3202 /*
3203 cout << "bla bla bla" << endl << endl;
3204 cout << endl;
3205 cout << "Environment:" << endl;
3206 cout << "environment" << endl;
3207 cout << endl;
3208 cout << "Examples:" << endl;
3209 cout << "test exam" << endl;
3210 cout << endl;
3211 cout << "Files:" << endl;
3212 cout << "files" << endl;
3213 cout << endl;
3214 */
3215}
3216
3217int main(int argc, const char* argv[])
3218{
3219 Configuration conf(argv[0]);
3220 conf.SetPrintUsage(PrintUsage);
3221 Main::SetupConfiguration(conf);
3222 SetupConfiguration(conf);
3223
3224 if (!conf.DoParse(argc, argv, PrintHelp))
3225 return 127;
3226
3227 //try
3228 {
3229 // No console access at all
3230 if (!conf.Has("console"))
3231 {
3232 if (conf.Get<bool>("no-dim"))
3233 return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
3234 else
3235 return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
3236 }
3237 // Cosole access w/ and w/o Dim
3238 if (conf.Get<bool>("no-dim"))
3239 {
3240 if (conf.Get<int>("console")==0)
3241 return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
3242 else
3243 return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
3244 }
3245 else
3246 {
3247 if (conf.Get<int>("console")==0)
3248 return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
3249 else
3250 return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
3251 }
3252 }
3253 /*catch (std::exception& e)
3254 {
3255 cerr << "Exception: " << e.what() << endl;
3256 return -1;
3257 }*/
3258
3259 return 0;
3260}
Note: See TracBrowser for help on using the repository browser.