Changeset 19436 for trunk/FACT++/src/drivectrl.cc

Timestamp:

02/05/19 16:01:50 (6 years ago)

Author:

tbretz

Message:

Added a new state to simplify detection of close-to-target 'Approaching'

File:

• trunk/FACT++/src/drivectrl.cc (modified) (8 diffs)

trunk/FACT++/src/drivectrl.cc

@@ -1625 +1625 @@
     uint16_t fDeviationMax;
 
+    float fApproachingLimit;
+
     vector<double> fDevBuffer;
     uint64_t       fDevCount;
@@ -2562 +2564 @@
         if (fDrive.HasWarning() || fDrive.HasError())
         {
-            if (T::GetCurrentState()==State::kOnTrack  ||
-                T::GetCurrentState()==State::kTracking ||
-                T::GetCurrentState()==State::kMoving   ||
+            if (T::GetCurrentState()==State::kOnTrack     ||
+                T::GetCurrentState()==State::kTracking    ||
+                T::GetCurrentState()==State::kMoving      ||
+                T::GetCurrentState()==State::kApproaching ||
                 T::GetCurrentState()==State::kParking)
                 return StopMovement();
@@ -2640 +2643 @@
             return State::kArmed;
 
-        if ((T::GetCurrentState()==State::kMoving ||
+        if ((T::GetCurrentState()==State::kMoving || T::GetCurrentState()==State::kApproaching ||
              T::GetCurrentState()==State::kParking) && !fDrive.IsMoving())
         {
@@ -2730 +2733 @@
         }
 
+        if (T::GetCurrentState()==State::kMoving && fDrive.IsMoving() && fIsTracking)
+        {
+            // 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);
+
+            if (absdev<fApproachingLimit)
+                return State::kApproaching;
+        }
+
         return T::GetCurrentState()>=State::kInitialized ?
             T::GetCurrentState() : State::kInitialized;
@@ -2772 +2793 @@
         T::AddStateName(State::kMoving, "Moving",
                         "Telescope moving");
+        T::AddStateName(State::kApproaching, "Approaching",
+                        "Telescope approaching destination");
         T::AddStateName(State::kTracking, "Tracking",
                         "Telescope in tracking mode");
@@ -2877 +2900 @@
 
         // FIXME: What to do in error state?
-        T::AddEvent("PARK", State::kInitialized, State::kMoving, State::kTracking, State::kOnTrack, State::kHardwareWarning)
+        T::AddEvent("PARK")(State::kInitialized)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)(State::kHardwareWarning)
             (bind(&StateMachineDrive::Park, this))
             ("Park the telescope");
 
-        T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kTracking)(State::kOnTrack)
+        T::AddEvent("STOP")(State::kUnavailable)(State::kAvailable)(State::kArmed)(State::kInitialized)(State::kStopping)(State::kParking)(State::kMoving)(State::kApproaching)(State::kTracking)(State::kOnTrack)
             (bind(&StateMachineDrive::StopMovement, this))
             ("Stop any kind of movement.");
@@ -3043 +3066 @@
             return 4;
         }
+
+        fApproachingLimit = conf.Get<float>("approaching-limit");
 
         fDeviationLimit   = conf.Get<uint16_t>("deviation-limit");
@@ -3159 +3184 @@
         ("acceleration.max.zd",      var<double>(0.09),         "Maximum allowed acceleration value for zenith axis")
         ("weather-timeout",          var<uint16_t>(300),        "Timeout [sec] for weather data (after timeout default values are used)")
+        ("approaching-limit",        var<float>(2.25),          "Limit to get 'Approaching' state")
         ("deviation-limit",          var<uint16_t>(90),         "Deviation limit in arcsec to get 'OnTrack'")
         ("deviation-count",          var<uint16_t>(3),          "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")