Index: /trunk/FACT++/src/drivectrl.cc =================================================================== --- /trunk/FACT++/src/drivectrl.cc (revision 18362) +++ /trunk/FACT++/src/drivectrl.cc (revision 18363) @@ -194,22 +194,4 @@ 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) @@ -286,53 +268,5 @@ 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) { @@ -541,47 +475,7 @@ 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) -*/ - @@ -640,7 +534,4 @@ 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, @@ -1150,6 +1041,4 @@ 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; @@ -2090,52 +1979,4 @@ } -/* - 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) { @@ -2510,103 +2351,5 @@ 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()) { @@ -2768,44 +2511,5 @@ 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; @@ -2930,20 +2634,4 @@ (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", @@ -3052,9 +2740,5 @@ ("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))