template<class T , class S >

int StateMachineDrive< T, S >::Execute ( )

inlineprivatevirtual

Implements StateMachineAsio< T >.

Definition at line 2545 of file drivectrl.cc.

References Encoder::Abs(), Local::az, data, RaDec::dec, Error(), Time::GetNextSunRise(), Drive::State::kArmed, MessageImp::kError, Drive::State::kInitialized, BIAS::State::kLocked, Drive::State::kMoving, Drive::State::kOnTrack, Drive::State::kParking, Drive::State::kPositioningFailed, Drive::State::kStopping, Drive::State::kTracking, Time::Mjd(), PointingData::pointing, RaDec::ra, PointingSetup::start, t, time, StateMachineDrive< T, S >::TrackingLoop(), and Local::zd.

    {
        const Time now;
        if (now>fSunRise && T::GetCurrentState()!=State::kParking)
        {
            fSunRise = now.GetNextSunRise();

            ostringstream msg;
            msg << "Next sun-rise will be at " << fSunRise;
            T::Info(msg);

            if (T::GetCurrentState()>State::kArmed && T::GetCurrentState()!=StateMachineImp::kError)
                return Park();
        }

        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;

        // 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;
    }

Here is the call graph for this function: