Index: trunk/FACT++/src/drivectrl.cc =================================================================== --- trunk/FACT++/src/drivectrl.cc (revision 18353) +++ trunk/FACT++/src/drivectrl.cc (revision 18353) @@ -0,0 +1,3435 @@ +#include +#include + +#ifdef HAVE_SQL +#include "Database.h" +#endif + +#include "FACT.h" +#include "Dim.h" +#include "Event.h" +#include "Shell.h" +#include "StateMachineDim.h" +#include "StateMachineAsio.h" +#include "Connection.h" +#include "LocalControl.h" +#include "Configuration.h" +#include "Timers.h" +#include "Console.h" + +#include "HeadersDrive.h" + +#include "pal.h" +#include "externals/nova.h" + +namespace ba = boost::asio; +namespace bs = boost::system; + +using namespace std; +using namespace Drive; + +// ------------------------------------------------------------------------ + +// The Nova classes are in degree. This is to be used in rad +struct RaDec +{ + double ra; // [rad] + double dec; // [rad] + RaDec() : ra(0), dec(0) { } + RaDec(double _ra, double _dec) : ra(_ra), dec(_dec) { } +}; + +struct RaDecHa : RaDec +{ + double ha; // [rad] + RaDecHa() : ha(0) { } + RaDecHa(double _ra, double _dec, double _ha) : RaDec(_ra, _dec), ha(_ha) { } +}; + +struct Local +{ + double zd; + double az; + + Local(double _zd=0, double _az=0) : zd(_zd), az(_az) { } +}; + +struct Velocity : Local +{ + Velocity(double _zd=0, double _az=0) : Local(_zd, _az) { } + Velocity operator/(double f) const { return Velocity(zd/f, az/f); } + Velocity operator*(double f) const { return Velocity(zd*f, az*f); } +}; + +struct Encoder : Local // [units: revolutions] +{ + Encoder(double _zd=0, double _az=0) : Local(_zd, _az) { } + + Encoder &operator*=(double f) { zd*=f; az*=f; return *this; } + Encoder &operator-=(const Encoder &enc) { zd-=enc.zd; az-=enc.az; return *this; } + Encoder operator*(double f) const { return Encoder(zd*f, az*f); } + Velocity operator/(double t) const { return Velocity(zd/t, az/t); } + Encoder Abs() const { return Encoder(fabs(zd), fabs(az)); } +}; + +struct ZdAz : Local // [units: rad] +{ + ZdAz(double _zd=0, double _az=0) : Local(_zd, _az) { } + ZdAz operator*(const double &f) const { return ZdAz(zd*f, az*f); } +}; + +struct Acceleration : Local +{ + Acceleration(double _zd=0, double _az=0) : Local(_zd, _az) { } + bool operator>(const Acceleration &a) const + { + return zd>a.zd || az>a.az; + } +}; + +Encoder operator-(const Encoder &a, const Encoder &b) +{ + return Encoder(a.zd-b.zd, a.az-b.az); +} +Velocity operator-(const Encoder &a, const Velocity &b) +{ + return Velocity(a.zd-b.zd, a.az-b.az); +} +Velocity operator-(const Velocity &a, const Velocity &b) +{ + return Velocity(a.zd-b.zd, a.az-b.az); +} +Encoder operator/(const Encoder &a, const Encoder &b) +{ + return Encoder(a.zd/b.zd, a.az/b.az); +} + +struct Weather +{ + float hum; + float temp; + float press; + Time time; +}; + +struct Source +{ + Source() : ra(0), dec(0), mag(0), offset(0) + { + angles[0] = -90; + angles[1] = 90; + } + + string name; + double ra; // [h] + double dec; // [deg] + double mag; + + double offset; + array angles; +}; + +enum Planets_t +{ + kENone = -1, + kESun = 0, + kEMercury = 1, + kEVenus = 2, + kEMoon = 3, // earth moon barycentre + kEMars = 4, + kEJupiter = 5, + kESaturn = 6, + kEUranus = 7, + kENeptune = 8, + kEPluto = 9, +}; + +// ------------------------------------------------------------------------ + +struct PointingSetup +{ + Source source; // Informations about source to track [h/deg] + Planets_t planet; // Id of the planet if tracking a planet + double start; // Starting time of wobble observation [mjd] + double orbit_period; // Time for one revolution (0:off) [day] + double wobble_offset; // Distance of wobble position [rad] + double wobble_angle; // Starting phi angle of wobble observation [rad] + + PointingSetup(Planets_t p=kENone) : planet(p), start(Time::none), orbit_period(0) { } +}; + +struct PointingData +{ + // Pointing direction of the opticl axis of the telescope + RaDec source; // Informations about source to track [rad/rad] + RaDec pointing; // Catalog coordinates (J2000, FK5) [rad/rad] pointing position + RaDecHa apparent; // Apparent position on the sky [rad/rad] + ZdAz sky; // Apparent position on the sky [rad/rad] + Encoder mount; // Encoder position corresponding to 'sky' [deg/deg] + double mjd; +}; + +class PointingModel +{ +private: + double fIe; // [rad] Index Error in Elevation + double fIa; // [rad] Index Error in Azimuth + double fFlop; // [rad] Vertical Sag + double fNpae; // [rad] Az-El Nonperpendicularity + double fCa; // [rad] Left-Right Collimation Error + double fAn; // [rad] Azimuth Axis Misalignment (N-S, 1st order) + double fAw; // [rad] Azimuth Axis Misalignment (E-W, 1st order) + double fAn2; // [rad] Azimuth Axis Misalignment (N-S, 2nd order) + double fAw2; // [rad] Azimuth Axis Misalignment (E-W, 2nd order) + double fTf; // [rad] Tube fluxture (sin) + double fTx; // [rad] Tube fluxture (tan) + double fNrx; // [rad] Nasmyth rotator displacement, horizontal + double fNry; // [rad] Nasmyth rotator displacement, vertical + double fCrx; // [rad] Alt/Az Coude Displacement (N-S) + double fCry; // [rad] Alt/Az Coude Displacement (E-W) + double fEces; // [rad] Elevation Centering Error (sin) + double fAces; // [rad] Azimuth Centering Error (sin) + double fEcec; // [rad] Elevation Centering Error (cos) + double fAcec; // [rad] Azimuth Centering Error (cos) + +public: + /* + Source source; // Informations about source to track + Planets_t planet; // Id of the planet if tracking a planet + double start; // Starting time of wobble observation [mjd] + double orbit_period; // Time for one revolution (0:off) [day] + double wobble_offset; // Distance of wobble position [rad] + double wobble_angle; // Starting phi angle of wobble observation [rad] + + // Source to track with all constraints + //RaDec source; // Catalog coordinates (J2000, FK5) [rad/rad] source position + + // Pointing direction of the opticl axis of the telescope + RaDec pointing; // Catalog coordinates (J2000, FK5) [rad/rad] pointing position + RaDecHa apparent; // Apparent position on the sky [rad/rad] + ZdAz sky; // Apparent position on the sky [rad/rad] + Encoder mount; // Encoder position corresponding to 'sky' [deg/deg] + double mjd; + */ + + void Load(const string &name) + { + /* + ! MMT 1987 July 8 + ! T 36 7.3622 41.448 -0.0481 + ! IA -37.5465 20.80602 + ! IE -13.9180 1.25217 + ! NPAE +7.0751 26.44763 + ! CA -6.9149 32.05358 + ! AN +0.5053 1.40956 + ! AW -2.2016 1.37480 + ! END + */ + + ifstream fin(name); + if (!fin) + throw runtime_error("Cannot open file "+name+": "+strerror(errno)); + + map coeff; + + string buf; + while (getline(fin, buf)) + { + buf = Tools::Trim(buf); + + vector vec; + boost::split(vec, buf, boost::is_any_of(" "), boost::token_compress_on); + if (vec.size()<2) + continue; + + coeff[vec[0]] = atof(vec[1].c_str()) * M_PI/180; + } + + fIe = coeff["IE"]; // [rad] Index Error in Elevation + fIa = coeff["IA"]; // [rad] Index Error in Azimuth + fFlop = coeff["FLOP"]; // [rad] Vertical Sag + fNpae = coeff["NPAE"]; // [rad] Az-El Nonperpendicularity + fCa = coeff["CA"]; // [rad] Left-Right Collimation Error + fAn = coeff["AN"]; // [rad] Azimuth Axis Misalignment (N-S, 1st order) + fAw = coeff["AW"]; // [rad] Azimuth Axis Misalignment (E-W, 1st order) + fAn2 = coeff["AN2"]; // [rad] Azimuth Axis Misalignment (N-S, 2nd order) + fAw2 = coeff["AW2"]; // [rad] Azimuth Axis Misalignment (E-W, 2nd order) + fTf = coeff["TF"]; // [rad] Tube fluxture (sin) + fTx = coeff["TX"]; // [rad] Tube fluxture (tan) + fNrx = coeff["NRX"]; // [rad] Nasmyth rotator displacement, horizontal + fNry = coeff["NRY"]; // [rad] Nasmyth rotator displacement, vertical + fCrx = coeff["CRX"]; // [rad] Alt/Az Coude Displacement (N-S) + fCry = coeff["CRY"]; // [rad] Alt/Az Coude Displacement (E-W) + fEces = coeff["ECES"]; // [rad] Elevation Centering Error (sin) + fAces = coeff["ACES"]; // [rad] Azimuth Centering Error (sin) + fEcec = coeff["ECEC"]; // [rad] Elevation Centering Error (cos) + fAcec = coeff["ACEC"]; // [rad] Azimuth Centering Error (cos) + } + + struct AltAz + { + double alt; + double az; + + AltAz(double _alt, double _az) : alt(_alt), az(_az) { } + AltAz(const ZdAz &za) : alt(M_PI/2-za.zd), az(za.az) { } + + AltAz &operator+=(const AltAz &aa) { alt += aa.alt; az+=aa.az; return *this; } + AltAz &operator-=(const AltAz &aa) { alt -= aa.alt; az-=aa.az; return *this; } + }; + + double Sign(double val, double alt) const + { + // Some pointing corrections are defined as Delta ZA, which + // is (P. Wallace) defined [0,90]deg while Alt is defined + // [0,180]deg + return (M_PI/2-alt < 0 ? -val : val); + } +/* + AltAz CalcAnAw(const AltAz &p) const + { + // Corrections for AN and AW without approximations + // as done by Patrick Wallace. The approximation cannot + // be used for MAGIC because the correction angle + // AW (~1.5deg) is not small enough. + + const double theta0 = M_PI/2-p.alt; + const double phi0 = p.az; + + const double st = sin(theta0); + const double ct = cos(theta0); + const double sp = sin(phi0); + const double cp = cos(phi0); + + double x = st * cp; + double y = st * sp; + double z = ct; + + // RotateY + const double sy = sin(fAn); + const double cy = cos(fAn); + const double zz = z; + z = cy*zz - sy*x; + x = sy*zz + cy*x; + + // RotateX + const double sx = sin(fAw); + const double cx = cos(fAw); + const double yy = y; + y = cx*yy - sx*z; + z = sx*yy + cx*z; + + const double xy = hypot(x, y); + const double theta1 = atan2(xy, z); + const double phi1 = atan2(y, x); + + double dalt = theta0 - theta1; + double daz = phi1 - phi0; + + if (daz>M_PI) + daz -= 2*M_PI; + if (daz<-M_PI) + daz += 2*M_PI; + + return AltAz(dalt, daz); + } +*/ + Encoder SkyToMount(AltAz p) + { + const AltAz CRX(-fCrx*sin(p.az-p.alt), fCrx*cos(p.az-p.alt)/cos(p.alt)); + const AltAz CRY(-fCry*cos(p.az-p.alt), -fCry*sin(p.az-p.alt)/cos(p.alt)); + p += CRX; + p += CRY; + + const AltAz NRX(fNrx*sin(p.alt), -fNrx); + const AltAz NRY(fNry*cos(p.alt), -fNry*tan(p.alt)); + p += NRX; + p += NRY; + + const AltAz CES(-fEces*sin(p.alt), -fAces*sin(p.az)); + const AltAz CEC(-fEcec*cos(p.alt), -fAcec*cos(p.az)); + p += CES; + p += CEC; + + const AltAz TX(Sign(fTx/tan(p.alt), p.alt), 0); + const AltAz TF(Sign(fTf*cos(p.alt), p.alt), 0); + //p += TX; + p += TF; + + const AltAz CA(0, -fCa/cos(p.alt)); + p += CA; + + const AltAz NPAE(0, -fNpae*tan(p.alt)); + p += NPAE; + + const AltAz AW2( fAw2*sin(p.az*2), -fAw2*cos(p.az*2)*tan(p.alt)); + const AltAz AN2(-fAn2*cos(p.az*2), -fAn2*sin(p.az*2)*tan(p.alt)); + const AltAz AW1( fAw *sin(p.az), -fAw *cos(p.az) *tan(p.alt)); + const AltAz AN1(-fAn *cos(p.az), -fAn *sin(p.az) *tan(p.alt)); + p += AW2; + p += AN2; + p += AW1; + p += AN1; + + const AltAz FLOP(Sign(fFlop, p.alt), 0); + p += FLOP; + + const AltAz I(fIe, fIa); + p += I; + + return Encoder(90 - p.alt*180/M_PI, p.az *180/M_PI); + } + + ZdAz MountToSky(const Encoder &mnt) const + { + AltAz p(M_PI/2-mnt.zd*M_PI/180, mnt.az*M_PI/180); + + const AltAz I(fIe, fIa); + p -= I; + + const AltAz FLOP(Sign(fFlop, p.alt), 0); + p -= FLOP; + + const AltAz AW1( fAw *sin(p.az), -fAw *cos(p.az) *tan(p.alt)); + const AltAz AN1(-fAn *cos(p.az), -fAn *sin(p.az) *tan(p.alt)); + const AltAz AW2( fAw2*sin(p.az*2), -fAw2*cos(p.az*2)*tan(p.alt)); + const AltAz AN2(-fAn2*cos(p.az*2), -fAn2*sin(p.az*2)*tan(p.alt)); + p -= AW1; + p -= AN1; + p -= AW2; + p -= AN2; + + const AltAz NPAE(0, -fNpae*tan(p.alt)); + p -= NPAE; + + const AltAz CA(0, -fCa/cos(p.alt)); + p -= CA; + + const AltAz TF(Sign(fTf*cos(p.alt), p.alt), 0); + const AltAz TX(Sign(fTx/tan(p.alt), p.alt), 0); + p -= TF; + //p -= TX; + + const AltAz CEC(-fEcec*cos(p.alt), -fAcec*cos(p.az)); + const AltAz CES(-fEces*sin(p.alt), -fAces*sin(p.az)); + p -= CEC; + p -= CES; + + const AltAz NRY(fNry*cos(p.alt), -fNry*tan(p.alt)); + const AltAz NRX(fNrx*sin(p.alt), -fNrx); + p -= NRY; + p -= NRX; + + const AltAz CRY(-fCry*cos(p.az-p.alt), -fCry*sin(p.az-p.alt)/cos(p.alt)); + const AltAz CRX(-fCrx*sin(p.az-p.alt), fCrx*cos(p.az-p.alt)/cos(p.alt)); + p -= CRY; + p -= CRX; + + return ZdAz(M_PI/2-p.alt, p.az); + } + + PointingData CalcPointingPos(const PointingSetup &setup, double _mjd, const Weather &weather, uint16_t timeout, bool tpoint=false) + { + PointingData out; + out.mjd = _mjd; + + const double elong = Nova::ORM().lng * M_PI/180; + const double lat = Nova::ORM().lat * M_PI/180; + const double height = 2200; + + const bool valid = weather.time+boost::posix_time::seconds(timeout) > Time(); + + const double temp = valid ? weather.temp : 10; + const double hum = valid ? weather.hum : 0.25; + const double press = valid ? weather.press : 780; + + const double dtt = palDtt(_mjd); // 32.184 + 35 + + const double tdb = _mjd + dtt/3600/24; + const double dut = 0; + + // prepare calculation: Mean Place to geocentric apperent + // (UTC would also do, except for the moon?) + double fAmprms[21]; + palMappa(2000.0, tdb, fAmprms); // Epoche, TDB + + // prepare: Apperent to observed place + double fAoprms[14]; + palAoppa(_mjd, dut, // mjd, Delta UT=UT1-UTC + elong, lat, height, // long, lat, height + 0, 0, // polar motion x, y-coordinate (radians) + 273.155+temp, press, hum, // temp, pressure, humidity + 0.40, 0.0065, // wavelength, tropo lapse rate + fAoprms); + + out.source.ra = setup.source.ra * M_PI/ 12; + out.source.dec = setup.source.dec * M_PI/180; + + if (setup.planet!=kENone) + { + // coordinates of planet: topocentric, equatorial, J2000 + // One can use TT instead of TDB for all planets (except the moon?) + double ra, dec, diam; + palRdplan(tdb, setup.planet, elong, lat, &ra, &dec, &diam); + + // ---- apparent to mean ---- + palAmpqk(ra, dec, fAmprms, &out.source.ra, &out.source.dec); + } + + if (setup.wobble_offset<=0 || tpoint) + { + out.pointing.dec = out.source.dec; + out.pointing.ra = out.source.ra; + } + else + { + const double dphi = + setup.orbit_period==0 ? 0 : 2*M_PI*(_mjd-setup.start)/setup.orbit_period; + + const double phi = setup.wobble_angle + dphi; + + const double cosdir = cos(phi); + const double sindir = sin(phi); + const double cosoff = cos(setup.wobble_offset); + const double sinoff = sin(setup.wobble_offset); + const double cosdec = cos(out.source.dec); + const double sindec = sin(out.source.dec); + + const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir; + + const double costheta = sintheta>1 ? 0 : sqrt(1 - sintheta*sintheta); + + const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta); + const double sindeltara = sindir*sinoff/costheta; + + out.pointing.dec = asin(sintheta); + out.pointing.ra = atan2(sindeltara, cosdeltara) + out.source.ra; + } + + // ---- Mean to apparent ---- + double r=0, d=0; + palMapqkz(out.pointing.ra, out.pointing.dec, fAmprms, &r, &d); + + // + // Doesn't work - don't know why + // + // slaMapqk (radec.Ra(), radec.Dec(), rdpm.Ra(), rdpm.Dec(), + // 0, 0, (double*)fAmprms, &r, &d); + // + + // -- apparent to observed -- + palAopqk(r, d, fAoprms, + &out.sky.az, // observed azimuth (radians: N=0,E=90) [-pi, pi] + &out.sky.zd, // observed zenith distance (radians) [-pi/2, pi/2] + &out.apparent.ha, // observed hour angle (radians) + &out.apparent.dec, // observed declination (radians) + &out.apparent.ra); // observed right ascension (radians) + + // ----- fix ambiguity ----- + if (out.sky.zd<0) + { + out.sky.zd = -out.sky.zd; + out.sky.az += out.sky.az<0 ? M_PI : -M_PI; + } + + // Star culminating behind zenith and Az between ~90 and ~180deg + if (out.source.dec0) + out.sky.az -= 2*M_PI; + + out.mount = SkyToMount(out.sky); + + return out; + } + + //PointingModel(double ra, double dec) : planet(kENone), pointing(ra, dec) { } + //PointingModel(const Nova::ZdAzPosn &za) : planet(kENone), sky(za.zd* M_PI/180,za.az* M_PI/180) { SkyToMount(); } + //PointingModel(const Nova::EquPosn &rd) : planet(kENone), pointing(rd.ra* M_PI/180, rd.dec* M_PI/180) { } +}; + +// ------------------------------------------------------------------------ + +/* +#define kEMERGENCY 0x1 +#define kPDO1_TX 0x3 +#define kPDO2_TX 0x5 +#define kPDO3_TX 0x7 +#define kPDO4_TX 0x9 +#define kSDO_RX 0xb // this is used to set data of the shaft encoder +#define kSDO_TX 0xc // this is used to request data from the shaft encoder +#define kNodeguard 0xe + +// NMT: no answer to NMT command +// cob-id=0, command (byte), id (byte) +#define kNMT_START 0x01 // change to operational state (start) +#define kNMT_STOP 0x02 // change to prepared state (stop) +#define kNMT_PREOP 0x80 // enter pre operational state +#define kNMT_RESET 0x81 // reset node (set parameter to power on values) +#define kNMT_REINIT 0x82 // reset communication of node (set communication parameters to power on values) + +// command for SDOs +#define kSDO_LEN4 0x3 +#define kSDO_LEN2 0xb +#define kSDO_LEN1 0xf + +#define kSDO_RXm4 0x22 // this is used with SDO_TX to send a maximum of 4 bytes +#define kSDO_RX4 0x20|kSDO_LEN4 // this is used with SDO_TX to send 4 bytes +#define kSDO_RX2 0x20|kSDO_LEN2 // this is used with SDO_TX to send 2 bytes +#define kSDO_RX1 0x20|kSDO_LEN1 // this is used with SDO_TX to send 1 byte +#define kSDO_RX_DATA 0x40 // this is used to request parameters from the encoder +#define kSDO_TX4 0x40|kSDO_LEN4 // answer to 0x40 with 4 bytes of data +#define kSDO_TX3 0x40|kSDO_LEN2 // answer to 0x40 with 2 bytes of data +#define kSDO_TX1 0x40|kSDO_LEN1 // answer to 0x40 with 1 byte of data +#define kSDO_TX_OK 0x60 // answer to a SDO_TX message +#define kSDO_TX_ERROR 0x80 // error message (instead of 0x60) +*/ + + + +class ConnectionDrive : public Connection +{ + uint16_t fVerbosity; + +public: + virtual void UpdatePointing(const Time &, const array &) + { + } + + virtual void UpdateTracking(const Time &, const array &) + { + } + + virtual void UpdateStatus(const Time &, const array &) + { + } + + virtual void UpdateTPoint(const Time &, const DimTPoint &, const string &) + { + } + + virtual void UpdateSource(const Time &, const string &, bool) + { + } + virtual void UpdateSource(const Time &,const array &, const string& = "") + { + } + +private: + enum NodeId_t + { + kNodeAz = 1, + kNodeZd = 3 + }; + + enum + { + kRxNodeguard = 0xe, + kRxPdo1 = 3, + kRxPdo2 = 5, + kRxPdo3 = 7, + kRxPdo4 = 9, + kRxSdo = 0xb, + kRxSdo4 = 0x40|0x3, + kRxSdo2 = 0x40|0xb, + kRxSdo1 = 0x40|0xf, + kRxSdoOk = 0x60, + kRxSdoErr = 0x80, + + kTxSdo = 0x40, + kTxSdo4 = 0x20|0x3, + kTxSdo2 = 0x20|0xb, + kTxSdo1 = 0x20|0xf, + }; + + //const Int_t id1 = env.GetValue("Az_Id", 1); + //const Int_t id2 = env.GetValue("Zd_Id", 3); + + void SendCanFrame(uint16_t cobid, + uint8_t m0=0, uint8_t m1=0, uint8_t m2=0, uint8_t m3=0, + uint8_t m4=0, uint8_t m5=0, uint8_t m6=0, uint8_t m7=0) + { + const uint16_t desc = (cobid<<5) | 8; + + vector data(11); + data[0] = 10; + data[1] = desc>>8; + data[2] = desc&0xff; + + const uint8_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; + memcpy(data.data()+3, msg, 8); + + PostMessage(data); + } + + enum Index_t + { + kReqArmed = 0x1000, + kReqPDO = 0x1001, + kReqErrStat = 0x1003, + kReqSoftVer = 0x100a, + kReqKeepAlive = 0x100b, + kReqVel = 0x2002, + kReqVelRes = 0x6002, + kReqVelMax = 0x6003, + kReqPos = 0x6004, + kReqPosRes = 0x6501, + + kSetArmed = 0x1000, + kSetPointVel = 0x2002, + kSetAcc = 0x2003, + kSetRpmMode = 0x3006, + kSetTrackVel = 0x3007, + kSetLedVoltage = 0x4000, + kSetPosition = 0x6004, + }; + + static uint32_t String(uint8_t b0=0, uint8_t b1=0, uint8_t b2=0, uint8_t b3=0) + { + return uint32_t(b0)<<24 | uint32_t(b1)<<16 | uint32_t(b2)<<8 | uint32_t(b3); + } + + uint32_t fVelRes[2]; + uint32_t fVelMax[2]; + uint32_t fPosRes[2]; + + uint32_t fErrCode[2]; + + void HandleSdo(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx, + const uint32_t &val, const Time &tv) + { + if (fVerbosity>0) + { + ostringstream out; + out << hex; + out << "SDO[" << int(node) << "] " << idx << "/" << int(subidx) << ": " << val << dec; + Out() << out.str() << endl; + } + + switch (idx) + { + case kReqArmed: + //fArmed = val==1; + return; + + case kReqErrStat: + { + fErrCode[node/2] = (val>>8); + LogErrorCode(node); + } + return; + + case kReqSoftVer: + //fSoftVersion = val; + return; + + case kReqKeepAlive: + // Do not display, this is used for CheckConnection + fIsInitialized[node/2] = true; + return; + + case kReqVel: + //fVel = val; + return; + + case kReqPos: + switch (subidx) + { + case 0: + fPdoPos1[node/2] = val; + fPdoTime1[node/2] = tv; + fHasChangedPos1[node/2] = true; + return; + case 1: + fPdoPos2[node/2] = val; + fPdoTime2[node/2] = tv; + fHasChangedPos2[node/2] = true; + return; + } + break; + + case kReqVelRes: + fVelRes[node/2] = val; + return; + + case kReqVelMax: + fVelMax[node/2] = val; + return; + + case kReqPosRes: + fPosRes[node/2] = val; + return; + } + + ostringstream str; + str << "HandleSDO: Idx=0x"<< hex << idx << "/" << (int)subidx; + str << ", val=0x" << val; + Warn(str); + } + + void HandleSdoOk(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx, + const Time &) + { + ostringstream out; + out << hex; + out << "SDO-OK[" << int(node) << "] " << idx << "/" << int(subidx) << dec << " "; + + switch (idx) + { + case kSetArmed: + out << "(Armed state set)"; + break; + /* + case 0x1001: + Out() << inf2 << "- " << GetNodeName() << ": PDOs requested." << endl; + return; + */ + case kSetPointVel: + out << "(Pointing velocity set)"; + break; + + case kSetAcc: + out << "(Acceleration set)"; + break; + + case kSetRpmMode: + out << "(RPM mode set)"; + break; + + case kSetLedVoltage: + out << "(LED Voltage set)"; + Info(out); + return; + /* + case 0x3007: + //Out() << inf2 << "- Velocity set (" << GetNodeName() << ")" << endl; + return; + + case 0x4000: + HandleNodeguard(tv); + return; + + case 0x6000: + Out() << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl; + return; + + case 0x6002: + Out() << inf2 << "- " << GetNodeName() << ": Velocity resolution set." << endl; + return; + */ + case kSetPosition: + out << "(Absolute positioning started)"; + break; + /* + case 0x6005: + Out() << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl; + fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO + return;*/ + } + /* + Out() << warn << setfill('0') << "WARNING - Nodedrv::HandleSDOOK: "; + Out() << "Node #" << dec << (int)fId << ": Sdo=" << hex << idx << "/" << (int)subidx << " set."; + Out() << endl; + */ + + if (fVerbosity>1) + Out() << out.str() << endl; + } + + void HandleSdoError(const uint8_t &node, const uint16_t &idx, const uint8_t &subidx, + const Time &) + { + ostringstream out; + out << hex; + out << "SDO-ERR[" << int(node) << "] " << idx << "/" << int(subidx) << dec; + Out() << out.str() << endl; + } + + + int32_t fPdoPos1[2]; + int32_t fPdoPos2[2]; + + Time fPdoTime1[2]; +public: + Time fPdoTime2[2]; +private: + bool fHasChangedPos1[2]; + bool fHasChangedPos2[2]; + + void HandlePdo1(const uint8_t &node, const uint8_t *data, const Time &tv) + { + const uint32_t pos1 = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0]; + const uint32_t pos2 = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4]; + + if (fVerbosity>2) + Out() << Time().GetAsStr("%M:%S.%f") << " PDO1[" << (int)node << "] " << 360.*int32_t(pos1)/fPosRes[node/2] << " " << 360.*int32_t(pos2)/fPosRes[node/2] << endl; + + // Once every few milliseconds! + + fPdoPos1[node/2] = pos1; + fPdoTime1[node/2] = tv; + fHasChangedPos1[node/2] = true; + + fPdoPos2[node/2] = pos2; + fPdoTime2[node/2] = tv; + fHasChangedPos2[node/2] = true; + } + + uint8_t fStatusAxis[2]; + uint8_t fStatusSys; + + enum { + kUpsAlarm = 0x01, // UPS Alarm (FACT only) + kUpsBattery = 0x02, // UPS on battery (FACT only) + kUpsCharging = 0x04, // UPS charging (FACT only) + kEmergencyOk = 0x10, // Emergency button released + kOvervoltOk = 0x20, // Overvoltage protection ok + kManualMode = 0x40, // Manual mode button pressed + + kAxisBb = 0x01, // IndraDrive reports Bb (Regler betriebsbereit) + kAxisMoving = 0x02, // SPS reports + kAxisRpmMode = 0x04, // SPS reports + kAxisRf = 0x20, // IndraDrive reports Rf (Regler freigegeben) + kAxisHasPower = 0x80 // IndraDrive reports axis power on + }; + + //std::function&)> fUpdateStatus; + + void HandlePdo3(const uint8_t &node, const uint8_t *data, const Time &tv) + { + /* + TX1M_STATUS.0 := 1; + 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; + TX1M_STATUS.2 := X_PC_VStart; + TX1M_STATUS.6 := NOT X_ist_freigegeben; + + TX3M_STATUS.0 := X_ist_betriebsbereit; + TX3M_STATUS.1 := 1; + TX3M_STATUS.2 := Not_Aus_IO; + TX3M_STATUS.3 := UeberspannungsSchutz_OK; + TX3M_STATUS.4 := FB_soll_drehen_links OR FB_soll_drehen_rechts OR FB_soll_schwenk_auf OR FB_soll_schwenk_ab; + TX3M_STATUS.5 := X_ist_freigegeben; + TX3M_STATUS.6 := 1; + TX3M_STATUS.7 := LeistungEinAz; + + TX3M_STATUS.8 := NOT UPS_ALARM; + TX3M_STATUS.9 := UPS_BattMode; + TX3M_STATUS.10 := UPS_Charging; + */ + + const uint8_t sys = ((data[0] & 0x1c)<<2) | (data[1]); + if (fStatusSys!=sys) + { + fStatusSys = sys; + + const bool alarm = sys&kUpsAlarm; // 01 TX3M.8 100 + const bool batt = sys&kUpsBattery; // 02 TX3M.9 200 + const bool charge = sys&kUpsCharging; // 04 TX3M.10 400 + const bool emcy = sys&kEmergencyOk; // 10 TX3M.2 04 + const bool vltg = sys&kOvervoltOk; // 20 TX3M.3 08 + const bool mode = sys&kManualMode; // 40 TX3M.4 10 + + ostringstream out; + if (alarm) out << " UPS-PowerLoss"; + if (batt) out << " UPS-OnBattery"; + if (charge) out << " UPS-Charging"; + if (emcy) out << " EmcyOk"; + if (vltg) out << " OvervoltOk"; + if (mode) out << " ManualMove"; + + Info("New system status["+to_string(node)+"]:"+out.str()); + if (fVerbosity>1) + Out() << "PDO3[" << (int)node << "] StatusSys=" << hex << (int)fStatusSys << dec << endl; + } + + const uint8_t axis = (data[0]&0xa1) | (data[3]&0x06); + if (fStatusAxis[node/2]!=axis) + { + fStatusAxis[node/2] = axis; + + const bool ready = axis&kAxisBb; // 01 + const bool move = axis&kAxisMoving; // 02 + const bool rpm = axis&kAxisRpmMode; // 04 + const bool rf = axis&kAxisRf; // 20 + const bool power = axis&kAxisHasPower; // 80 + + ostringstream out; + if (ready) out << " DKC-Ready"; + if (move) out << " Moving"; + if (rpm) out << " RpmMode"; + if (rf) out << " RF"; + if (power) out << " PowerOn"; + + Info("New axis status["+to_string(node)+"]:"+out.str()); + if (fVerbosity>1) + Out() << "PDO3[" << (int)node << "] StatusAxis=" << hex << (int)fStatusAxis[node/2] << dec << endl; + } + + array arr = {{ fStatusAxis[0], fStatusAxis[1], fStatusSys }}; + UpdateStatus(tv, arr); + } + + string ErrCodeToString(uint32_t code) const + { + switch (code) + { + case 0: return "offline"; + case 0xa000: case 0xa0000: + case 0xa001: case 0xa0001: + case 0xa002: case 0xa0002: + case 0xa003: case 0xa0003: return "Communication phase "+to_string(code&0xf); + case 0xa010: case 0xa0010: return "Drive HALT"; + case 0xa012: case 0xa0012: return "Control and power section ready for operation"; + case 0xa013: case 0xa0013: return "Ready for power on"; + case 0xa100: case 0xa0100: return "Drive in Torque mode"; + case 0xa101: case 0xa0101: return "Drive in Velocity mode"; + case 0xa102: case 0xa0102: return "Position control mode with encoder 1"; + case 0xa103: case 0xa0103: return "Position control mode with encoder 2"; + case 0xa104: case 0xa0104: return "Position control mode with encoder 1, lagless"; + case 0xa105: case 0xa0105: return "Position control mode with encoder 2, lagless"; + case 0xa106: case 0xa0106: return "Drive controlled interpolated positioning with encoder 1"; + case 0xa107: case 0xa0107: return "Drive controlled interpolated positioning with encoder 2"; + case 0xa108: case 0xa0108: return "Drive controlled interpolated positioning with encoder 1, lagless"; + case 0xa109: case 0xa0109: return "Drive controlled interpolated positioning with encoder 2, lagless"; + //case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1"; + //case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2"; + //case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1"; + //case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2"; + case 0xa150: case 0xa0150: return "Drive controlled positioning with encoder 1"; + case 0xa151: case 0xa0151: return "Drive controlled positioning with encoder 1, lagless"; + case 0xa152: case 0xa0152: return "Drive controlled positioning with encoder 2"; + case 0xa153: case 0xa0153: return "Drive controlled positioning with encoder 2, lagless"; + case 0xa208: return "Jog mode positive"; + case 0xa218: return "Jog mode negative"; + case 0xa400: case 0xa4000: return "Automatic drive check and adjustment"; + case 0xa401: case 0xa4001: return "Drive decelerating to standstill"; + case 0xa800: case 0xa0800: return "Unknown operation mode"; + case 0xc217: return "Motor encoder reading error"; + case 0xc218: return "Shaft encoder reading error"; + case 0xc220: return "Motor encoder initialization error"; + case 0xc221: return "Shaft encoder initialization error"; + case 0xc300: return "Command: set absolute measure"; + case 0xc400: case 0xc0400: return "Switching to parameter mode"; + case 0xc401: case 0xc0401: return "Drive active, switching mode not allowed"; + case 0xc500: case 0xc0500: return "Error reset"; + case 0xc600: case 0xc0600: return "Drive controlled homing procedure"; + case 0xe225: return "Motor overload"; + case 0xe249: case 0xe2049: return "Positioning command velocity exceeds limit bipolar"; + case 0xe250: return "Drive overtemp warning"; + case 0xe251: return "Motor overtemp warning"; + case 0xe252: return "Bleeder overtemp warning"; + case 0xe257: return "Continous current limit active"; + case 0xe2819: return "Main power failure"; + case 0xe259: return "Command velocity limit active"; + case 0xe8260: return "Torque limit active"; + case 0xe264: return "Target position out of numerical range"; + case 0xe829: case 0xe8029: return "Positive position limit exceeded"; + case 0xe830: case 0xe8030: return "Negative position limit exceeded"; + case 0xe831: return "Position limit reached during jog"; + case 0xe834: return "Emergency-Stop"; + case 0xe842: return "Both end-switches activated"; + case 0xe843: return "Positive end-switch activated"; + case 0xe844: return "Negative end-switch activated"; + case 0xf218: case 0xf2018: return "Amplifier overtemp shutdown"; + case 0xf219: case 0xf2019: return "Motor overtemp shutdown"; + case 0xf220: return "Bleeder overload shutdown"; + case 0xf221: case 0xf2021: return "Motor temperature surveillance defective"; + case 0xf2022: return "Unit temperature surveillance defective"; + case 0xf224: return "Maximum breaking time exceeded"; + case 0xf2025: return "Drive not ready for power on"; + case 0xf228: case 0xf2028: return "Excessive control deviation"; + case 0xf250: return "Overflow of target position preset memory"; + case 0xf257: case 0xf2057: return "Command position out of range"; + case 0xf269: return "Error during release of the motor holding brake"; + case 0xf276: return "Absolute encoder out of allowed window"; + case 0xf2174: return "Lost reference of motor encoder"; + case 0xf409: case 0xf4009: return "Bus error on Profibus interface"; + case 0xf434: return "Emergency-Stop"; + case 0xf629: return "Positive position limit exceeded"; + case 0xf630: return "Negative position limit exceeded"; + case 0xf634: return "Emergency-Stop"; + case 0xf643: return "Positive end-switch activated"; + case 0xf644: return "Negative end-switch activated"; + case 0xf8069: return "15V DC error"; + case 0xf870: case 0xf8070: return "24V DC error"; + case 0xf878: case 0xf8078: return "Velocity loop error"; + case 0xf8079: return "Velocity limit exceeded"; + case 0xf2026: return "Undervoltage in power section"; + } + return "unknown"; + } + + void LogErrorCode(uint32_t node) + { + const uint8_t typ = fErrCode[node/2]>>16; + + ostringstream out; + out << "IndraDrive "; + out << (node==1?"Az":"Zd"); + out << " [" << hex << fErrCode[node/2]; + out << "]: "; + out << ErrCodeToString(fErrCode[node/2]); + out << (typ==0xf || typ==0xe ? "!" : "."); + + switch (typ) + { + case 0xf: Error(out); break; + case 0xe: Warn(out); break; + case 0xa: Info(out); break; + case 0x0: + case 0xc: + case 0xd: Message(out); break; + default: Fatal(out); break; + } + } + + void HandlePdo2(const uint8_t &node, const uint8_t *data, const Time &) + { + fErrCode[node/2] = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; + + if (fVerbosity>0) + Out() << "PDO2[" << int(node) << "] err=" << hex << fErrCode[node/2] << endl; + + LogErrorCode(node); + } + + struct SDO + { + uint8_t node; + uint8_t req; + uint16_t idx; + uint8_t subidx; + uint32_t val; + + SDO(uint8_t n, uint8_t r, uint16_t i, uint8_t s, uint32_t v=0) + : node(n), req(r&0xf), idx(i), subidx(s), val(v) { } + + bool operator==(const SDO &s) const + { + return node==s.node && idx==s.idx && subidx==s.subidx; + } + }; + + struct Timeout_t : SDO, ba::deadline_timer + { + + Timeout_t(ba::io_service& ioservice, + uint8_t n, uint8_t r, uint16_t i, uint8_t s, uint32_t v, uint16_t millisec) : SDO(n, r, i, s, v), + ba::deadline_timer(ioservice) + { + expires_from_now(boost::posix_time::milliseconds(millisec)); + } + // get_io_service() + }; + + std::list fTimeouts; + + vector fData; + + void HandleReceivedData(const boost::system::error_code& err, size_t bytes_received, int) + { + // Do not schedule a new read if the connection failed. + if (bytes_received!=11 || fData[0]!=10 || err) + { + if (err==ba::error::eof) + Warn("Connection closed by remote host (cosy)."); + + // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category)) + // 125: Operation canceled + if (err && err!=ba::error::eof && // Connection closed by remote host + err!=ba::error::basic_errors::not_connected && // Connection closed by remote host + err!=ba::error::basic_errors::operation_aborted) // Connection closed by us + { + ostringstream str; + str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl; + Error(str); + } + PostClose(err!=ba::error::basic_errors::operation_aborted); + return; + } + + Time now; + + const uint16_t desc = fData[1]<<8 | fData[2]; + //const uint8_t rtr = (desc>>4)&1; + //const uint8_t len = desc&0xf; + const uint16_t cobid = desc>>5; + + const uint8_t *data = fData.data()+3; + + const uint16_t fcode = cobid >> 7; + const uint8_t node = cobid & 0x1f; + + switch (fcode) + { + case kRxNodeguard: + Out() << "Received nodeguard" << endl; + //HandleNodeguard(node, now); + break; + + case kRxSdo: + { + const uint8_t cmd = data[0]; + const uint16_t idx = data[1] | (data[2]<<8); + const uint8_t subidx = data[3]; + const uint32_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24); + + const auto it = find(fTimeouts.begin(), fTimeouts.end(), SDO(node, cmd, idx, subidx)); + if (it!=fTimeouts.end()) + { + // This will call the handler and in turn remove the object from the list + it->cancel(); + } + + switch (cmd) + { + case kRxSdo4: // answer to 0x40 with 4 bytes of data + HandleSdo(node, idx, subidx, dat, now); + break; + + case kRxSdo2: // answer to 0x40 with 2 bytes of data + HandleSdo(node, idx, subidx, dat&0xffff, now); + break; + + case kRxSdo1: // answer to 0x40 with 1 byte of data + HandleSdo(node, idx, subidx, dat&0xff, now); + break; + + case kRxSdoOk: // answer to a SDO_TX message + HandleSdoOk(node, idx, subidx, now); + break; + + case kRxSdoErr: // error message + HandleSdoError(node, idx, subidx, now); + break; + + default: + { + ostringstream out; + out << "Invalid SDO command code " << hex << cmd << " received."; + Error(out); + PostClose(false); + return; + } + } + } + break; + + case kRxPdo1: + HandlePdo1(node, data, now); + break; + + case kRxPdo2: + HandlePdo2(node, data, now); + break; + + case kRxPdo3: + HandlePdo3(node, data, now); + break; + + default: + { + ostringstream out; + out << "Invalid function code " << hex << fcode << " received."; + Error(out); + PostClose(false); + return; + } + } + + StartReadReport(); + } + + void StartReadReport() + { + ba::async_read(*this, ba::buffer(fData), + boost::bind(&ConnectionDrive::HandleReceivedData, this, + ba::placeholders::error, ba::placeholders::bytes_transferred, 0)); + + //AsyncWait(fInTimeout, 35000, &Connection::HandleReadTimeout); // 30s + } + + bool fIsInitialized[2]; + + // This is called when a connection was established + void ConnectionEstablished() + { + //Info("Connection to PLC established."); + + fIsInitialized[0] = false; + fIsInitialized[1] = false; + + SendSdo(kNodeZd, kSetArmed, 1); + SendSdo(kNodeAz, kSetArmed, 1); + + RequestSdo(kNodeZd, kReqErrStat); + RequestSdo(kNodeAz, kReqErrStat); + + SetRpmMode(false); + + RequestSdo(kNodeZd, kReqPosRes); + RequestSdo(kNodeAz, kReqPosRes); + + RequestSdo(kNodeZd, kReqVelRes); + RequestSdo(kNodeAz, kReqVelRes); + + RequestSdo(kNodeZd, kReqVelMax); + RequestSdo(kNodeAz, kReqVelMax); + + RequestSdo(kNodeZd, kReqPos, 0); + RequestSdo(kNodeAz, kReqPos, 0); + RequestSdo(kNodeZd, kReqPos, 1); + RequestSdo(kNodeAz, kReqPos, 1); + + RequestSdo(kNodeZd, kReqKeepAlive); + RequestSdo(kNodeAz, kReqKeepAlive); + + StartReadReport(); + } + + void HandleTimeoutImp(const std::list::iterator &ref, const bs::error_code &error) + { + if (error==ba::error::basic_errors::operation_aborted) + return; + + if (error) + { + ostringstream str; + str << "SDO timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl; + Error(str); + + //PostClose(); + return; + } + + if (!is_open()) + { + // For example: Here we could schedule a new accept if we + // would not want to allow two connections at the same time. + return; + } + + // Check whether the deadline has passed. We compare the deadline + // against the current time since a new asynchronous operation + // may have moved the deadline before this actor had a chance + // to run. + if (ref->expires_at() > ba::deadline_timer::traits_type::now()) + return; + + ostringstream str; + str << hex; + str << "SDO timeout ("; + str << uint32_t(ref->node) << ": "; + str << (ref->req==kTxSdo?"RX ":"TX "); + str << ref->idx << "/" << uint32_t(ref->subidx) << " [" << ref->val << "] "; + str << to_simple_string(ref->expires_from_now()); + str << ")"; + + Warn(str); + + //PostClose(); + } + + void HandleTimeout(const std::list::iterator &ref, const bs::error_code &error) + { + HandleTimeoutImp(ref, error); + fTimeouts.erase(ref); + } + + void SendSdoRequest(uint8_t node, uint8_t req, + uint16_t idx, uint8_t subidx, uint32_t val=0) + { + if (fVerbosity>1) + Out() << "SDO-" << (req==kTxSdo?"REQ":"SET") << "[" << int(node) << "] " << idx << "/" << int(subidx) << " = " << val << endl; + + + SendCanFrame(0x600|(node&0x1f), req, idx&0xff, idx>>8, subidx, + val&0xff, (val>>8)&0xff, (val>>16)&0xff, (val>>24)&0xff); + + // - The boost::asio::basic_deadline_timer::expires_from_now() + // function cancels any pending asynchronous waits, and returns + // the number of asynchronous waits that were cancelled. If it + // returns 0 then you were too late and the wait handler has + // already been executed, or will soon be executed. If it + // returns 1 then the wait handler was successfully cancelled. + // - If a wait handler is cancelled, the bs::error_code passed to + // it contains the value bs::error::operation_aborted. + + const uint32_t milliseconds = 3000; + fTimeouts.emplace_front(get_io_service(), node, req, idx, subidx, val, milliseconds); + + const std::list::iterator &timeout = fTimeouts.begin(); + + timeout->async_wait(boost::bind(&ConnectionDrive::HandleTimeout, this, timeout, ba::placeholders::error)); + } + +public: + ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()), + fVerbosity(0), fData(11) + { + SetLogStream(&imp); + } + + void SetVerbosity(const uint16_t &v) + { + fVerbosity = v; + } + + uint16_t GetVerbosity() const + { + return fVerbosity; + } + + void RequestSdo(uint8_t node, uint16_t idx, uint8_t subidx=0) + { + SendSdoRequest(node, kTxSdo, idx, subidx); + } + void SendSdo(uint8_t node, uint16_t idx, uint8_t subidx, uint32_t val) + { + SendSdoRequest(node, kTxSdo4, idx, subidx, val); + } + + void SendSdo(uint8_t node, uint16_t idx, uint32_t val) + { + SendSdo(node, idx, 0, val); + } + + bool IsMoving() const + { + return (fStatusAxis[0]&kAxisMoving) || (fStatusAxis[1]&kAxisMoving) + || (fStatusAxis[0]&kAxisRpmMode) || (fStatusAxis[1]&kAxisRpmMode); + } + + bool IsInitialized() const + { + // All important information has been successfully requested from the + // SPS and the power control units are in RF (Regler freigegeben) + return fIsInitialized[0] && fIsInitialized[1]; + } + + bool HasError() const + { + const uint8_t typ0 = fErrCode[0]>>16; + const uint8_t typ1 = fErrCode[1]>>16; + return typ0==0xe || typ0==0xf || typ1==0xe || typ1==0xf; + } + + bool IsOnline() const + { + return fErrCode[0]!=0 && fErrCode[1]!=0; + } + + bool IsReady() const + { + return fStatusAxis[0]&kAxisRf && fStatusAxis[1]&kAxisRf; + } + + bool IsBlocked() const + { + return (fStatusSys&0x10)==0 || (fStatusSys&kManualMode); + } + + Encoder GetSePos() const // [rev] + { + return Encoder(double(fPdoPos2[1])/fPosRes[1], double(fPdoPos2[0])/fPosRes[0]); + } + + double GetSeTime() const // [rev] + { + // The maximum difference here should not be larger than 100ms. + // So th error we make on both axes should not exceed 50ms; + return (Time(fPdoTime2[0]).Mjd()+Time(fPdoTime2[1]).Mjd())/2; + } + + Encoder GetVelUnit() const + { + return Encoder(fVelMax[1], fVelMax[0]); + } + + void SetRpmMode(bool mode) + { + const uint32_t val = mode ? String('s','t','r','t') : String('s','t','o','p'); + SendSdo(kNodeAz, kSetRpmMode, val); + SendSdo(kNodeZd, kSetRpmMode, val); + } + + void SetAcceleration(const Acceleration &acc) + { + SendSdo(kNodeAz, kSetAcc, lrint(acc.az*1000000000+0.5)); + SendSdo(kNodeZd, kSetAcc, lrint(acc.zd*1000000000+0.5)); + } + + void SetPointingVelocity(const Velocity &vel, double scale=1) + { + SendSdo(kNodeAz, kSetPointVel, lrint(vel.az*fVelMax[0]*scale)); + SendSdo(kNodeZd, kSetPointVel, lrint(vel.zd*fVelMax[1]*scale)); + } + void SetTrackingVelocity(const Velocity &vel) + { + SendSdo(kNodeAz, kSetTrackVel, lrint(vel.az*fVelRes[0])); + SendSdo(kNodeZd, kSetTrackVel, lrint(vel.zd*fVelRes[1])); + } + + void StartAbsolutePositioning(const Encoder &enc, bool zd, bool az) + { + if (az) SendSdo(kNodeAz, kSetPosition, lrint(enc.az*fPosRes[0])); + if (zd) SendSdo(kNodeZd, kSetPosition, lrint(enc.zd*fPosRes[1])); + + // Make sure that the status is set correctly already before the first PDO + if (az) fStatusAxis[0] |= 0x02; + if (zd) fStatusAxis[1] |= 0x02; + + // FIXME: UpdateDim? + } + + void SetLedVoltage(uint32_t v1, uint32_t v2) + { + SendSdo(kNodeAz, 0x4000, v1); + SendSdo(kNodeZd, 0x4000, v2); + } +}; + + +// ------------------------------------------------------------------------ + +#include "DimDescriptionService.h" + +class ConnectionDimDrive : public ConnectionDrive +{ +private: + DimDescribedService fDimPointing; + DimDescribedService fDimTracking; + DimDescribedService fDimSource; + DimDescribedService fDimTPoint; + DimDescribedService fDimStatus; + + // Update dim from a different thread to ensure that these + // updates cannot block the main eventloop which eventually + // also checks the timeouts + Queue>> fQueuePointing; + Queue>> fQueueTracking; + Queue,bool>> fQueueSource; + Queue>> fQueueTPoint; + Queue>> fQueueStatus; + + bool SendPointing(const pair> &p) + { + fDimPointing.setData(p.second); + fDimPointing.Update(p.first); + return true; + } + + bool SendTracking(const pair> &p) + { + fDimTracking.setData(p.second); + fDimTracking.Update(p.first); + return true; + } + + bool SendSource(const tuple,bool> &t) + { + const Time &time = get<0>(t); + const vector &data = get<1>(t); + const bool &tracking = get<2>(t); + + fDimSource.setQuality(tracking); + fDimSource.setData(data); + fDimSource.Update(time); + return true; + } + + bool SendStatus(const pair> &p) + { + fDimStatus.setData(p.second); + fDimStatus.Update(p.first); + return true; + } + + bool SendTPoint(const pair> &p) + { + fDimTPoint.setData(p.second); + fDimTPoint.Update(p.first); + return true; + } + +public: + void UpdatePointing(const Time &t, const array &arr) + { + fQueuePointing.emplace(t, arr); + } + + void UpdateTracking(const Time &t,const array &arr) + { + fQueueTracking.emplace(t, arr); + } + + void UpdateStatus(const Time &t, const array &arr) + { + fQueueStatus.emplace(t, arr); + } + + void UpdateTPoint(const Time &t, const DimTPoint &data, + const string &name) + { + vector dim(sizeof(data)+name.length()+1); + memcpy(dim.data(), &data, sizeof(data)); + memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1); + + fQueueTPoint.emplace(t, dim); + } + + void UpdateSource(const Time &t, const string &name, bool tracking) + { + vector dat(5*sizeof(double)+31, 0); + strncpy(dat.data()+5*sizeof(double), name.c_str(), 30); + + fQueueSource.emplace(t, dat, tracking); + } + + void UpdateSource(const Time &t, const array &arr, const string &name="") + { + vector dat(5*sizeof(double)+31, 0); + memcpy(dat.data(), arr.data(), 5*sizeof(double)); + strncpy(dat.data()+5*sizeof(double), name.c_str(), 30); + + fQueueSource.emplace(t, dat, true); + } + +public: + ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) : + ConnectionDrive(ioservice, imp), + fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1", + "|Zd[deg]:Zenith distance (derived from encoder readout)" + "|Az[deg]:Azimuth angle (derived from encoder readout)"), + 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", + "|Ra[h]:Command right ascension pointing direction (J2000)" + "|Dec[deg]:Command declination pointing direction (J2000)" + "|Ha[h]:Hour angle pointing direction" + "|SrcRa[h]:Right ascension source (J2000)" + "|SrcDec[deg]:Declination source (J2000)" + "|SrcHa[h]:Hour angle source" + "|Zd[deg]:Nominal zenith distance" + "|Az[deg]:Nominal azimuth angle" + "|dZd[deg]:Control deviation Zd" + "|dAz[deg]:Control deviation Az" + "|dev[arcsec]:Absolute control deviation" + "|avgdev[arcsec]:Average control deviation used to define OnTrack"), + fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;C:31", + "|Ra_src[h]:Source right ascension" + "|Dec_src[deg]:Source declination" + "|Offset[deg]:Wobble offset" + "|Angle[deg]:Wobble angle" + "|Period[min]:Time for one orbit" + "|Name[string]:Source name if available"), + 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", + "|Ra[h]:Command right ascension" + "|Dec[deg]:Command declination" + "|Zd_nom[deg]:Nominal zenith distance" + "|Az_nom[deg]:Nominal azimuth angle" + "|Zd_cur[deg]:Current zenith distance (calculated from image)" + "|Az_cur[deg]:Current azimuth angle (calculated from image)" + "|Zd_enc[deg]:Feedback zenith axis (from encoder)" + "|Az_enc[deg]:Feedback azimuth angle (from encoder)" + "|N_leds[cnt]:Number of detected LEDs" + "|N_rings[cnt]:Number of rings used to calculate the camera center" + "|Xc[pix]:X position of center in CCD camera frame" + "|Yc[pix]:Y position of center in CCD camera frame" + "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)" + "|Xs[pix]:X position of start in CCD camera frame" + "|Ys[pix]:Y position of star in CCD camera frame" + "|Ms[mag]:Artifical magnitude of star (calculated from image)" + "|Phi[deg]:Rotation angle of image derived from detected LEDs" + "|Mc[mag]:Catalog magnitude of star" + "|Dx[arcsec]:De-rotated dx" + "|Dy[arcsec]:De-rotated dy" + "|Name[string]:Name of star"), + fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", ""), + fQueuePointing(std::bind(&ConnectionDimDrive::SendPointing, this, placeholders::_1)), + fQueueTracking(std::bind(&ConnectionDimDrive::SendTracking, this, placeholders::_1)), + fQueueSource( std::bind(&ConnectionDimDrive::SendSource, this, placeholders::_1)), + fQueueTPoint( std::bind(&ConnectionDimDrive::SendTPoint, this, placeholders::_1)), + fQueueStatus( std::bind(&ConnectionDimDrive::SendStatus, this, placeholders::_1)) + { + } + + // 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 +}; + +// ------------------------------------------------------------------------ + +template +class StateMachineDrive : public StateMachineAsio +{ +private: + S fDrive; + + ba::deadline_timer fTrackingLoop; + + string fDatabase; + + typedef map sources; + sources fSources; + + Weather fWeather; + uint16_t fWeatherTimeout; + + ZdAz fParkingPos; + + PointingModel fPointingModel; + PointingSetup fPointingSetup; + Encoder fMovementTarget; + + Time fSunRise; + + Encoder fPointingMin; + Encoder fPointingMax; + + uint16_t fDeviationLimit; + uint16_t fDeviationCounter; + uint16_t fDeviationMax; + + vector fDevBuffer; + uint64_t fDevCount; + + uint64_t fTrackingCounter; + + + // --------------------- DIM Sending ------------------ + + bool CheckEventSize(size_t has, const char *name, size_t size) + { + if (has==size) + return true; + + ostringstream msg; + msg << name << " - Received event has " << has << " bytes, but expected " << size << "."; + T::Fatal(msg); + return false; + } + + // --------------------- DIM Receiving ------------------ + + int HandleWeatherData(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "HandleWeatherData", 7*4+2)) + { + fWeather.time = Time(Time::none); + return T::GetCurrentState(); + } + + const float *ptr = evt.Ptr(2); + + fWeather.temp = ptr[0]; + fWeather.hum = ptr[2]; + fWeather.press = ptr[3]; + fWeather.time = evt.GetTime(); + + return T::GetCurrentState(); + } + + int HandleTPoint(const EventImp &evt) + { + // Skip disconnect events + if (evt.GetSize()==0) + return T::GetCurrentState(); + + // skip invalid events + if (!CheckEventSize(evt.GetSize(), "HandleTPoint", 11*8)) + return T::GetCurrentState(); + + // skip event which are older than one minute + if (Time().UnixTime()-evt.GetTime().UnixTime()>60) + return T::GetCurrentState(); + + // Original code in slaTps2c: + // + // From the tangent plane coordinates of a star of known RA,Dec, + // determine the RA,Dec of the tangent point. + + const double *ptr = evt.Ptr(); + + // Tangent plane rectangular coordinates + const double dx = ptr[0] * M_PI/648000; // [arcsec -> rad] + const double dy = ptr[1] * M_PI/648000; // [arcsec -> rad] + + const PointingData data = fPointingModel.CalcPointingPos(fPointingSetup, evt.GetTime().Mjd(), fWeather, fWeatherTimeout, true); + + const double x2 = dx*dx; + const double y2 = 1 + dy*dy; + + const double sd = cos(data.sky.zd);//sin(M_PI/2-sky.zd); + const double cd = sin(data.sky.zd);//cos(M_PI/2-sky.zd); + const double sdf = sd*sqrt(x2+y2); + const double r2 = cd*cd*y2 - sd*sd*x2; + + // Case of no solution ("at the pole") or + // two solutions ("over the pole solution") + if (r2<0 || fabs(sdf)>=1) + { + T::Warn("Could not determine pointing direction from TPoint."); + return T::GetCurrentState(); + } + + const double r = sqrt(r2); + const double s = sdf - dy * r; + const double c = sdf * dy + r; + const double phi = atan2(dx, r); + + // Spherical coordinates of tangent point + const double az = fmod(data.sky.az-phi + 2*M_PI, 2*M_PI); + const double zd = M_PI/2 - atan2(s, c); + + const Encoder dev = fDrive.GetSePos()*360 - data.mount; + + // --- Output TPoint --- + + const string fname = "tpoints-"+to_string(evt.GetTime().NightAsInt())+".txt"; + //time.GetAsStr("/%Y/%m/%d"); + + const bool exist = boost::filesystem::exists(fname); + + ofstream fout(fname, ios::app); + if (!exist) + { + fout << "FACT Model TPOINT data file" << endl; + fout << ": ALTAZ" << endl; + fout << "49 48 0 "; + fout << evt.GetTime() << endl; + } + fout << setprecision(7); + fout << fmod(az*180/M_PI+360, 360) << " "; + fout << 90-zd*180/M_PI << " "; + fout << fmod(data.mount.az+360, 360) << " "; + fout << 90-data.mount.zd << " "; + fout << dev.az << " "; // delta az + fout << -dev.zd << " "; // delta el + fout << 90-data.sky.zd * 180/M_PI << " "; + fout << data.sky.az * 180/M_PI << " "; + fout << setprecision(10); + fout << data.mjd << " "; + fout << setprecision(7); + fout << ptr[6] << " "; // center.mag + fout << ptr[9] << " "; // star.mag + fout << ptr[4] << " "; // center.x + fout << ptr[5] << " "; // center.y + fout << ptr[7] << " "; // star.x + fout << ptr[8] << " "; // star.y + fout << ptr[2] << " "; // num leds + fout << ptr[3] << " "; // num rings + fout << ptr[0] << " "; // dx (de-rotated) + fout << ptr[1] << " "; // dy (de-rotated) + fout << ptr[10] << " "; // rotation angle + fout << fPointingSetup.source.mag << " "; + fout << fPointingSetup.source.name; + fout << endl; + + DimTPoint dim; + dim.fRa = data.pointing.ra * 12/M_PI; + dim.fDec = data.pointing.dec * 180/M_PI; + dim.fNominalZd = data.sky.zd * 180/M_PI; + dim.fNominalAz = data.sky.az * 180/M_PI; + dim.fPointingZd = zd * 180/M_PI; + dim.fPointingAz = az * 180/M_PI; + dim.fFeedbackZd = data.mount.zd; + dim.fFeedbackAz = data.mount.az; + dim.fNumLeds = uint16_t(ptr[2]); + dim.fNumRings = uint16_t(ptr[3]); + dim.fCenterX = ptr[4]; + dim.fCenterY = ptr[5]; + dim.fCenterMag = ptr[6]; + dim.fStarX = ptr[7]; + dim.fStarY = ptr[8]; + dim.fStarMag = ptr[9]; + dim.fRotation = ptr[10]; + dim.fDx = ptr[0]; + dim.fDy = ptr[1]; + dim.fRealMag = fPointingSetup.source.mag; + + fDrive.UpdateTPoint(evt.GetTime(), dim, fPointingSetup.source.name); + + ostringstream txt; + txt << "TPoint recorded [" << zd*180/M_PI << "/" << az*180/M_PI << " | " + << data.sky.zd*180/M_PI << "/" << data.sky.az*180/M_PI << " | " + << data.mount.zd << "/" << data.mount.az << " | " + << dx*180/M_PI << "/" << dy*180/M_PI << "]"; + T::Info(txt); + + return T::GetCurrentState(); + } + + // -------------------------- Helpers ----------------------------------- + + double GetDevAbs(double nomzd, double meszd, double devaz) + { + nomzd *= M_PI/180; + meszd *= M_PI/180; + devaz *= M_PI/180; + + const double x = sin(meszd) * sin(nomzd) * cos(devaz); + const double y = cos(meszd) * cos(nomzd); + + return acos(x + y) * 180/M_PI; + } + + double ReadAngle(istream &in) + { + char sgn; + uint16_t d, m; + float s; + + in >> sgn >> d >> m >> s; + + const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.; + return sgn=='-' ? -ret : ret; + } + + bool CheckRange(ZdAz pos) + { + if (pos.zdfPointingMax.zd) + { + T::Error("Zenith distance "+to_string(pos.zd)+" exceeds limit "+to_string(fPointingMax.zd)); + return false; + } + + if (pos.azfPointingMax.az) + { + T::Error("Azimuth angle "+to_string(pos.az)+" exceeds limit "+to_string(fPointingMax.az)); + return false; + } + + return true; + } + + PointingData CalcPointingPos(double mjd) + { + return fPointingModel.CalcPointingPos(fPointingSetup, mjd, fWeather, fWeatherTimeout); + } + + // ----------------------------- SDO Commands ------------------------------ + + int RequestSdo(const EventImp &evt) + { + // FIXME: STop telescope + if (!CheckEventSize(evt.GetSize(), "RequestSdo", 6)) + return T::kSM_FatalError; + + const uint16_t node = evt.Get(); + const uint16_t index = evt.Get(2); + const uint16_t subidx = evt.Get(4); + + if (node!=1 && node !=3) + { + T::Error("Node id must be 1 (az) or 3 (zd)."); + return T::GetCurrentState(); + } + + if (subidx>0xff) + { + T::Error("Subindex must not be larger than 255."); + return T::GetCurrentState(); + } + + fDrive.RequestSdo(node, index, subidx); + + return T::GetCurrentState(); + } + + int SendSdo(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "SendSdo", 6+8)) + return T::kSM_FatalError; + + const uint16_t node = evt.Get(); + const uint16_t index = evt.Get(2); + const uint16_t subidx = evt.Get(4); + const uint64_t value = evt.Get(6); + + if (node!=1 && node!=3) + { + T::Error("Node id must be 1 (az) or 3 (zd)."); + return T::GetCurrentState(); + } + + if (subidx>0xff) + { + T::Error("Subindex must not be larger than 255."); + return T::GetCurrentState(); + } + + fDrive.SendSdo(node, index, subidx, value); + + return T::GetCurrentState(); + } + + // --------------------- Moving and tracking --------------------- + + uint16_t fStep; + bool fIsTracking; + Acceleration fAccPointing; + Acceleration fAccTracking; + Acceleration fAccMax; + double fMaxPointingResidual; + double fPointingVelocity; + + int InitMovement(const ZdAz &sky, bool tracking=false, const string &name="") + { + fMovementTarget = fPointingModel.SkyToMount(sky); + + // Check whether bending is valid! + if (!CheckRange(sky*(180/M_PI))) + return StopMovement(); + + fStep = 0; + fIsTracking = tracking; + + fDrive.SetRpmMode(false); // *NEW* (Stop a previous tracking to avoid the pointing command to be ignored) + fDrive.SetAcceleration(fAccPointing); + + if (!tracking) + fDrive.UpdateSource(Time(), name, false); + else + { + const array dim = + {{ + fPointingSetup.source.ra, + fPointingSetup.source.dec, + fPointingSetup.wobble_offset * 180/M_PI, + fPointingSetup.wobble_angle * 180/M_PI, + fPointingSetup.orbit_period * 24*60 + }}; + fDrive.UpdateSource(fPointingSetup.start, dim, fPointingSetup.source.name); + } + + return State::kMoving; + } + + int MoveTo(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "MoveTo", 16)) + return T::kSM_FatalError; + + const double *dat = evt.Ptr(); + + ostringstream out; + out << "Pointing telescope to Zd=" << dat[0] << "deg Az=" << dat[1] << "deg"; + T::Message(out); + + return InitMovement(ZdAz(dat[0]*M_PI/180, dat[1]*M_PI/180)); + } + + int InitTracking() + { + fPointingSetup.start = Time().Mjd(); + + const PointingData data = CalcPointingPos(fPointingSetup.start); + + ostringstream out; + out << "Tracking position now at Zd=" << data.sky.zd*180/M_PI << "deg Az=" << data.sky.az*180/M_PI << "deg"; + T::Info(out); + + return InitMovement(data.sky, true); + } + + int StartTracking(const Source &src, double offset, double angle, double period=0) + { + ostringstream out; + out << "Tracking Ra=" << src.ra << "h Dec=" << src.dec << "deg"; + if (!src.name.empty()) + out << " [" << src.name << "]"; + T::Info(out); + + fPointingSetup.planet = kENone; + fPointingSetup.source = src; + fPointingSetup.orbit_period = period / 1440; // [min->day] + fPointingSetup.wobble_angle = angle * M_PI/180; // [deg->rad] + fPointingSetup.wobble_offset = offset * M_PI/180; // [deg->rad] + + return InitTracking(); + } + + int TrackCelest(const Planets_t &p) + { + switch (p) + { + case kEMoon: fPointingSetup.source.name = "Moon"; break; + case kEVenus: fPointingSetup.source.name = "Venus"; break; + case kEMars: fPointingSetup.source.name = "Mars"; break; + case kEJupiter: fPointingSetup.source.name = "Jupiter"; break; + case kESaturn: fPointingSetup.source.name = "Saturn"; break; + default: + T::Error("TrackCelest - Celestial object "+to_string(p)+" not yet supported."); + return T::GetCurrentState(); + } + + fPointingSetup.planet = p; + fPointingSetup.wobble_offset = 0; + + fDrive.UpdateSource(Time(), fPointingSetup.source.name, true); + + return InitTracking(); + } + + int Park() + { + ostringstream out; + out << "Parking telescope at Zd=" << fParkingPos.zd << "deg Az=" << fParkingPos.az << "deg"; + T::Message(out); + + const int rc = InitMovement(ZdAz(fParkingPos.zd*M_PI/180, fParkingPos.az*M_PI/180), false, "Park"); + return rc==State::kMoving ? State::kParking : rc; + } + +/* + int StartWobble(const double &srcra, const double &srcdec, + const double &woboff, const double &wobang, + const string name="") + { + + + const double ra = srcra *M_PI/ 12; + const double dec = srcdec*M_PI/180; + const double off = woboff*M_PI/180; + const double dir = wobang*M_PI/180; + + if (off==0) + { + const array dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }}; + UpdateSource(dim, src.name); + return StartTracking(src);//srcra, srcdec); + } + + const double cosdir = cos(dir); + const double sindir = sin(dir); + const double cosoff = cos(off); + const double sinoff = sin(off); + const double cosdec = cos(dec); + const double sindec = sin(dec); + + const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir; + if (sintheta >= 1) + { + T::Error("cos(Zd) > 1"); + return T::GetCurrentState(); + } + + const double costheta = sqrt(1 - sintheta*sintheta); + + const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta); + const double sindeltara = sindir*sinoff/costheta; + + const double ndec = asin(sintheta)*180/M_PI; + const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI; + + const array dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }}; + UpdateSource(dim, src.name); + + return StartTracking(nra, ndec); + } +*/ + + int Wobble(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "Wobble", 32)) + return T::kSM_FatalError; + + const double *dat = evt.Ptr(); + + Source src; + src.ra = dat[0]; + src.dec = dat[1]; + return StartTracking(src, dat[2], dat[3]); + } + + int Orbit(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "Orbit", 40)) + return T::kSM_FatalError; + + const double *dat = evt.Ptr(); + + Source src; + src.ra = dat[0]; + src.dec = dat[1]; + return StartTracking(src, dat[2], dat[3], dat[4]); + } + + const sources::const_iterator GetSourceFromDB(const char *ptr, const char *last) + { + if (find(ptr, last, '\0')==last) + { + T::Fatal("TrackWobble - The name transmitted by dim is not null-terminated."); + throw uint32_t(T::kSM_FatalError); + } + + const string name(ptr); + + const sources::const_iterator it = fSources.find(name); + if (it==fSources.end()) + { + T::Error("Source '"+name+"' not found in list."); + throw uint32_t(T::GetCurrentState()); + } + + return it; + } + + int TrackWobble(const EventImp &evt) + { + if (evt.GetSize()<2) + { + ostringstream msg; + msg << "TrackWobble - Received event has " << evt.GetSize() << " bytes, but expected at least 3."; + T::Fatal(msg); + return T::kSM_FatalError; + } + + if (evt.GetSize()==2) + { + ostringstream msg; + msg << "TrackWobble - Source name missing."; + T::Error(msg); + return T::GetCurrentState(); + } + + const uint16_t wobble = evt.GetUShort(); + if (wobble!=1 && wobble!=2) + { + ostringstream msg; + msg << "TrackWobble - Wobble id " << wobble << " undefined, only 1 and 2 allowed."; + T::Error(msg); + return T::GetCurrentState(); + } + + const char *ptr = evt.Ptr(2); + const char *last = ptr+evt.GetSize()-2; + + try + { + const sources::const_iterator it = GetSourceFromDB(ptr, last); + + const Source &src = it->second; + return StartTracking(src, src.offset, src.angles[wobble-1]); + } + catch (const uint32_t &e) + { + return e; + } + } + + int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0, double time=0) + { + const char *last = ptr+size; + + try + { + const sources::const_iterator it = GetSourceFromDB(ptr, last); + + const Source &src = it->second; + return StartTracking(src, offset<0?0.6/*src.offset*/:offset, angle, time); + } + catch (const uint32_t &e) + { + return e; + } + } + + int Track(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "Track", 16)) + return T::kSM_FatalError; + + Source src; + + src.name = ""; + src.ra = evt.Get(0); + src.dec = evt.Get(8); + + return StartTracking(src, 0, 0); + } + + int TrackSource(const EventImp &evt) + { + if (evt.GetSize()<16) + { + ostringstream msg; + msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 17."; + T::Fatal(msg); + return T::kSM_FatalError; + } + + if (evt.GetSize()==16) + { + ostringstream msg; + msg << "TrackOn - Source name missing."; + T::Error(msg); + return T::GetCurrentState(); + } + + const double offset = evt.Get(0); + const double angle = evt.Get(8); + + return StartTrackWobble(evt.Ptr(16), evt.GetSize()-16, offset, angle); + } + + int TrackOn(const EventImp &evt) + { + if (evt.GetSize()==0) + { + ostringstream msg; + msg << "TrackOn - Source name missing."; + T::Error(msg); + return T::GetCurrentState(); + } + + return StartTrackWobble(evt.Ptr(), evt.GetSize()); + } + + int TrackOrbit(const EventImp &evt) + { + if (evt.GetSize()<16) + { + ostringstream msg; + msg << "TrackOrbit - Received event has " << evt.GetSize() << " bytes, but expected at least 17."; + T::Fatal(msg); + return T::kSM_FatalError; + } + if (evt.GetSize()==16) + { + ostringstream msg; + msg << "TrackOrbit - Source name missing."; + T::Error(msg); + return T::GetCurrentState(); + } + + const double angle = evt.Get(0); + const double time = evt.Get(8); + + return StartTrackWobble(evt.Ptr(16), evt.GetSize()-16, -1, angle, time); + } + + int StopMovement() + { + fDrive.SetAcceleration(fAccMax); + fDrive.SetRpmMode(false); + + fTrackingLoop.cancel(); + + fDrive.UpdateSource(Time(), "", false); + + return State::kStopping; + } + + // --------------------- Others --------------------- + + int TPoint() + { + T::Info("TPoint initiated."); + Dim::SendCommandNB("TPOINT/EXECUTE"); + return T::GetCurrentState(); + } + + int SetLedBrightness(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8)) + return T::kSM_FatalError; + + const uint32_t *led = evt.Ptr(); + + fDrive.SetLedVoltage(led[0], led[1]); + + return T::GetCurrentState(); + } + + int SetLedsOff() + { + fDrive.SetLedVoltage(0, 0); + return T::GetCurrentState(); + } + + // --------------------- Internal --------------------- + + int SetVerbosity(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 2)) + return T::kSM_FatalError; + + fDrive.SetVerbosity(evt.GetUShort()); + + return T::GetCurrentState(); + } + + int Print() + { + for (auto it=fSources.begin(); it!=fSources.end(); it++) + { + const string &name = it->first; + const Source &src = it->second; + + T::Out() << name << ","; + T::Out() << src.ra << "," << src.dec << "," << src.offset << ","; + T::Out() << src.angles[0] << "," << src.angles[1] << endl; + } + return T::GetCurrentState(); + } + + int Unlock() + { + const int rc = CheckState(); + return rc<0 ? State::kInitialized : rc; + } + + int ReloadSources() + { + try + { + ReadDatabase(); + } + catch (const exception &e) + { + T::Error("Reading sources from databse failed: "+string(e.what())); + } + return T::GetCurrentState(); + } + + int Disconnect() + { + // Close all connections + fDrive.PostClose(false); + + /* + // Now wait until all connection have been closed and + // all pending handlers have been processed + poll(); + */ + + return T::GetCurrentState(); + } + + int Reconnect(const EventImp &evt) + { + // Close all connections to supress the warning in SetEndpoint + fDrive.PostClose(false); + + // Now wait until all connection have been closed and + // all pending handlers have been processed + ba::io_service::poll(); + + if (evt.GetBool()) + fDrive.SetEndpoint(evt.GetString()); + + // Now we can reopen the connection + fDrive.PostClose(true); + + return T::GetCurrentState(); + } + + // ========================= Tracking code ============================= + + int UpdateTrackingPosition() + { + // First calculate deviation between + // command position and nominal position + //fPointing.mount = sepos; // [deg] ref pos for alignment + const PointingData data = CalcPointingPos(fDrive.GetSeTime()); + + // Get current position and calculate deviation + const Encoder sepos = fDrive.GetSePos()*360; // [deg] + const Encoder dev = sepos - data.mount; + + // Calculate absolut deviation on the sky + const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az)*3600; + + // Smoothing + fDevBuffer[fDevCount++%5] = absdev; + + // Calculate average + const uint8_t cnt = fDevCount<5 ? fDevCount : 5; + const double avgdev = accumulate(fDevBuffer.begin(), fDevBuffer.begin()+cnt, 0.)/cnt; + + // Count the consecutive number of avgdev below fDeviationLimit + if (avgdev dim; + dim[0] = data.pointing.ra * 12/M_PI; // Ra [h] optical axis + dim[1] = data.pointing.dec * 180/M_PI; // Dec [deg] optical axis + dim[2] = ha - data.pointing.ra; // Ha [h] optical axis + dim[3] = data.source.ra * 12/M_PI; // SrcRa [h] source position + dim[4] = data.source.dec * 180/M_PI; // SrcDec [deg] source position + dim[5] = ha - data.source.ra; // SrcHa [h] source position + dim[6] = data.sky.zd * 180/M_PI; // Zd [deg] optical axis + dim[7] = data.sky.az * 180/M_PI; // Az [deg] optical axis + dim[8] = dev.zd; // dZd [deg] control deviation + dim[9] = dev.az; // dAz [deg] control deviation + dim[10] = absdev; // dev [arcsec] absolute control deviation + dim[11] = avgdev; // dev [arcsec] average control deviation + + fDrive.UpdateTracking(fDrive.GetSeTime(), dim); + + if (fDrive.GetVerbosity()) + T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Deviation [deg] " << absdev << "\"|" << avgdev << "\"|" << fDevCount<< " dZd=" << dev.zd*3600 << "\" dAz=" << dev.az*3600 << "\"" << endl; + + // Maximum deviation execeeded -> fall back to Tracking state + if (T::GetCurrentState()==State::kOnTrack && avgdev>fDeviationMax) + return State::kTracking; + + // Condition for OnTrack state achieved -> enhance to OnTrack state + if (T::GetCurrentState()==State::kTracking && fTrackingCounter>=fDeviationCounter) + return State::kOnTrack; + + // No state change + return T::GetCurrentState(); + } + + void UpdatePointingPosition() + { + const Encoder sepos = fDrive.GetSePos()*360; // [deg] ref pos for alignment + + const ZdAz pos = fPointingModel.MountToSky(sepos); + + array data; + data[0] = pos.zd*180/M_PI; // Zd [deg] + data[1] = pos.az*180/M_PI; // Az [deg] + fDrive.UpdatePointing(fDrive.GetSeTime(), data); + + if (fDrive.GetVerbosity()) + T::Out() << Time().GetAsStr(" %H:%M:%S.%f") << " - Position [deg] " << pos.zd*180/M_PI << " " << pos.az*180/M_PI << endl; + } +/* + void TrackingLoop2(const boost::system::error_code &error=boost::system::error_code()) + { + if (error==ba::error::basic_errors::operation_aborted) + return; + + if (error) + { + ostringstream str; + str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl; + T::Error(str); + return; + } + + if (T::GetCurrentState()!=State::kTracking && + T::GetCurrentState()!=State::kOnTrack) + return; + + // + // Update speed as often as possible. + // make sure, that dt is around 10 times larger than the + // update time + // + // The loop should not be executed faster than the ramp of + // a change in the velocity can be followed. + // + fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(2000/8)); + + // We want to reach the theoretical position exactly in about 7.6s + const float dt = 2.0; + + // Get current position and corresponding time + const double mjd = fDrive.GetSeTime(); + //const Encoder sepos = fDrive.GetSePos(); // [rev] + + // Request Target position for Now+dt + //fPointing.mount = sepos*360; // [deg] ref pos for alignment + + fPointing.Update(mjd+dt/24/3600, fWeather, fWeatherTimeout); + if (!CheckRange(fPointing.sky)) + { + StopMovement(); + T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop"); + return; + } + + // Destination position at t+dt in re-units + const Encoder dest = fPointing.mount*(1./360); // [rev] + + // Current position + const Encoder sepos = fDrive.GetSePos(); // [rev] + + // Now calculate the distance to move from now + // to a time in t+dt. + const Encoder dist = dest-sepos; // [rev] + + // This is to avoid that we are always too slow + // On average 30ms have been passed since the last report was + // received, it is fair to assume that on average als0 30ms + // will pass until this report arrives. Without that correction + // we were always too fast (60ms = 3% von 2000ms) + const double deltat = 60./1000/60;//(Time().Mjd() - mjd)/1440; //[min] + + // Assume a mainly linear movements. We can estimate the position + // at which we are now + static Velocity last = 0; + + // Velocity to go [rev/min] to reach the right position at time t+dt + // correct for the duration of RaDec2AltAz + const Velocity vt = (dist - last*deltat) / (dt/60 - deltat); //[rev/min] + + last = vt; + + if (fDrive.GetVerbosity()>1) + { + T::Out() << "Destination [deg] " << dest.zd *360 << " " << dest.az *360 << endl; + T::Out() << "Position [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl; + T::Out() << "Distance [arcmin] " << dist.zd *360*60 << " " << dist.az *360*60 << endl; + T::Out() << "Velocity [rpm] " << vt.zd << " " << vt.az << endl; + T::Out() << "Delta T (enc) [ms] " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl; + T::Out() << "Delta T (now) [ms] " << (Time().Mjd()-mjd)*24*3600*1000 << endl; + } + + // Tracking loop every 250ms + // Vorsteuerung 2s + // Delta T (enc) 5ms, every 5th, 25ms + // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s) + + // + // FIXME: check if the drive is fast enough to follow the star + // + // Velocity units (would be 100 for %) + + fDrive.SetTrackingVelocity(vt); + + fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop, + this, ba::placeholders::error)); + } +*/ + void TrackingLoop(const boost::system::error_code &error=boost::system::error_code()) + { + if (error==ba::error::basic_errors::operation_aborted) + return; + + if (error) + { + ostringstream str; + str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl; + T::Error(str); + return; + } + + if (T::GetCurrentState()!=State::kTracking && + T::GetCurrentState()!=State::kOnTrack) + return; + + // + // Update speed as often as possible. + // make sure, that dt is around 10 times larger than the + // update time + // + // The loop should not be executed faster than the ramp of + // a change in the velocity can be followed. + // + fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(250)); + + const double mjd = Time().Mjd(); + + // I assume that it takes about 50ms for the value to be + // transmitted and the drive needs time to follow as well (maybe + // more than 50ms), therefore the calculated speec is calculated + // for a moment 50ms in the future + const PointingData data = CalcPointingPos(fDrive.GetSeTime()); + const PointingData data0 = CalcPointingPos(mjd-0.45/24/3600); + const PointingData data1 = CalcPointingPos(mjd+0.55/24/3600); + + const Encoder dest = data.mount *(1./360); // [rev] + const Encoder dest0 = data0.mount*(1./360); // [rev] + const Encoder dest1 = data1.mount*(1./360); // [rev] + + if (!CheckRange(data1.sky)) + { + StopMovement(); + T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop"); + return; + } + + // Current position + const Encoder sepos = fDrive.GetSePos(); // [rev] + + // Now calculate the current velocity + const Encoder dist = dest1 - dest0; // [rev] Distance between t-1s and t+1s + const Velocity vel = dist/(1./60); // [rev/min] Actual velocity of the pointing position + + const Encoder dev = sepos - dest; // [rev] Current control deviation + const Velocity vt = vel - dev/(1./60); // [rev/min] Correct velocity by recent control deviation + // correct control deviation with 5s + if (fDrive.GetVerbosity()>1) + { + T::Out() << "Ideal position [deg] " << dest.zd *360 << " " << dest.az *360 << endl; + T::Out() << "Encoder pos. [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl; + T::Out() << "Deviation [arcmin] " << dev.zd *360*60 << " " << dev.az *360*60 << endl; + T::Out() << "Distance 1s [arcmin] " << dist.zd *360*60 << " " << dist.az *360*60 << endl; + T::Out() << "Velocity 1s [rpm] " << vt.zd << " " << vt.az << endl; + T::Out() << "Delta T (enc) [ms] " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl; + T::Out() << "Delta T (now) [ms] " << (Time().Mjd()-mjd)*24*3600*1000 << endl; + } + + // Tracking loop every 250ms + // Vorsteuerung 2s + // Delta T (enc) 5ms, every 5th, 25ms + // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s) + + // + // FIXME: check if the drive is fast enough to follow the star + // + // Velocity units (would be 100 for %) + + fDrive.SetTrackingVelocity(vt); + + fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop, + this, ba::placeholders::error)); + } + + // ===================================================================== + + int CheckState() + { + if (!fDrive.IsConnected()) + return State::kDisconnected; + + if (!fDrive.IsOnline()) + return State::kUnavailable; + + // FIXME: This can prevent parking in case e.g. + // of e8029 Position limit exceeded + if (fDrive.HasError()) + { + if (T::GetCurrentState()==State::kOnTrack || + T::GetCurrentState()==State::kTracking || + T::GetCurrentState()==State::kMoving || + T::GetCurrentState()==State::kParking) + return StopMovement(); + + if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving()) + return State::kStopping; + + return StateMachineImp::kSM_Error; + } + + // This can happen if one of the drives is not in RF. + // Usually this only happens when the drive is not yet in RF + // or an error was just cleared. Usually there is no way that + // a drive goes below the RF state during operation without + // a warning or error message. + if (fDrive.IsOnline() && !fDrive.IsReady()) + return State::kAvailable; + + if (fDrive.IsOnline() && fDrive.IsBlocked()) + return State::kBlocked; + + // This is the case as soon as the init commands were send + // after a connection to the SPS was established + if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized()) + return State::kArmed; + + return -1; + } + + int Execute() + { + const Time now; + if (now>fSunRise) + { + if (T::GetCurrentState()>State::kLocked) + return Park(); + + if (T::GetCurrentState()==State::kLocked) + { + fSunRise = now.GetNextSunRise(); + + ostringstream msg; + msg << "Next sun-rise will be at " << fSunRise; + T::Info(msg); + + return State::kLocked; + } + } + + if (T::GetCurrentState()==State::kLocked) + return State::kLocked; + + // FIXME: Send STOP if IsPositioning or RpmActive but no + // Moving or Tracking state + + const int rc = CheckState(); + if (rc>0) + return rc; + /* + + if (!fDrive.IsConnected()) + return State::kDisconnected; + + if (!fDrive.IsOnline()) + return State::kUnavailable; + + // FIXME: This can prevent parking in case e.g. + // of e8029 Position limit exceeded + if (fDrive.HasError()) + { + if (T::GetCurrentState()==State::kOnTrack || + T::GetCurrentState()==State::kTracking || + T::GetCurrentState()==State::kMoving || + T::GetCurrentState()==State::kParking) + return StopMovement(); + + if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving()) + return State::kStopping; + + return StateMachineImp::kSM_Error; + } + + // This can happen if one of the drives is not in RF. + // Usually this only happens when the drive is not yet in RF + // or an error was just cleared. Usually there is no way that + // a drive goes below the RF state during operation without + // a warning or error message. + if (fDrive.IsOnline() && !fDrive.IsReady()) + return State::kAvailable; + + if (fDrive.IsOnline() && fDrive.IsBlocked()) + return State::kBlocked; + + // This is the case as soon as the init commands were send + // after a connection to the SPS was established + if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized()) + return State::kArmed; +*/ + // Once every second + static time_t lastTime = 0; + const time_t tm = time(NULL); + if (lastTime!=tm && fDrive.IsInitialized()) + { + lastTime=tm; + + UpdatePointingPosition(); + + if (T::GetCurrentState()==State::kTracking || T::GetCurrentState()==State::kOnTrack) + return UpdateTrackingPosition(); + } + + if (T::GetCurrentState()==State::kStopping && !fDrive.IsMoving()) + return State::kArmed; + + if ((T::GetCurrentState()==State::kMoving || + T::GetCurrentState()==State::kParking) && !fDrive.IsMoving()) + { + if (fIsTracking && fStep==1) + { + // Init tracking + fDrive.SetAcceleration(fAccTracking); + fDrive.SetRpmMode(true); + + fDevCount = 0; + fTrackingCounter = 0; + + fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(1)); + fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop, + this, ba::placeholders::error)); + + fPointingSetup.start = Time().Mjd(); + + const PointingData data = CalcPointingPos(fPointingSetup.start); + + ostringstream out; + out << "Start tracking at Ra=" << data.pointing.ra*12/M_PI << "h Dec=" << data.pointing.dec*180/M_PI << "deg"; + T::Info(out); + + return State::kTracking; + } + + // Get feedback 2 + const Encoder dest = fMovementTarget*(1./360); // [rev] + const Encoder sepos = fDrive.GetSePos(); // [rev] + + // Calculate residual to move deviation + const Encoder dist = dest - sepos; // [rev] + + // Check which axis should still be moved + Encoder cd = dist; // [rev] + cd *= 1./fMaxPointingResidual; // Scale to units of the maximum residual + cd = cd.Abs(); + + // Check if there is a control deviation on the axis + const bool cdzd = cd.zd>1; + const bool cdaz = cd.az>1; + + if (!fIsTracking) + { + // check if we reached the correct position already + if (!cdzd && !cdaz) + { + T::Info("Target position reached in "+to_string(fStep)+" steps."); + return T::GetCurrentState()==State::kParking ? State::kLocked : State::kArmed; + } + + if (fStep==10) + { + T::Error("Target position not reached in "+to_string(fStep)+" steps."); + return State::kPositioningFailed; + } + } + + const Encoder t = dist.Abs()/fDrive.GetVelUnit(); + + const Velocity vel = + t.zd > t.az ? + Velocity(1, t.zd==0?0:t.az/t.zd) : + Velocity(t.az==0?0:t.zd/t.az, 1); + + if (fDrive.GetVerbosity()) + { + T::Out() << "Moving step " << fStep << endl; + T::Out() << "Encoder [deg] " << sepos.zd*360 << " " << sepos.az*360 << endl; + T::Out() << "Destination [deg] " << dest.zd *360 << " " << dest.az *360 << endl; + T::Out() << "Residual [deg] " << dist.zd *360 << " " << dist.az *360 << endl; + T::Out() << "Residual/max [1] " << cd.zd << " " << cd.az << endl; + T::Out() << "Rel. time [1] " << t.zd << " " << t.az << endl; + T::Out() << "Rel. velocity [1] " << vel.zd << " " << vel.az << endl; + } + + fDrive.SetPointingVelocity(vel, fPointingVelocity); + fDrive.StartAbsolutePositioning(dest, cdzd, cdaz); + + ostringstream out; + if (fStep==0) + out << "Moving to encoder Zd=" << dest.zd*360 << "deg Az=" << dest.az*360 << "deg"; + else + out << "Moving residual of dZd=" << dist.zd*360*60 << "' dAz=" << dist.az*360*60 << "'"; + T::Info(out); + + fStep++; + } + + return T::GetCurrentState()>=State::kInitialized ? + T::GetCurrentState() : State::kInitialized; + } + +public: + StateMachineDrive(ostream &out=cout) : + StateMachineAsio(out, "DRIVE_CONTROL"), fDrive(*this, *this), + fTrackingLoop(*this), fSunRise(Time().GetNextSunRise()), fDevBuffer(5) + { + + T::Subscribe("MAGIC_WEATHER/DATA") + (bind(&StateMachineDrive::HandleWeatherData, this, placeholders::_1)); + + T::Subscribe("TPOINT/DATA") + (bind(&StateMachineDrive::HandleTPoint, this, placeholders::_1)); + + /* + kDisconnected = 1, + kConnected, + kLocked, + kUnavailable, // IndraDrives not connected + kAvailable, // IndraDrives connected, but not in Af + kBlocked, // Drive blocked by manual operation of emergency button + kArmed, // IndraDrives Af, not yet initialized + kInitialized, // IndraDrives Af, initialized + kStopping, + kParking, + kMoving, + kTracking, + kOnTrack, + + */ + // State names + T::AddStateName(State::kDisconnected, "Disconnected", + "No connection to SPS"); + T::AddStateName(State::kConnected, "Connected", + "Connection to SPS, no information received yet"); + + T::AddStateName(State::kLocked, "Locked", + "Drive system is locked (will not accept commands)"); + + T::AddStateName(State::kUnavailable, "Unavailable", + "Connected to SPS, no connection to at least one IndraDrives"); + T::AddStateName(State::kAvailable, "Available", + "Connected to SPS and to IndraDrives, but at least one drive not in RF"); + T::AddStateName(State::kBlocked, "Blocked", + "Drive system is blocked by manual operation or an released emergeny button"); + T::AddStateName(State::kArmed, "Armed", + "Connected to SPS and IndraDrives in RF, but not yet initialized"); + T::AddStateName(State::kInitialized, "Initialized", + "Connected to SPS and IndraDrives in RF and initialized"); + + T::AddStateName(State::kStopping, "Stopping", + "Stop command sent, waiting for telescope to be still"); + T::AddStateName(State::kParking, "Parking", + "Telescope in parking operation, waiting for telescope to be still"); + T::AddStateName(State::kMoving, "Moving", + "Telescope moving"); + T::AddStateName(State::kTracking, "Tracking", + "Telescope in tracking mode"); + T::AddStateName(State::kOnTrack, "OnTrack", + "Telescope tracking stable"); + + + T::AddEvent("REQUEST_SDO", "S:3", State::kArmed) + (bind(&StateMachineDrive::RequestSdo, this, placeholders::_1)) + ("Request an SDO from the drive" + "|node[uint32]:Node identifier (1:az, 3:zd)" + "|index[uint32]:SDO index" + "|subindex[uint32]:SDO subindex"); + + T::AddEvent("SET_SDO", "S:3;X:1", State::kArmed) + (bind(&StateMachineDrive::SendSdo, this, placeholders::_1)) + ("Request an SDO from the drive" + "|node[uint32]:Node identifier (1:az, 3:zd)" + "|index[uint32]:SDO index" + "|subindex[uint32]:SDO subindex" + "|value[uint64]:Value"); + + // Drive Commands + T::AddEvent("MOVE_TO", "D:2", State::kInitialized) // ->ZDAZ + (bind(&StateMachineDrive::MoveTo, this, placeholders::_1)) + ("Move the telescope to the given local sky coordinates" + "|Zd[deg]:Zenith distance" + "|Az[deg]:Azimuth"); + + T::AddEvent("TRACK", "D:2", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::Track, this, placeholders::_1)) + ("Move the telescope to the given sky coordinates and start tracking them" + "|Ra[h]:Right ascension" + "|Dec[deg]:Declination"); + + T::AddEvent("WOBBLE", "D:4", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::Wobble, this, placeholders::_1)) + ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them" + "|Ra[h]:Right ascension" + "|Dec[deg]:Declination" + "|Offset[deg]:Wobble offset" + "|Angle[deg]:Wobble angle"); + + T::AddEvent("ORBIT", "D:5", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::Orbit, this, placeholders::_1)) + ("Move the telescope in a circle around the source" + "|Ra[h]:Right ascension" + "|Dec[deg]:Declination" + "|Offset[deg]:Wobble offset" + "|Angle[deg]:Starting angle" + "|Period[min]:Time for one orbit"); + + T::AddEvent("TRACK_SOURCE", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::TrackSource, this, placeholders::_1)) + ("Move the telescope to the given wobble position around the given source and start tracking" + "|Offset[deg]:Wobble offset" + "|Angle[deg]:Wobble angle" + "|Name[string]:Source name"); + + T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1)) + ("Move the telescope to the given wobble position around the given source and start tracking" + "|Id:Wobble angle id (1 or 2)" + "|Name[string]:Source name"); + + T::AddEvent("TRACK_ORBIT", "D:2;C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::TrackOrbit, this, placeholders::_1)) + ("Move the telescope in a circle around the source" + "|Angle[deg]:Starting angle" + "|Period[min]:Time for one orbit" + "|Name[string]:Source name"); + + T::AddEvent("TRACK_ON", "C", State::kInitialized, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::TrackOn, this, placeholders::_1)) + ("Move the telescope to the given position and start tracking" + "|Name[string]:Source name"); +/* + T::AddEvent("RESUME", StateMachineImp::kSM_Error) + (bind(&StateMachineDrive::Resume, this)) + ("If drive is in Error state, this can b used to resume the last tracking command, if the last command sent to cosy was a tracking command."); +*/ + T::AddEvent("MOON", State::kInitialized, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::TrackCelest, this, kEMoon)) + ("Start tracking the moon"); + T::AddEvent("VENUS", State::kInitialized, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::TrackCelest, this, kEVenus)) + ("Start tracking Venus"); + T::AddEvent("MARS", State::kInitialized, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::TrackCelest, this, kEMars)) + ("Start tracking Mars"); + T::AddEvent("JUPITER", State::kInitialized, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::TrackCelest, this, kEJupiter)) + ("Start tracking Jupiter"); + T::AddEvent("SATURN", State::kInitialized, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::TrackCelest, this, kESaturn)) + ("Start tracking Saturn"); + + // FIXME: What to do in error state? + T::AddEvent("PARK", State::kInitialized, State::kMoving, State::kTracking, State::kOnTrack) + (bind(&StateMachineDrive::Park, this)) + ("Park the telescope"); + + T::AddEvent("STOP") + (bind(&StateMachineDrive::StopMovement, this)) + ("Stop any kind of movement."); + + T::AddEvent("TPOINT", State::kOnTrack) + (bind(&StateMachineDrive::TPoint, this)) + ("Take a TPoint"); + + T::AddEvent("SET_LED_BRIGHTNESS", "I:2") + (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1)) + ("Set the LED brightness of the top and bottom leds" + "|top[au]:Allowed range 0-32767 for top LEDs" + "|bot[au]:Allowed range 0-32767 for bottom LEDs"); + + T::AddEvent("LEDS_OFF") + (bind(&StateMachineDrive::SetLedsOff, this)) + ("Switch off TPoint LEDs"); + + T::AddEvent("UNLOCK", Drive::State::kLocked) + (bind(&StateMachineDrive::Unlock, this)) + ("Unlock locked state."); + + // Verbosity commands + T::AddEvent("SET_VERBOSITY", "S:1") + (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1)) + ("Set verbosity state" + "|verbosity[uint16]:disable or enable verbosity for received data (yes/no), except dynamic data"); + + // Conenction commands + T::AddEvent("DISCONNECT", State::kConnected) + (bind(&StateMachineDrive::Disconnect, this)) + ("disconnect from ethernet"); + + T::AddEvent("RECONNECT", "O", State::kDisconnected, State::kConnected) + (bind(&StateMachineDrive::Reconnect, this, placeholders::_1)) + ("(Re)connect Ethernet connection to SPS, a new address can be given" + "|[host][string]:new ethernet address in the form "); + + + T::AddEvent("PRINT") + (bind(&StateMachineDrive::Print, this)) + ("Print source list."); + + T::AddEvent("RELOAD_SOURCES", State::kDisconnected, State::kConnected, State::kArmed, State::kInitialized, State::kLocked) + (bind(&StateMachineDrive::ReloadSources, this)) + ("Reload sources from database after database has changed.."); + + + //fDrive.SetUpdateStatus(std::bind(&StateMachineDrive::UpdateStatus, this, placeholders::_1, placeholders::_2)); + fDrive.StartConnect(); + } + + void SetEndpoint(const string &url) + { + fDrive.SetEndpoint(url); + } + + bool AddSource(const string &name, const Source &src) + { + const auto it = fSources.find(name); + if (it!=fSources.end()) + T::Warn("Source '"+name+"' already in list... overwriting."); + + fSources[name] = src; + return it==fSources.end(); + } + + void ReadDatabase(bool print=true) + { +#ifdef HAVE_SQL + Database db(fDatabase); + + T::Message("Connected to '"+db.uri()+"'"); + + const mysqlpp::StoreQueryResult res = + db.query("SELECT fSourceName, fRightAscension, fDeclination, fWobbleOffset, fWobbleAngle0, fWobbleAngle1, fMagnitude FROM Source").store(); + + fSources.clear(); + for (vector::const_iterator v=res.begin(); v("quiet")); + + fMaxPointingResidual = conf.Get("pointing.max.residual"); + fPointingVelocity = conf.Get("pointing.velocity"); + + fPointingMin = Encoder(conf.Get("pointing.min.zd"), + conf.Get("pointing.min.az")); + fPointingMax = Encoder(conf.Get("pointing.max.zd"), + conf.Get("pointing.max.az")); + + fParkingPos.zd = conf.Has("parking-pos.zd") ? conf.Get("parking-pos.zd") : 90; + fParkingPos.az = conf.Has("parking-pos.az") ? conf.Get("parking-pos.az") : 0; + + if (!CheckRange(fParkingPos)) + return 2; + + fAccPointing = Acceleration(conf.Get("pointing.acceleration.zd"), + conf.Get("pointing.acceleration.az")); + fAccTracking = Acceleration(conf.Get("tracking.acceleration.zd"), + conf.Get("tracking.acceleration.az")); + fAccMax = Acceleration(conf.Get("acceleration.max.zd"), + conf.Get("acceleration.max.az")); + + fWeatherTimeout = conf.Get("weather-timeout"); + + if (fAccPointing>fAccMax) + { + T::Error("Pointing acceleration exceeds maximum acceleration."); + return 3; + } + + if (fAccTracking>fAccMax) + { + T::Error("Tracking acceleration exceeds maximum acceleration."); + return 4; + } + + fDeviationLimit = conf.Get("deviation-limit"); + fDeviationCounter = conf.Get("deviation-count"); + fDeviationMax = conf.Get("deviation-max"); + + const string fname = conf.Get("pointing.model-file"); + + try + { + fPointingModel.Load(fname); + } + catch (const exception &e) + { + T::Error(e.what()); + return 5; + } + + const vector &vec = conf.Vec("source"); + + for (vector::const_iterator it=vec.begin(); it!=vec.end(); it++) + { + istringstream stream(*it); + + string name; + + int i=0; + + Source src; + + string buffer; + while (getline(stream, buffer, ',')) + { + istringstream is(buffer); + + switch (i++) + { + case 0: name = buffer; break; + case 1: src.ra = ReadAngle(is); break; + case 2: src.dec = ReadAngle(is); break; + case 3: is >> src.offset; break; + case 4: is >> src.angles[0]; break; + case 5: is >> src.angles[1]; break; + } + + if (is.fail()) + break; + } + + if (i==3 || i==6) + { + AddSource(name, src); + continue; + } + + T::Warn("Resource 'source' not correctly formatted: '"+*it+"'"); + } + + //fAutoResume = conf.Get("auto-resume"); + + if (conf.Has("source-database")) + { + fDatabase = conf.Get("source-database"); + ReadDatabase(); + } + + if (fSunRise.IsValid()) + { + ostringstream msg; + msg << "Next sun-rise will be at " << fSunRise; + T::Message(msg); + } + + // The possibility to connect should be last, so that + // everything else is already initialized. + SetEndpoint(conf.Get("addr")); + + return -1; + } +}; + +// ------------------------------------------------------------------------ + +#include "Main.h" + + +template +int RunShell(Configuration &conf) +{ + return Main::execute>(conf); +} + +void SetupConfiguration(Configuration &conf) +{ + po::options_description control("Drive control options"); + control.add_options() + ("quiet,q", po_bool(), "Disable debug messages") + ("no-dim,d", po_switch(), "Disable dim services") + ("addr,a", var("sps:5357"), "Network address of cosy") + ("verbosity,v", var(0), "Vervosity level (0=off; 1=major updates; 2=most updates; 3=frequent updates)") + ("pointing.model-file", var()->required(), "") + ("pointing.max.zd", var( 104.9), "") + ("pointing.max.az", var( 85.0), "") + ("pointing.min.zd", var(-104.9), "") + ("pointing.min.az", var(-295.0), "") + ("pointing.max.residual", var(1./32768), "Maximum residual for a pointing operation [revolutions]") + ("pointing.velocity", var(0.3), "") + ("pointing.acceleration.az", var(0.01), "") + ("pointing.acceleration.zd", var(0.03), "") + ("tracking.acceleration.az", var(0.01), "") + ("tracking.acceleration.zd", var(0.01), "") + ("parking-pos.zd", var(101), "") + ("parking-pos.az", var(0), "") + ("acceleration.max.az", var(0.03), "") + ("acceleration.max.zd", var(0.09), "") + ("weather-timeout", var(300), "") + ("deviation-limit", var(90), "Deviation limit in arcsec to get 'OnTrack'") + ("deviation-count", var(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'") + ("deviation-max", var(180), "Maximum deviation in arcsec allowed to keep status 'OnTrack'") + ("source-database", var(), "Database link as in\n\tuser:password@server[:port]/database.") + ("source", vars(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"") + ; + + conf.AddOptions(control); +} + +/* + Extract usage clause(s) [if any] for SYNOPSIS. + Translators: "Usage" and "or" here are patterns (regular expressions) which + are used to match the usage synopsis in program output. An example from cp + (GNU coreutils) which contains both strings: + Usage: cp [OPTION]... [-T] SOURCE DEST + or: cp [OPTION]... SOURCE... DIRECTORY + or: cp [OPTION]... -t DIRECTORY SOURCE... + */ +void PrintUsage() +{ + cout << + "The drivectrl is an interface to cosy.\n" + "\n" + "The default is that the program is started without user intercation. " + "All actions are supposed to arrive as DimCommands. Using the -c " + "option, a local shell can be initialized. With h or help a short " + "help message about the usuage can be brought to the screen.\n" + "\n" + "Usage: drivectrl [-c type] [OPTIONS]\n" + " or: drivectrl [OPTIONS]\n"; + cout << endl; +} + +void PrintHelp() +{ + Main::PrintHelp>(); + + /* Additional help text which is printed after the configuration + options goes here */ + + /* + cout << "bla bla bla" << endl << endl; + cout << endl; + cout << "Environment:" << endl; + cout << "environment" << endl; + cout << endl; + cout << "Examples:" << endl; + cout << "test exam" << endl; + cout << endl; + cout << "Files:" << endl; + cout << "files" << endl; + cout << endl; + */ +} + +int main(int argc, const char* argv[]) +{ + Configuration conf(argv[0]); + conf.SetPrintUsage(PrintUsage); + Main::SetupConfiguration(conf); + SetupConfiguration(conf); + + if (!conf.DoParse(argc, argv, PrintHelp)) + return 127; + + //try + { + // No console access at all + if (!conf.Has("console")) + { + if (conf.Get("no-dim")) + return RunShell(conf); + else + return RunShell(conf); + } + // Cosole access w/ and w/o Dim + if (conf.Get("no-dim")) + { + if (conf.Get("console")==0) + return RunShell(conf); + else + return RunShell(conf); + } + else + { + if (conf.Get("console")==0) + return RunShell(conf); + else + return RunShell(conf); + } + } + /*catch (std::exception& e) + { + cerr << "Exception: " << e.what() << endl; + return -1; + }*/ + + return 0; +}