template<class T , class S >

void StateMachineDrive< T, S >::TrackingLoop ( const boost::system::error_code &  error = boost::system::error_code() )

inlineprivate

Definition at line 2415 of file drivectrl.cc.

References Local::az, data, dev, Error(), Drive::State::kAllowedRangeExceeded, Drive::State::kOnTrack, Drive::State::kTracking, Time::Mjd(), PointingData::mount, PointingData::sky, str, and Local::zd.

Referenced by StateMachineDrive< T, S >::Execute().

    {
        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));
    }

Here is the call graph for this function:

Here is the caller graph for this function: