Index: trunk/FACT++/src/drivectrl.cc
===================================================================
--- trunk/FACT++/src/drivectrl.cc	(revision 16885)
+++ trunk/FACT++/src/drivectrl.cc	(revision 16887)
@@ -127,4 +127,7 @@
     uint16_t fDeviationMax;
 
+    vector<double> fDevBuffer;
+    uint64_t       fDevCount;
+
     uint64_t fTrackingCounter;
 
@@ -362,9 +365,15 @@
 
         // Calculate absolut deviation on the sky
+
         const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
+
+        fDevBuffer[fDevCount++%5] = dev;
+
+        const uint8_t cnt    = fDevCount<5 ? fDevCount : 5;
+        const double  avgdev = accumulate(fDevBuffer.begin(), fDevBuffer.begin()+cnt, 0)/cnt;
 
         // If any other state than tracking or a deviation
         // larger than 60, reset the counter
-        if (fState!=State::kTracking || dev>fDeviationLimit)
+        if (fState!=State::kTracking || avgdev>fDeviationLimit)
             fTrackingCounter = 0;
         else
@@ -377,6 +386,9 @@
 
         // Having th state as Tracking will reset the counter
-        if (fState==State::kOnTrack && dev>fDeviationMax)
+        if (fState==State::kOnTrack && avgdev>fDeviationMax)
             fState = State::kTracking;
+
+        if (fState!=State::kTracking && avgdev!=State::kOnTrack)
+            fDevCount = 0;
 
         // 206 206         ce ce       pwr vlt emcy fs        |  pwr vlt emcy fs
@@ -624,5 +636,8 @@
 public:
     ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
-        fState(-1), fIsVerbose(true), fDeviationLimit(120), fDeviationCounter(5), fDeviationMax(240), fTrackingCounter(0), fKeepAlive(ioservice)
+        fState(-1), fIsVerbose(true),
+        fDeviationLimit(120), fDeviationCounter(5), fDeviationMax(240),
+        fDevBuffer(5), fDevCount(0),
+        fTrackingCounter(0), fKeepAlive(ioservice)
     {
         SetLogStream(&imp);
@@ -640,4 +655,5 @@
         fDeviationMax     = max;
     }
+
     int GetState() const
     {
@@ -790,4 +806,5 @@
     string fLastCommand;  // Last tracking (RADEC) command
     int fAutoResume;      // 0: disabled, 1: enables, 2: resuming
+    Time fAutoResumeTime;
 
     // Status 0: Error
@@ -873,4 +890,6 @@
         }
 
+        
+        T::Info("Resume: "+fLastCommand);
         return SendCommand(fLastCommand, false);
     }
@@ -1220,5 +1239,8 @@
         }*/
 
-        if (Time()>fSunRise)
+        const Time now;
+
+
+        if (now>fSunRise)
         {
             if (T::GetCurrentState()>Drive::State::kLocked)
@@ -1227,5 +1249,5 @@
             if (T::GetCurrentState()==Drive::State::kLocked)
             {
-                fSunRise = Time().GetNextSunRise();
+                fSunRise = now.GetNextSunRise();
 
                 ostringstream msg;
@@ -1246,14 +1268,26 @@
             // If auto resume is enabled and the drive is in error,
             // resume tracking
-            if (fAutoResume==1 && state==StateMachineImp::kSM_Error)
+            if (state==StateMachineImp::kSM_Error)
             {
-                Resume();
-                fAutoResume = 2;
+                if (fAutoResume==1)
+                {
+                    Resume();
+                    fAutoResume = 2;
+                    fAutoResumeTime = now;
+                }
+
+                if (fAutoResume==2 && fAutoResumeTime+boost::posix_time::seconds(5)<now)
+                {
+                    Resume();
+                    fAutoResume = 3;
+                }
             }
-
-            // If drive got out of the error state,
-            // enable auto resume again
-            if (fAutoResume==2 && state!=StateMachineImp::kSM_Error)
-                fAutoResume = 1;
+            else
+            {
+                // If drive got out of the error state,
+                // enable auto resume again
+                if (fAutoResume>1)
+                    fAutoResume = 1;
+            }
         }