From 1ea5fb623daccebd99e8ea8965c03cb35a7bf918 Mon Sep 17 00:00:00 2001
From: Edward Emelianov <eddy@sao.ru>
Date: Fri, 14 Nov 2025 14:07:07 +0300
Subject: [PATCH] fixed model for STOPPED state

---
 LibSidServo/examples/dump.c |  7 +++-
 LibSidServo/serial.c        | 78 ++++++++++++++++++++++++-------------
 2 files changed, 56 insertions(+), 29 deletions(-)

diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c
index 6bb6620..70e78c4 100644
--- a/LibSidServo/examples/dump.c
+++ b/LibSidServo/examples/dump.c
@@ -137,18 +137,23 @@ void waitmoving(int N){
     mountdata_t mdata;
     int ctr = -1;
     uint32_t millis = 0;
-    double xlast = 0., ylast = 0.;
+    //double xlast = 0., ylast = 0.;
+    DBG("Wait moving for %d stopped times", N);
     while(ctr < N){
         usleep(10000);
         if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
         if(mdata.millis == millis) continue;
         millis = mdata.millis;
+        if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
+        else ++ctr;
+        /*
         if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
             xlast = mdata.motXposition.val;
             ylast = mdata.motYposition.val;
             //DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
             ctr = 0;
         }else ++ctr;
+        */
     }
 }
 
diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c
index 5b5c410..a9bfc77 100644
--- a/LibSidServo/serial.c
+++ b/LibSidServo/serial.c
@@ -390,6 +390,22 @@ void data_free(data_t **x){
     *x = NULL;
 }
 
+static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
+    if(isnan(*prev)){
+        *stat = AXIS_STOPPED;
+        DBG("START");
+    }else if(*stat != AXIS_STOPPED){
+        if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){
+            *stat = AXIS_STOPPED;
+            DBG("AXIS stopped");
+        }
+    }else if(*prev != cur){
+        DBG("AXIS moving");
+        *nstopped = 0;
+    }
+    *prev = cur;
+}
+
 // main mount thread
 static void *mountthread(void _U_ *u){
     int errctr = 0;
@@ -399,34 +415,40 @@ static void *mountthread(void _U_ *u){
     double t0 = nanotime(), tstart = t0;
     static double oldmt = -100.; // old `millis measurement` time
     static uint32_t oldmillis = 0;
-    if(Conf.RunModel) while(1){
-        coordval_pair_t c;
-        movestate_t xst, yst;
-        // now change data
-        getModData(&c, &xst, &yst);
-        pthread_mutex_lock(&datamutex);
-        double tnow = c.X.t;
-        mountdata.encXposition.t = mountdata.encYposition.t = tnow;
-        mountdata.encXposition.val = c.X.val;
-        mountdata.encYposition.val = c.Y.val;
-        //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
-        if(tnow - oldmt > Conf.MountReqInterval){
-            oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
-            mountdata.motYposition.t = mountdata.motXposition.t = tnow;
-            if(xst == ST_MOVE)
-                mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
-            else
-                mountdata.motXposition.val = c.X.val;
-            if(yst == ST_MOVE)
-                mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
-            else
-                mountdata.motYposition.val = c.Y.val;
-            oldmt = tnow;
-        }else mountdata.millis = oldmillis;
-        pthread_mutex_unlock(&datamutex);
-        getXspeed(); getYspeed();
-        while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
-        t0 = nanotime();
+    if(Conf.RunModel){
+        double Xprev = NAN, Yprev = NAN; // previous coordinates
+        int xcnt = 0, ycnt = 0;
+        while(1){
+            coordval_pair_t c;
+            movestate_t xst, yst;
+            // now change data
+            getModData(&c, &xst, &yst);
+            pthread_mutex_lock(&datamutex);
+            double tnow = c.X.t;
+            mountdata.encXposition.t = mountdata.encYposition.t = tnow;
+            mountdata.encXposition.val = c.X.val;
+            mountdata.encYposition.val = c.Y.val;
+            //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
+            if(tnow - oldmt > Conf.MountReqInterval){
+                oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
+                mountdata.motYposition.t = mountdata.motXposition.t = tnow;
+                if(xst == ST_MOVE)
+                    mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
+                else
+                    mountdata.motXposition.val = c.X.val;
+                if(yst == ST_MOVE)
+                    mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
+                else
+                    mountdata.motYposition.val = c.Y.val;
+                oldmt = tnow;
+            }else mountdata.millis = oldmillis;
+            chkModStopped(&Xprev, c.X.val, &xcnt, &mountdata.Xstate);
+            chkModStopped(&Yprev, c.Y.val, &ycnt, &mountdata.Ystate);
+            pthread_mutex_unlock(&datamutex);
+            getXspeed(); getYspeed();
+            while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
+            t0 = nanotime();
+        }
     }
     // data to get
     data_t d = {.buf = buf, .maxlen = sizeof(buf)};