seems like PID works on real telescope

Auxiliary_utils/LibSidServo/CMakeLists.txt

@@ -15,6 +15,8 @@ set(CMAKE_COLOR_MAKEFILE ON)
 
 option(DEBUG "Compile in debug mode" OFF)
 option(EXAMPLES "Compile also some examples" ON)
+option(BUILD_SHARED "Build shared libarary" OFF)
+
 
 # cmake -DDEBUG=on -> debugging
 if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
@@ -52,7 +54,13 @@ aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES)
 #list(APPEND ${PROJ}_LIBRARIES "-lfftw3_threads")
 
 # library
+
+if(BUILD_SHARED)
     add_library(${PROJ} SHARED ${SOURCES})
+else()
+    add_library(${PROJ} STATIC ${SOURCES})
+endif()
+
 # library header files
 set(LIBHEADER "sidservo.h")
 # -I

Auxiliary_utils/LibSidServo/PID.c

@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <float.h>
 #include <math.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -25,34 +26,70 @@
 #include "PID.h"
 #include "serial.h"
 
-PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
+typedef struct {
+    PIDpar_t gain;      // PID gains
+    double prev_error;  // Previous error
+    double prev_tagpos; // previous target position
+    double integral;    // Integral term
+    double *pidIarray;  // array for Integral
+    struct timespec prevT; // time of previous correction
+    size_t pidIarrSize; // it's size
+    size_t curIidx;     // and index of current element
+} PIDController_t;
+
+typedef struct{
+    axis_status_t state;
+    coordval_t position;
+    coordval_t speed;
+} axisdata_t;
+
+static PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
     if(!gain || Iarrsz < 3) return NULL;
     PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
     pid->gain = *gain;
     DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
     pid->pidIarrSize = Iarrsz;
     pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
+    curtime(&pid->prevT);
     return pid;
 }
 
 // don't clear lastT!
-void pid_clear(PIDController_t *pid){
+static void pid_clear(PIDController_t *pid){
     if(!pid) return;
     DBG("CLEAR PID PARAMETERS");
     bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
     pid->integral = 0.;
     pid->prev_error = 0.;
     pid->curIidx = 0;
+    curtime(&pid->prevT);
 }
-
+/*
-void pid_delete(PIDController_t **pid){
+static void pid_delete(PIDController_t **pid){
     if(!pid || !*pid) return;
     if((*pid)->pidIarray) free((*pid)->pidIarray);
     free(*pid);
     *pid = NULL;
-}
+}*/
 
-double pid_calculate(PIDController_t *pid, double error, double dt){
+// calculate new motor speed
+static double pid_calculate(PIDController_t *pid, double axispos, const coordval_t *target){
+    double dtpid = timediff(&target->t, &pid->prevT);
+    if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
+        DBG("time diff too big: clear PID");
+        pid_clear(pid);
+        pid->prev_tagpos = target->val;
+        return 0.;
+    }
+    double dt = timediff(&target->t, &pid->prevT);
+    if(dt < FLT_EPSILON){
+        DBG("Target time in past");
+        return 0.;
+    }
+    pid->prevT = target->t;
+    double error = target->val - axispos;
+    double tagspeed = (target->val - pid->prev_tagpos) / dt;
+    pid->prev_tagpos = target->val;
     // calculate flowing integral
     double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
     //DBG("oldi/new: %g, %g", oldi, newi);
@@ -61,43 +98,33 @@ double pid_calculate(PIDController_t *pid, double error, double dt){
     pid->integral += newi - oldi;
     double derivative = (error - pid->prev_error) / dt;
     pid->prev_error = error;
-    double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
+    DBG("pid pars: P=%g, I=%g, D=%f", pid->gain.P, pid->gain.I, pid->gain.D);
-    DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
+    double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative + tagspeed;
+    DBG("tagspeed=%g, P=%g, I=%g, D=%g; sum=%g", tagspeed, pid->gain.P * error,
+        pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
     return sum;
 }
 
-typedef struct{
-    PIDController_t *PIDC;
-    PIDController_t *PIDV;
-} PIDpair_t;
-
-typedef struct{
-    axis_status_t state;
-    coordval_t position;
-    coordval_t speed;
-} axisdata_t;
 /**
  * @brief process - Process PID for given axis
  * @param tagpos - given coordinate of target position
  * @param endpoint - endpoint for this coordinate
  * @param pid - pid itself
- * @return calculated new speed or -1 for max speed
+ * @return calculated NEW SPEED or NAN for max speed
  */
-static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
+static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_t *axis){
     double dt = timediff(&tagpos->t, &axis->position.t);
     if(dt < 0 || dt > Conf.PIDMaxDt){
         DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt);
         return axis->speed.val; // data is too old or wrong
     }
     double error = tagpos->val - axis->position.val, fe = fabs(error);
-    DBG("error: %g", error);
+    DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI);
-    PIDController_t *pid = NULL;
     switch(axis->state){
         case AXIS_SLEWING:
-            if(fe < Conf.MaxPointingErr){
+            if(fe < Conf.MaxFinePointingErr){
                 axis->state = AXIS_POINTING;
                 DBG("--> Pointing");
-                pid = pidpair->PIDC;
             }else{
                 DBG("Slewing...");
                 return NAN; // max speed for given axis
@@ -107,28 +134,26 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
             if(fe < Conf.MaxFinePointingErr){
                 axis->state = AXIS_GUIDING;
                 DBG("--> Guiding");
-                pid = pidpair->PIDV;
             }else if(fe > Conf.MaxPointingErr){
                 DBG("--> Slewing");
                 axis->state = AXIS_SLEWING;
                 return NAN;
-            } else pid = pidpair->PIDC;
+            }
             break;
         case AXIS_GUIDING:
-            pid = pidpair->PIDV;
             if(fe > Conf.MaxFinePointingErr){
                 DBG("--> Pointing");
                 axis->state = AXIS_POINTING;
-                pid = pidpair->PIDC;
             }else if(fe < Conf.MaxGuidingErr){
                 DBG("At target");
                 // TODO: we can point somehow that we are at target or introduce new axis state
-            }else DBG("Current error: %g", fe);
+            }else DBG("Current abs error: %g", fe);
             break;
+        case AXIS_GONNASTOP:
         case AXIS_STOPPED: // start pointing to target; will change speed next time
             DBG("AXIS STOPPED!!!! --> Slewing");
             axis->state = AXIS_SLEWING;
-            return getspeed(tagpos, pidpair, axis);
+            return getspeed(tagpos, pid, axis);
         case AXIS_ERROR:
             DBG("Can't move from erroneous state");
             return 0.;
@@ -137,17 +162,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
         DBG("WTF? Where is a PID?");
         return axis->speed.val;
     }
-    double dtpid = timediff(&tagpos->t, &pid->prevT);
+    return pid_calculate(pid, axis->position.val, tagpos);
-    if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
-        DBG("time diff too big: clear PID");
-        pid_clear(pid);
-    }
-    if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
-    pid->prevT = tagpos->t;
-    DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
-    double tagspeed = pid_calculate(pid, error, dtpid);
-    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
-    return tagspeed; // coordinate-based
 }
 
 /**
@@ -157,18 +172,14 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
  * @return error code
  */
 mcc_errcodes_t correct2(const coordval_pair_t *target){
-    static PIDpair_t pidX = {0}, pidY = {0};
+    static PIDController_t *pidX = NULL, *pidY = NULL;
-    if(!pidX.PIDC){
+    if(!pidX){
-        pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+        pidX = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
-        if(!pidX.PIDC) return MCC_E_FATAL;
+        if(!pidX) return MCC_E_FATAL;
-        pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
-        if(!pidX.PIDV) return MCC_E_FATAL;
     }
-    if(!pidY.PIDC){
+    if(!pidY){
-        pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+        pidY = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
-        if(!pidY.PIDC) return MCC_E_FATAL;
+        if(!pidY) return MCC_E_FATAL;
-        pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
-        if(!pidY.PIDV) return MCC_E_FATAL;
     }
     mountdata_t m;
     coordpair_t tagspeed; // absolute value of speed
@@ -179,7 +190,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
     axis.state = m.Xstate;
     axis.position = m.encXposition;
     axis.speed = m.encXspeed;
-    tagspeed.X = getspeed(&target->X, &pidX, &axis);
+    tagspeed.X = getspeed(&target->X, pidX, &axis);
     if(isnan(tagspeed.X)){ // max speed
         if(target->X.val < axis.position.val) Xsign = -1.;
         tagspeed.X = Xlimits.max.speed;
@@ -191,7 +202,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
     axis.state = m.Ystate;
     axis.position = m.encYposition;
     axis.speed = m.encYspeed;
-    tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
+    tagspeed.Y = getspeed(&target->Y, pidY, &axis);
     if(isnan(tagspeed.Y)){ // max speed
         if(target->Y.val < axis.position.val) Ysign = -1.;
         tagspeed.Y = Ylimits.max.speed;
@@ -205,6 +216,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
         setStat(xstate, ystate);
     }
     coordpair_t endpoint;
+#if 0
     // allow at least PIDMaxDt moving with target speed
     double dv = fabs(tagspeed.X - m.encXspeed.val);
     double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
@@ -216,6 +228,16 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
             + Conf.PIDMaxDt * tagspeed.Y
             + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
     endpoint.Y = m.encYposition.val + Ysign * adder;
+#endif
+    // allow 10s moving but not more than 10deg and not less than 1deg
+    double adder = fabs(tagspeed.X) * 10.;
+    if(adder > 0.17453) adder = 0.17453;
+    else if(adder < 0.017453) adder = 0.017453;
+    endpoint.X = m.encXposition.val + Xsign * adder;
+    adder = fabs(tagspeed.Y) * 10.;
+    if(adder > 0.17453) adder = 0.17453;
+    else if(adder < 0.017453) adder = 0.017453;
+    endpoint.Y = m.encYposition.val + Ysign * adder;
     DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
     return Mount.moveWspeed(&endpoint, &tagspeed);
 }

Auxiliary_utils/LibSidServo/PID.h

@@ -22,19 +22,10 @@
 
 #include "sidservo.h"
 
-typedef struct {
+/*
-    PIDpar_t gain;      // PID gains
-    double prev_error;  // Previous error
-    double integral;    // Integral term
-    double *pidIarray;  // array for Integral
-    struct timespec prevT; // time of previous correction
-    size_t pidIarrSize; // it's size
-    size_t curIidx;     // and index of current element
-} PIDController_t;
-
 PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
 void pid_clear(PIDController_t *pid);
 void pid_delete(PIDController_t **pid);
 double pid_calculate(PIDController_t *pid, double error, double dt);
-
+*/
 mcc_errcodes_t  correct2(const coordval_pair_t *target);

Auxiliary_utils/LibSidServo/examples/scmd_traectory.c

@@ -109,17 +109,27 @@ static void runtraectory(traectory_fn tfn){
         }
         if(!Mount.currentT(&tcur)) continue;
         if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
-        DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
         tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
         double t = Mount.timeFromStart();
-        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
+        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax){
-        if(!traectory_point(&traectXY, t)) break;
+            if(fabs(telXY.X.val) > G.Xmax) DBG("X over maximal limit!");
+            if(fabs(telXY.Y.val) > G.Ymax) DBG("Y over maximal limit!");
+            if(t - tstart > G.tmax) DBG("Time over...");
+            break;
+        }
+        if(!traectory_point(&traectXY, t)){
+            DBG("Error in 'traectory_point', time from start=%g", t);
+            break;
+        }
+        DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f); traectory: %g'/%g' (%.6f/%.6f)",
+            RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val),
+            RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y), RAD2DEG(traectXY.X), RAD2DEG(traectXY.Y));
         target.X.val = traectXY.X; target.Y.val = traectXY.Y;
         target.X.t = target.Y.t = tcur;
         if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
         else{
             //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
-            DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+            DBG("%g, target-telescope: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
             //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
             if(errlog)
                 fprintf(errlog, "%10.4f  %10.4f  %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
@@ -146,10 +156,12 @@ int main(int argc, char **argv){
             ERRX("Can't open error log %s", G.errlog);
         else
             fprintf(errlog, "#    time      Xerr''      Yerr''   // target - real\n");
+        setbuf(errlog, NULL);
     }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
+        setbuf(fcoords, NULL);
     }else fcoords = stdout;
     Config = readServoConf(G.conffile);
     if(!Config || G.dumpconf){
@@ -164,21 +176,22 @@ int main(int argc, char **argv){
         return 1;
     }
     coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
-    if(!init_traectory(tfn, &c)){
-        ERRX("Can't init traectory");
-        return 1;
-    }
     mcc_errcodes_t e = Mount.init(Config);
     if(e != MCC_E_OK){
         WARNX("Can't init devices");
         return 1;
     }
+    // run this function only after mount inited!
+    if(!init_traectory(tfn, &c)){
+        ERRX("Can't init traectory");
+        return 1;
+    }
     signal(SIGTERM, signals); // kill (-15) - quit
     signal(SIGHUP, SIG_IGN);  // hup - ignore
     signal(SIGINT, signals);  // ctrl+C - quit
     signal(SIGQUIT, signals); // ctrl+\ - quit
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-    chk0(G.Ncycles);
+//    chk0(G.Ncycles);
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
     ;

Auxiliary_utils/LibSidServo/examples/traectories.c

@@ -42,6 +42,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
     cur_traectory = f;
     XYstart = *XY0;
     tstart = Mount.timeFromStart();
+    DBG("inited @ %gs from start", tstart);
     mountdata_t mdata;
     int ntries = 0;
     for(; ntries < 10; ++ntries){
@@ -58,11 +59,21 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
  * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
  */
 int traectory_point(coordpair_t *nextpt, double t){
-    if(t < 0. || !cur_traectory) return FALSE;
+    if(t < 0. || !cur_traectory){
+        if(t < 0.) DBG("time in past!");
+        else DBG("no current traectory selected!");
+        return FALSE;
+    }
     coordpair_t pt;
-    if(!cur_traectory(&pt, t)) return FALSE;
+    if(!cur_traectory(&pt, t)){
+        DBG("error in cur_traectory");
+        return FALSE;
+    }
     if(nextpt) *nextpt = pt;
-    if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
+    if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI){
+        DBG("some points is over limits, pt.x=%g, pt.y=%g degrees", RAD2DEG(pt.X), RAD2DEG(pt.Y));
+        return FALSE;
+    }
     return TRUE;
 }
 
@@ -88,6 +99,7 @@ int telpos(coordval_pair_t *curpos){
 // X=X0+1'/s, Y=Y0+15''/s
 int Linear(coordpair_t *nextpt, double t){
     coordpair_t pt;
+    DBG("t=%g, tfromstart=%g, dt=%g", t, tstart, t-tstart);
     pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
     pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
     if(nextpt) *nextpt = pt;
@@ -98,7 +110,7 @@ int Linear(coordpair_t *nextpt, double t){
 int SinCos(coordpair_t *nextpt, double t){
     coordpair_t pt;
     pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
-    pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
+    pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/10.*2*M_PI);
     if(nextpt) *nextpt = pt;
     return TRUE;
 }
@@ -111,7 +123,7 @@ typedef struct{
 
 static tr_names names[] = {
     {Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
-    {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
+    {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+1'*cos(t/10*2pi)"},
     {NULL, NULL, NULL}
 };
 

Auxiliary_utils/LibSidServo/kalman.c

@@ -0,0 +1,169 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+
+#include "kalman.h"
+
+void kalman3_init(Kalman3 *kf, double dt, double enc_var){
+    kf->dt = dt;
+
+    kf->x[0] = 0;
+    kf->x[1] = 0;
+    kf->x[2] = 0;
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++)
+            kf->P[i][j] = 0;
+
+    kf->P[0][0] = 1;
+    kf->P[1][1] = 1;
+    kf->P[2][2] = 1;
+
+    // process noise
+    kf->Q[0][0] = 1e-6;
+    kf->Q[1][1] = 1e-4;
+    kf->Q[2][2] = 1e-3;
+
+    kf->R = enc_var; // encoder noise variance
+}
+
+void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j){
+    double dt = kf->dt;
+
+    double dt2 = dt*dt;
+    double dt3 = dt2*dt;
+    double dt4 = dt3*dt;
+    double dt5 = dt4*dt;
+
+    double q = sigma_j * sigma_j;
+
+    kf->Q[0][0] = q * dt5 / 20.0;
+    kf->Q[0][1] = q * dt4 / 8.0;
+    kf->Q[0][2] = q * dt3 / 6.0;
+
+    kf->Q[1][0] = q * dt4 / 8.0;
+    kf->Q[1][1] = q * dt3 / 3.0;
+    kf->Q[1][2] = q * dt2 / 2.0;
+
+    kf->Q[2][0] = q * dt3 / 6.0;
+    kf->Q[2][1] = q * dt2 / 2.0;
+    kf->Q[2][2] = q * dt;
+}
+
+void kalman3_predict(Kalman3 *kf){
+    double dt = kf->dt;
+    double dt2 = 0.5 * dt * dt;
+
+    double theta = kf->x[0];
+    double omega = kf->x[1];
+    double alpha = kf->x[2];
+
+    // state prediction
+    kf->x[0] = theta + omega*dt + alpha*dt2;
+    kf->x[1] = omega + alpha*dt;
+    kf->x[2] = alpha;
+
+    // transition matrix
+    double F[3][3] =
+        {
+            {1, dt, dt2},
+            {0, 1,  dt},
+            {0, 0,  1}
+        };
+
+    // P = FPF^T + Q
+
+    double FP[3][3];
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++){
+            FP[i][j] =
+                F[i][0]*kf->P[0][j] +
+                F[i][1]*kf->P[1][j] +
+                F[i][2]*kf->P[2][j];
+        }
+
+    double Pnew[3][3];
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++){
+            Pnew[i][j] =
+                FP[i][0]*F[j][0] +
+                FP[i][1]*F[j][1] +
+                FP[i][2]*F[j][2] +
+                kf->Q[i][j];
+        }
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++)
+            kf->P[i][j] = Pnew[i][j];
+}
+
+void kalman3_update(Kalman3 *kf, double z){
+    // innovation
+    double y = z - kf->x[0];
+
+    // S = HPH^T + R
+    double S = kf->P[0][0] + kf->R;
+
+    // Kalman gain
+    double K[3];
+
+    K[0] = kf->P[0][0] / S;
+    K[1] = kf->P[1][0] / S;
+    K[2] = kf->P[2][0] / S;
+
+    // state update
+    kf->x[0] += K[0] * y;
+    kf->x[1] += K[1] * y;
+    kf->x[2] += K[2] * y;
+
+    // covariance update
+    double P00 = kf->P[0][0];
+    double P01 = kf->P[0][1];
+    double P02 = kf->P[0][2];
+
+    double P10 = kf->P[1][0];
+    double P11 = kf->P[1][1];
+    double P12 = kf->P[1][2];
+
+    double P20 = kf->P[2][0];
+    double P21 = kf->P[2][1];
+    double P22 = kf->P[2][2];
+
+    kf->P[0][0] = P00 - K[0]*P00;
+    kf->P[0][1] = P01 - K[0]*P01;
+    kf->P[0][2] = P02 - K[0]*P02;
+
+    kf->P[1][0] = P10 - K[1]*P00;
+    kf->P[1][1] = P11 - K[1]*P01;
+    kf->P[1][2] = P12 - K[1]*P02;
+
+    kf->P[2][0] = P20 - K[2]*P00;
+    kf->P[2][1] = P21 - K[2]*P01;
+    kf->P[2][2] = P22 - K[2]*P02;
+}
+
+
+// estimation of the R
+double encoder_noise(int counts){
+    double d = 2.0*M_PI / counts;
+    return d*d / 12.0;
+}
+

Auxiliary_utils/LibSidServo/kalman.h

@@ -0,0 +1,34 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct{
+    double x[3];        // [theta, omega, alpha]
+    double P[3][3];     // covariance
+    double Q[3][3];     // process noise
+    double R;           // measurement noise
+    double dt;
+} Kalman3;
+
+
+double encoder_noise(int counts);
+void kalman3_update(Kalman3 *kf, double z);
+void kalman3_predict(Kalman3 *kf);
+void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j);
+void kalman3_init(Kalman3 *kf, double dt, double enc_var);

Auxiliary_utils/LibSidServo/libsidservo.creator.user

@@ -1,10 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 18.0.1, 2026-01-26T22:24:32. -->
+<!-- Written by QtCreator 18.0.0, 2026-03-23T09:21:54. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
-  <value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
+  <value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
  </data>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -40,9 +40,9 @@
    <value type="int" key="EditorConfiguration.PaddingMode">1</value>
    <value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
    <value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
-   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
    <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
-   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
    <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
    <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
    <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
@@ -51,10 +51,10 @@
    <value type="bool" key="EditorConfiguration.UseIndenter">false</value>
    <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
    <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
-   <value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
+   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
    <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
    <value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
-   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+   <value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
    <value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
    <value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
   </valuemap>
@@ -79,7 +79,7 @@
     <value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
     <value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
     <value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
-    <value type="int" key="ClangTools.ParallelJobs">8</value>
+    <value type="int" key="ClangTools.ParallelJobs">4</value>
     <value type="bool" key="ClangTools.PreferConfigFile">true</value>
     <valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
     <valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
@@ -96,12 +96,12 @@
    <value type="bool" key="HasPerBcDcs">true</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
-   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
-    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/tmp/robo5/mountcontrol.git/LibSidServo</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
       <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
@@ -133,7 +133,7 @@
     <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
     <value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
     <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
-    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
     <value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
     <value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
@@ -168,7 +168,6 @@
      <value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
      <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
      <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
-     <value type="QString" key="RunConfiguration.WorkingDirectory.default">%{RunConfig:Executable:Path}</value>
     </valuemap>
     <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
    </valuemap>
@@ -204,7 +203,6 @@
     <value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
-    <value type="QString" key="RunConfiguration.WorkingDirectory.default">%{RunConfig:Executable:Path}</value>
    </valuemap>
    <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
   </valuemap>

Auxiliary_utils/LibSidServo/libsidservo.files

@@ -13,12 +13,14 @@ examples/dumpswing.c
 examples/goto.c
 examples/scmd_traectory.c
 examples/simpleconv.h
+kalman.c
 main.c
 sidservo.h
 serial.c
 examples/CMakeLists.txt
 examples/traectories.c
 examples/traectories.h
+kalman.h
 main.h
 movingmodel.c
 movingmodel.h

Auxiliary_utils/LibSidServo/main.c

@@ -161,6 +161,8 @@ double LS_calc_slope(less_square_t *l, double x, double t){
     if(!l) return 0.;
     size_t idx = l->idx;
     double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
+    /*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
+        l->xsum, l->tsum, l->t2sum, l->xtsum);*/
     double t2 = t * t, xt = x * t;
     l->x[idx] = x; l->t2[idx] = t2;
     l->t[idx] = t; l->xt[idx] = xt;
@@ -172,9 +174,9 @@ double LS_calc_slope(less_square_t *l, double x, double t){
     l->xtsum += xt - oldxt;
     double n = (double)l->arraysz;
     double denominator = n * l->t2sum - l->tsum * l->tsum;
-    //DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator);
     if(fabs(denominator) < 1e-7) return 0.;
     double numerator = n * l->xtsum - l->xsum * l->tsum;
+    //DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
     // point: (sum_x  - slope * sum_t) / n;
     return (numerator / denominator);
 }
@@ -200,6 +202,7 @@ static mcc_errcodes_t init(conf_t *c){
         if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
         return MCC_E_OK;
     }
+    DBG("Try to open mount device");
     if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
@@ -207,33 +210,53 @@ static mcc_errcodes_t init(conf_t *c){
         DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
         ret = MCC_E_MOUNTDEV;
     }
-    if(Conf.SepEncoder){
-        if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
-            DBG("Define encoder device path");
-            ret = MCC_E_BADFORMAT;
-        }else if(!openEncoder()){
-            DBG("Can't open encoder device");
-            ret = MCC_E_ENCODERDEV;
-        }
-    }
     // TODO: read hardware configuration on init
     if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
         DBG("Wrong speed interval");
         ret = MCC_E_BADFORMAT;
     }
-    if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
     if(ret != MCC_E_OK) return ret;
+    DBG("Exit ACM, exit manual mode");
+    SSrawcmd(CMD_EXITACM, NULL);
+    SStextcmd(CMD_AUTOX, NULL);
+    SStextcmd(CMD_AUTOY, NULL);
     // read HW config to update constants
     hardware_configuration_t HW;
-    if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
+    DBG("Read hardware configuration");
+    ret = MCC_E_FAILED;
+    for(int i = 0; i < MAX_ERR_CTR; ++i){
+        DBG("TRY %d..", i);
+        ret = get_hwconf(&HW);
+        if(ret == MCC_E_OK) break;
+    }
+    if(MCC_E_OK != ret) return ret;
     // make a pause for actual encoder's values
+    DBG("Check encoders");
+    if(Conf.SepEncoder){
+        if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
+            DBG("Define encoder device path");
+            ret = MCC_E_BADFORMAT;
+        }else{
+            ret = MCC_E_ENCODERDEV;
+            for(int i = 0; i < MAX_ERR_CTR; ++i){
+                if(openEncoder()){
+                    ret = MCC_E_OK;
+                    break;
+                }
+            }
+        }
+    }
+    if(MCC_E_OK != ret) return ret;
     double t0 = timefromstart();
-    while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
+    DBG("Wait for first encoders' measurement");
+    while(timefromstart() - t0 < Conf.EncoderReqInterval * 15.) usleep(1000);
+    DBG("Update motor position");
     mcc_errcodes_t e = updateMotorPos();
     // and refresh data after updating
     DBG("Wait for next mount reading");
     t0 = timefromstart();
-    while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
+    while(timefromstart() - t0 < Conf.MountReqInterval * 5.) usleep(1000);
+    DBG("ALL READY!");
     return e;
 }
 
@@ -288,10 +311,6 @@ static mcc_errcodes_t move2(const coordpair_t *target){
     cmd.Ymot = target->Y;
     cmd.Xspeed = Xlimits.max.speed;
     cmd.Yspeed = Ylimits.max.speed;
-    /*mcc_errcodes_t r = shortcmd(&cmd);
-    if(r != MCC_E_OK) return r;
-    setslewingstate();
-    return MCC_E_OK;*/
     return shortcmd(&cmd);
 }
 
@@ -328,10 +347,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
     cmd.Ymot = target->Y;
     cmd.Xspeed = speed->X;
     cmd.Yspeed = speed->Y;
-    mcc_errcodes_t r = shortcmd(&cmd);
+    return shortcmd(&cmd);
-    if(r != MCC_E_OK) return r;
-    setslewingstate();
-    return MCC_E_OK;
 }
 
 /**

Auxiliary_utils/LibSidServo/serial.c

@@ -31,6 +31,7 @@
 #include <sys/stat.h>
 #include <unistd.h>
 
+#include "kalman.h"
 #include "main.h"
 #include "movingmodel.h"
 #include "serial.h"
@@ -49,7 +50,13 @@ static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
 // encoders thread and mount thread
 static pthread_t encthread, mntthread;
 // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
-static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
+// this values will be modified later
+static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder reading timeout
+    mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading
+    mntRtmout =  {.tv_sec = 0, .tv_usec = 50000}; // next readings
+
+static volatile int GlobExit = 0;
+
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -71,7 +78,6 @@ void getXspeed(){
         mountdata.encXspeed.val = speed;
         mountdata.encXspeed.t = mountdata.encXposition.t;
     }
-    //DBG("Xspeed=%g", mountdata.encXspeed.val);
 }
 void getYspeed(){
     static less_square_t *ls = NULL;
@@ -138,77 +144,6 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
     //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
 }
 
-#if 0
-/**
- * @brief getencval - get uint64_t data from encoder
- * @param fd - encoder fd
- * @param val - value read
- * @param t - measurement time
- * @return amount of data read or 0 if problem
- */
-static int getencval(int fd, double *val, struct timespec *t){
-    if(fd < 0){
-        DBG("Encoder fd < 0!");
-        return FALSE;
-    }
-    char buf[128];
-    int got = 0, Lmax = 127;
-    double t0 = timefromstart();
-    //DBG("start: %.6g", t0);
-    do{
-        fd_set rfds;
-        FD_ZERO(&rfds);
-        FD_SET(fd, &rfds);
-        struct timeval tv = encRtmout;
-        int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
-        if(!retval){
-            //DBG("select()==0 - timeout, %.6g", timefromstart());
-            break;
-        }
-        if(retval < 0){
-            if(errno == EINTR){
-                DBG("EINTR");
-                continue;
-            }
-            DBG("select() < 0");
-            return 0;
-        }
-        if(FD_ISSET(fd, &rfds)){
-            ssize_t l = read(fd, &buf[got], Lmax);
-            if(l < 1){
-                DBG("read() < 0");
-                return 0; // disconnected ??
-            }
-            got += l; Lmax -= l;
-            buf[got] = 0;
-        } else continue;
-        if(buf[got-1] == '\n') break; // got EOL as last symbol
-    }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
-    if(got == 0){
-        //DBG("No data from encoder, tfs=%.6g", timefromstart());
-        return 0;
-    }
-    char *estr = strrchr(buf, '\n');
-    if(!estr){
-        DBG("No EOL");
-        return 0;
-    }
-    *estr = 0;
-    char *bgn = strrchr(buf, '\n');
-    if(bgn) ++bgn;
-    else bgn = buf;
-    char *eptr;
-    long data = strtol(bgn, &eptr, 10);
-    if(eptr != estr){
-        DBG("NAN");
-        return 0; // wrong number
-    }
-    if(val) *val = (double) data;
-    if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
-    return got;
-}
-#endif
-
 // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
 static int getencbyte(){
     if(encfd[0] < 0) return -1;
@@ -232,43 +167,60 @@ static int getencbyte(){
     }while(1);
     return (int)byte;
 }
-// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
+
-static int getmntbyte(){
+/**
-    if(mntfd < 0) return -1;
+ * @brief readmntdata - read data
-    uint8_t byte;
+ * @param buffer - input buffer
+ * @param maxlen - maximal buffer length
+ * @return amount of bytes read or -1 in case of error
+ */
+static int readmntdata(uint8_t *buffer, int maxlen){
+    if(mntfd < 0){
+        DBG("mntfd non opened");
+        return -1;
+    }
+    if(!buffer || maxlen < 1) return 0;
+    //DBG("ask for %d bytes", maxlen);
+    int got = 0;
     fd_set rfds;
-   /* ssize_t l = read(mntfd, &byte, 1);
+    struct timeval tv = mnt1Rtmout;
-    //DBG("MNT read=%zd byte=0x%X", l, byte);
-    if(l == 0) return -1;
-    if(l != 1) return -2; // disconnected ??
-    return (int) byte;*/
     do{
         FD_ZERO(&rfds);
         FD_SET(mntfd, &rfds);
-        struct timeval tv = mntRtmout;
+        //DBG("select");
         int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
+        //DBG("returned %d", retval);
         if(retval < 0){
             if(errno == EINTR) continue;
             DBG("Error in select()");
             return -1;
         }
-        //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
         if(FD_ISSET(mntfd, &rfds)){
-            ssize_t l = read(mntfd, &byte, 1);
+            ssize_t l = read(mntfd, buffer, maxlen);
-            //DBG("MNT read=%zd byte=0x%X", l, byte);
+            if(l == 0){
-            if(l != 1){
+                DBG("read ZERO");
+                continue;
+            }
+            if(l < 0){
                 DBG("Mount disconnected?");
                 return -2; // disconnected ??
             }
+            buffer += l;
+            maxlen -= l;
+            got += l;
+        }else{
+            DBG("no new data after %d bytes (%s)", got, buffer - got);
             break;
-        } else return -1;
-    }while(1);
-    return (int)byte;
         }
+        tv = mntRtmout;
+    }while(maxlen);
+    return got;
+}
+
 // clear data from input buffer
 static void clrmntbuf(){
     if(mntfd < 0) return;
-    uint8_t byte;
+    uint8_t bytes[256];
     fd_set rfds;
     do{
         FD_ZERO(&rfds);
@@ -281,8 +233,9 @@ static void clrmntbuf(){
             break;
         }
         if(FD_ISSET(mntfd, &rfds)){
-            ssize_t l = read(mntfd, &byte, 1);
+            ssize_t l = read(mntfd, &bytes, 256);
-            if(l != 1) break;
+            if(l < 1) break;
+            DBG("clr got %zd bytes: %s", l, bytes);
         }else break;
     }while(1);
 }
@@ -293,7 +246,7 @@ static void *encoderthread1(void _U_ *u){
     uint8_t databuf[ENC_DATALEN];
     int wridx = 0, errctr = 0;
     struct timespec tcur;
-    while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
+    while(encfd[0] > -1 && errctr < MAX_ERR_CTR && !GlobExit){
         int b = getencbyte();
         if(b == -2) ++errctr;
         if(b < 0) continue;
@@ -328,9 +281,12 @@ typedef struct{
 
 // write to buffer next data portion; return FALSE in case of error
 static int readstrings(buf_t *buf, int fd){
-    FNAME();
     if(!buf){DBG("Empty buffer"); return FALSE;}
     int L = XYBUFSZ - buf->len;
+    if(L < 0){
+        DBG("buf not initialized!");
+        buf->len = 0;
+    }
     if(L == 0){
         DBG("buffer  overfull: %d!", buf->len);
         char *lastn = strrchr(buf->buf, '\n');
@@ -344,6 +300,7 @@ static int readstrings(buf_t *buf, int fd){
         }else buf->len = 0;
         L = XYBUFSZ - buf->len;
     }
+    //DBG("read %d bytes from %d", L, fd);
     int got = read(fd, &buf->buf[buf->len], L);
     if(got < 0){
         DBG("read()");
@@ -351,13 +308,16 @@ static int readstrings(buf_t *buf, int fd){
     }else if(got == 0){ DBG("NO data"); return TRUE; }
     buf->len += got;
     buf->buf[buf->len] = 0;
-    DBG("buf[%d]: %s", buf->len, buf->buf);
+    //DBG("buf[%d]: %s", buf->len, buf->buf);
     return TRUE;
 }
 
 // return TRUE if got, FALSE if no data found
 static int getdata(buf_t *buf, long *out){
-    if(!buf || buf->len < 1) return FALSE;
+    if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){
+        return FALSE;
+    }
+//    DBG("got data");
     // read record between last '\n' and previous (or start of string)
     char *last = &buf->buf[buf->len - 1];
     //DBG("buf: _%s_", buf->buf);
@@ -378,7 +338,7 @@ static int getdata(buf_t *buf, long *out){
 
 // try to write '\n' asking new data portion; return FALSE if failed
 static int asknext(int fd){
-    FNAME();
+    //FNAME();
     if(fd < 0) return FALSE;
     int i = 0;
     for(; i < 5; ++i){
@@ -399,12 +359,24 @@ static void *encoderthread2(void _U_ *u){
     pfds[0].fd = encfd[0]; pfds[0].events = POLLIN;
     pfds[1].fd = encfd[1]; pfds[1].events = POLLIN;
     double t0[2], tstart;
-    buf_t strbuf[2];
+    buf_t strbuf[2] = {0};
     long msrlast[2]; // last encoder data
     double mtlast[2]; // last measurement time
     asknext(encfd[0]); asknext(encfd[1]);
     t0[0] = t0[1] = tstart = timefromstart();
     int errctr = 0;
+
+    // init Kalman for both axes
+    Kalman3 kf[2];
+    double dt = Conf.EncoderReqInterval; // 1ms encoders step
+    double sigma_jx = 1e-6, sigma_jy = 1e-6; // "jerk" sigma
+    double xnoice = encoder_noise(X_ENC_STEPSPERREV);
+    double ynoice = encoder_noise(Y_ENC_STEPSPERREV);
+    kalman3_init(&kf[0], dt, xnoice);
+    kalman3_init(&kf[1], dt, ynoice);
+    kalman3_set_jerk_noise(&kf[0], sigma_jx);
+    kalman3_set_jerk_noise(&kf[1], sigma_jy);
+
     do{ // main cycle
         if(poll(pfds, 2, 0) < 0){
             DBG("poll()");
@@ -414,6 +386,7 @@ static void *encoderthread2(void _U_ *u){
         for(int i = 0; i < 2; ++i){
             if(pfds[i].revents && POLLIN){
                 if(!readstrings(&strbuf[i], encfd[i])){
+                    DBG("ERR");
                     ++errctr;
                     break;
                 }
@@ -421,16 +394,37 @@ static void *encoderthread2(void _U_ *u){
             double curt = timefromstart();
             if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
             if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
+                //DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
                 if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
+                    //DBG("time OK");
                     pthread_mutex_lock(&datamutex);
+                    double pos = (double)msrlast[i];
                     if(i == 0){
-                        mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
+                        pos = Xenc2rad(pos);
+                        // Kalman filtering
+                        kalman3_predict(&kf[i]);
+                        kalman3_update(&kf[i], pos);
+                        //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
+                        //    pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
+                        mountdata.encXposition.val = kf[i].x[0];
                         curtime(&mountdata.encXposition.t);
+                        /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
+                            msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
+                            X_ENC_ZERO, X_ENC_STEPSPERREV);*/
                         getXspeed();
+                        //mountdata.encXspeed.val = kf[i].x[1];
+                        //mountdata.encXspeed.t = mountdata.encXposition.t;
                     }else{
-                        mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
+                        pos = Yenc2rad(pos);
+                        kalman3_predict(&kf[i]);
+                        kalman3_update(&kf[i], pos);
+                        //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
+                        //    pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
+                        mountdata.encYposition.val = kf[i].x[0];
                         curtime(&mountdata.encYposition.t);
                         getYspeed();
+                        //mountdata.encYspeed.val = kf[i].x[1];
+                        //mountdata.encYspeed.t = mountdata.encYposition.t;
                     }
                     pthread_mutex_unlock(&datamutex);
                 }
@@ -443,7 +437,7 @@ static void *encoderthread2(void _U_ *u){
             }
         }
         if(got == 2) errctr = 0;
-    }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
+    }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR && !GlobExit);
     DBG("\n\nEXIT: ERRCTR=%d", errctr);
     for(int i = 0; i < 2; ++i){
         if(encfd[i] > -1){
@@ -490,7 +484,7 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t
 // main mount thread
 static void *mountthread(void _U_ *u){
     int errctr = 0;
-    uint8_t buf[2*sizeof(SSstat)];
+    uint8_t buf[sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
     bzero(&mountdata, sizeof(mountdata));
     double t0 = timefromstart(), tstart = t0, tcur = t0;
@@ -499,7 +493,7 @@ static void *mountthread(void _U_ *u){
     if(Conf.RunModel){
         double Xprev = NAN, Yprev = NAN; // previous coordinates
         int xcnt = 0, ycnt = 0;
-        while(1){
+        while(!GlobExit){
             coordpair_t c;
             movestate_t xst, yst;
             // now change data
@@ -508,20 +502,20 @@ static void *mountthread(void _U_ *u){
             if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
             pthread_mutex_lock(&datamutex);
             mountdata.encXposition.t = mountdata.encYposition.t = tnow;
-            mountdata.encXposition.val = c.X;
+            mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
-            mountdata.encYposition.val = c.Y;
+            mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
             //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
             if(tcur - oldmt > Conf.MountReqInterval){
                 oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
                 mountdata.motYposition.t = mountdata.motXposition.t = tnow;
                 if(xst == ST_MOVE)
                     mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
-                else
+                //else
-                    mountdata.motXposition.val = c.X;
+                //    mountdata.motXposition.val = c.X;
                 if(yst == ST_MOVE)
                     mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
-                else
+                //else
-                    mountdata.motYposition.val = c.Y;
+                //    mountdata.motYposition.val = c.Y;
                 oldmt = tcur;
             }else mountdata.millis = oldmillis;
             chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
@@ -537,7 +531,7 @@ static void *mountthread(void _U_ *u){
     // cmd to send
     data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
     if(!cmd_getstat) goto failed;
-    while(mntfd > -1 && errctr < MAX_ERR_CTR){
+    while(mntfd > -1 && errctr < MAX_ERR_CTR && !GlobExit){
         // read data to status
         struct timespec tcur;
         if(!curtime(&tcur)) continue;
@@ -594,8 +588,8 @@ static int ttyopen(const char *path, speed_t speed){
     tty.c_cflag     = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
     tty.c_ispeed = speed;
     tty.c_ospeed = speed;
-    //tty.c_cc[VMIN]  = 0;  // non-canonical mode
+    tty.c_cc[VMIN]  = 0;  // non-canonical mode
-    //tty.c_cc[VTIME] = 5;
+    tty.c_cc[VTIME] = 5;
     if(ioctl(fd, TCSETS2, &tty)){
         DBG("Can't set TTY settings");
         close(fd);
@@ -610,15 +604,18 @@ static int ttyopen(const char *path, speed_t speed){
 
 // return FALSE if failed
 int openEncoder(){
+    // TODO: open real devices in "model" mode too!
     if(Conf.RunModel) return TRUE;
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
+    /*
+    encRtmout.tv_sec = 0;
+    encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
+*/
     if(Conf.SepEncoder == 1){ // only one device
         DBG("One device");
         if(encfd[0] > -1) close(encfd[0]);
         encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
         if(encfd[0] < 0) return FALSE;
-        encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
         if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
             close(encfd[0]);
             encfd[0] = -1;
@@ -632,8 +629,6 @@ int openEncoder(){
             encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
             if(encfd[i] < 0) return FALSE;
         }
-        encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
         if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
             for(int i = 0; i < 2; ++i){
                 close(encfd[i]);
@@ -648,6 +643,7 @@ int openEncoder(){
 
 // return FALSE if failed
 int openMount(){
+    // TODO: open real devices in "model" mode too!
     if(Conf.RunModel) goto create_thread;
     if(mntfd > -1) close(mntfd);
     DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
@@ -655,16 +651,13 @@ int openMount(){
     if(mntfd < 0) return FALSE;
     DBG("mntfd=%d", mntfd);
     // clear buffer
-    while(getmntbyte() > -1);
+    clrmntbuf();
-    /*int g = write(mntfd, "XXS\r", 4);
+    /*
-    DBG("Written %d", g);
+    mnt1Rtmout.tv_sec = 0;
-    uint8_t buf[100];
+    mnt1Rtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
-    do{
-        ssize_t l = read(mntfd, buf, 100);
-        DBG("got %zd", l);
-    }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
+    mntRtmout.tv_usec = mnt1Rtmout.tv_usec / 50;
+*/
 create_thread:
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
         DBG("Can't create mount thread");
@@ -680,15 +673,17 @@ create_thread:
 
 // close all opened serial devices and quit threads
 void closeSerial(){
-    // TODO: close devices in "model" mode too!
+    GlobExit = 1;
-    if(Conf.RunModel) return;
+    DBG("Give 100ms to proper close");
+    usleep(100000);
+    DBG("Force closed all devices");
     if(mntfd > -1){
         DBG("Cancel mount thread");
         pthread_cancel(mntthread);
         DBG("join mount thread");
         pthread_join(mntthread, NULL);
         DBG("close mount fd");
-        close(mntfd);
+        if(mntfd > -1) close(mntfd);
         mntfd = -1;
     }
     if(encfd[0] > -1){
@@ -697,13 +692,14 @@ void closeSerial(){
         DBG("join encoder thread");
         pthread_join(encthread, NULL);
         DBG("close encoder's fd");
-        close(encfd[0]);
+        if(encfd[0] > -1) close(encfd[0]);
         encfd[0] = -1;
         if(Conf.SepEncoder == 2 && encfd[1] > -1){
             close(encfd[1]);
             encfd[1] = -1;
         }
     }
+    GlobExit = 0;
 }
 
 // get fresh encoder information
@@ -731,26 +727,29 @@ static int wr(const data_t *out, data_t *in, int needeol){
         DBG("Wrong arguments or no mount fd");
         return FALSE;
     }
+    //DBG("clrbuf");
     clrmntbuf();
     if(out){
+        //DBG("write %zd bytes (%s)", out->len, out->buf);
         if(out->len != (size_t)write(mntfd, out->buf, out->len)){
             DBG("written bytes not equal to need");
             return FALSE;
         }
+        //DBG("eol, mntfd=%d", mntfd);
         if(needeol){
             int g = write(mntfd, "\r", 1); // add EOL
             (void) g;
         }
-        usleep(50000); // add little pause so that the idiot has time to swallow
+        //usleep(50000); // add little pause so that the idiot has time to swallow
     }
-    if(!in) return TRUE;
+    if(!in || in->maxlen < 1) return TRUE;
+    int got = readmntdata(in->buf, in->maxlen);
+    if(got < 0){
+        DBG("Error reading mount data!");
         in->len = 0;
-    for(size_t i = 0; i < in->maxlen; ++i){
+        return FALSE;
-        int b = getmntbyte();
-        if(b < 0) break; // nothing to read -> go out
-        in->buf[in->len++] = (uint8_t) b;
     }
-    while(getmntbyte() > -1);
+    in->len = got;
     return TRUE;
 }
 
@@ -761,22 +760,24 @@ static int wr(const data_t *out, data_t *in, int needeol){
  * @return FALSE if failed
  */
 int MountWriteRead(const data_t *out, data_t *in){
-    if(Conf.RunModel) return -1;
+    if(Conf.RunModel) return FALSE;
+    //double t0 = timefromstart();
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 1);
     pthread_mutex_unlock(&mntmutex);
+    //DBG("Got %gus", (timefromstart()-t0)*1e6);
     return ret;
 }
 // send binary data - without EOL
 int MountWriteReadRaw(const data_t *out, data_t *in){
-    if(Conf.RunModel) return -1;
+    if(Conf.RunModel) return FALSE;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 0);
     pthread_mutex_unlock(&mntmutex);
     return ret;
 }
 
-#ifdef EBUG
+#if 0
 static void logscmd(SSscmd *c){
     printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
     printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits);
@@ -799,31 +800,37 @@ static int bincmd(uint8_t *cmd, int len){
     if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
     int ret = FALSE;
     pthread_mutex_lock(&mntmutex);
-    // dummy buffer to clear trash in input
-    //char ans[300];
-    //data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
     if(len == sizeof(SSscmd)){
         ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
-        DBG("Short command");
+        //DBG("Short command");
-#ifdef EBUG
+#if 0
         logscmd((SSscmd*)cmd);
 #endif
         if(!wr(dscmd, NULL, 1)) goto rtn;
     }else if(len == sizeof(SSlcmd)){
         ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
-        DBG("Long command");
+       // DBG("Long command");
-#ifdef EBUG
+#if 0
         loglcmd((SSlcmd*)cmd);
 #endif
         if(!wr(dlcmd, NULL, 1)) goto rtn;
     }else{
         goto rtn;
     }
-    data_t d;
+    SSstat ans;
+    data_t d, in;
     d.buf = cmd;
     d.len = d.maxlen = len;
-    ret = wr(&d, NULL, 0);
+    in.buf = (uint8_t*)&ans; in.maxlen = sizeof(SSstat);
+    ret = wr(&d, &in, 0);
     DBG("%s", ret ? "SUCCESS" : "FAIL");
+    if(ret){
+        SSscmd *sc = (SSscmd*)cmd;
+        mountdata.Xtarget = sc->Xmot;
+        mountdata.Ytarget = sc->Ymot;
+        DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d; Xtag/Ytag: %d/%d",
+            ans.Xmot, ans.Ymot, ans.XLast, ans.YLast, mountdata.Xtarget, mountdata.Ytarget);
+    }
 rtn:
     pthread_mutex_unlock(&mntmutex);
     return ret;

Auxiliary_utils/LibSidServo/servo_real.conf

@@ -0,0 +1,23 @@
+MountDevPath=/dev/ttyUSB0
+MountDevSpeed=19200
+EncoderDevSpeed=1000000
+MountReqInterval=0.1
+EncoderReqInterval=0.001
+SepEncoder=2
+EncoderXDevPath=/dev/encoder_X0
+EncoderYDevPath=/dev/encoder_Y0
+EncoderSpeedInterval=0.05
+RunModel=0
+# telescope is in "pointing state" when coordinate error less than MaxFinePointingErr and goes to "slewing state"
+#   when this error greater than MaxPointingErr
+MaxPointingErr = 0.3490658504 # "pointing zone" - 20 degr
+MaxFinePointingErr = 0.1745329252 # "guiding zone" - 10 degr
+MaxGuidingErr = 4.8481368e-6 # "on target zone" - 1 arcsec
+XPIDVP=0.9
+XPIDVI=0.0005
+XPIDVD=0.0
+YPIDVP=0.5
+YPIDVI=0.005
+YPIDVD=0.
+XEncZero=36627112
+YEncZero=36067741

Auxiliary_utils/LibSidServo/sidservo.h

@@ -136,10 +136,11 @@ typedef struct{
 } extradata_t;
 
 typedef enum{
-    AXIS_STOPPED,
+    AXIS_STOPPED,   // stop
-    AXIS_SLEWING,
+    AXIS_GONNASTOP, // stop command run
-    AXIS_POINTING,
+    AXIS_SLEWING,   // go to target with maximal speed
-    AXIS_GUIDING,
+    AXIS_POINTING,  // axis is in pointing zone, use PID
+    AXIS_GUIDING,   // near target
     AXIS_ERROR,
 } axis_status_t;
 
@@ -157,6 +158,9 @@ typedef struct{
     uint32_t millis;
     double temperature;
     double voltage;
+    // target X/Y position by last `short` or `long` command
+    int32_t Xtarget; // in SidServo's counts
+    int32_t Ytarget; // -//-
 } mountdata_t;
 
 typedef struct{

Auxiliary_utils/LibSidServo/ssii.c

@@ -26,8 +26,12 @@
 #include "serial.h"
 #include "ssii.h"
 
-int X_ENC_ZERO, Y_ENC_ZERO;
+int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; // will be filled later from config
-double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.;
+// defaults until read from controller
+double  X_MOT_STEPSPERREV = 13312000.,
+        Y_MOT_STEPSPERREV = 17578668.,
+        X_ENC_STEPSPERREV = 67108864.,
+        Y_ENC_STEPSPERREV = 67108864.;
 
 uint16_t SScalcChecksum(uint8_t *buf, int len){
     uint16_t checksum = 0;
@@ -41,27 +45,36 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
 }
 
 // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
-static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
+static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){
     if(*prev == INT32_MAX){
-        *stat = AXIS_STOPPED;
+        stat = AXIS_STOPPED;
         DBG("START");
-    }else if(*stat != AXIS_STOPPED){
+    }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target
-        if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
+        if(*prev == cur){
-            *stat = AXIS_STOPPED;
+            DBG("Test for stop, nstopped=%d", *nstopped);
+            if(++(*nstopped) > MOTOR_STOPPED_CNT){
+                stat = AXIS_STOPPED;
                 DBG("AXIS stopped");
             }
+        }else *nstopped = 0;
     }else if(*prev != cur){
         DBG("AXIS moving");
         *nstopped = 0;
     }
     *prev = cur;
+    return stat;
 }
 // check for stopped/pointing states
 static void ChkStopped(const SSstat *s, mountdata_t *m){
     static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
     static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
-    chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
+    axis_status_t Xstat, Ystat;
-    chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
+    Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate);
+    Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate);
+    if(Xstat != m->Xstate || Ystat != m->Ystate){
+        DBG("Status changed");
+        setStat(Xstat, Ystat);
+    }
 }
 
 /**
@@ -78,6 +91,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
     m->motXposition.t = m->motYposition.t = *t;
     // fill encoder data from here, as there's no separate enc thread
     if(!Conf.SepEncoder){
+        DBG("ENCODER from SSII");
         m->encXposition.val = Xenc2rad(s->Xenc);
         DBG("encx: %g", m->encXposition.val);
         m->encYposition.val = Yenc2rad(s->Yenc);
@@ -165,14 +179,17 @@ int SSsetterI(const char *cmd, int32_t ival){
 }
 
 int SSstop(int emerg){
+    FNAME();
     int i = 0;
     const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
     const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
+    setStat(AXIS_GONNASTOP, AXIS_GONNASTOP);
     for(; i < 10; ++i){
         if(!SStextcmd(cmdx, NULL)) continue;
         if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
+    DBG("Stopped");
     return TRUE;
 }
 

Auxiliary_utils/LibSidServo/ssii.h

@@ -209,12 +209,14 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE
 
 // convert angle in radians to +-pi
 static inline __attribute__((always_inline)) double ang2half(double ang){
+    ang = fmod(ang, 2.*M_PI);
     if(ang < -M_PI) ang += 2.*M_PI;
     else if(ang > M_PI) ang -= 2.*M_PI;
     return ang;
 }
 // convert to only positive: 0..2pi
 static inline __attribute__((always_inline)) double ang2full(double ang){
+    ang = fmod(ang, 2.*M_PI);
     if(ang < 0.) ang += 2.*M_PI;
     else if(ang > 2.*M_PI) ang -= 2.*M_PI;
     return ang;