Changeset 19436


Ignore:
Timestamp:
Feb 5, 2019, 4:01:50 PM (13 days ago)
Author:
tbretz
Message:
Added a new state to simplify detection of close-to-target 'Approaching'
Location:
trunk/FACT++/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/FACT++/src/HeadersDrive.h

    r18804 r19436  
    1919            kParking,
    2020            kMoving,
     21            kApproaching,
    2122            kTracking,
    2223            kOnTrack,
  • trunk/FACT++/src/drivectrl.cc

    r19416 r19436  
    16251625    uint16_t fDeviationMax;
    16261626
     1627    float fApproachingLimit;
     1628
    16271629    vector<double> fDevBuffer;
    16281630    uint64_t       fDevCount;
     
    25622564        if (fDrive.HasWarning() || fDrive.HasError())
    25632565        {
    2564             if (T::GetCurrentState()==State::kOnTrack  ||
    2565                 T::GetCurrentState()==State::kTracking ||
    2566                 T::GetCurrentState()==State::kMoving   ||
     2566            if (T::GetCurrentState()==State::kOnTrack     ||
     2567                T::GetCurrentState()==State::kTracking    ||
     2568                T::GetCurrentState()==State::kMoving      ||
     2569                T::GetCurrentState()==State::kApproaching ||
    25672570                T::GetCurrentState()==State::kParking)
    25682571                return StopMovement();
     
    26402643            return State::kArmed;
    26412644
    2642         if ((T::GetCurrentState()==State::kMoving ||
     2645        if ((T::GetCurrentState()==State::kMoving || T::GetCurrentState()==State::kApproaching ||
    26432646             T::GetCurrentState()==State::kParking) && !fDrive.IsMoving())
    26442647        {
     
    27302733        }
    27312734
     2735        if (T::GetCurrentState()==State::kMoving && fDrive.IsMoving() && fIsTracking)
     2736        {
     2737            // First calculate deviation between
     2738            // command position and nominal position
     2739            //fPointing.mount = sepos; // [deg] ref pos for alignment
     2740            const PointingData data = CalcPointingPos(fDrive.GetSeTime());
     2741
     2742            // Get current position and calculate deviation
     2743            const Encoder sepos = fDrive.GetSePos()*360; // [deg]
     2744            const Encoder dev   = sepos - data.mount;
     2745
     2746            // Calculate absolut deviation on the sky
     2747            const double absdev = GetDevAbs(data.mount.zd, sepos.zd, dev.az);
     2748
     2749            if (absdev<fApproachingLimit)
     2750                return State::kApproaching;
     2751        }
     2752
    27322753        return T::GetCurrentState()>=State::kInitialized ?
    27332754            T::GetCurrentState() : State::kInitialized;
     
    27722793        T::AddStateName(State::kMoving, "Moving",
    27732794                        "Telescope moving");
     2795        T::AddStateName(State::kApproaching, "Approaching",
     2796                        "Telescope approaching destination");
    27742797        T::AddStateName(State::kTracking, "Tracking",
    27752798                        "Telescope in tracking mode");
     
    28772900
    28782901        // FIXME: What to do in error state?
    2879         T::AddEvent("PARK", State::kInitialized, State::kMoving, State::kTracking, State::kOnTrack, State::kHardwareWarning)
     2902        T::AddEvent("PARK")(State::kInitialized)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)(State::kHardwareWarning)
    28802903            (bind(&StateMachineDrive::Park, this))
    28812904            ("Park the telescope");
    28822905
    2883         T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kTracking)(State::kOnTrack)
     2906        T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)
    28842907            (bind(&StateMachineDrive::StopMovement, this))
    28852908            ("Stop any kind of movement.");
     
    30433066            return 4;
    30443067        }
     3068
     3069        fApproachingLimit = conf.Get<float>("approaching-limit");
    30453070
    30463071        fDeviationLimit   = conf.Get<uint16_t>("deviation-limit");
     
    31593184        ("acceleration.max.zd",      var<double>(0.09),         "Maximum allowed acceleration value for zenith axis")
    31603185        ("weather-timeout",          var<uint16_t>(300),        "Timeout [sec] for weather data (after timeout default values are used)")
     3186        ("approaching-limit",        var<float>(2.25),          "Limit to get 'Approaching' state")
    31613187        ("deviation-limit",          var<uint16_t>(90),         "Deviation limit in arcsec to get 'OnTrack'")
    31623188        ("deviation-count",          var<uint16_t>(3),          "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
Note: See TracChangeset for help on using the changeset viewer.