fix encoders

.gitignore

@@ -23,3 +23,6 @@
 *.so
 *.so.*
 
+# build dirs
+mk/
+build/

Auxiliary_utils/Hydreon/cmdlnopts.c

@@ -49,7 +49,7 @@ static glob_pars const Gdefault = {
  * Define command line options by filling structure:
  *  name        has_arg     flag    val     type        argptr              help
 */
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
 // common options
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        _("show this help")},
     {"device",  NEED_ARG,   NULL,   'd',    arg_string, APTR(&G.device),    _("serial device name (default: )" DEFAULT_DEV ")")},
@@ -75,10 +75,10 @@ glob_pars *parse_args(int argc, char **argv){
     char helpstring[1024], *hptr = helpstring;
     snprintf(hptr, hlen, "Usage: %%s [args]\n\n\tWhere args are:\n");
     // format of help: "Usage: progname [args]\n"
-    change_helpstring(helpstring);
+    sl_helpstring(helpstring);
     // parse arguments
-    parseargs(&argc, &argv, cmdlnopts);
-    if(help) showhelp(-1, cmdlnopts);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(help) sl_showhelp(-1, cmdlnopts);
     if(argc > 0){
         WARNX("Ignore %d unknown parameters: ");
         for (i = 0; i < argc; i++)

Auxiliary_utils/Hydreon/dumpregs.c

@@ -57,7 +57,7 @@ static void dumpRchanges(rg11 *new, rg11 *old){
     int start = 1;
     for(int i = 0; i < RREGNUM; ++i){
         if(o[i] != n[i]){
-            sl_putlogt(start, globlog, LOGLEVEL_MSG, "%s=%d", regname(i), n[i]);
+            sl_putlogt(start, sl_globlog, LOGLEVEL_MSG, "%s=%d", regname(i), n[i]);
             DBG("%s=%d", regname(i), n[i]);
             if(start) start = 0;
         }
@@ -68,7 +68,7 @@ static void dumpRchanges(rg11 *new, rg11 *old){
         uint8_t f = 1;
         for(int i = 0; i < RGBITNUM; ++i, f <<= 1){
             if(xOr & f){
-                sl_putlogt(start, globlog, LOGLEVEL_MSG, "%s=%d", rgbitname(i), (new->RGBits & f) ? 1 : 0);
+                sl_putlogt(start, sl_globlog, LOGLEVEL_MSG, "%s=%d", rgbitname(i), (new->RGBits & f) ? 1 : 0);
                 DBG("%s=%d", rgbitname(i), (new->RGBits & f) ? 1 : 0);
                 if(start) start = 0;
             }
@@ -82,7 +82,7 @@ static void dumpSchanges(slowregs *new, slowregs *old){
     int start = 1;
     for(int i = 0; i < SREGNUM; ++i){
         if(o[i] != n[i]){
-            sl_putlogt(start, globlog, LOGLEVEL_MSG, "%s=%d", slowname(i), n[i]);
+            sl_putlogt(start, sl_globlog, LOGLEVEL_MSG, "%s=%d", slowname(i), n[i]);
             DBG("%s=%d", slowname(i), n[i]);
             if(start) start = 0;
         }
@@ -147,12 +147,12 @@ static void puttotable(rg11 *R, slowregs *S){
 }
 
 int main(int argc, char **argv){
-    initial_setup();
+    sl_init();
     char *self = strdup(argv[0]);
     G = parse_args(argc, argv);
     if(G->timeout < 5) ERRX("Timeout should be not less than 5 seconds");
     if(!G->logfile && !G->outfile) ERRX("Point at least log or output file name");
-    check4running(self, G->pidfile);
+    sl_check4running(self, G->pidfile);
     if(!hydreon_open(G->device)) return 1;
     if(G->logfile) OPENLOG(G->logfile, LOGLEVEL_ANY, 0);
     if(G->outfile){
@@ -166,9 +166,9 @@ int main(int argc, char **argv){
     signal(SIGINT, signals);  // ctrl+C - quit
     signal(SIGQUIT, signals); // ctrl+\ - quit
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-    double t0 = dtime();
+    double t0 = sl_dtime();
     puttotable(NULL, NULL);
-    while(dtime() - t0 < (double)G->timeout){ // dump only changes
+    while(sl_dtime() - t0 < (double)G->timeout){ // dump only changes
         if(!hydreon_getpacket(&Rregs, &Sregs)) continue;
         int changes = FALSE;
         if(memcmp(&Rregs, &oRregs, RREGNUM + 1)){ // Rregs changed -> log changes
@@ -182,7 +182,7 @@ int main(int argc, char **argv){
             changes = TRUE;
         }
         if(changes) puttotable(&Rregs, &Sregs);
-        t0 = dtime();
+        t0 = sl_dtime();
     }
     signals(-1); // never reached
     return 0;

Auxiliary_utils/Hydreon/hydreon.c

@@ -20,7 +20,7 @@
 
 #include "hydreon.h"
 
-static TTY_descr *dev = NULL;
+static sl_tty_t *dev = NULL;
 
 // regular registers names
 static const char* rregnames[RREGNUM] = {
@@ -121,7 +121,7 @@ int hydreon_getpacket(rg11 *Rregs, slowregs *Sregs){
     if(!dev) return 0;
     static int buflen = 0;
     static char strbuf[BUFLEN];
-    int l = read_tty(dev);
+    int l = sl_tty_read(dev);
     if(l < 1) return FALSE;
     char s = dev->buf[0];
     if(s == 's'){ // start of new packet -> encode old
@@ -146,13 +146,13 @@ int hydreon_getpacket(rg11 *Rregs, slowregs *Sregs){
  * @return TRUE or FALSE if failed
  */
 int hydreon_open(const char *devname){
-    dev = new_tty((char*)devname, 1200, 1);
+    dev = sl_tty_new((char*)devname, 1200, 1);
     if(!dev) return FALSE;
-    dev = tty_open(dev, 1);
+    dev = sl_tty_open(dev, 1);
     if(!dev) return FALSE;
     return TRUE;
 }
 
 void hydreon_close(){
-    if(dev) close_tty(&dev);
+    if(dev) sl_tty_close(&dev);
 }

Auxiliary_utils/LibSidServo/PID.c

@@ -0,0 +1,221 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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 <stdio.h>
+#include <stdlib.h>
+#include <strings.h>
+
+#include "main.h"
+#include "PID.h"
+#include "serial.h"
+
+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));
+    return pid;
+}
+
+// don't clear lastT!
+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;
+}
+
+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 flowing integral
+    double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
+    //DBG("oldi/new: %g, %g", oldi, newi);
+    pid->pidIarray[pid->curIidx++] = newi;
+    if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
+    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("P=%g, I=%g, D=%g; sum=%g", 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
+ */
+static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, 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);
+    PIDController_t *pid = NULL;
+    switch(axis->state){
+        case AXIS_SLEWING:
+            if(fe < Conf.MaxPointingErr){
+                axis->state = AXIS_POINTING;
+                DBG("--> Pointing");
+                pid = pidpair->PIDC;
+            }else{
+                DBG("Slewing...");
+                return NAN; // max speed for given axis
+            }
+            break;
+        case AXIS_POINTING:
+            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);
+            break;
+        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);
+        case AXIS_ERROR:
+            DBG("Can't move from erroneous state");
+            return 0.;
+    }
+    if(!pid){
+        DBG("WTF? Where is a PID?");
+        return axis->speed.val;
+    }
+    double dtpid = timediff(&tagpos->t, &pid->prevT);
+    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
+}
+
+/**
+ * @brief correct2 - recalculate PID and move telescope to new point with new speed
+ * @param target - target position (for error calculations)
+ * @param endpoint - stop point (some far enough point to stop in case of hang)
+ * @return error code
+ */
+mcc_errcodes_t correct2(const coordval_pair_t *target){
+    static PIDpair_t pidX = {0}, pidY = {0};
+    if(!pidX.PIDC){
+        pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+        if(!pidX.PIDC) return MCC_E_FATAL;
+        pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+        if(!pidX.PIDV) return MCC_E_FATAL;
+    }
+    if(!pidY.PIDC){
+        pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
+        if(!pidY.PIDC) 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
+    double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
+    if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
+    axisdata_t axis;
+    DBG("state: %d/%d", m.Xstate, m.Ystate);
+    axis.state = m.Xstate;
+    axis.position = m.encXposition;
+    axis.speed = m.encXspeed;
+    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;
+    }else{
+        if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
+        if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
+    }
+    axis_status_t xstate = axis.state;
+    axis.state = m.Ystate;
+    axis.position = m.encYposition;
+    axis.speed = m.encYspeed;
+    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;
+    }else{
+        if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
+        if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
+    }
+    axis_status_t ystate = axis.state;
+    if(m.Xstate != xstate || m.Ystate != ystate){
+        DBG("State changed");
+        setStat(xstate, ystate);
+    }
+    coordpair_t endpoint;
+    // 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
+                   + Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
+                   + tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
+    endpoint.X = m.encXposition.val + Xsign * adder;
+    dv = fabs(tagspeed.Y - m.encYspeed.val);
+    adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
+            + Conf.PIDMaxDt * tagspeed.Y
+            + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
+    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

@@ -0,0 +1,40 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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
+
+#include <stddef.h>
+
+#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/PID_test/Dramp.c

@@ -0,0 +1,141 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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 <stdio.h>
+#include <usefull_macros.h>
+
+#include "Dramp.h"
+
+static movestate_t state = ST_STOP;
+static moveparam_t target, Min, Max;
+static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate
+static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
+static double T1 = -1.; // time to switch into minimal speed for best coord tolerance
+
+typedef enum{
+    STAGE_NORMALSPEED,
+    STAGE_MINSPEED,
+    STAGE_STOPPED
+} movingsage_t;
+
+static movingsage_t movingstage = STAGE_STOPPED;
+
+static int initlims(limits_t *lim){
+    if(!lim) return FALSE;
+    Min = lim->min;
+    Max = lim->max;
+    return TRUE;
+}
+
+static int calc(moveparam_t *x, double t){
+    DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed);
+    if(!x || t < 0.) return FALSE;
+    if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE;
+    double adist = fabs(x->coord - curparams.coord);
+    DBG("want dist: %g", adist);
+    if(adist < coord_tolerance) return TRUE; // we are at place
+    if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters
+    target = *x;
+    if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position
+    if(target.speed < Min.speed) target.speed = Min.speed;
+    DBG("Approximate tag speed: %g", target.speed);
+    T0 = t;
+    // calculate time to switch into minimal speed
+    T1 = -1.; // no min speed phase
+    if(target.speed > Min.speed){
+        double dxpertick = target.speed * time_tick;
+        DBG("dX per one tick: %g", dxpertick);
+        double ticks_need = floor(adist / dxpertick);
+        DBG("ticks need: %g", ticks_need);
+        if(ticks_need < 1.) return FALSE; // cannot reach
+        if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){
+            DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need);
+            double dxpersmtick = Min.speed * time_tick;
+            DBG("dX per smallest tick: %g", dxpersmtick);
+            while(--ticks_need > 1.){
+                double part = adist - ticks_need * dxpertick;
+                double smticks = floor(part / dxpersmtick);
+                double least = part - smticks * dxpersmtick;
+                if(least < coord_tolerance) break;
+            }
+            DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick);
+            T1 = t + ticks_need * time_tick;
+        }
+    }
+    state = ST_MOVE;
+    Xlast0 = curparams.coord;
+    if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED;
+    else movingstage = STAGE_MINSPEED;
+    if(x->coord < curparams.coord) target.speed *= -1.; // real speed
+    curparams.speed = target.speed;
+    return TRUE;
+}
+
+static void stop(double _U_ t){
+    T0 = -1.;
+    curparams.accel = 0.;
+    curparams.speed = 0.;
+    state = ST_STOP;
+    movingstage = STAGE_STOPPED;
+}
+
+static movestate_t proc(moveparam_t *next, double t){
+    if(T0 < 0.) return ST_STOP;
+    curparams.coord = Xlast0 + (t - T0) * curparams.speed;
+    //DBG("coord: %g (dTmon: %g, speed: %g)", curparams.coord, t-T0, curparams.speed);
+    int ooops = FALSE; // oops - we are over target!
+    if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;}
+    else{ if(curparams.coord > target.coord) ooops = TRUE; }
+    if(ooops){
+        DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord);
+        stop(t);
+        if(next) *next = curparams;
+        return state;
+    }
+    if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1
+        if(t >= T1){
+            DBG("T1=%g, t=%g -->", T1, t);
+            curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed;
+            movingstage = STAGE_MINSPEED;
+            Xlast0 = curparams.coord;
+            T0 = T1;
+            DBG("Go further with minimal speed");
+        }
+    }
+    if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place
+        DBG("OK, we are in place");
+        stop(t);
+    }
+    if(next) *next = curparams;
+    return state;
+}
+
+static movestate_t getst(moveparam_t *cur){
+    if(cur) *cur = curparams;
+    return state;
+}
+
+movemodel_t dumb = {
+    .init_limits = initlims,
+    .calculate = calc,
+    .proc_move = proc,
+    .stop = stop,
+    .emergency_stop = stop,
+    .get_state = getst,
+};

Auxiliary_utils/LibSidServo/PID_test/Dramp.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include "moving_private.h"
+
+extern movemodel_t dumb;

Auxiliary_utils/LibSidServo/PID_test/Makefile

@@ -0,0 +1,57 @@
+# run `make DEF=...` to add extra defines
+PROGRAM := moving
+LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
+LDFLAGS += -lusefull_macros -lm
+SRCS := $(wildcard *.c)
+DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
+OBJDIR := mk
+CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
+OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
+DEPS := $(OBJS:.o=.d)
+TARGFILE := $(OBJDIR)/TARGET
+CC = gcc
+#TARGET := RELEASE
+
+ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
+	TARGET := $(file < $(TARGFILE))
+else
+	TARGET := RELEASE
+endif
+
+ifeq ($(TARGET), DEBUG)
+	.DEFAULT_GOAL := debug
+endif
+
+release: $(PROGRAM)
+
+debug: CFLAGS += -DEBUG -Werror
+debug: TARGET := DEBUG
+debug: $(PROGRAM)
+
+$(TARGFILE): $(OBJDIR)
+	@echo -e "\t\tTARGET: $(TARGET)"
+	@echo "$(TARGET)" > $(TARGFILE)
+
+$(PROGRAM) : $(TARGFILE) $(OBJS)
+	@echo -e "\t\tLD $(PROGRAM)"
+	$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
+
+$(OBJDIR):
+	@mkdir $(OBJDIR)
+
+ifneq ($(MAKECMDGOALS),clean)
+-include $(DEPS)
+endif
+
+$(OBJDIR)/%.o: %.c
+	@echo -e "\t\tCC $<"
+	$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ 
+
+clean:
+	@echo -e "\t\tCLEAN"
+	@rm -rf $(OBJDIR) 2>/dev/null || true
+
+xclean: clean
+	@rm -f $(PROGRAM)
+
+.PHONY: clean xclean

Auxiliary_utils/LibSidServo/PID_test/PID.c

@@ -0,0 +1,64 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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 <stdio.h>
+#include <stdlib.h>
+#include <strings.h>
+#include <usefull_macros.h>
+
+#include "PID.h"
+
+PIDController_t *pid_create(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;
+    pid->pidIarrSize = Iarrsz;
+    pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
+    return pid;
+}
+
+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;
+}
+
+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 flowing integral
+    double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
+    DBG("oldi/new: %g, %g", oldi, newi);
+    pid->pidIarray[pid->curIidx++] = newi;
+    if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
+    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("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
+    return sum;
+}

Auxiliary_utils/LibSidServo/PID_test/PID.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include <stddef.h>
+
+typedef struct{
+    double P, I, D;
+} PIDpar_t;
+
+typedef struct {
+    PIDpar_t gain;      // PID gains
+    double prev_error;  // Previous error
+    double integral;    // Integral term
+    double *pidIarray;  // array for Integral
+    size_t pidIarrSize; // it's size
+    size_t curIidx;     // and index of current element
+} PIDController_t;
+
+PIDController_t *pid_create(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);

Auxiliary_utils/LibSidServo/PID_test/Sramp.c

@@ -0,0 +1,27 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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/>.
+ */
+
+// quasi non-jerk s-ramp
+
+#include <stdio.h>
+#include <usefull_macros.h>
+
+#include "Sramp.h"
+
+
+movemodel_t s_shaped = { 0 };

Auxiliary_utils/LibSidServo/PID_test/Sramp.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include "moving_private.h"
+
+extern movemodel_t s_shaped;

Auxiliary_utils/LibSidServo/PID_test/Tramp.c

@@ -0,0 +1,223 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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/>.
+ */
+
+// simplest trapezioidal ramp
+
+#include <math.h>
+#include <stdio.h>
+#include <strings.h>
+#include <usefull_macros.h>
+
+#include "Tramp.h"
+
+#undef DBG
+#define DBG(...)
+
+static movestate_t state = ST_STOP;
+static moveparam_t Min, Max; // `Min` acceleration not used!
+
+typedef enum{
+    STAGE_ACCEL,        // start from zero speed and accelerate to Max speed
+    STAGE_MAXSPEED,     // go with target speed
+    STAGE_DECEL,        // go from target speed to zero
+    STAGE_STOPPED,      // stop
+    STAGE_AMOUNT
+} movingstage_t;
+
+static movingstage_t movingstage = STAGE_STOPPED;
+static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
+static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
+static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
+
+static int initlims(limits_t *lim){
+    if(!lim) return FALSE;
+    Min = lim->min;
+    Max = lim->max;
+    return TRUE;
+}
+
+static void emstop(double _U_ t){
+    curparams.accel = 0.;
+    curparams.speed = 0.;
+    bzero(Times, sizeof(Times));
+    bzero(Params, sizeof(Params));
+    state = ST_STOP;
+    movingstage = STAGE_STOPPED;
+}
+
+static void stop(double t){
+    if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
+    movingstage = STAGE_DECEL;
+    state = ST_MOVE;
+    Times[STAGE_DECEL] = t;
+    Params[STAGE_DECEL].speed = curparams.speed;
+    if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
+    else Params[STAGE_DECEL].accel = Max.accel;
+    Params[STAGE_DECEL].coord = curparams.coord;
+    // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
+    Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
+    // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
+    double dt = Times[STAGE_STOPPED] - t;
+    Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
+                                  Params[STAGE_DECEL].accel * dt * dt / 2.;
+}
+
+/**
+ * @brief calc - moving calculation
+ * @param x - using max speed (>0!!!) and coordinate
+ * @param t - current time value
+ * @return FALSE if can't move with given parameters
+ */
+static int calc(moveparam_t *x, double t){
+    if(!x) return FALSE;
+    if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
+    if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
+    double Dx = fabs(x->coord - curparams.coord); // full distance
+    double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
+    // we have two variants: with or without stage with constant speed
+    double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
+    double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
+    DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
+    double setspeed = x->speed; // new max speed (we can change it if need)
+    double dt01, dx01; // we'll fill them depending on starting conditions
+    Times[0] = t;
+    Params[0].speed = curparams.speed;
+    Params[0].coord = curparams.coord;
+
+    double curspeed = fabs(curparams.speed);
+    double dt0s = curspeed / Max.accel; // time of stopping phase
+    double dx0s = curspeed * dt0s / 2.; // distance
+    DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
+    if(dx0s > Dx){
+        WARNX("distance too short");
+        return FALSE;
+    }
+    if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
+        DBG("Distance good to just stop");
+        stop(t);
+        return TRUE;
+    }
+    if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
+        // after stop we will have full profile
+        double dxs3 = Dx - dx0s;
+        double newspeed = sqrt(Max.accel * dxs3);
+        if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
+        DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
+        dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
+        Params[0].accel = sign * Max.accel;
+        if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
+        else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
+        DBG("dx01=%g, dt01=%g", dx01, dt01);
+    }else{ // increase or decrease speed without stopping phase
+        dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
+        double a = sign * Max.accel;
+        if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
+        else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
+        //double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
+        dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
+        DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
+        if(dx01 + dx23 > Dx){ // calculate max speed
+            setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
+            if(setspeed < curspeed){
+                setspeed = curparams.speed;
+                dt01 = 0.; dx01 = 0.;
+                Params[0].accel = 0.;
+            }else{
+                Params[0].accel = a;
+                dt01 = fabs(setspeed - curspeed) / Max.accel;
+                dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
+            }
+        }else Params[0].accel = a;
+    }
+    if(setspeed < Min.speed){
+        WARNX("New speed should be too small");
+        return FALSE;
+    }
+    moveparam_t *p = &Params[STAGE_MAXSPEED];
+    p->accel = 0.; p->speed = sign * setspeed;
+    p->coord = curparams.coord + dx01 * sign;
+    Times[STAGE_MAXSPEED] = Times[0] + dt01;
+    dt23 = setspeed / Max.accel;
+    dx23 = setspeed * dt23 / 2.;
+    // calculate dx12 and dt12
+    double dx12 = Dx - dx01 - dx23;
+    if(dx12 < -coord_tolerance){
+        WARNX("Oops, WTF dx12=%g?", dx12);
+        return FALSE;
+    }
+    double dt12 = dx12 / setspeed;
+    p = &Params[STAGE_DECEL];
+    p->accel = -sign * Max.accel;
+    p->speed = sign * setspeed;
+    p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
+    Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
+    p = &Params[STAGE_STOPPED];
+    p->accel = 0.; p->speed = 0.; p->coord = x->coord;
+    Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
+    for(int i = 0; i < 4; ++i)
+        DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
+            Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
+    state = ST_MOVE;
+    movingstage = STAGE_ACCEL;
+    return TRUE;
+}
+
+static movestate_t proc(moveparam_t *next, double t){
+    if(state == ST_STOP) goto ret;
+    for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
+        if(Times[s] <= t){ // check time for current stage
+            movingstage = s;
+            break;
+        }
+    }
+    if(movingstage == STAGE_STOPPED){
+        curparams.coord = Params[STAGE_STOPPED].coord;
+        emstop(t);
+        goto ret;
+    }
+    // calculate current parameters
+    double dt = t - Times[movingstage];
+    double a = Params[movingstage].accel;
+    double v0 = Params[movingstage].speed;
+    double x0 = Params[movingstage].coord;
+    curparams.accel = a;
+    curparams.speed = v0 + a * dt;
+    curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
+ret:
+    if(next) *next = curparams;
+    return state;
+}
+
+static movestate_t getst(moveparam_t *cur){
+    if(cur) *cur = curparams;
+    return state;
+}
+
+static double gettstop(){
+    return Times[STAGE_STOPPED];
+}
+
+movemodel_t trapez = {
+    .init_limits = initlims,
+    .stop = stop,
+    .emergency_stop = emstop,
+    .get_state = getst,
+    .calculate = calc,
+    .proc_move = proc,
+    .stoppedtime = gettstop,
+};

Auxiliary_utils/LibSidServo/PID_test/Tramp.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include "moving_private.h"
+
+extern movemodel_t trapez;

Auxiliary_utils/LibSidServo/PID_test/main.c

@@ -0,0 +1,222 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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 <stdio.h>
+#include <strings.h>
+#include <usefull_macros.h>
+
+#include "moving.h"
+#include "PID.h"
+
+// errors for states: slewing/pointing/guiding
+// 10-degrees zone - Coordinate-driven PID
+#define MAX_POINTING_ERR    (36000.)
+// 1-arcminute zone - Velocity-dtiven PID
+#define MAX_GUIDING_ERR     (60.)
+// timeout to "forget" old data from I sum array; seconds
+#define PID_I_PERIOD        (3.)
+
+// PID for coordinate-driven and velocity-driven parts
+static PIDController_t *pidC = NULL, *pidV = NULL;
+static movemodel_t *model = NULL;
+static FILE *coordslog = NULL;
+
+typedef enum{
+    Slewing,
+    Pointing,
+    Guiding
+} state_t;
+
+static state_t state = Slewing;
+
+typedef struct{
+    int help;
+    char *ramptype;
+    char *xlog;
+    double dTmon;
+    double dTcorr;
+    double Tend;
+    double minerr;
+    double startcoord;
+    double error;
+    PIDpar_t gainC, gainV;
+} pars;
+
+static pars G = {
+    .dTmon = 0.01,
+    .dTcorr = 0.05,
+    .Tend = 100.,
+    .minerr = 0.1,
+    .gainC.P = 0.1,
+    .gainV.P = 0.1,
+    .startcoord = 100.,
+};
+
+static limits_t limits = {
+    .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
+    .max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
+};
+
+
+static sl_option_t opts[] = {
+    {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),          "show this help"},
+    {"tmon",    NEED_ARG,   NULL,   'T',    arg_double, APTR(&G.dTmon),         "time interval for monitoring (seconds, default: 0.001)"},
+    {"tcor",    NEED_ARG,   NULL,   't',    arg_double, APTR(&G.dTcorr),        "time interval for corrections (seconds, default: 0.05)"},
+    {"xlog",    NEED_ARG,   NULL,   'l',    arg_string, APTR(&G.xlog),          "log file name for coordinates logging"},
+    {"tend",    NEED_ARG,   NULL,   'e',    arg_double, APTR(&G.Tend),          "end time of monitoring (seconds, default: 100)"},
+    {"minerr",  NEED_ARG,   NULL,   'm',    arg_double, APTR(&G.minerr),        "minimal error for corrections (units, default: 0.1)"},
+    {"propC",   NEED_ARG,   NULL,   'P',    arg_double, APTR(&G.gainC.P),       "P-coefficient of coordinate-driven PID"},
+    {"integC",  NEED_ARG,   NULL,   'I',    arg_double, APTR(&G.gainC.I),       "I-coefficient of coordinate-driven PID"},
+    {"diffC",   NEED_ARG,   NULL,   'D',    arg_double, APTR(&G.gainC.D),       "D-coefficient of coordinate-driven PID"},
+    {"propV",   NEED_ARG,   NULL,   'p',    arg_double, APTR(&G.gainV.P),       "P-coefficient of velocity-driven PID"},
+    {"integV",  NEED_ARG,   NULL,   'i',    arg_double, APTR(&G.gainV.I),       "I-coefficient of velocity-driven PID"},
+    {"diffV",   NEED_ARG,   NULL,   'd',    arg_double, APTR(&G.gainV.D),       "D-coefficient of velocity-driven PID"},
+    {"xstart",  NEED_ARG,   NULL,   '0',    arg_double, APTR(&G.startcoord),    "starting coordinate of target"},
+    {"error",   NEED_ARG,   NULL,   'E',    arg_double, APTR(&G.error),         "error range"},
+    // TODO: add parameters for limits setting
+    end_option
+};
+
+// calculate coordinate target for given time (starting from zero)
+static double target_coord(double t){
+    if(t > 20. && t < 30.) return 0.;
+    //double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5);
+    double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5);
+    return pos;
+}
+
+static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
+    double error = targcoord - p->coord, fe = fabs(error);
+    PIDController_t *pid = NULL;
+    switch(state){
+        case Slewing:
+            if(fe < MAX_POINTING_ERR){
+                pid_clear(pidC);
+                state = Pointing;
+                green("--> Pointing\n");
+                pid = pidC;
+            }else{
+                red("Slewing...\n");
+                return (error > 0.) ? limits.max.speed : -limits.max.speed;
+            }
+            break;
+        case Pointing:
+            if(fe < MAX_GUIDING_ERR){
+                pid_clear(pidV);
+                state = Guiding;
+                green("--> Guiding\n");
+                pid = pidV;
+            }else if(fe > MAX_POINTING_ERR){
+                red("--> Slewing\n");
+                state = Slewing;
+                return (error > 0.) ? limits.max.speed : -limits.max.speed;
+            } else pid = pidC;
+            break;
+        case Guiding:
+            pid= pidV;
+            if(fe > MAX_GUIDING_ERR){
+                red("--> Pointing\n");
+                state = Pointing;
+                pid_clear(pidC);
+                pid = pidC;
+            }else if(fe < G.minerr){
+                    green("At target\n");
+            }else printf("Current error: %g\n", fe);
+            break;
+    }
+    if(!pid){
+        WARNX("where is PID?"); return p->speed;
+    }
+    double tagspeed = pid_calculate(pid, error, dt);
+    if(state == Guiding) return p->speed + tagspeed;
+    return tagspeed;
+}
+// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
+// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
+
+static void start_model(double Tend){
+    double T = 0., Tcorr = 0.;
+    moveparam_t target;
+    uint64_t N = 0;
+    double errmax = 0., errsum = 0., errsum2 = 0.;
+    while(T <= Tend){
+        moveparam_t p;
+        movestate_t st = model->get_state(&p);
+        if(st == ST_MOVE) st = model->proc_move(&p, T);
+        double nextcoord = target_coord(T);
+        double error = nextcoord - p.coord;
+        if(state == Guiding){
+            double ae = fabs(error);
+            if(ae > errmax) errmax = ae;
+            errsum += error; errsum2 += error * error;
+            ++N;
+        }
+        if(T - Tcorr >= G.dTcorr){ // check correction
+            double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
+            target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
+            target.speed = fabs(speed);
+            double res_speed = limits.max.speed / 2.;
+            if(target.speed > limits.max.speed){
+                target.speed = limits.max.speed;
+                res_speed = limits.max.speed / 4.;
+            }else if(target.speed < limits.min.speed){
+                target.speed = limits.min.speed;
+                res_speed = limits.min.speed * 4.;
+            }
+            if(!move_to(&target, T)){
+                target.speed = res_speed;
+                if(!move_to(&target, T))
+                    WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed);
+            }
+            DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error);
+            Tcorr = T;
+        }
+        // make log
+        fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
+                T, nextcoord, p.coord, p.speed, p.accel, error);
+        T += G.dTmon;
+    }
+    printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n");
+    double mean = errsum / (double)N;
+    printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean));
+}
+
+int main(int argc, char **argv){
+    sl_init();
+    sl_parseargs(&argc, &argv, opts);
+    if(G.help) sl_showhelp(-1, opts);
+    if(G.xlog){
+        coordslog = fopen(G.xlog, "w");
+        if(!coordslog) ERR("Can't open %s", G.xlog);
+    } else coordslog = stdout;
+    if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
+    if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
+    if(G.Tend <= 0.) ERRX("tend should be > 0.");
+    pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
+    pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
+    if(!pidC || !pidV) ERRX("Can't init PID regulators");
+    model = init_moving(&limits);
+    if(!model) ERRX("Can't init moving model: check parameters");
+    fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
+    start_model(G.Tend);
+    pid_delete(&pidC);
+    pid_delete(&pidV);
+    fclose(coordslog);
+    return 0;
+}

Auxiliary_utils/LibSidServo/PID_test/moving.c

@@ -0,0 +1,89 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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 <stdio.h>
+#include <usefull_macros.h>
+#include <pthread.h>
+#include <time.h>
+
+#include "moving.h"
+#include "moving_private.h"
+#include "Tramp.h"
+
+static movemodel_t *model = &trapez;
+double coord_tolerance = COORD_TOLERANCE_DEFAULT;
+double time_tick = TIME_TICK_DEFAULT;
+
+// difference of time from first call, using nanoseconds
+double nanot(){
+    static struct timespec *start = NULL;
+    struct timespec now;
+    if(!start){
+        start = MALLOC(struct timespec, 1);
+        if(!start) return -1.;
+        if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
+    }
+    if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
+    //DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
+    double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
+    double sd = (double)now.tv_sec - (double)start->tv_sec;
+    return sd + nd;
+}
+
+static void chkminmax(double *min, double *max){
+    if(*min <= *max) return;
+    double t = *min;
+    *min = *max;
+    *max = t;
+}
+
+movemodel_t *init_moving(limits_t *l){
+    if(!l) return FALSE;
+    if(!model->init_limits) return NULL;
+    moveparam_t *max = &l->max, *min = &l->min;
+    if(min->speed < 0.) min->speed = -min->speed;
+    if(max->speed < 0.) max->speed = -max->speed;
+    if(min->accel < 0.) min->accel = -min->accel;
+    if(max->accel < 0.) max->accel = -max->accel;
+    chkminmax(&min->coord, &max->coord);
+    chkminmax(&min->speed, &max->speed);
+    chkminmax(&min->accel, &max->accel);
+    if(!model->init_limits(l)) return NULL;
+    return model;
+}
+
+int move_to(moveparam_t *target, double t){
+    if(!target || !model) return FALSE;
+    DBG("MOVE to %g at speed %g", target->coord, target->speed);
+    // only positive velocity
+    if(target->speed < 0.) target->speed = -target->speed;
+    // don't mind about acceleration - user cannot set it now
+    return model->calculate(target, t);
+}
+
+int init_coordtol(double tolerance){
+    if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
+    coord_tolerance = tolerance;
+    return TRUE;
+}
+int init_timetick(double tick){
+    if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
+    time_tick = tick;
+    return TRUE;
+}

Auxiliary_utils/LibSidServo/PID_test/moving.h

@@ -0,0 +1,62 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+// tolerance, time ticks
+#define COORD_TOLERANCE_DEFAULT (0.01)
+#define COORD_TOLERANCE_MIN     (0.0001)
+#define COORD_TOLERANCE_MAX     (10.)
+#define TIME_TICK_DEFAULT       (0.0001)
+#define TIME_TICK_MIN           (1e-9)
+#define TIME_TICK_MAX           (10.)
+
+typedef enum{
+    ST_STOP,            // stopped
+    ST_MOVE,            // moving
+    ST_AMOUNT
+} movestate_t;
+
+typedef struct{ // all values could be both as positive and negative
+    double coord;
+    double speed;
+    double accel;
+} moveparam_t;
+
+typedef struct{
+    moveparam_t min;
+    moveparam_t max;
+} limits_t;
+
+typedef struct{
+    int (*init_limits)(limits_t *lim);                      // init values of limits, jerk
+    int (*calculate)(moveparam_t *target, double t);        // calculate stages of traectory beginning from t
+    movestate_t (*proc_move)(moveparam_t *next, double t);  // calculate next model point for time t
+    movestate_t (*get_state)(moveparam_t *cur);             // get current moving state
+    void (*stop)(double t);                                 // stop by ramp
+    void (*emergency_stop)(double t);                       // stop with highest acceleration
+    double (*stoppedtime)();                                // time when moving will ends
+} movemodel_t;
+
+extern double coord_tolerance;
+
+double nanot();
+movemodel_t *init_moving(limits_t *l);
+int init_coordtol(double tolerance);
+int init_timetick(double tick);
+int move_to(moveparam_t *target, double t);

Auxiliary_utils/LibSidServo/PID_test/moving_model.cflags

@@ -0,0 +1 @@
+-std=c17

Auxiliary_utils/LibSidServo/PID_test/moving_model.config

@@ -0,0 +1,4 @@
+// Add predefined macros for your project here. For example:
+// #define THE_ANSWER 42
+#define _XOPEN_SOURCE 666
+#define EBUG

Auxiliary_utils/LibSidServo/PID_test/moving_model.creator

@@ -0,0 +1 @@
+[General]

Auxiliary_utils/LibSidServo/PID_test/moving_model.creator.user

@@ -0,0 +1,221 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
+<qtcreator>
+ <data>
+  <variable>EnvironmentId</variable>
+  <value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.ActiveTarget</variable>
+  <value type="qlonglong">0</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.EditorSettings</variable>
+  <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoDetect">true</value>
+   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+   <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+    <value type="QString" key="language">Cpp</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+    </valuemap>
+   </valuemap>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+    <value type="QString" key="language">QmlJS</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+    </valuemap>
+   </valuemap>
+   <value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
+   <value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
+   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+   <value type="int" key="EditorConfiguration.IndentSize">4</value>
+   <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+   <value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
+   <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+   <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+   <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.ShowMargin">false</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>
+   <value type="int" key="EditorConfiguration.TabSize">8</value>
+   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+   <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.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.skipTrailingWhitespace">true</value>
+   <value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.PluginSettings</variable>
+  <valuemap type="QVariantMap">
+   <valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
+    <value type="bool" key="AutoTest.Framework.Boost">true</value>
+    <value type="bool" key="AutoTest.Framework.CTest">false</value>
+    <value type="bool" key="AutoTest.Framework.Catch">true</value>
+    <value type="bool" key="AutoTest.Framework.GTest">true</value>
+    <value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
+    <value type="bool" key="AutoTest.Framework.QtTest">true</value>
+   </valuemap>
+   <value type="bool" key="AutoTest.ApplyFilter">false</value>
+   <valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
+   <valuelist type="QVariantList" key="AutoTest.PathFilters"/>
+   <value type="int" key="AutoTest.RunAfterBuild">0</value>
+   <value type="bool" key="AutoTest.UseGlobal">true</value>
+   <valuemap type="QVariantMap" key="ClangTools">
+    <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">1</value>
+    <value type="bool" key="ClangTools.PreferConfigFile">true</value>
+    <valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
+    <valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
+    <valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
+    <value type="bool" key="ClangTools.UseGlobalSettings">true</value>
+   </valuemap>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Target.0</variable>
+  <valuemap type="QVariantMap">
+   <value type="QString" key="DeviceType">Desktop</value>
+   <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">{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">/home/eddy/C-files/mountcontrol.git/moving_model</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
+     </valuemap>
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
+       <value type="QString">clean</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
+     </valuemap>
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <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">По умолчанию</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>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+      <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
+     <value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+    </valuemap>
+    <value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+     <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+     <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+     <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
+     <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+     <value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
+     <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
+     <valuelist type="QVariantList" key="CustomOutputParsers"/>
+     <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+     <value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
+     <value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+     <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
+     <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+    </valuemap>
+    <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
+    <value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+    <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
+    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
+    <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
+    <valuelist type="QVariantList" key="CustomOutputParsers"/>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+    <value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
+    <value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+    <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
+    <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.TargetCount</variable>
+  <value type="qlonglong">1</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+  <value type="int">22</value>
+ </data>
+ <data>
+  <variable>Version</variable>
+  <value type="int">22</value>
+ </data>
+</qtcreator>

Auxiliary_utils/LibSidServo/PID_test/moving_model.creator.user.7bd84e3

@@ -0,0 +1,184 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
+<qtcreator>
+ <data>
+  <variable>EnvironmentId</variable>
+  <value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.ActiveTarget</variable>
+  <value type="qlonglong">0</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.EditorSettings</variable>
+  <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoDetect">true</value>
+   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+   <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+    <value type="QString" key="language">Cpp</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+    </valuemap>
+   </valuemap>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+    <value type="QString" key="language">QmlJS</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+    </valuemap>
+   </valuemap>
+   <value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
+   <value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
+   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+   <value type="int" key="EditorConfiguration.IndentSize">4</value>
+   <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+   <value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
+   <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+   <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+   <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.ShowMargin">false</value>
+   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</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>
+   <value type="int" key="EditorConfiguration.TabSize">8</value>
+   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+   <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.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.skipTrailingWhitespace">true</value>
+   <value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.PluginSettings</variable>
+  <valuemap type="QVariantMap">
+   <valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
+    <value type="bool" key="AutoTest.Framework.Boost">true</value>
+    <value type="bool" key="AutoTest.Framework.CTest">false</value>
+    <value type="bool" key="AutoTest.Framework.Catch">true</value>
+    <value type="bool" key="AutoTest.Framework.GTest">true</value>
+    <value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
+    <value type="bool" key="AutoTest.Framework.QtTest">true</value>
+   </valuemap>
+   <value type="bool" key="AutoTest.ApplyFilter">false</value>
+   <valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
+   <valuelist type="QVariantList" key="AutoTest.PathFilters"/>
+   <value type="int" key="AutoTest.RunAfterBuild">0</value>
+   <value type="bool" key="AutoTest.UseGlobal">true</value>
+   <valuemap type="QVariantMap" key="ClangTools">
+    <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="bool" key="ClangTools.PreferConfigFile">true</value>
+    <valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
+    <valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
+    <valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
+    <value type="bool" key="ClangTools.UseGlobalSettings">true</value>
+   </valuemap>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Target.0</variable>
+  <valuemap type="QVariantMap">
+   <value type="QString" key="DeviceType">Desktop</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="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">/Big/Data/00__Small_tel/moving_model</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
+     </valuemap>
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
+       <value type="QString">clean</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
+     </valuemap>
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <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.Id">GenericProjectManager.GenericBuildConfiguration</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
+    <value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+    <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
+    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
+    <valuelist type="QVariantList" key="CustomOutputParsers"/>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+    <value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
+    <value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+    <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
+    <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+   </valuemap>
+   <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.TargetCount</variable>
+  <value type="qlonglong">1</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+  <value type="int">22</value>
+ </data>
+ <data>
+  <variable>Version</variable>
+  <value type="int">22</value>
+ </data>
+</qtcreator>

Auxiliary_utils/LibSidServo/PID_test/moving_model.cxxflags

@@ -0,0 +1 @@
+-std=c++17

Auxiliary_utils/LibSidServo/PID_test/moving_model.files

@@ -0,0 +1,12 @@
+Dramp.c
+Dramp.h
+PID.c
+PID.h
+Sramp.c
+Sramp.h
+Tramp.c
+Tramp.h
+main.c
+moving.c
+moving.h
+moving_private.h

Auxiliary_utils/LibSidServo/PID_test/moving_private.h

@@ -0,0 +1,26 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include "moving.h"
+
+extern double coord_tolerance;
+extern double time_tick;
+
+

Auxiliary_utils/LibSidServo/PID_test/plot

@@ -0,0 +1,4 @@
+#!/usr/bin/gnuplot
+
+plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
+pause mouse

Auxiliary_utils/LibSidServo/PID_test/plot.cont

@@ -0,0 +1,8 @@
+#!/usr/bin/gnuplot
+
+#set term pdf
+#set output "output.pdf"
+while(1){
+    plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
+    pause 1
+}

Auxiliary_utils/LibSidServo/PID_test/plot_jpg

@@ -0,0 +1,6 @@
+#!/usr/bin/gnuplot
+
+set terminal jpeg size 1000,500
+set output "all.jpg"
+
+plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader

Auxiliary_utils/LibSidServo/PID_test/plot_pdf

@@ -0,0 +1,5 @@
+#!/usr/bin/gnuplot
+
+set term pdf
+set output "output.pdf"
+plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader

Auxiliary_utils/LibSidServo/PID_test/plotacc

@@ -0,0 +1,4 @@
+#!/usr/bin/gnuplot
+
+plot 'coords' using 1:5 with lines title columnheader
+pause mouse

Auxiliary_utils/LibSidServo/PID_test/ploterr

@@ -0,0 +1,4 @@
+#!/usr/bin/gnuplot
+
+plot 'coords' using 1:6 with lines title columnheader
+pause mouse

Auxiliary_utils/LibSidServo/PID_test/ploterr.cont

@@ -0,0 +1,6 @@
+#!/usr/bin/gnuplot
+
+while(1){
+    plot 'coords' using 1:6 with lines title columnheader
+    pause 1
+}

Auxiliary_utils/LibSidServo/PID_test/ploterr_jpg

@@ -0,0 +1,5 @@
+#!/usr/bin/gnuplot
+
+set term jpeg size 1000,500
+set output "error.jpg"
+plot 'coords' using 1:6 with lines title columnheader

Auxiliary_utils/LibSidServo/TODO

@@ -0,0 +1,3 @@
+fix encoders opening for several tries
+encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request)
+Read HW config even in model mode

Auxiliary_utils/LibSidServo/dbg.h

@@ -59,6 +59,6 @@ extern conf_t Conf;
         fprintf(stderr, "\n");} while(0)
 
 #else  // EBUG
-    #define FNAME()  do{}while(0)
-    #define DBG(...) do{}while(0)
+    #define FNAME()
+    #define DBG(...)
 #endif // EBUG

Auxiliary_utils/LibSidServo/examples/CMakeLists.txt

@@ -11,3 +11,4 @@ add_executable(dump_s dumpmoving_scmd.c dump.c conf.c)
 add_executable(dumpswing dumpswing.c dump.c conf.c)
 add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c)
 add_executable(SSIIconf SSIIconf.c conf.c)
+add_executable(slewNtrack dumpmoving_dragNtrack.c dump.c conf.c)

Auxiliary_utils/LibSidServo/examples/SSIIconf.c

@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <math.h>
 #include <stdio.h>
 #include <usefull_macros.h>
 
@@ -33,7 +34,9 @@ typedef struct{
 
 static hardware_configuration_t HW = {0};
 
-static parameters G = {0};
+static parameters G = {
+    .conffile = "servo.conf",
+};
 
 static sl_option_t cmdlnopts[] = {
     {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
@@ -50,6 +53,86 @@ static sl_option_t confopts[] = {
     end_option
 };
 
+static void dumpaxis(char axis, axis_config_t *c){
+#define STRUCTPAR(p)    (c)->p
+#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0)
+#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
+    DUMPD(accel);
+    DUMPD(backlash);
+    DUMPD(errlimit);
+    DUMP(propgain);
+    DUMP(intgain);
+    DUMP(derivgain);
+    DUMP(outplimit);
+    DUMP(currlimit);
+    DUMP(intlimit);
+    DUMP(motor_stepsperrev);
+    DUMP(axis_stepsperrev);
+#undef DUMP
+#undef DUMPD
+}
+
+static void dumpxbits(xbits_t *c){
+#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
+    DUMPBIT(motrev);
+    DUMPBIT(motpolarity);
+    DUMPBIT(encrev);
+    DUMPBIT(dragtrack);
+    DUMPBIT(trackplat);
+    DUMPBIT(handpaden);
+    DUMPBIT(newpad);
+    DUMPBIT(guidemode);
+#undef DUMPBIT
+}
+
+static void dumpybits(ybits_t *c){
+#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
+    DUMPBIT(motrev);
+    DUMPBIT(motpolarity);
+    DUMPBIT(encrev);
+    DUMPBIT(slewtrack);
+    DUMPBIT(digin_sens);
+    printf("Ydigin=%d\n", c->digin);
+#undef DUMPBIT
+}
+
+static void dumpHWconf(){
+#undef STRUCTPAR
+#define STRUCTPAR(p)    (HW).p
+#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
+#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
+#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
+#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
+    green("X axis configuration:\n");
+    dumpaxis('X', &HW.Xconf);
+    green("X bits:\n");
+    dumpxbits(&HW.xbits);
+    green("Y axis configuration:\n");
+    dumpaxis('Y', &HW.Yconf);
+    green("Y bits:\n");
+    dumpybits(&HW.ybits);
+    green("Other:\n");
+    printf("address=%d\n", HW.address);
+    DUMP(eqrate);
+    DUMP(eqadj);
+    DUMP(trackgoal);
+    DUMPD(latitude);
+    DUMPU32(Xsetpr);
+    DUMPU32(Ysetpr);
+    DUMPU32(Xmetpr);
+    DUMPU32(Ymetpr);
+    DUMPD(Xslewrate);
+    DUMPD(Yslewrate);
+    DUMPD(Xpanrate);
+    DUMPD(Ypanrate);
+    DUMPD(Xguiderate);
+    DUMPD(Yguiderate);
+    DUMPU32(baudrate);
+    DUMPD(locsdeg);
+    DUMPD(locsspeed);
+    DUMPD(backlspd);
+}
+
 int main(int argc, char** argv){
     sl_init();
     sl_parseargs(&argc, &argv, cmdlnopts);
@@ -64,13 +147,17 @@ int main(int argc, char** argv){
     }
     if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
     if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
+    /*
     char *c = sl_print_opts(confopts, TRUE);
     green("Got configuration:\n");
     printf("%s\n", c);
     FREE(c);
+    */
+    dumpHWconf();
+    /*
     if(G.hwconffile && G.writeconf){
         ;
-    }
+    }*/
     Mount.quit();
     return 0;
 }

Auxiliary_utils/LibSidServo/examples/conf.c

@@ -24,10 +24,32 @@
 static conf_t Config = {
     .MountDevPath = "/dev/ttyUSB0",
     .MountDevSpeed = 19200,
-    .EncoderDevPath = "/dev/ttyUSB1",
+    .EncoderXDevPath = "/dev/encoder_X0",
+    .EncoderYDevPath = "/dev/encoder_Y0",
     .EncoderDevSpeed = 153000,
     .MountReqInterval = 0.1,
-    .SepEncoder = 1
+    .EncoderReqInterval = 0.001,
+    .SepEncoder = 2,
+    .EncoderSpeedInterval = 0.05,
+    .EncodersDisagreement = 1e-5, // 2''
+    .PIDMaxDt = 1.,
+    .PIDRefreshDt = 0.1,
+    .PIDCycleDt = 5.,
+    .XPIDC.P = 0.5,
+    .XPIDC.I = 0.1,
+    .XPIDC.D = 0.2,
+    .XPIDV.P = 0.09,
+    .XPIDV.I = 0.0,
+    .XPIDV.D = 0.05,
+    .YPIDC.P = 0.5,
+    .YPIDC.I = 0.1,
+    .YPIDC.D = 0.2,
+    .YPIDV.P = 0.09,
+    .YPIDV.I = 0.0,
+    .YPIDV.D = 0.05,
+    .MaxPointingErr = 0.13962634,
+    .MaxFinePointingErr = 0.026179939,
+    .MaxGuidingErr = 4.8481368e-7,
 };
 
 static sl_option_t opts[] = {
@@ -35,8 +57,35 @@ static sl_option_t opts[] = {
     {"MountDevSpeed",   NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.MountDevSpeed),    "serial speed of mount device"},
     {"EncoderDevPath",  NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderDevPath),   "path to encoder device"},
     {"EncoderDevSpeed", NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.EncoderDevSpeed),  "serial speed of encoder device"},
+    {"SepEncoder",      NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device (1 - one device, 2 - two devices)"},
+    {"EncoderXDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderXDevPath),  "path to X encoder (/dev/encoderX0)"},
+    {"EncoderYDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderYDevPath),  "path to Y encoder (/dev/encoderY0)"},
+    {"EncodersDisagreement", NEED_ARG,NULL, 0,  arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
     {"MountReqInterval",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
-    {"SepEncoder",      NO_ARGS,    NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device"},
+    {"EncoderReqInterval",NEED_ARG, NULL,   0,  arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
+    {"EncoderSpeedInterval", NEED_ARG,NULL, 0,  arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
+    {"RunModel",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.RunModel),         "instead of real hardware run emulation"},
+    {"PIDMaxDt",        NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.PIDMaxDt),         "maximal PID refresh time interval (if larger all old data will be cleared)"},
+    {"PIDRefreshDt",    NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.PIDRefreshDt),     "normal PID refresh interval by master process"},
+    {"PIDCycleDt",      NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.PIDCycleDt),       "PID I cycle time (analog of \"RC\" for PID on opamps)"},
+    {"XPIDCP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.P),          "P of X PID (coordinate driven)"},
+    {"XPIDCI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.I),          "I of X PID (coordinate driven)"},
+    {"XPIDCD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.D),          "D of X PID (coordinate driven)"},
+    {"YPIDCP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.P),          "P of Y PID (coordinate driven)"},
+    {"YPIDCI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.I),          "I of Y PID (coordinate driven)"},
+    {"YPIDCD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.D),          "D of Y PID (coordinate driven)"},
+    {"XPIDVP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.P),          "P of X PID (velocity driven)"},
+    {"XPIDVI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.I),          "I of X PID (velocity driven)"},
+    {"XPIDVD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.D),          "D of X PID (velocity driven)"},
+    {"YPIDVP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.P),          "P of Y PID (velocity driven)"},
+    {"YPIDVI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.I),          "I of Y PID (velocity driven)"},
+    {"YPIDVD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.D),          "D of Y PID (velocity driven)"},
+    {"MaxPointingErr",  NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MaxPointingErr),   "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
+    {"MaxFinePointingErr",NEED_ARG, NULL,   0,  arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
+    {"MaxGuidingErr",   NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MaxGuidingErr),    "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
+    {"XEncZero",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.XEncZero),         "X axis encoder approximate zero position"},
+    {"YEncZero",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.YEncZero),         "Y axis encoder approximate zero position"},
+    // {"",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.), ""},
     end_option
 };
 
@@ -61,5 +110,19 @@ void dumpConf(){
 }
 
 void confHelp(){
-    sl_showhelp(-1, opts);
+    sl_conf_showhelp(-1, opts);
+}
+
+const char* errcodes[MCC_E_AMOUNT] = {
+    [MCC_E_OK] = "OK",
+    [MCC_E_FATAL] = "Fatal error",
+    [MCC_E_BADFORMAT] = "Wrong data format",
+    [MCC_E_ENCODERDEV] = "Encoder error",
+    [MCC_E_MOUNTDEV] = "Mount error",
+    [MCC_E_FAILED] = "Failed to run"
+};
+// return string with error code
+const char *EcodeStr(mcc_errcodes_t e){
+    if(e >= MCC_E_AMOUNT) return "Wrong error code";
+    return errcodes[e];
 }

Auxiliary_utils/LibSidServo/examples/conf.h

@@ -25,3 +25,4 @@
 void confHelp();
 conf_t *readServoConf(const char *filename);
 void dumpConf();
+const char *EcodeStr(mcc_errcodes_t e);

Auxiliary_utils/LibSidServo/examples/dump.c

@@ -23,6 +23,51 @@
 #include "dump.h"
 #include "simpleconv.h"
 
+// starting dump time (to conform different logs)
+static struct timespec dumpT0 = {0};
+
+#if 0
+// amount of elements used for encoders' data filtering
+#define NFILT	(10)
+
+static double filterK[NFILT];
+static double lastvals[2][NFILT] = {0};
+static int need2buildFilter = 1;
+
+static void buildFilter(){
+    filterK[NFILT-1] = 1.;
+    double sum = 1.;
+    for(int i = NFILT-2; i > -1; --i){
+        filterK[i] = (filterK[i+1] + 1.) * 1.1;
+        sum += filterK[i];
+    }
+    for(int i = 0; i < NFILT; ++i) filterK[i] /= sum;
+}
+
+static double filter(double val, int idx){
+    if(need2buildFilter){
+        buildFilter();
+        need2buildFilter = 0;
+    }
+    static int ctr[2] = {0};
+    for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1];
+    lastvals[idx][0] = val;
+    double r = 0.;
+    if(ctr[idx] < NFILT){
+        ++ctr[idx];
+        return val;
+    }
+    for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i];
+    return r;
+}
+#endif
+
+// return starting time of dump
+void dumpt0(struct timespec *t){
+    if(t) *t = dumpT0;
+}
+
+
 /**
  * @brief logmnt - log mount data into file
  * @param fcoords - file to dump
@@ -31,18 +76,16 @@
 void logmnt(FILE *fcoords, mountdata_t *m){
     if(!fcoords) return;
     //DBG("LOG %s", m ? "data" : "header");
-    static double t0 = -1.;
     if(!m){ // write header
-        fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
+        fprintf(fcoords, "      time    Xmot(deg)   Ymot(deg) Xenc(deg)  Yenc(deg)   VX(d/s)    VY(d/s)     millis\n");
         return;
-    }
-    if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6;
-    double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6 - t0;
+    }else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
     // write data
-    fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
-            t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
-            RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
-            m->millis, m->temperature, m->voltage);
+    fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
+            Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
+            RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
+            RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
+            m->millis);
     fflush(fcoords);
 }
 
@@ -64,45 +107,48 @@ void dumpmoving(FILE *fcoords, double t, int N){
         WARNX("Can't get mount data");
         LOGWARN("Can't get mount data");
     }
-    uint32_t millis = mdata.encposition.msrtime.tv_usec;
+    uint32_t mdmillis = mdata.millis;
+    struct timespec encXt = mdata.encXposition.t;
     int ctr = -1;
-    double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
-    double t0 = sl_dtime();
-    //DBG("millis = %u", millis);
-    while(sl_dtime() - t0 < t && ctr < N){
+    double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
+    double t0 = Mount.timeFromStart();
+    while(Mount.timeFromStart() - t0 < t && ctr < N){
         usleep(1000);
         if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
-        if(mdata.encposition.msrtime.tv_usec == millis) continue;
-        //DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
-        millis = mdata.encposition.msrtime.tv_usec;
+        //double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
+        struct timespec msrt = mdata.encXposition.t;
+        if(msrt.tv_nsec == encXt.tv_nsec) continue;
+        encXt = msrt;
         if(fcoords) logmnt(fcoords, &mdata);
-        if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
-            xlast = mdata.motposition.X;
-            ylast = mdata.motposition.Y;
+        if(mdata.millis == mdmillis) continue;
+        //DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
+        mdmillis = mdata.millis;
+        if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
+            xlast = mdata.motXposition.val;
+            ylast = mdata.motYposition.val;
             ctr = 0;
         }else ++ctr;
     }
+    DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
 }
 
 /**
- * @brief waitmoving - wait until moving by both axes stops at least for N cycles
+ * @brief waitmoving - wait until moving by both axiss stops at least for N cycles
  * @param N - amount of stopped cycles
  */
 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.motposition.X != xlast || mdata.motposition.Y != ylast){
-            xlast = mdata.motposition.X;
-            ylast = mdata.motposition.Y;
-            ctr = 0;
-        }else ++ctr;
+        if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
+        else ++ctr;
     }
 }
 
@@ -112,7 +158,7 @@ void waitmoving(int N){
  * @param Y (o) - encoder position (or NULL)
  * @return FALSE if failed
  */
-int getPos(coords_t *mot, coords_t *enc){
+int getPos(coordval_pair_t *mot, coordval_pair_t *enc){
     mountdata_t mdata = {0};
     int errcnt = 0;
     do{
@@ -126,19 +172,25 @@ int getPos(coords_t *mot, coords_t *enc){
         WARNX("Can't read mount status");
         return FALSE;
     }
-    if(mot) *mot = mdata.motposition;
-    if(enc) *enc = mdata.encposition;
+    if(mot){
+        mot->X = mdata.motXposition;
+        mot->Y = mdata.motYposition;
+    }
+    if(enc){
+        enc->X = mdata.encXposition;
+        enc->Y = mdata.encYposition;
+    }
     return TRUE;
 }
 
 // check current position and go to 0 if non-zero
 void chk0(int ncycles){
-    coords_t M;
+    coordval_pair_t M;
     if(!getPos(&M, NULL)) signals(2);
-    if(M.X || M.Y){
+    if(M.X.val || M.Y.val){
         WARNX("Mount position isn't @ zero; moving");
-        double zero = 0.;
-        Mount.moveTo(&zero, &zero);
+        coordpair_t zero = {0., 0.};
+        Mount.moveTo(&zero);
         waitmoving(ncycles);
         green("Now mount @ zero\n");
     }

Auxiliary_utils/LibSidServo/examples/dump.h

@@ -25,5 +25,6 @@
 void logmnt(FILE *fcoords, mountdata_t *m);
 void dumpmoving(FILE *fcoords, double t, int N);
 void waitmoving(int N);
-int getPos(coords_t *mot, coords_t *enc);
+int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
 void chk0(int ncycles);
+void dumpt0(struct timespec *t);

Auxiliary_utils/LibSidServo/examples/dumpmoving.c

@@ -73,6 +73,7 @@ int main(int argc, char **argv){
     conf_t *Config = readServoConf(G.conffile);
     if(!Config){
         dumpConf();
+        confHelp();
         return 1;
     }
     if(G.coordsoutput){
@@ -85,18 +86,19 @@ int main(int argc, char **argv){
     LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed);
     LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed);
     if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices");
-    coords_t M;
+    coordval_pair_t M;
     if(!getPos(&M, NULL)) ERRX("Can't get current position");
     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
-    double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y;
-    if(MCC_E_OK != Mount.moveTo(&tagx, &tagy))
+    coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val};
+    if(MCC_E_OK != Mount.moveTo(&tag))
         ERRX("Can't move to 45, 45");
     dumpmoving(fcoords, 30., G.Ncycles);
-    Mount.moveTo(&M.X, &M.Y);
+    tag.X = M.X.val; tag.Y = M.Y.val;
+    Mount.moveTo(&tag);
     dumpmoving(fcoords, 30., G.Ncycles);
     signals(0);
     return 0;

Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c

@@ -0,0 +1,228 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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/>.
+ */
+
+// move telescope to target using short command and force it to track mode
+// also do some corrections while moving
+
+#include <math.h>
+#include <pthread.h>
+#include <signal.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <usefull_macros.h>
+
+#include "conf.h"
+#include "dump.h"
+#include "sidservo.h"
+#include "simpleconv.h"
+
+// Original XXI=6827
+// XXD=136546
+// XXB=4915666
+
+typedef struct{
+    int help;
+    int Ncycles;
+    double reqint;
+    char *coordsoutput;
+    char *conffile;
+} parameters;
+
+static parameters G = {
+    .Ncycles = 40,
+    .reqint = -1.,
+};
+
+static FILE *fcoords = NULL;
+
+static sl_option_t cmdlnopts[] = {
+    {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
+    {"ncycles",     NEED_ARG,   NULL,   'n',    arg_int,    APTR(&G.Ncycles),   "N cycles in stopped state (default: 40)"},
+    {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
+    {"reqinterval", NEED_ARG,   NULL,   'i',    arg_double, APTR(&G.reqint),    "mount requests interval (default: 0.1)"},
+    {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
+    end_option
+};
+
+static mcc_errcodes_t return2zero();
+
+void signals(int sig){
+    if(sig){
+        signal(sig, SIG_IGN);
+        DBG("Get signal %d, quit.\n", sig);
+    }
+    return2zero();
+    sleep(5);
+    Mount.quit();
+    exit(sig);
+}
+
+// dump thread
+static void *dumping(void _U_ *u){
+    dumpmoving(fcoords, 3600., G.Ncycles);
+    return NULL;
+}
+
+// return TRUE if motor position is reached +- 0.01 degrees
+#define XYcount (DEG2RAD(0.3))
+// tag in degrees!
+static int Wait(double tag, int isX){
+    mountdata_t mdata;
+    red("Wait for %g degrees\n", tag);
+    tag = DEG2RAD(tag);
+    int errcnt = 0;
+    uint32_t millis = 0;
+    double curpos = 0.;
+    double t0 = sl_dtime();
+    do{
+        if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
+        else{
+            errcnt = 0;
+            if(mdata.millis == millis) continue;
+            millis = mdata.millis;
+            if(isX) curpos = mdata.motXposition.val;
+            else curpos = mdata.motYposition.val;
+        }
+        double t = sl_dtime();
+        if(t - t0 > 1.){
+            t0 = t;
+            printf("\t\tCurrent MOT X/Y: %g / %g deg\n", RAD2DEG(mdata.motXposition.val),
+                    RAD2DEG(mdata.motYposition.val));
+        }
+    }while(fabs(curpos - tag) > XYcount && errcnt < 10);
+    if(errcnt >= 10){
+        WARNX("Too much errors");
+        return FALSE;
+    }
+    green("%s reached position %g degrees\n", (isX) ? "X" : "Y", RAD2DEG(tag));
+    fflush(stdout);
+    return TRUE;
+}
+
+// previous GOTO coords/speeds for `mkcorr`
+static coordpair_t lastTag = {0}, lastSpeed = {0};
+
+// slew to given position and start tracking
+// pos/speed in deg and deg/s
+static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){
+    short_command_t cmd = {0};
+    DBG("Try to move to (%g, %g) with speed (%g, %g)",
+        target->X, target->Y, speed->X, speed->Y);
+    cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
+    cmd.Xspeed = DEG2RAD(speed->X);
+    cmd.Yspeed = DEG2RAD(speed->Y);
+    lastTag = *target;
+    lastSpeed = *speed;
+    /*cmd.xychange = 1;
+    cmd.XBits = 108;
+    cmd.YBits = 28;*/
+    return Mount.shortCmd(&cmd);
+}
+
+static mcc_errcodes_t return2zero(){
+    short_command_t cmd = {0};
+    DBG("Try to move to zero");
+    cmd.Xmot = 0.; cmd.Ymot = 0.;
+    coordpair_t maxspd;
+    if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
+    cmd.Xspeed = maxspd.X;
+    cmd.Yspeed = maxspd.Y;
+    /*cmd.xychange = 1;
+    cmd.XBits = 100;
+    cmd.YBits = 20;*/
+    return Mount.shortCmd(&cmd);
+}
+
+static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
+    long_command_t cmd = {0};
+    cmd.Xspeed = DEG2RAD(lastSpeed.X);
+    cmd.Yspeed = DEG2RAD(lastSpeed.Y);
+    cmd.Xmot = DEG2RAD(lastTag.X);
+    cmd.Ymot = DEG2RAD(lastTag.Y);
+    cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
+    cmd.Xatime = time->X; cmd.Yatime = time->Y;
+    return Mount.longCmd(&cmd);
+}
+
+int main(int argc, char **argv){
+    sl_init();
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(G.help) sl_showhelp(-1, cmdlnopts);
+    if(G.coordsoutput){
+        if(!(fcoords = fopen(G.coordsoutput, "w")))
+            ERRX("Can't open %s", G.coordsoutput);
+    }else fcoords = stdout;
+    conf_t *Config = readServoConf(G.conffile);
+    if(!Config){
+        dumpConf();
+        return 1;
+    }
+    if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
+    if(MCC_E_OK != Mount.init(Config)){
+        WARNX("Can't init devices");
+        return 1;
+    }
+    //if(!getPos(&M, NULL)) ERRX("Can't get current position");
+    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
+    // move to X=40 degr with different speeds
+    pthread_t dthr;
+    logmnt(fcoords, NULL);
+    if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
+    // move to 10/10
+    coordpair_t coords, speeds, adders, tadd;
+    coords = (coordpair_t){.X = 10., .Y = 20.};
+    speeds = (coordpair_t){.X = 1., .Y = 2.};
+    adders = (coordpair_t){.X = 0.01, .Y = 0.01};
+    tadd = (coordpair_t){.X = 1., .Y = 2.};
+    green("Goto\n");
+    if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
+    DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
+    green("Waiting X==4\n");
+    Wait(4., 1);
+    // now we are at point by Y but still moving by X; make small correction by X/Y into '+'
+    green("Mkcorr 1\n");
+    if(MCC_E_OK != mkcorr(&adders, &tadd)) ERRX("Can't make corr");
+    green("Waiting X==6\n");
+    Wait(6., 1);
+    green("Goto more\n");
+    coords = (coordpair_t){.X = 20., .Y = 30.};
+    if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
+    DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
+    green("Waiting Y==14\n");
+    Wait(14., 0);
+    // now we are @ point, make the same small correction again
+    green("Mkcorr 2\n");
+    if(MCC_E_OK != mkcorr(&coords, &speeds)) ERRX("Can't make corr");
+    // wait for 5 seconds
+    green("Wait for 5 seconds\n");
+    sleep(5);
+    // return to zero and wait
+    green("Return 2 zero and wait\n");
+    if(MCC_E_OK != return2zero()) ERRX("Can't return");
+    Wait(0., 0);
+    Wait(0., 1);
+    // wait moving ends
+    pthread_join(dthr, NULL);
+    signals(0);
+    return 0;
+}

Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c

@@ -49,7 +49,7 @@ static parameters G = {
 
 static FILE *fcoords = NULL;
 
-static coords_t M;
+static coordval_pair_t M;
 
 static sl_option_t cmdlnopts[] = {
     {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
@@ -77,8 +77,8 @@ static void *dumping(void _U_ *u){
     return NULL;
 }
 
-// return TRUE if motor position is reached +- 0.1 degrees
-#define XYcount (DEG2RAD(0.1))
+// return TRUE if motor position is reached +- 0.01 degrees
+#define XYcount (DEG2RAD(0.01))
 static int Wait(double tag){
     mountdata_t mdata;
     red("Wait for %g degrees\n", RAD2DEG(tag));
@@ -92,8 +92,8 @@ static int Wait(double tag){
             errcnt = 0;
             if(mdata.millis == millis) continue;
             millis = mdata.millis;
-            if(*G.axis == 'X') curpos = mdata.motposition.X;
-            else curpos = mdata.motposition.Y;
+            if(*G.axis == 'X') curpos = mdata.motXposition.val;
+            else curpos = mdata.motYposition.val;
             if(sign == 0.) sign = (curpos > tag) ? 1. : -1.;
             //printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos),
             //       RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.);
@@ -104,27 +104,26 @@ static int Wait(double tag){
         return FALSE;
     }
     green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
+    fflush(stdout);
     return TRUE;
 }
 
 // move X/Y to 40 degr with given speed until given coord
 static void move(double target, double limit, double speed){
-#define SCMD()   do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
     green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
     short_command_t cmd = {0};
     if(*G.axis == 'X' || *G.axis == 'B'){
-        cmd.Xmot = DEG2RAD(target) + M.X;
+        cmd.Xmot = DEG2RAD(target) + M.X.val;
         cmd.Xspeed = DEG2RAD(speed);
-        limit = DEG2RAD(limit) + M.X;
+        limit = DEG2RAD(limit) + M.X.val;
     }
     if(*G.axis == 'Y' || *G.axis == 'B'){
-        cmd.Ymot = DEG2RAD(target) + M.Y;
+        cmd.Ymot = DEG2RAD(target) + M.Y.val;
         cmd.Yspeed = DEG2RAD(speed);
-        limit = DEG2RAD(limit) + M.Y;
+        if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val;
     }
-    SCMD();
+    if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
     if(!Wait(limit)) signals(9);
-#undef SCMD
 }
 
 
@@ -160,21 +159,19 @@ int main(int argc, char **argv){
     pthread_t dthr;
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
-    // goto 1 degr with 1'/s
-    move(10., 1., 1./60.);
-    // goto 2 degr with 2'/s
-    move(10., 2., 2./60.);
-    // goto 3 degr with 5'/s
-    move(10., 3., 5./60.);
-    // goto 4 degr with 10'/s
-    move(10., 4., 10./60.);
-    // and go back with 5deg/s
-    move(0., 0., 5.);
-    // be sure to move @ 0,0
-    Mount.moveTo(&M.X, &M.Y);
+    // goto 30' with 5'/s
+    move(10., 30./60., 5./60.);
+    // goto 1' with 10'/s
+    move(10., 1., 10./60.);
+    // goto 3degr with 15'/s
+    move(10., 3., 15./60.);
+    // and go back with 7deg/s
+    move(0., 0., 7.);
+    // be sure to move @ starting position
+    coordpair_t tag = {.X = M.X.val, .Y = M.Y.val};
+    Mount.moveTo(&tag);
     // wait moving ends
     pthread_join(dthr, NULL);
-#undef SCMD
     signals(0);
     return 0;
 }

Auxiliary_utils/LibSidServo/examples/dumpswing.c

@@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = {
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
     {"axis",        NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.axis),      "axis to move (X or Y)"},
     {"period",      NEED_ARG,   NULL,   'p',    arg_double, APTR(&G.period),    "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
-    {"amplitude",   NEED_ARG,   NULL,   'A',    arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
+    {"amplitude",   NEED_ARG,   NULL,   'A',    arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
     {"nswings",     NEED_ARG,   NULL,   'N',    arg_int,    APTR(&G.Nswings),   "amount of swing periods (default: 10)"},
     {"conffile",    NEED_ARG,   NULL,   'C',    arg_int,    APTR(&G.conffile),  "configuration file name"},
     end_option
@@ -83,18 +83,18 @@ void waithalf(double t){
     uint32_t millis = 0;
     double xlast = 0., ylast = 0.;
     while(ctr < 5){
-        if(sl_dtime() >= t) return;
+        if(Mount.timeFromStart() >= t) return;
         usleep(1000);
         if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
         if(mdata.millis == millis) continue;
         millis = mdata.millis;
-        if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
-            DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
-            xlast = mdata.motposition.X;
-            ylast = mdata.motposition.Y;
+        if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
+            //DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
+            xlast = mdata.motXposition.val;
+            ylast = mdata.motYposition.val;
             ctr = 0;
         }else{
-            DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
+            //DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
             ++ctr;
         }
     }
@@ -110,15 +110,28 @@ int main(int argc, char **argv){
         return 1;
     }
     if(G.coordsoutput){
-        if(!(fcoords = fopen(G.coordsoutput, "w")))
-            ERRX("Can't open %s", G.coordsoutput);
+        if(!(fcoords = fopen(G.coordsoutput, "w"))){
+            WARNX("Can't open %s", G.coordsoutput);
+            return 1;
+        }
     }else fcoords = stdout;
-    if(G.Ncycles < 7) ERRX("Ncycles should be >7");
-    if(G.amplitude < 0.01 || G.amplitude > 45.)
-        ERRX("Amplitude should be from 0.01 to 45 degrees");
-    if(G.period < 0.1 || G.period > 900.)
-        ERRX("Period should be from 0.1 to 900s");
-    if(G.Nswings < 1) ERRX("Nswings should be more than 0");
+    if(G.Ncycles < 2){
+        WARNX("Ncycles should be >2");
+        return 1;
+    }
+    double absamp = fabs(G.amplitude);
+    if(absamp < 0.01 || absamp > 45.){
+        WARNX("Amplitude should be from 0.01 to 45 degrees");
+        return 1;
+    }
+    if(G.period < 0.1 || G.period > 900.){
+        WARNX("Period should be from 0.1 to 900s");
+        return 1;
+    }
+    if(G.Nswings < 1){
+        WARNX("Nswings should be more than 0");
+        return 1;
+    }
     conf_t *Config = readServoConf(G.conffile);
     if(!Config){
         dumpConf();
@@ -145,25 +158,30 @@ int main(int argc, char **argv){
     }else{
         tagX = 0.; tagY = DEG2RAD(G.amplitude);
     }
-    double t = sl_dtime(), t0 = t;
-    double divide = 2., rtagX = -tagX, rtagY = -tagY;
+    double t = Mount.timeFromStart(), t0 = t;
+    coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
+    double divide = 2.;
     for(int i = 0; i < G.Nswings; ++i){
-        Mount.moveTo(&tagX, &tagY);
-        DBG("CMD: %g", sl_dtime()-t0);
+        Mount.moveTo(&tag);
+        DBG("CMD: %g", Mount.timeFromStart()-t0);
         t += G.period / divide;
         divide = 1.;
         waithalf(t);
         DBG("Moved to +, t=%g", t-t0);
-        DBG("CMD: %g", sl_dtime()-t0);
-        Mount.moveTo(&rtagX, &rtagY);
+        DBG("CMD: %g", Mount.timeFromStart()-t0);
+        Mount.moveTo(&rtag);
         t += G.period;
         waithalf(t);
         DBG("Moved to -, t=%g", t-t0);
-        DBG("CMD: %g", sl_dtime()-t0);
+        DBG("CMD: %g", Mount.timeFromStart()-t0);
     }
-    double zero = 0.;
+    green("Move to zero @ %g\n", Mount.timeFromStart());
+    tag = (coordpair_t){0};
     // be sure to move @ 0,0
-    Mount.moveTo(&zero, &zero);
+    if(MCC_E_OK != Mount.moveTo(&tag)){
+        Mount.emergStop();
+        Mount.moveTo(&tag);
+    }
     // wait moving ends
     pthread_join(dthr, NULL);
 #undef SCMD

Auxiliary_utils/LibSidServo/examples/goto.c

@@ -40,7 +40,7 @@ typedef struct{
 } parameters;
 
 static parameters G = {
-    .Ncycles = 40,
+    .Ncycles = 10,
     .X = NAN,
     .Y = NAN,
 };
@@ -61,12 +61,13 @@ static FILE* fcoords = NULL;
 static pthread_t dthr;
 
 void signals(int sig){
-    pthread_cancel(dthr);
     if(sig){
         signal(sig, SIG_IGN);
         DBG("Get signal %d, quit.\n", sig);
     }
+    DBG("Quit");
     Mount.quit();
+    DBG("close");
     if(fcoords) fclose(fcoords);
     exit(sig);
 }
@@ -88,46 +89,56 @@ int main(int _U_ argc, char _U_ **argv){
         return 1;
     }
     if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
-    coords_t M;
-    if(!getPos(&M, NULL)) ERRX("Can't get current position");
+    coordval_pair_t M, E;
+    if(!getPos(&M, &E)) ERRX("Can't get current position");
+    printf("Current time: %.10f\n", Mount.timeFromStart());
     if(G.coordsoutput){
-        if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
+        if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
         G.wait = 1;
-    }
-    if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
         logmnt(fcoords, NULL);
         if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
     }
-    printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
+    M.X.val = RAD2DEG(M.X.val);
+    M.Y.val = RAD2DEG(M.Y.val);
+    printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val,
+           RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
     if(isnan(G.X) && isnan(G.Y)) goto out;
-    double *xtag = NULL, *ytag = NULL, xr, yr;
-    if(!isnan(G.X)){
-        xr = DEG2RAD(G.X);
-        if(G.relative) xr += M.X;
-        xtag = &xr;
+    coordpair_t tag;
+    if(isnan(G.X)){
+        if(G.relative) G.X = 0.;
+        else G.X = M.X.val;
     }
-    if(!isnan(G.Y)){
-        yr = DEG2RAD(G.Y);
-        if(G.relative) yr += M.Y;
-        ytag = &yr;
+    if(isnan(G.Y)){
+        if(G.relative) G.Y = 0.;
+        else G.Y = M.Y.val;
+    }
+    if(G.relative){
+        G.X += M.X.val;
+        G.Y += M.Y.val;
+    }
+    printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
+    tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
+    mcc_errcodes_t e = Mount.moveTo(&tag);
+    if(MCC_E_OK != e){
+        WARNX("Cant go to given coordinates: %s\n", EcodeStr(e));
+        goto out;
     }
-    printf("Moving to ");
-    if(xtag) printf("X=%gdeg ", G.X);
-    if(ytag) printf("Y=%gdeg", G.Y);
-    printf("\n");
-    Mount.moveTo(xtag, ytag);
     if(G.wait){
         sleep(1);
         waitmoving(G.Ncycles);
         if(!getPos(&M, NULL)) WARNX("Can't get current position");
-        else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
+        else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
     }
 out:
+    DBG("JOIN");
     if(G.coordsoutput) pthread_join(dthr, NULL);
+    DBG("QUIT");
     if(G.wait){
-        if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
+        usleep(250000); // pause to refresh coordinates
+        if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val),
+            RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
         Mount.quit();
     }
     return 0;

Auxiliary_utils/LibSidServo/examples/scmd_traectory.c

@@ -30,6 +30,7 @@
 
 typedef struct{
     int help;
+    int dumpconf;
     int Ncycles;        // n cycles to wait stop
     double reqint;      // requests interval (seconds)
     double Xmax;        // maximal X to stop
@@ -38,11 +39,13 @@ typedef struct{
     double X0;          // starting point of traectory (-30..30 degr)
     double Y0;          // -//-
     char *coordsoutput; // dump file
+    char *errlog;       // log with position errors
     char *tfn;          // traectory function name
     char *conffile;
 } parameters;
 
-static FILE *fcoords = NULL;
+static conf_t *Config = NULL;
+static FILE *fcoords = NULL, *errlog = NULL;
 static pthread_t dthr;
 static parameters G = {
     .Ncycles = 40,
@@ -61,21 +64,24 @@ static sl_option_t cmdlnopts[] = {
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
     {"reqinterval", NEED_ARG,   NULL,   'i',    arg_double, APTR(&G.reqint),    "mount requests interval (default: 0.1 second)"},
     {"traectory",   NEED_ARG,   NULL,   't',    arg_string, APTR(&G.tfn),       "used traectory function (default: sincos)"},
-    {"xmax",        NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.Xmax),      "maximal X coordinate for traectory (default: 45 degrees)"},
-    {"ymax",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Ymax),      "maximal X coordinate for traectory (default: 45 degrees)"},
+    {"xmax",        NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.Xmax),      "maximal abs X coordinate for traectory (default: 45 degrees)"},
+    {"ymax",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Ymax),      "maximal abs Y coordinate for traectory (default: 45 degrees)"},
     {"tmax",        NEED_ARG,   NULL,   'T',    arg_double, APTR(&G.tmax),      "maximal duration time of emulation (default: 300 seconds)"},
     {"x0",          NEED_ARG,   NULL,   '0',    arg_double, APTR(&G.X0),        "starting X-coordinate of traectory (default: 10 degrees)"},
     {"y0",          NEED_ARG,   NULL,   '1',    arg_double, APTR(&G.Y0),        "starting Y-coordinate of traectory (default: 10 degrees)"},
     {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
+    {"errlog",      NEED_ARG,   NULL,   'e',    arg_string, APTR(&G.errlog),    "file with errors log"},
+    {"dumpconf",    NO_ARGS,    NULL,   'D',    arg_int,    APTR(&G.dumpconf),  "dump current configuration"},
     end_option
 };
 
 void signals(int sig){
-    pthread_cancel(dthr);
     if(sig){
         signal(sig, SIG_IGN);
         DBG("Get signal %d, quit.\n", sig);
     }
+    Mount.stop();
+    sleep(1);
     Mount.quit();
     if(fcoords) fclose(fcoords);
     exit(sig);
@@ -89,22 +95,40 @@ static void *dumping(void _U_ *u){
 // calculate
 static void runtraectory(traectory_fn tfn){
     if(!tfn) return;
-    coords_t telXY, traectXY;
-    double t0 = sl_dtime();
-    uint32_t susec_last = 0;
+    coordval_pair_t telXY;
+    coordval_pair_t target;
+    coordpair_t traectXY;
+    double tlast = 0., tstart = Mount.timeFromStart();
+    long tlastXnsec = 0, tlastYnsec = 0;
+    struct timespec tcur, t0 = {0};
+    dumpt0(&t0);
     while(1){
         if(!telpos(&telXY)){
             WARNX("No next telescope position");
             return;
         }
-        if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind
-        susec_last = telXY.msrtime.tv_usec;
-        double t = sl_dtime();
-        if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break;
+        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(!traectory_point(&traectXY, t)) break;
-        DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.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("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));
+        }
+        if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
+        while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
+        tlast = t;
     }
-    WARNX("No next traectory point");
+    WARNX("No next traectory point or emulation ends");
 }
 
 int main(int argc, char **argv){
@@ -117,12 +141,18 @@ int main(int argc, char **argv){
     G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
     if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
         ERRX("X0 and Y0 should be -30..30 degrees");
+    if(G.errlog){
+        if(!(errlog = fopen(G.errlog, "w")))
+            ERRX("Can't open error log %s", G.errlog);
+        else
+            fprintf(errlog, "#    time      Xerr''      Yerr''   // target - real\n");
+    }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
-    conf_t *Config = readServoConf(G.conffile);
-    if(!Config){
+    Config = readServoConf(G.conffile);
+    if(!Config || G.dumpconf){
         dumpConf();
         return 1;
     }
@@ -133,7 +163,7 @@ int main(int argc, char **argv){
         print_tr_names();
         return 1;
     }
-    coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
+    coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
     if(!init_traectory(tfn, &c)){
         ERRX("Can't init traectory");
         return 1;

Auxiliary_utils/LibSidServo/examples/servo.conf

@@ -1,8 +1,25 @@
+# Current configuration
 MountDevPath=/dev/ttyUSB0
 MountDevSpeed=19200
+EncoderDevPath=(null)
+EncoderDevSpeed=1000000
+MountReqInterval=0.1
+EncoderReqInterval=0.001
+SepEncoder=2
 EncoderXDevPath=/dev/encoder_X0
 EncoderYDevPath=/dev/encoder_Y0
-MountReqInterval=0.05
-SepEncoder=2
-EncoderReqInterval=0.01
-EncoderDevSpeed=1000000
+EncoderSpeedInterval=0.05
+RunModel=1
+XPIDCP=0.8
+XPIDCI=0.0
+XPIDCD=0.0
+YPIDCP=0.5
+YPIDCI=0.0
+YPIDCD=0.0
+XPIDVP=0.2
+XPIDVI=0.1
+XPIDVD=0.0
+YPIDVP=0.2
+YPIDVI=0.1
+YPIDVD=0.0
+

Auxiliary_utils/LibSidServo/examples/simpleconv.h

@@ -20,10 +20,10 @@
 
 #include <math.h>
 
-#define DEG2RAD(d)  (d/180.*M_PI)
-#define ASEC2RAD(d) (d/180.*M_PI/3600.)
-#define AMIN2RAD(d) (d/180.*M_PI/60.)
-#define RAD2DEG(r)  (r/M_PI*180.)
-#define RAD2ASEC(r) (r/M_PI*180.*3600.)
-#define RAD2AMIN(r) (r/M_PI*180.*60.)
+#define DEG2RAD(d)  ((d)/180.*M_PI)
+#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
+#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
+#define RAD2DEG(r)  ((r)/M_PI*180.)
+#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
+#define RAD2AMIN(r) ((r)/M_PI*180.*60.)
 

Auxiliary_utils/LibSidServo/examples/traectories.c

@@ -28,12 +28,8 @@
 
 static traectory_fn cur_traectory = NULL;
 // starting point of traectory
-static coords_t XYstart = {0};
+static coordpair_t XYstart = {0};
 static double tstart = 0.;
-// convert Xe/Ye to approximate motor coordinates:
-// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
-// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
-static coords_t XYcor = {0};
 
 /**
  * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@@ -41,20 +37,17 @@ static coords_t XYcor = {0};
  * @param XY0 - starting point
  * @return FALSE if failed
  */
-int init_traectory(traectory_fn f, coords_t *XY0){
+int init_traectory(traectory_fn f, coordpair_t *XY0){
     if(!f || !XY0) return FALSE;
     cur_traectory = f;
     XYstart = *XY0;
-    tstart = sl_dtime();
+    tstart = Mount.timeFromStart();
     mountdata_t mdata;
     int ntries = 0;
     for(; ntries < 10; ++ntries){
         if(MCC_E_OK == Mount.getMountData(&mdata)) break;
     }
     if(ntries == 10) return FALSE;
-    XYcor.X = mdata.motposition.X - mdata.encposition.X;
-    XYcor.Y = mdata.motposition.X + mdata.encposition.Y;
-    DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y));
     return TRUE;
 }
 
@@ -64,12 +57,10 @@ int init_traectory(traectory_fn f, coords_t *XY0){
  * @param t - UNIX-time of event
  * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
  */
-int traectory_point(coords_t *nextpt, double t){
+int traectory_point(coordpair_t *nextpt, double t){
     if(t < 0. || !cur_traectory) return FALSE;
-    coords_t pt;
+    coordpair_t pt;
     if(!cur_traectory(&pt, t)) return FALSE;
-    pt.msrtime.tv_sec = floor(t);
-    pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec;
     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;
     return TRUE;
@@ -77,34 +68,36 @@ int traectory_point(coords_t *nextpt, double t){
 
 // current telescope position according to starting motor coordinates
 // @return FALSE if failed to get current coordinates
-int telpos(coords_t *curpos){
+int telpos(coordval_pair_t *curpos){
     mountdata_t mdata;
     int ntries = 0;
     for(; ntries < 10; ++ntries){
         if(MCC_E_OK == Mount.getMountData(&mdata)) break;
     }
     if(ntries == 10) return FALSE;
-    coords_t pt;
-    pt.X = XYcor.X + mdata.encposition.X;
-    pt.Y = XYcor.Y + mdata.encposition.Y;
-    pt.msrtime = mdata.encposition.msrtime;
+    coordval_pair_t pt;
+    //DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
+    pt.X.val = mdata.encXposition.val;
+    pt.Y.val = mdata.encYposition.val;
+    pt.X.t = mdata.encXposition.t;
+    pt.Y.t = mdata.encYposition.t;
     if(curpos) *curpos = pt;
     return TRUE;
 }
 
 // X=X0+1'/s, Y=Y0+15''/s
-int Linear(coords_t *nextpt, double t){
-    coords_t pt;
-    pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
+int Linear(coordpair_t *nextpt, double t){
+    coordpair_t pt;
+    pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
     pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
     if(nextpt) *nextpt = pt;
     return TRUE;
 }
 
 // X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
-int SinCos(coords_t *nextpt, double t){
-    coords_t pt;
-    pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
+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);
     if(nextpt) *nextpt = pt;
     return TRUE;
@@ -117,8 +110,8 @@ typedef struct{
 } tr_names;
 
 static tr_names names[] = {
-    {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
-    {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
+    {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)"},
     {NULL, NULL, NULL}
 };
 

Auxiliary_utils/LibSidServo/examples/traectories.h

@@ -21,12 +21,12 @@
 #include "sidservo.h"
 
 // traectory
-typedef int (*traectory_fn)(coords_t *, double);
+typedef int (*traectory_fn)(coordpair_t *, double);
 
-int init_traectory(traectory_fn f, coords_t *XY0);
+int init_traectory(traectory_fn f, coordpair_t *XY0);
 traectory_fn traectory_by_name(const char *name);
 void print_tr_names();
-int traectory_point(coords_t *nextpt, double t);
-int telpos(coords_t *curpos);
-int Linear(coords_t *nextpt, double t);
-int SinCos(coords_t *nextpt, double t);
+int traectory_point(coordpair_t *nextpt, double t);
+int telpos(coordval_pair_t *curpos);
+int Linear(coordpair_t *nextpt, double t);
+int SinCos(coordpair_t *nextpt, double t);

Auxiliary_utils/LibSidServo/libsidservo.config

@@ -1,6 +1,7 @@
 // Add predefined macros for your project here. For example:
 // #define THE_ANSWER 42
 #define EBUG
-#define _POSIX_C_SOURCE
+#define _POSIX_C_SOURCE  11111111
 #define PACKAGE_VERSION  "0.0.1"
-
+#define _XOPEN_SOURCE    666
+#define _DEFAULT_SOURCE

Auxiliary_utils/LibSidServo/libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.1, 2025-02-27T22:45:06. -->
+<!-- Written by QtCreator 18.0.1, 2026-01-26T22:24:32. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -13,8 +13,8 @@
  <data>
   <variable>ProjectExplorer.Project.EditorSettings</variable>
   <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoDetect">true</value>
    <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
-   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
    <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
    <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
     <value type="QString" key="language">Cpp</value>
@@ -75,8 +75,6 @@
    <valuelist type="QVariantList" key="AutoTest.PathFilters"/>
    <value type="int" key="AutoTest.RunAfterBuild">0</value>
    <value type="bool" key="AutoTest.UseGlobal">true</value>
-   <valuelist type="QVariantList" key="ClangCodeModel.CustomCommandLineKey"/>
-   <value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
    <valuemap type="QVariantMap" key="ClangTools">
     <value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
     <value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
@@ -88,12 +86,14 @@
     <valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
     <value type="bool" key="ClangTools.UseGlobalSettings">true</value>
    </valuemap>
+   <value type="int" key="RcSync">0</value>
   </valuemap>
  </data>
  <data>
   <variable>ProjectExplorer.Project.Target.0</variable>
   <valuemap type="QVariantMap">
    <value type="QString" key="DeviceType">Desktop</value>
+   <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>
@@ -101,7 +101,7 @@
    <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">/Big/Data/00__Small_tel/C-sources/erfa</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/tmp/robo5/mountcontrol.git/LibSidServo</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,8 +133,44 @@
     <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">По умолчанию</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</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>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+      <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
+     <value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+    </valuemap>
+    <value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+     <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+     <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+     <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
+     <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+     <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
+     <valuelist type="QVariantList" key="CustomOutputParsers"/>
+     <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+     <value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
+     <value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+     <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
+     <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+     <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>
    <value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
@@ -153,7 +189,9 @@
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
     <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
     <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
     <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
     <valuelist type="QVariantList" key="CustomOutputParsers"/>
     <value type="int" key="PE.EnvironmentAspect.Base">2</value>
     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
@@ -163,8 +201,10 @@
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
     <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
     <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+    <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>
@@ -173,10 +213,6 @@
   <variable>ProjectExplorer.Project.TargetCount</variable>
   <value type="qlonglong">1</value>
  </data>
- <data>
-  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
-  <value type="int">22</value>
- </data>
  <data>
   <variable>Version</variable>
   <value type="int">22</value>

Auxiliary_utils/LibSidServo/libsidservo.files

@@ -1,11 +1,13 @@
 CMakeLists.txt
-dbg.h
+PID.c
+PID.h
 examples/SSIIconf.c
 examples/conf.c
 examples/conf.h
 examples/dump.c
 examples/dump.h
 examples/dumpmoving.c
+examples/dumpmoving_dragNtrack.c
 examples/dumpmoving_scmd.c
 examples/dumpswing.c
 examples/goto.c
@@ -17,6 +19,12 @@ serial.c
 examples/CMakeLists.txt
 examples/traectories.c
 examples/traectories.h
+main.h
+movingmodel.c
+movingmodel.h
+polltest/main.c
+ramp.c
+ramp.h
 serial.h
 ssii.c
 ssii.h

Auxiliary_utils/LibSidServo/main.c

@@ -16,26 +16,169 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * main functions to fill struct `mount_t`
+ */
+
 #include <inttypes.h>
+#include <strings.h>
+#include <time.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
+#include "movingmodel.h"
 #include "serial.h"
 #include "ssii.h"
+#include "PID.h"
+
+// adder for monotonic time by realtime: inited any call of init()
+static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC
+    t0 = {0},           // curtime() for initstarttime() call
+    starttime = {0};    // starting time by monotonic (for timefromstart())
 
 conf_t Conf = {0};
+// parameters for model
+static movemodel_t *Xmodel, *Ymodel;
+// limits for model and/or real mount (in latter case data should be read from mount on init)
+// radians, rad/sec, rad/sec^2
+// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
+// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
+limits_t
+    Xlimits = {
+        .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
+    Ylimits = {
+        .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
+;
+static mcc_errcodes_t shortcmd(short_command_t *cmd);
+static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
+
+/**
+ * @brief curtime - monotonic time from first run
+ * @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
+ * @return TRUE if all OK
+ * FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
+ */
+int curtime(struct timespec *t){
+    struct timespec now;
+    if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
+    now.tv_sec += timeadder.tv_sec;
+    now.tv_nsec += timeadder.tv_nsec;
+    if(now.tv_nsec > 999999999L){
+        ++now.tv_sec;
+        now.tv_nsec -= 1000000000L;
+    }
+    if(t) *t = now;
+    return TRUE;
+}
+
+// init starttime; @return TRUE if all OK
+static int initstarttime(){
+    struct timespec start;
+    if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
+    if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
+    timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
+    timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
+    if(timeadder.tv_nsec < 0){
+        --timeadder.tv_sec;
+        timeadder.tv_nsec += 1000000000L;
+    }
+    curtime(&t0);
+    return TRUE;
+}
+
+// return difference (in seconds) between time1 and time0
+double timediff(const struct timespec *time1, const struct timespec *time0){
+    if(!time1 || !time0) return -1.;
+    return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
+}
+// difference between given time and  last initstarttime() call
+double timediff0(const struct timespec *time1){
+    return timediff(time1, &t0);
+}
+// time from last initstarttime() call
+double timefromstart(){
+    struct timespec now;
+    if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
+    return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
+}
 
 /**
  * @brief quit - close all opened and return to default state
+ * TODO: close serial devices even in "model" mode
  */
 static void quit(){
-    DBG("Close serial devices");
+    if(Conf.RunModel) return;
     for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
+    DBG("Close all serial devices");
     closeSerial();
     DBG("Exit");
 }
 
+void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
+    if(!c || !Xmodel || !Ymodel) return;
+    double tnow = timefromstart();
+    moveparam_t Xp, Yp;
+    movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
+    //DBG("Xstate = %d", Xst);
+    if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
+    movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
+    if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
+    c->X = Xp.coord;
+    c->Y = Yp.coord;
+    if(xst) *xst = Xst;
+    if(yst) *yst = Yst;
+}
+
+/**
+ * less square calculations of speed
+ */
+less_square_t *LS_init(size_t Ndata){
+    if(Ndata < 5){
+        DBG("Ndata=%zd - TOO SMALL", Ndata);
+        return NULL;
+    }
+    DBG("Init less squares: %zd", Ndata);
+    less_square_t *l = calloc(1, sizeof(less_square_t));
+    l->x = calloc(Ndata, sizeof(double));
+    l->t2 = calloc(Ndata, sizeof(double));
+    l->t = calloc(Ndata, sizeof(double));
+    l->xt = calloc(Ndata, sizeof(double));
+    l->arraysz = Ndata;
+    return l;
+}
+void LS_delete(less_square_t **l){
+    if(!l || !*l) return;
+    free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
+    free(*l);
+    *l = NULL;
+}
+// add next data portion and calculate current slope
+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];
+    double t2 = t * t, xt = x * t;
+    l->x[idx] = x; l->t2[idx] = t2;
+    l->t[idx] = t; l->xt[idx] = xt;
+    ++idx;
+    l->idx = (idx >= l->arraysz) ? 0 : idx;
+    l->xsum += x - oldx;
+    l->t2sum += t2 - oldt2;
+    l->tsum += t - oldt;
+    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;
+    // point: (sum_x  - slope * sum_t) / n;
+    return (numerator / denominator);
+}
+
 /**
  * @brief init - open serial devices and do other job
  * @param c - initial configuration
@@ -44,55 +187,90 @@ static void quit(){
 static mcc_errcodes_t init(conf_t *c){
     FNAME();
     if(!c) return MCC_E_BADFORMAT;
+    if(!initstarttime()) return MCC_E_FAILED;
     Conf = *c;
     mcc_errcodes_t ret = MCC_E_OK;
-    if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
-        DBG("Define mount device path and speed");
-        ret = MCC_E_BADFORMAT;
-    }else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){
-        DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
-        ret = MCC_E_MOUNTDEV;
-    }
-    if(Conf.SepEncoder){
-        if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){
-            DBG("Define encoder device path and speed");
-            ret = MCC_E_BADFORMAT;
-        }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){
-            DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed);
-            ret = MCC_E_ENCODERDEV;
-        }
-    }
+    Xmodel = model_init(&Xlimits);
+    Ymodel = model_init(&Ylimits);
     if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
         DBG("Bad value of MountReqInterval");
         ret = MCC_E_BADFORMAT;
     }
-    uint8_t buf[1024];
-    data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
-    // read input data as there may be some trash on start
-    if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
-    if(ret != MCC_E_OK) quit();
-    return ret;
+    if(Conf.RunModel){
+        if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
+        return MCC_E_OK;
+    }
+    if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
+        DBG("Define mount device path and speed");
+        ret = MCC_E_BADFORMAT;
+    }else if(!openMount()){
+        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;
+    // read HW config to update constants
+    hardware_configuration_t HW;
+    if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
+    // make a pause for actual encoder's values
+    double t0 = timefromstart();
+    while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
+    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);
+    return e;
 }
 
-// check coordinates and speeds; return FALSE if failed
+// check coordinates (rad) and speeds (rad/s); return FALSE if failed
 // TODO fix to real limits!!!
 static int chkX(double X){
-    if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
+    if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
     return TRUE;
 }
 static int chkY(double Y){
-    if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
+    if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
     return TRUE;
 }
 static int chkXs(double s){
-    if(s < 0. || s > X_SPEED_MAX) return FALSE;
+    if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
     return TRUE;
 }
 static int chkYs(double s){
-    if(s < 0. || s > Y_SPEED_MAX) return FALSE;
+    if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
     return TRUE;
 }
 
+// set SLEWING state if axis was stopped
+static void setslewingstate(){
+    //FNAME();
+    mountdata_t d;
+    if(MCC_E_OK == getMD(&d)){
+        axis_status_t newx = d.Xstate, newy = d.Ystate;
+        //DBG("old state: %d/%d", d.Xstate, d.Ystate);
+        if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
+        if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
+        if(newx != d.Xstate || newy != d.Ystate){
+            DBG("Started moving -> slew");
+            setStat(newx, newy);
+        }
+    }else DBG("CAN't GET MOUNT DATA!");
+}
 
 /**
  * @brief move2 - simple move to given point and stop
@@ -100,41 +278,36 @@ static int chkYs(double s){
  * @param Y - new Y coordinate (radians: -pi..pi) or NULL
  * @return error code
  */
-static mcc_errcodes_t move2(const double *X, const double *Y){
-    if(!X && !Y) return MCC_E_BADFORMAT;
-    if(X){
-        if(!chkX(*X)) return MCC_E_BADFORMAT;
-        int32_t tag = X_RAD2MOT(*X);
-        DBG("X: %g, tag: %d", *X, tag);
-        if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED;
-    }
-    if(Y){
-        if(!chkY(*Y)) return MCC_E_BADFORMAT;
-        int32_t tag = Y_RAD2MOT(*Y);
-        DBG("Y: %g, tag: %d", *Y, tag);
-        if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED;
-    }
-    return MCC_E_OK;
+static mcc_errcodes_t move2(const coordpair_t *target){
+    if(!target) return MCC_E_BADFORMAT;
+    if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
+    if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
+    short_command_t cmd = {0};
+    DBG("x,y: %g, %g", target->X, target->Y);
+    cmd.Xmot = target->X;
+    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);
 }
 
 /**
- * @brief setspeed - set maximal speed over axis
+ * @brief setspeed - set maximal speed over axis by text command
  * @param X (i) - max speed or NULL
  * @param Y (i) - -//-
  * @return errcode
  */
-static mcc_errcodes_t setspeed(const double *X, const double *Y){
-    if(!X && !Y) return MCC_E_BADFORMAT;
-    if(X){
-        if(!chkXs(*X)) return MCC_E_BADFORMAT;
-        int32_t spd = X_RS2MOTSPD(*X);
-        if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
-    }
-    if(Y){
-        if(!chkYs(*Y)) return MCC_E_BADFORMAT;
-        int32_t spd = Y_RS2MOTSPD(*Y);
-        if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
-    }
+static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
+    if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
+    int32_t spd = X_RS2MOTSPD(tagspeed->X);
+    if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
+    spd = Y_RS2MOTSPD(tagspeed->Y);
+    if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 
@@ -144,19 +317,20 @@ static mcc_errcodes_t setspeed(const double *X, const double *Y){
  * @param speed (i) - speed or NULL
  * @return
  */
-static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
-    if(!target && !speed) return MCC_E_BADFORMAT;
-    if(!target) return setspeed(&speed->X, &speed->Y);
-    if(!speed) return move2(&target->X, &target->Y);
-    if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y))
-        return MCC_E_BADFORMAT;
-    char buf[128];
-    int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X);
-    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTX, tag, CMD_MOTXYS, spd);
-    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
-    spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y);
-    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTY, tag, CMD_MOTXYS, spd);
-    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
+static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){
+    if(!target || !speed) return MCC_E_BADFORMAT;
+    if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
+    if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
+    // updateMotorPos() here can make a problem; TODO: remove?
+    if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
+    short_command_t cmd = {0};
+    cmd.Xmot = target->X;
+    cmd.Ymot = target->Y;
+    cmd.Xspeed = speed->X;
+    cmd.Yspeed = speed->Y;
+    mcc_errcodes_t r = shortcmd(&cmd);
+    if(r != MCC_E_OK) return r;
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -165,11 +339,25 @@ static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
+    FNAME();
+    if(Conf.RunModel){
+        double curt = timefromstart();
+        Xmodel->emergency_stop(Xmodel, curt);
+        Ymodel->emergency_stop(Ymodel, curt);
+        return MCC_E_OK;
+    }
     if(!SSstop(TRUE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 // normal stop
 static mcc_errcodes_t stop(){
+    FNAME();
+    if(Conf.RunModel){
+        double curt = timefromstart();
+        Xmodel->stop(Xmodel, curt);
+        Ymodel->stop(Ymodel,curt);
+        return MCC_E_OK;
+    }
     if(!SSstop(FALSE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
@@ -181,8 +369,18 @@ static mcc_errcodes_t stop(){
  */
 static mcc_errcodes_t shortcmd(short_command_t *cmd){
     if(!cmd) return MCC_E_BADFORMAT;
+    if(Conf.RunModel){
+        double curt = timefromstart();
+        moveparam_t param = {0};
+        param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
+        if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
+        param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
+        if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
+        setslewingstate();
+        return MCC_E_OK;
+    }
     SSscmd s = {0};
-    DBG("xmot=%g, ymot=%g", cmd->Xmot, cmd->Ymot);
+    DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
     s.Xmot = X_RAD2MOT(cmd->Xmot);
     s.Ymot = Y_RAD2MOT(cmd->Ymot);
     s.Xspeed = X_RS2MOTSPD(cmd->Xspeed);
@@ -192,23 +390,27 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
     s.YBits = cmd->YBits;
     DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
     if(!cmdS(&s)) return MCC_E_FAILED;
-    cmd->Xmot = X_MOT2RAD(s.Xmot);
-    cmd->Ymot = Y_MOT2RAD(s.Ymot);
-    cmd->Xspeed = X_MOTSPD2RS(s.Xspeed);
-    cmd->Yspeed = Y_MOTSPD2RS(s.Yspeed);
-    cmd->xychange = s.xychange;
-    cmd->XBits = s.XBits;
-    cmd->YBits = s.YBits;
+    setslewingstate();
     return MCC_E_OK;
 }
 
 /**
- * @brief shortcmd - send and receive long binary command
+ * @brief longcmd - send and receive long binary command
  * @param cmd (io) - command
  * @return errcode
  */
 static mcc_errcodes_t longcmd(long_command_t *cmd){
     if(!cmd) return MCC_E_BADFORMAT;
+    if(Conf.RunModel){
+        double curt = timefromstart();
+        moveparam_t param = {0};
+        param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
+        if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
+        param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
+        if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
+        setslewingstate();
+        return MCC_E_OK;
+    }
     SSlcmd l = {0};
     l.Xmot = X_RAD2MOT(cmd->Xmot);
     l.Ymot = Y_RAD2MOT(cmd->Ymot);
@@ -219,31 +421,175 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
     l.Xatime = S2ADDER(cmd->Xatime);
     l.Yatime = S2ADDER(cmd->Yatime);
     if(!cmdL(&l)) return MCC_E_FAILED;
-    cmd->Xmot = X_MOT2RAD(l.Xmot);
-    cmd->Ymot = Y_MOT2RAD(l.Ymot);
-    cmd->Xspeed = X_MOTSPD2RS(l.Xspeed);
-    cmd->Yspeed = Y_MOTSPD2RS(l.Yspeed);
-    cmd->Xadder = X_MOTSPD2RS(l.Xadder);
-    cmd->Yadder = Y_MOTSPD2RS(l.Yadder);
-    cmd->Xatime = ADDER2S(l.Xatime);
-    cmd->Yatime = ADDER2S(l.Yatime);
+    setslewingstate();
     return MCC_E_OK;
 }
 
-mcc_errcodes_t get_hwconf(hardware_configuration_t *c){
-    if(!c) return MCC_E_BADFORMAT;
-    SSconfig conf;
-    if(!cmdC(&conf, FALSE)) return MCC_E_FAILED;
-    // and bored transformations
-    DBG("Xacc=%u", conf.Xconf.accel);
-    DBG("Yacc=%u", conf.Yconf.accel);
-    c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel);
-    DBG("cacc: %g", c->Xconf.accel);
-    c->Xconf.backlash = conf.Xconf.backlash;
-    // ...
-    c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel);
-    c->Xconf.backlash = conf.Xconf.backlash;
-    // ...
+static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
+    if(!hwConfig) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
+    SSconfig config;
+    DBG("Read HW configuration");
+    if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
+    // Convert acceleration (ticks per loop^2 to rad/s^2)
+    hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
+    hwConfig->Yconf.accel = Y_MOTACC2RS(config.Yconf.accel);
+    // Convert backlash (ticks to radians)
+    hwConfig->Xconf.backlash = X_MOT2RAD(config.Xconf.backlash);
+    hwConfig->Yconf.backlash = Y_MOT2RAD(config.Yconf.backlash);
+    // Convert error limit (ticks to radians)
+    hwConfig->Xconf.errlimit = X_MOT2RAD(config.Xconf.errlimit);
+    hwConfig->Yconf.errlimit = Y_MOT2RAD(config.Yconf.errlimit);
+    // Proportional, integral, and derivative gains are unitless, so no conversion needed
+    hwConfig->Xconf.propgain = (double)config.Xconf.propgain;
+    hwConfig->Yconf.propgain = (double)config.Yconf.propgain;
+    hwConfig->Xconf.intgain = (double)config.Xconf.intgain;
+    hwConfig->Yconf.intgain = (double)config.Yconf.intgain;
+    hwConfig->Xconf.derivgain = (double)config.Xconf.derivgain;
+    hwConfig->Yconf.derivgain = (double)config.Yconf.derivgain;
+    // Output limit is a percentage (0-100)
+    hwConfig->Xconf.outplimit = (double)config.Xconf.outplimit / 255.0 * 100.0;
+    hwConfig->Yconf.outplimit = (double)config.Yconf.outplimit / 255.0 * 100.0;
+    // Current limit in amps
+    hwConfig->Xconf.currlimit = (double)config.Xconf.currlimit / 100.0;
+    hwConfig->Yconf.currlimit = (double)config.Yconf.currlimit / 100.0;
+    // Integral limit is unitless
+    hwConfig->Xconf.intlimit = (double)config.Xconf.intlimit;
+    hwConfig->Yconf.intlimit = (double)config.Yconf.intlimit;
+    // Copy XBits and YBits (no conversion needed)
+    hwConfig->xbits = config.xbits;
+    hwConfig->ybits = config.ybits;
+    // Copy address
+    hwConfig->address = config.address;
+
+    // TODO: What to do with eqrate, eqadj and trackgoal?
+    config.latitude = __bswap_16(config.latitude);
+    // Convert latitude (degrees * 100 to radians)
+    hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
+    // Copy ticks per revolution
+    hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
+    hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
+    hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
+    hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
+    // Convert slew rates (ticks per loop to rad/s)
+    hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
+    hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
+    // Convert pan rates (ticks per loop to rad/s)
+    hwConfig->Xpanrate = X_MOTSPD2RS(config.Xpanrate);
+    hwConfig->Ypanrate = Y_MOTSPD2RS(config.Ypanrate);
+    // Convert guide rates (ticks per loop to rad/s)
+    hwConfig->Xguiderate = X_MOTSPD2RS(config.Xguiderate);
+    hwConfig->Yguiderate = Y_MOTSPD2RS(config.Yguiderate);
+    // copy baudrate
+    hwConfig->baudrate = (uint32_t) config.baudrate;
+    // Convert local search degrees (degrees * 100 to radians)
+    hwConfig->locsdeg = (double)config.locsdeg / 100.0 * M_PI / 180.0;
+    // Convert local search speed (arcsec per second to rad/s)
+    hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
+    // Convert backlash speed (ticks per loop to rad/s)
+    hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
+    // now read text commands
+    int64_t i64;
+    double Xticks, Yticks;
+    DBG("SERIAL");
+    // motor's encoder ticks per rev
+    if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED;
+    Xticks = ((double) i64); // divide by 4 as these values stored ???
+    if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED;
+    Yticks = ((double) i64);
+    X_ENC_ZERO = Conf.XEncZero;
+    Y_ENC_ZERO = Conf.YEncZero;
+    DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
+    X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
+    Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
+    DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
+    // axis encoder ticks per rev
+    if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
+    Xticks = (double) i64;
+    if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
+    Yticks = (double) i64;
+    DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
+    X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
+    Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
+    DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
+    return MCC_E_OK;
+}
+
+static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
+    SSconfig config;
+    if(Conf.RunModel) return MCC_E_FAILED;
+    // Convert acceleration (rad/s^2 to ticks per loop^2)
+    config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
+    config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
+    // Convert backlash (radians to ticks)
+    config.Xconf.backlash = X_RAD2MOT(hwConfig->Xconf.backlash);
+    config.Yconf.backlash = Y_RAD2MOT(hwConfig->Yconf.backlash);
+    // Convert error limit (radians to ticks)
+    config.Xconf.errlimit = X_RAD2MOT(hwConfig->Xconf.errlimit);
+    config.Yconf.errlimit = Y_RAD2MOT(hwConfig->Yconf.errlimit);
+    // Proportional, integral, and derivative gains are unitless, so no conversion needed
+    config.Xconf.propgain = (uint16_t)hwConfig->Xconf.propgain;
+    config.Yconf.propgain = (uint16_t)hwConfig->Yconf.propgain;
+    config.Xconf.intgain = (uint16_t)hwConfig->Xconf.intgain;
+    config.Yconf.intgain = (uint16_t)hwConfig->Yconf.intgain;
+    config.Xconf.derivgain = (uint16_t)hwConfig->Xconf.derivgain;
+    config.Yconf.derivgain = (uint16_t)hwConfig->Yconf.derivgain;
+    // Output limit is a percentage (0-100), so convert back to 0-255
+    config.Xconf.outplimit = (uint8_t)(hwConfig->Xconf.outplimit / 100.0 * 255.0);
+    config.Yconf.outplimit = (uint8_t)(hwConfig->Yconf.outplimit / 100.0 * 255.0);
+    // Current limit is in amps (convert back to *100)
+    config.Xconf.currlimit = (uint16_t)(hwConfig->Xconf.currlimit * 100.0);
+    config.Yconf.currlimit = (uint16_t)(hwConfig->Yconf.currlimit * 100.0);
+    // Integral limit is unitless, so no conversion needed
+    config.Xconf.intlimit = (uint16_t)hwConfig->Xconf.intlimit;
+    config.Yconf.intlimit = (uint16_t)hwConfig->Yconf.intlimit;
+    // Copy XBits and YBits (no conversion needed)
+    config.xbits = hwConfig->xbits;
+    config.ybits = hwConfig->ybits;
+    // Convert latitude (radians to degrees * 100)
+    config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
+    // Convert slew rates (rad/s to ticks per loop)
+    config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
+    config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
+    // Convert pan rates (rad/s to ticks per loop)
+    config.Xpanrate = X_RS2MOTSPD(hwConfig->Xpanrate);
+    config.Ypanrate = Y_RS2MOTSPD(hwConfig->Ypanrate);
+    // Convert guide rates (rad/s to ticks per loop)
+    config.Xguiderate = X_RS2MOTSPD(hwConfig->Xguiderate);
+    config.Yguiderate = Y_RS2MOTSPD(hwConfig->Yguiderate);
+    // Convert local search degrees (radians to degrees * 100)
+    config.locsdeg = (uint32_t)(hwConfig->locsdeg * 180.0 / M_PI * 100.0);
+    // Convert local search speed (rad/s to arcsec per second)
+    config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
+    // Convert backlash speed (rad/s to ticks per loop)
+    config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
+    config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
+    config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
+    config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
+    config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
+    // todo - also write text params
+    // TODO - next
+    (void) config;
+    return MCC_E_OK;
+}
+
+// getters of max/min speed and acceleration
+mcc_errcodes_t maxspeed(coordpair_t *v){
+    if(!v) return MCC_E_BADFORMAT;
+    v->X = Xlimits.max.speed;
+    v->Y = Ylimits.max.speed;
+    return MCC_E_OK;
+}
+mcc_errcodes_t minspeed(coordpair_t *v){
+    if(!v) return MCC_E_BADFORMAT;
+    v->X = Xlimits.min.speed;
+    v->Y = Ylimits.min.speed;
+    return MCC_E_OK;
+}
+mcc_errcodes_t acceleration(coordpair_t *a){
+    if(!a) return MCC_E_BADFORMAT;
+    a->X = Xlimits.max.accel;
+    a->Y = Ylimits.max.accel;
     return MCC_E_OK;
 }
 
@@ -260,4 +606,14 @@ mount_t Mount = {
     .shortCmd = shortcmd,
     .longCmd = longcmd,
     .getHWconfig = get_hwconf,
+    .saveHWconfig = write_hwconf,
+    .currentT = curtime,
+    .timeFromStart = timefromstart,
+    .timeDiff = timediff,
+    .timeDiff0 = timediff0,
+    .correctTo = correct2,
+    .getMaxSpeed = maxspeed,
+    .getMinSpeed = minspeed,
+    .getAcceleration = acceleration,
 };
+

Auxiliary_utils/LibSidServo/main.h

@@ -0,0 +1,81 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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/>.
+ */
+
+/*
+ * Almost all here used for debug purposes
+ */
+
+#pragma once
+
+#include <stdlib.h>
+
+#include "movingmodel.h"
+#include "sidservo.h"
+
+extern conf_t Conf;
+extern limits_t Xlimits, Ylimits;
+int curtime(struct timespec *t);
+double timediff(const struct timespec *time1, const struct timespec *time0);
+double timediff0(const struct timespec *time1);
+double timefromstart();
+void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst);
+typedef struct{
+    double *x, *t, *t2, *xt; // arrays of coord/time and multiply
+    double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
+    size_t idx; // index of current data in array
+    size_t arraysz; // size of arrays
+} less_square_t;
+
+less_square_t *LS_init(size_t Ndata);
+void LS_delete(less_square_t **ls);
+double LS_calc_slope(less_square_t *l, double x, double t);
+
+// unused arguments of functions
+#define _U_         __attribute__((__unused__))
+// weak functions
+#define WEAK        __attribute__ ((weak))
+
+#ifndef DBL_EPSILON
+#define DBL_EPSILON        (2.2204460492503131e-16)
+#endif
+
+#ifndef FALSE
+#define FALSE (0)
+#endif
+
+#ifndef TRUE
+#define TRUE (1)
+#endif
+
+
+#ifdef EBUG
+#include <stdio.h>
+    #define COLOR_RED           "\033[1;31;40m"
+    #define COLOR_GREEN         "\033[1;32;40m"
+    #define COLOR_OLD           "\033[0;0;0m"
+    #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
+    fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
+    #define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
+        fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
+        fprintf(stderr, __VA_ARGS__);           \
+        fprintf(stderr, "\n");} while(0)
+
+#else  // EBUG
+    #define FNAME()
+    #define DBG(...)
+#endif // EBUG

Auxiliary_utils/LibSidServo/moving_model/Tramp.c

@@ -122,7 +122,10 @@ static int calc(moveparam_t *x, double t){
         DBG("dx01=%g, dt01=%g", dx01, dt01);
     }else{ // increase or decrease speed without stopping phase
         dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
-        double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
+        double a = sign * Max.accel;
+        if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
+        else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
+        //double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
         dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
         DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
         if(dx01 + dx23 > Dx){ // calculate max speed
@@ -150,7 +153,7 @@ static int calc(moveparam_t *x, double t){
     dx23 = setspeed * dt23 / 2.;
     // calculate dx12 and dt12
     double dx12 = Dx - dx01 - dx23;
-    if(dx12 < 0.){
+    if(dx12 < -coord_tolerance){
         DBG("Oops, WTF?");
         return FALSE;
     }

Auxiliary_utils/LibSidServo/moving_model/main.c

@@ -38,12 +38,12 @@ static pars G = {
 };
 
 static limits_t limits = {
-    .min = {.coord = -1e6, .speed = 0.1, .accel = 0.1},
-    .max = {.coord = 1e6, .speed = 1e3, .accel = 50.},
+    .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
+    .max = {.coord = 1e6, .speed = 20., .accel = 9.53523},
     .jerk = 10.
 };
 
-static myoption opts[] = {
+static sl_option_t opts[] = {
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),          "show this help"},
     {"ramp",    NEED_ARG,   NULL,   'r',    arg_string, APTR(&G.ramptype),      "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
     {"deltat",  NEED_ARG,   NULL,   't',    arg_int,    APTR(&G.dT),            "time interval for monitoring (microseconds, >0)"},
@@ -84,9 +84,9 @@ static void monit(double tnext){
 }
 
 int main(int argc, char **argv){
-    initial_setup();
-    parseargs(&argc, &argv, opts);
-    if(G.help) showhelp(-1, opts);
+    sl_init();
+    sl_parseargs(&argc, &argv, opts);
+    if(G.help) sl_showhelp(-1, opts);
     if(G.xlog){
         coordslog = fopen(G.xlog, "w");
         if(!coordslog) ERR("Can't open %s", G.xlog);
@@ -101,8 +101,9 @@ int main(int argc, char **argv){
     model = init_moving(ramp, &limits);
     if(!model) ERRX("Can't init moving model: check parameters");
     Tstart = nanot();
-    moveparam_t target = {.speed = 10., .coord = 20.};
-    if(move(&target)) monit(0.5);
+    moveparam_t target = {.speed = 8.0, .coord = 90.};
+    if(move(&target)) monit(60.);
+    /*
     for(int i = 0; i < 10; ++i){
         target.coord = -target.coord;
         if(move(&target)) monit(1.);
@@ -119,6 +120,7 @@ int main(int argc, char **argv){
     target.coord = 0.; target.speed = 20.;
     if(move(&target)) monit(1e6);
     usleep(5000);
+    */
     fclose(coordslog);
     return 0;
 }

Auxiliary_utils/LibSidServo/movingmodel.c

@@ -0,0 +1,73 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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 <stdio.h>
+#include <string.h>
+#include <pthread.h>
+
+#include "main.h"
+#include "movingmodel.h"
+#include "ramp.h"
+
+extern movemodel_t trapez;
+
+static void chkminmax(double *min, double *max){
+    if(*min <= *max) return;
+    double t = *min;
+    *min = *max;
+    *max = t;
+}
+
+movemodel_t *model_init(limits_t *l){
+    if(!l) return FALSE;
+    movemodel_t *m = calloc(1, sizeof(movemodel_t));
+    // we can't use memcpy or assign as Times/Params would be common for all
+    *m = trapez;
+    m->Times = calloc(STAGE_AMOUNT, sizeof(double));
+    m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
+    moveparam_t *max = &l->max, *min = &l->min;
+    if(min->speed < 0.) min->speed = -min->speed;
+    if(max->speed < 0.) max->speed = -max->speed;
+    if(min->accel < 0.) min->accel = -min->accel;
+    if(max->accel < 0.) max->accel = -max->accel;
+    chkminmax(&min->coord, &max->coord);
+    chkminmax(&min->speed, &max->speed);
+    chkminmax(&min->accel, &max->accel);
+    m->Min = l->min;
+    m->Max = l->max;
+    m->movingstage = STAGE_STOPPED;
+    m->state = ST_STOP;
+    pthread_mutex_init(&m->mutex, NULL);
+    DBG("model inited");
+    return m;
+}
+
+int model_move2(movemodel_t *model, moveparam_t *target, double t){
+    if(!target || !model) return FALSE;
+    DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
+    // only positive velocity
+    if(target->speed < 0.) target->speed = -target->speed;
+    if(fabs(target->speed) < model->Min.speed){
+        DBG("STOP");
+        model->stop(model, t);
+        return TRUE;
+    }
+    // don't mind about acceleration - user cannot set it now
+    return model->calculate(model, target, t);
+}

Auxiliary_utils/LibSidServo/movingmodel.h

@@ -0,0 +1,76 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+#include <pthread.h>
+
+#include "sidservo.h"
+
+// tolerance, time ticks
+#define COORD_TOLERANCE_DEFAULT (1e-8)
+#define COORD_TOLERANCE_MIN     (1e-12)
+#define COORD_TOLERANCE_MAX     (10.)
+#define TIME_TICK_DEFAULT       (0.0001)
+#define TIME_TICK_MIN           (1e-9)
+#define TIME_TICK_MAX           (10.)
+
+typedef enum{
+    ST_STOP,            // stopped
+    ST_MOVE,            // moving
+    ST_AMOUNT
+} movestate_t;
+
+typedef struct{
+    double coord;
+    double speed;
+    double accel;
+} moveparam_t;
+
+typedef struct{
+    moveparam_t min;
+    moveparam_t max;
+    //double acceleration;
+} limits_t;
+
+typedef enum{
+    STAGE_ACCEL,        // start from last speed and accelerate/decelerate to target speed
+    STAGE_MAXSPEED,     // go with target speed
+    STAGE_DECEL,        // go from target speed to zero
+    STAGE_STOPPED,      // stop
+    STAGE_AMOUNT
+} movingstage_t;
+
+typedef struct movemodel{
+    moveparam_t Min;
+    moveparam_t Max;
+    movingstage_t movingstage;
+    movestate_t state;
+    double *Times;
+    moveparam_t *Params;
+    moveparam_t curparams;                    // init values of limits, jerk
+    int (*calculate)(struct movemodel *m, moveparam_t *target, double t);        // calculate stages of traectory beginning from t
+    movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t);  // calculate next model point for time t
+    movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur);             // get current moving state
+    void (*stop)(struct movemodel *m, double t);                                 // stop by ramp
+    void (*emergency_stop)(struct movemodel *m, double t);                       // stop with highest acceleration
+    double (*stoppedtime)(struct movemodel *m);                                  // time when moving will ends
+    pthread_mutex_t mutex;
+} movemodel_t;
+
+movemodel_t *model_init(limits_t *l);
+int model_move2(movemodel_t *model, moveparam_t *target, double t);

Auxiliary_utils/LibSidServo/polltest/main.c

@@ -0,0 +1,237 @@
+/*
+ * 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 <fcntl.h>
+#include <poll.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <usefull_macros.h>
+
+#define XYBUFSZ     (2048)
+
+struct{
+    int help;
+    char *Xpath;
+    char *Ypath;
+    double dt;
+} G = {
+    .Xpath = "/dev/encoder_X0",
+    .Ypath = "/dev/encoder_Y0",
+    .dt = 0.001,
+};
+
+sl_option_t options[] = {
+    {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
+    {"Xpath",   NEED_ARG,   NULL,   'X',    arg_string, APTR(&G.Xpath),     "path to X encoder"},
+    {"Ypath",   NEED_ARG,   NULL,   'Y',    arg_string, APTR(&G.Ypath),     "path to Y encoder"},
+    {"dt",      NEED_ARG,   NULL,   'd',    arg_double, APTR(&G.dt),        "request interval (1e-4..10s)"},
+};
+
+typedef struct{
+    char buf[XYBUFSZ+1];
+    int len;
+} buf_t;
+
+static int Xfd = -1, Yfd = -1;
+
+void signals(int sig){
+    if(sig){
+        signal(sig, SIG_IGN);
+        DBG("Get signal %d, quit.\n", sig);
+    }
+    DBG("close");
+    if(Xfd > 0){ close(Xfd); Xfd = -1; }
+    if(Yfd > 0){ close(Yfd); Yfd = -1; }
+    exit(sig);
+}
+
+static int op(const char *nm){
+    int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK);
+    if(fd < 0) ERR("Can't open %s", nm);
+    struct termios2 tty;
+    if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings");
+    tty.c_lflag     = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
+    tty.c_iflag     = 0; // don't do any changes in input stream
+    tty.c_oflag     = 0; // don't do any changes in output stream
+    tty.c_cflag     = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
+    tty.c_ispeed = 1000000;
+    tty.c_ospeed = 1000000;
+    if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings");
+    // try to set exclusive
+    if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
+    return fd;
+}
+
+static int eolcnt(buf_t *buf){
+    if(!buf) return -1;
+    int cnt = 0;
+    for(int i = 0; i < buf->len; ++i)
+        if(buf->buf[i] == '\n') ++cnt;
+    return cnt;
+}
+// move last record (if any) into head of buffer
+static void movelast(buf_t *buf){
+    FNAME();
+    if(!buf) return;
+    DBG("buf was: %s", buf->buf);
+    int cnt = eolcnt(buf);
+    char *E = strrchr(buf->buf, '\n');
+    int idx = -1;
+    if(E){
+        idx = ++E - buf->buf; // position of symbol after '\n'
+    }else{
+        buf->len = strlen(buf->buf);
+        DBG("leave as is (%s)", buf->buf);
+        return;
+    }
+    DBG("cnt=%d, idx=%d", cnt, idx);
+    switch(cnt){
+        case 0: // EOL not found - clear buf
+            buf->len = 0;
+        break;
+        case 1: // only one record - move all after '\n'
+            if(idx > 0 && idx < XYBUFSZ){
+                buf->len = XYBUFSZ - idx;
+                memmove(buf->buf, E, buf->len);
+            }else buf->len = 0;
+        break;
+        default: // more than one record - move
+        {
+            int i = idx - 2;
+            for(; i > -1; --i)
+                if(buf->buf[i] == '\n') break;
+            ++i;
+            buf->len = XYBUFSZ - i;
+            memmove(buf->buf, &buf->buf[i], buf->len);
+        }
+    }
+    buf->buf[buf->len] = 0;
+    DBG("MOVED; now buf[%d]=%s", buf->len, buf->buf);
+}
+// write to buffer next data portion; return FALSE in case of error
+static int readstrings(buf_t *buf, int fd){
+    if(!buf){WARNX("Empty buffer"); return FALSE;}
+    int L = XYBUFSZ - buf->len;
+    if(L == 0){
+        DBG("len: %d", buf->len);
+        movelast(buf);
+        L = XYBUFSZ - buf->len;
+    }
+    int got = read(fd, &buf->buf[buf->len], L);
+    if(got < 0){
+        WARN("read()");
+        return FALSE;
+    }else if(got == 0) return TRUE;
+    buf->len += got;
+    buf->buf[buf->len] = 0;
+    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) return -1;
+    // read record between last '\n' and previous (or start of string)
+    int cnt = eolcnt(buf);
+    if(cnt < 1) return FALSE;
+    char *last = strrchr(buf->buf, '\n');
+    if(!last) return FALSE; // WTF?
+    *last = 0;
+    char *prev = buf->buf;
+    if(cnt > 1) prev = strrchr(buf->buf, '\n') + 1;
+    if(!prev) prev = buf->buf; // ??
+    if(out) *out = atol(prev);
+    int l = strlen(++last);
+    if(l < XYBUFSZ){
+        buf->len = l;
+        if(l){
+            memmove(buf->buf, last, l);
+            DBG("moved: %s", buf->buf);
+        }else DBG("empty line");
+    }else{
+        buf->len = 0;
+        DBG("buffer clear");
+    }
+    return TRUE;
+}
+// try to write '\n' asking new data portion; return FALSE if failed
+static int asknext(int fd){
+    FNAME();
+    if(fd < 0) return FALSE;
+    int i = 0;
+    for(; i < 5; ++i){
+        int l = write(fd, "\n", 1);
+        DBG("l=%d", l);
+        if(1 == l) return TRUE;
+        usleep(100);
+    }
+    DBG("5 tries... failed!");
+    return FALSE;
+}
+
+int main(int argc, char **argv){
+    buf_t xbuf, ybuf;
+    long xlast, ylast;
+    double xtlast, ytlast;
+    sl_init();
+    sl_parseargs(&argc, &argv, options);
+    if(G.help) sl_showhelp(-1, options);
+    if(G.dt < 1e-4) ERRX("dx too small");
+    if(G.dt > 10.) ERRX("dx too big");
+    Xfd = op(G.Xpath);
+    Yfd = op(G.Ypath);
+    struct pollfd pfds[2];
+    pfds[0].fd = Xfd; pfds[0].events = POLLIN;
+    pfds[1].fd = Yfd; pfds[1].events = POLLIN;
+    double t0x, t0y, tstart;
+    asknext(Xfd); asknext(Yfd);
+    t0x = t0y = tstart = sl_dtime();
+    DBG("Start");
+    do{ // main cycle
+        if(poll(pfds, 2, 1) < 0){
+            WARN("poll()");
+            break;
+        }
+        if(pfds[0].revents && POLLIN){
+            if(!readstrings(&xbuf, Xfd)) break;
+        }
+        if(pfds[1].revents && POLLIN){
+            if(!readstrings(&ybuf, Yfd)) break;
+        }
+        double curt = sl_dtime();
+        if(getdata(&xbuf, &xlast)) xtlast = curt;
+        if(curt - t0x >= G.dt){ // get last records
+            if(curt - xtlast < 1.5*G.dt)
+                printf("%-14.4fX=%ld\n", xtlast-tstart, xlast);
+            if(!asknext(Xfd)) break;
+            t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt;
+        }
+        curt = sl_dtime();
+        if(getdata(&ybuf, &ylast)) ytlast = curt;
+        if(curt - t0y >= G.dt){ // get last records
+            if(curt - ytlast < 1.5*G.dt)
+                printf("%-14.4fY=%ld\n", ytlast-tstart, ylast);
+            if(!asknext(Yfd)) break;
+            t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt;
+        }
+    }while(Xfd > 0 && Yfd > 0);
+    DBG("OOps: disconnected");
+    signals(0);
+    return 0;
+}

Auxiliary_utils/LibSidServo/ramp.c

@@ -0,0 +1,259 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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/>.
+ */
+
+// simplest trapezioidal ramp
+
+#include <math.h>
+#include <strings.h>
+
+#include "main.h"
+#include "ramp.h"
+
+#ifdef EBUG
+#undef DBG
+#define DBG(...)
+#undef FNAME
+#define FNAME()
+#endif
+
+static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
+
+static void emstop(movemodel_t *m, double _U_ t){
+    FNAME();
+    pthread_mutex_lock(&m->mutex);
+    m->curparams.accel = 0.;
+    m->curparams.speed = 0.;
+    bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
+    bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
+    m->state = ST_STOP;
+    m->movingstage = STAGE_STOPPED;
+    pthread_mutex_unlock(&m->mutex);
+}
+
+static void stop(movemodel_t *m, double t){
+    FNAME();
+    pthread_mutex_lock(&m->mutex);
+    if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
+    m->movingstage = STAGE_DECEL;
+    m->state = ST_MOVE;
+    m->Times[STAGE_DECEL] = t;
+    m->Params[STAGE_DECEL].speed = m->curparams.speed;
+    if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
+    else m->Params[STAGE_DECEL].accel = m->Max.accel;
+    m->Params[STAGE_DECEL].coord = m->curparams.coord;
+    // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
+    m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
+    // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
+    double dt = m->Times[STAGE_STOPPED] - t;
+    m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
+                                  m->Params[STAGE_DECEL].accel * dt * dt / 2.;
+ret:
+    pthread_mutex_unlock(&m->mutex);
+}
+
+// inner part of `calc`, could be called recoursively for hard case
+static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
+    // signs
+    double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage
+    // times
+    double dt01 = 0., dt12 = 0., dt23 = 0.;
+    // absolute speed at stage 23 (or in that point); absolute max acceleration
+    double abs_vset = x->speed, abs_a = m->Max.accel;
+    // absolute target movement
+    double abs_Dx = fabs(x->coord - m->curparams.coord);
+    if(m->state == ST_STOP && abs_Dx < coord_tolerance){
+        DBG("Movement too small -> stay at place");
+        return;
+    }
+    // signs of Dx and current speed
+    double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.;
+    double v0 = m->curparams.speed;
+    double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0);
+    if(v0 == 0.) sign_v0 = 0.;
+    // preliminary calculations (vset and dependent values could be changed)
+    dt01 = fabs(abs_v0 - abs_vset) / abs_a;
+    double abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
+    dt23 = abs_vset / abs_a;
+    double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a;
+    if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction
+        if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop
+            //DBG("Simplest case: stop");
+            dt01 = dt12 = 0.;
+            sign_a23 = -sign_v0;
+            dt23 = abs_v0 / abs_a;
+        }else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case
+            //DBG("Move with smaller speed");
+            sign_a01 = sign_a23 = -sign_v0;
+            sign_vset = sign_v0;
+            double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.;
+            double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
+            dt12 = abs_dx12 / abs_vset;
+        }else{// move with larget speed
+            //DBG("Move with larger speed");
+            double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
+            if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other
+                abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.);
+                dt01 = fabs(abs_v0 - abs_vset) / abs_a;
+                abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
+                dt23 = abs_vset / abs_a;
+                abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
+                DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset);
+            }
+            sign_a01 = sign_Dx; // sign_v0 could be ZERO!!!
+            sign_a23 = -sign_Dx;
+            sign_vset = sign_Dx;
+            double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
+            dt12 = abs_dx12 / abs_vset;
+        }
+    }else{
+        // if we are here, we have the worst case: change speed direction
+       // DBG("Hardest case: change speed direction");
+        // now we should calculate coordinate at which model stops and biuld new trapezium from that point
+        double x0 = m->curparams.coord, v0 = m->curparams.speed;
+        double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
+        m->state = ST_STOP;
+        m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.;
+        unlockedcalc(m, x, tstop); // calculate new ramp
+        // and change started conditions
+        m->curparams.coord = x0; m->curparams.speed = v0;
+        m->Times[STAGE_ACCEL] = t;
+        m->Params[STAGE_ACCEL].coord = x0;
+        m->Params[STAGE_ACCEL].speed = v0;
+       // DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
+        return;
+    }
+    m->state = ST_MOVE;
+    m->movingstage = STAGE_ACCEL;
+    // some knot parameters
+    double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a;
+    double v1, v2, x0, x1, x2;
+    v2 = v1 = sign_vset * abs_vset;
+    x0 = m->curparams.coord;
+    x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.;
+    x2 = x1 + v1 * dt12;
+    // fill knot parameters
+    moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed
+    p->accel = a01;
+    p->speed = m->curparams.speed;
+    p->coord = x0;
+    m->Times[STAGE_ACCEL] = t;
+    p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed
+    p->accel = 0.;
+    p->speed = v1;
+    p->coord = x1;
+    m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01;
+    p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed
+    p->accel = a23;
+    p->speed = v2;
+    p->coord = x2;
+    m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
+    p = &m->Params[STAGE_STOPPED]; // 3 - stop at target
+    p->accel = p->speed = 0.;
+    p->coord = x->coord;
+    m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
+}
+
+/**
+ * @brief calc - moving calculation
+ * @param x - using max speed (>0!!!) and coordinate
+ * @param t - current time value
+ * @return FALSE if can't move with given parameters
+ */
+static int calc(movemodel_t *m, moveparam_t *x, double t) {
+    //DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
+    if (!x || !m) return FALSE;
+    pthread_mutex_lock(&m->mutex);
+    int ret = FALSE;
+    // Validate input parameters
+    if(x->coord < m->Min.coord || x->coord > m->Max.coord){
+        DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
+        goto ret;
+    }
+    if(x->speed < m->Min.speed || x->speed > m->Max.speed){
+        DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
+        goto ret;
+    }
+    ret = TRUE; // now there's no chanses to make error
+    unlockedcalc(m, x, t);
+    // Debug output
+    /*for(int i = 0; i < STAGE_AMOUNT; i++){
+        DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
+            i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
+    }*/
+ret:
+    pthread_mutex_unlock(&m->mutex);
+    return ret;
+}
+
+static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
+    pthread_mutex_lock(&m->mutex);
+    if(m->state == ST_STOP) goto ret;
+    for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
+        if(m->Times[s] <= t){ // check time for current stage
+            m->movingstage = s;
+            break;
+        }
+    }
+    if(m->movingstage == STAGE_STOPPED){
+        m->curparams.coord = m->Params[STAGE_STOPPED].coord;
+        pthread_mutex_unlock(&m->mutex);
+       /* DBG("REACHED STOPping stage @ t=%g", t);
+        for(int s = STAGE_STOPPED; s >= 0; --s){
+            DBG("T[%d]=%g, ", s, m->Times[s]);
+        }*/
+        emstop(m, t);
+        goto ret;
+    }
+    // calculate current parameters
+    double dt = t - m->Times[m->movingstage];
+    double a = m->Params[m->movingstage].accel;
+    double v0 = m->Params[m->movingstage].speed;
+    double x0 = m->Params[m->movingstage].coord;
+    m->curparams.accel = a;
+    m->curparams.speed = v0 + a * dt;
+    m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
+ret:
+    if(next) *next = m->curparams;
+    movestate_t st = m->state;
+    pthread_mutex_unlock(&m->mutex);
+    return st;
+}
+
+static movestate_t getst(movemodel_t *m, moveparam_t *cur){
+    pthread_mutex_lock(&m->mutex);
+    if(cur) *cur = m->curparams;
+    movestate_t st = m->state;
+    pthread_mutex_unlock(&m->mutex);
+    return st;
+}
+
+static double gettstop(movemodel_t *m){
+    pthread_mutex_lock(&m->mutex);
+    double r = m->Times[STAGE_STOPPED];
+    pthread_mutex_unlock(&m->mutex);
+    return r;
+}
+
+movemodel_t trapez = {
+    .stop = stop,
+    .emergency_stop = emstop,
+    .get_state = getst,
+    .calculate = calc,
+    .proc_move = proc,
+    .stoppedtime = gettstop,
+};

Auxiliary_utils/LibSidServo/ramp.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of the moving_model project.
+ * Copyright 2025 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
+
+#include "movingmodel.h"
+
+extern movemodel_t trapez;

Auxiliary_utils/LibSidServo/serial.c

@@ -20,31 +20,36 @@
 #include <errno.h>
 #include <fcntl.h>
 #include <math.h>
+#include <poll.h>
 #include <pthread.h>
+#include <signal.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
-#include <time.h>
 #include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
+#include "movingmodel.h"
 #include "serial.h"
+#include "ssii.h"
 
 // serial devices FD
-static int encfd = -1, mntfd = -1;
+static int encfd[2] = {-1, -1}, mntfd = -1;
 // main mount data
 static mountdata_t mountdata = {0};
+// last encoders time and last encoders data - for speed measurement
+//static coordval_t lastXenc = {0}, lastYenc = {0};
 
 // mutexes for RW operations with mount device and data
 static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
                         datamutex = 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
-static struct timeval encRtmout = {0}, mntRtmout = {0};
+// 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};
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -53,34 +58,42 @@ typedef struct __attribute__((packed)){
     uint8_t CRC[4];
 } enc_t;
 
-/**
- * @brief dtime - UNIX time with microsecond
- * @return value
- */
-double dtime(){
-    double t;
-    struct timeval tv;
-    gettimeofday(&tv, NULL);
-    t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
-    return t;
+// calculate current X/Y speeds
+void getXspeed(){
+    static less_square_t *ls = NULL;
+    if(!ls){
+        ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
+        if(!ls) return;
+    }
+    double dt = timediff0(&mountdata.encXposition.t);
+    double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
+    if(fabs(speed) < 1.5 * Xlimits.max.speed){
+        mountdata.encXspeed.val = speed;
+        mountdata.encXspeed.t = mountdata.encXposition.t;
+    }
+    //DBG("Xspeed=%g", mountdata.encXspeed.val);
+}
+void getYspeed(){
+    static less_square_t *ls = NULL;
+    if(!ls){
+        ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
+        if(!ls) return;
+    }
+    double dt = timediff0(&mountdata.encYposition.t);
+    double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
+    if(fabs(speed) < 1.5 * Ylimits.max.speed){
+        mountdata.encYspeed.val = speed;
+        mountdata.encYspeed.t = mountdata.encYposition.t;
+    }
 }
 
-#if 0
-// init start time
-static void gttime(){
-    struct timeval tv;
-    gettimeofday(&tv, NULL);
-    tv_sec_got = tv.tv_sec;
-    tv_usec_got = tv.tv_usec;
-}
-#endif
-
 /**
- * @brief parse_encbuf - check encoder buffer (for separate encoder) and fill fresh data
+ * @brief parse_encbuf - check encoder buffer (for encoder data based on SSII proto) and fill fresh data
  * @param databuf - input buffer with 13 bytes of data
- * @param nexttime - time when databuf[0] got
+ * @param t - time when databuf[0] got
  */
-static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
+static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
+    if(!t) return;
     enc_t *edata = (enc_t*) databuf;
 /*
 #ifdef EBUG
@@ -115,31 +128,104 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
         return;
     }
     pthread_mutex_lock(&datamutex);
-    mountdata.encposition.X = X_ENC2RAD(edata->encX);
-    mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
-    mountdata.encposition.msrtime = *tv;
+    mountdata.encXposition.val = Xenc2rad(edata->encX);
+    mountdata.encYposition.val = Yenc2rad(edata->encY);
+    DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
+    mountdata.encXposition.t = *t;
+    mountdata.encYposition.t = *t;
+    getXspeed(); getYspeed();
     pthread_mutex_unlock(&datamutex);
     //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) return -1;
+    if(encfd[0] < 0) return -1;
     uint8_t byte = 0;
     fd_set rfds;
-    struct timeval tv;
     do{
         FD_ZERO(&rfds);
-        FD_SET(encfd, &rfds);
-        tv = encRtmout;
-        int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
+        FD_SET(encfd[0], &rfds);
+        struct timeval tv = encRtmout;
+        int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
         if(!retval) break;
         if(retval < 0){
             if(errno == EINTR) continue;
             return -1;
         }
-        if(FD_ISSET(encfd, &rfds)){
-            ssize_t l = read(encfd, &byte, 1);
+        if(FD_ISSET(encfd[0], &rfds)){
+            ssize_t l = read(encfd[0], &byte, 1);
             if(l != 1) return -2; // disconnected ??
             break;
         } else return -1;
@@ -151,7 +237,6 @@ static int getmntbyte(){
     if(mntfd < 0) return -1;
     uint8_t byte;
     fd_set rfds;
-    struct timeval tv;
    /* ssize_t l = read(mntfd, &byte, 1);
     //DBG("MNT read=%zd byte=0x%X", l, byte);
     if(l == 0) return -1;
@@ -160,7 +245,7 @@ static int getmntbyte(){
     do{
         FD_ZERO(&rfds);
         FD_SET(mntfd, &rfds);
-        tv = mntRtmout;
+        struct timeval tv = mntRtmout;
         int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
         if(retval < 0){
             if(errno == EINTR) continue;
@@ -171,19 +256,44 @@ static int getmntbyte(){
         if(FD_ISSET(mntfd, &rfds)){
             ssize_t l = read(mntfd, &byte, 1);
             //DBG("MNT read=%zd byte=0x%X", l, byte);
-            if(l != 1) return -2; // disconnected ??
+            if(l != 1){
+                DBG("Mount disconnected?");
+                return -2; // disconnected ??
+            }
             break;
         } else return -1;
     }while(1);
     return (int)byte;
 }
+// clear data from input buffer
+static void clrmntbuf(){
+    if(mntfd < 0) return;
+    uint8_t byte;
+    fd_set rfds;
+    do{
+        FD_ZERO(&rfds);
+        FD_SET(mntfd, &rfds);
+        struct timeval tv = {.tv_sec=0, .tv_usec=10};
+        int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
+        if(retval < 0){
+            if(errno == EINTR) continue;
+            DBG("Error in select()");
+            break;
+        }
+        if(FD_ISSET(mntfd, &rfds)){
+            ssize_t l = read(mntfd, &byte, 1);
+            if(l != 1) break;
+        } else break;
+    }while(1);
+}
 
 // main encoder thread (for separate encoder): read next data and make parsing
-static void *encoderthread(void _U_ *u){
+static void *encoderthread1(void _U_ *u){
+    if(Conf.SepEncoder != 1) return NULL;
     uint8_t databuf[ENC_DATALEN];
     int wridx = 0, errctr = 0;
-    struct timeval tv;
-    while(encfd > -1 && errctr < MAX_ERR_CTR){
+    struct timespec tcur;
+    while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
         int b = getencbyte();
         if(b == -2) ++errctr;
         if(b < 0) continue;
@@ -193,18 +303,153 @@ static void *encoderthread(void _U_ *u){
             if((uint8_t)b == ENC_MAGICK){
 //                DBG("Got magic -> start filling packet");
                 databuf[wridx++] = (uint8_t) b;
-                gettimeofday(&tv, NULL);
             }
             continue;
         }else databuf[wridx++] = (uint8_t) b;
         if(wridx == ENC_DATALEN){
-            parse_encbuf(databuf, &tv);
-            wridx = 0;
+            if(curtime(&tcur)){
+                parse_encbuf(databuf, &tcur);
+                wridx = 0;
+            }
         }
     }
-    if(encfd > -1){
-        close(encfd);
-        encfd = -1;
+    if(encfd[0] > -1){
+        close(encfd[0]);
+        encfd[0] = -1;
+    }
+    return NULL;
+}
+
+#define XYBUFSZ     (128)
+typedef struct{
+    char buf[XYBUFSZ+1];
+    int len;
+} buf_t;
+
+// 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("buffer  overfull: %d!", buf->len);
+        char *lastn = strrchr(buf->buf, '\n');
+        if(lastn){
+            fprintf(stderr, "BUFOVR: _%s_", buf->buf);
+            ++lastn;
+            buf->len = XYBUFSZ - (lastn - buf->buf);
+            DBG("Memmove %d", buf->len);
+            memmove(lastn, buf->buf, buf->len);
+            buf->buf[buf->len] = 0;
+        }else buf->len = 0;
+        L = XYBUFSZ - buf->len;
+    }
+    int got = read(fd, &buf->buf[buf->len], L);
+    if(got < 0){
+        DBG("read()");
+        return FALSE;
+    }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);
+    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;
+    // read record between last '\n' and previous (or start of string)
+    char *last = &buf->buf[buf->len - 1];
+    //DBG("buf: _%s_", buf->buf);
+    if(*last != '\n') return FALSE;
+    *last = 0;
+    //DBG("buf: _%s_", buf->buf);
+    char *prev = strrchr(buf->buf, '\n');
+    if(!prev) prev = buf->buf;
+    else{
+        fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
+        ++prev; // after last '\n'
+    }
+    if(out) *out = atol(prev);
+    // clear buffer
+    buf->len = 0;
+    return TRUE;
+}
+
+// try to write '\n' asking new data portion; return FALSE if failed
+static int asknext(int fd){
+    FNAME();
+    if(fd < 0) return FALSE;
+    int i = 0;
+    for(; i < 5; ++i){
+        int l = write(fd, "\n", 1);
+        //DBG("l=%d", l);
+        if(1 == l) return TRUE;
+        usleep(100);
+    }
+    DBG("5 tries... failed!");
+    return FALSE;
+}
+
+// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
+static void *encoderthread2(void _U_ *u){
+    if(Conf.SepEncoder != 2) return NULL;
+    DBG("Thread started");
+    struct pollfd pfds[2];
+    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];
+    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;
+    do{ // main cycle
+        if(poll(pfds, 2, 0) < 0){
+            DBG("poll()");
+            break;
+        }
+        int got = 0;
+        for(int i = 0; i < 2; ++i){
+            if(pfds[i].revents && POLLIN){
+                if(!readstrings(&strbuf[i], encfd[i])){
+                    ++errctr;
+                    break;
+                }
+            }
+            double curt = timefromstart();
+            if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
+            if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
+                if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
+                    pthread_mutex_lock(&datamutex);
+                    if(i == 0){
+                        mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
+                        curtime(&mountdata.encXposition.t);
+                        getXspeed();
+                    }else{
+                        mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
+                        curtime(&mountdata.encYposition.t);
+                        getYspeed();
+                    }
+                    pthread_mutex_unlock(&datamutex);
+                }
+                if(!asknext(encfd[i])){
+                    ++errctr;
+                    break;
+                }
+                t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
+                ++got;
+            }
+        }
+        if(got == 2) errctr = 0;
+    }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
+    DBG("\n\nEXIT: ERRCTR=%d", errctr);
+    for(int i = 0; i < 2; ++i){
+        if(encfd[i] > -1){
+            close(encfd[i]);
+            encfd[i] = -1;
+        }
     }
     return NULL;
 }
@@ -225,26 +470,78 @@ void data_free(data_t **x){
     *x = NULL;
 }
 
+static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
+    if(!prev || !nstopped || !stat) return;
+    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; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped);
+        }
+    }else if(*prev != cur){
+        DBG("AXIS moving");
+        *nstopped = 0;
+    }
+    *prev = cur;
+}
+
 // main mount thread
 static void *mountthread(void _U_ *u){
     int errctr = 0;
     uint8_t buf[2*sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
+    bzero(&mountdata, sizeof(mountdata));
+    double t0 = timefromstart(), tstart = t0, tcur = t0;
+    double oldmt = -100.; // old `millis measurement` time
+    static uint32_t oldmillis = 0;
+    if(Conf.RunModel){
+        double Xprev = NAN, Yprev = NAN; // previous coordinates
+        int xcnt = 0, ycnt = 0;
+        while(1){
+            coordpair_t c;
+            movestate_t xst, yst;
+            // now change data
+            getModData(&c, &xst, &yst);
+            struct timespec tnow;
+            if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
+            pthread_mutex_lock(&datamutex);
+            mountdata.encXposition.t = mountdata.encYposition.t = tnow;
+            mountdata.encXposition.val = c.X;
+            mountdata.encYposition.val = c.Y;
+            //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
+                    mountdata.motXposition.val = c.X;
+                if(yst == ST_MOVE)
+                    mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
+                else
+                    mountdata.motYposition.val = c.Y;
+                oldmt = tcur;
+            }else mountdata.millis = oldmillis;
+            chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
+            chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
+            getXspeed(); getYspeed();
+            pthread_mutex_unlock(&datamutex);
+            while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
+            t0 = timefromstart();
+        }
+    }
     // data to get
     data_t d = {.buf = buf, .maxlen = sizeof(buf)};
     // cmd to send
     data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
     if(!cmd_getstat) goto failed;
-    double t0 = dtime();
-/*
-#ifdef EBUG
-    double t00 = t0;
-#endif
-*/
     while(mntfd > -1 && errctr < MAX_ERR_CTR){
         // read data to status
-        struct timeval tgot;
-        if(0 != gettimeofday(&tgot, NULL)) continue;
+        struct timespec tcur;
+        if(!curtime(&tcur)) continue;
+        // 80 milliseconds to get answer on GETSTAT
         if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
 #ifdef EBUG
             DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
@@ -260,13 +557,13 @@ static void *mountthread(void _U_ *u){
         errctr = 0;
         pthread_mutex_lock(&datamutex);
         // now change data
-        SSconvstat(status, &mountdata, &tgot);
+        SSconvstat(status, &mountdata, &tcur);
         pthread_mutex_unlock(&datamutex);
         // allow writing & getters
-        //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
-        if(dtime() - t0 >= Conf.MountReqInterval) usleep(50);
-        while(dtime() - t0 < Conf.MountReqInterval);
-        t0 = dtime();
+        do{
+            usleep(500);
+        }while(timefromstart() - t0 < Conf.MountReqInterval);
+        t0 = timefromstart();
     }
     data_free(&cmd_getstat);
 failed:
@@ -282,8 +579,15 @@ static int ttyopen(const char *path, speed_t speed){
     int fd = -1;
     struct termios2 tty;
     DBG("Try to open %s @ %d", path, speed);
-    if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
-    if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
+    if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
+        DBG("Can't open device %s: %s", path, strerror(errno));
+        return -1;
+    }
+    if(ioctl(fd, TCGETS2, &tty)){
+        DBG("Can't read TTY settings");
+        close(fd);
+        return -1;
+    }
     tty.c_lflag     = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
     tty.c_iflag     = 0; // don't do any changes in input stream
     tty.c_oflag     = 0; // don't do any changes in output stream
@@ -292,8 +596,12 @@ static int ttyopen(const char *path, speed_t speed){
     tty.c_ospeed = speed;
     //tty.c_cc[VMIN]  = 0;  // non-canonical mode
     //tty.c_cc[VTIME] = 5;
-    if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
-    DBG("Check speed");
+    if(ioctl(fd, TCSETS2, &tty)){
+        DBG("Can't set TTY settings");
+        close(fd);
+        return -1;
+    }
+    DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
     if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
     // try to set exclusive
     if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
@@ -301,27 +609,49 @@ static int ttyopen(const char *path, speed_t speed){
 }
 
 // return FALSE if failed
-int openEncoder(const char *path, int speed){
+int openEncoder(){
+    if(Conf.RunModel) return TRUE;
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
-    if(encfd > -1) close(encfd);
-    encfd = ttyopen(path, (speed_t) speed);
-    if(encfd < 0) return FALSE;
-    encRtmout.tv_sec = 0;
-    encRtmout.tv_usec = 200000000 / speed; // 20 bytes
-    if(pthread_create(&encthread, NULL, encoderthread, NULL)){
-        close(encfd);
-        encfd = -1;
-        return FALSE;
-    }
+    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;
+            return FALSE;
+        }
+    }else if(Conf.SepEncoder == 2){
+        DBG("Two devices!");
+        const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath};
+        for(int i = 0; i < 2; ++i){
+            if(encfd[i] > -1) close(encfd[i]);
+            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]);
+                encfd[i] = -1;
+            }
+            return FALSE;
+        }
+    }else return FALSE;
     DBG("Encoder opened, thread started");
     return TRUE;
 }
 
 // return FALSE if failed
-int openMount(const char *path, int speed){
+int openMount(){
+    if(Conf.RunModel) goto create_thread;
     if(mntfd > -1) close(mntfd);
-    DBG("Open mount %s @ %d", path, speed);
-    mntfd = ttyopen(path, (speed_t) speed);
+    DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
+    mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
     if(mntfd < 0) return FALSE;
     DBG("mntfd=%d", mntfd);
     // clear buffer
@@ -334,11 +664,14 @@ int openMount(const char *path, int speed){
         DBG("got %zd", l);
     }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
+    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
+create_thread:
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
-        DBG("Can't create thread");
-        close(mntfd);
-        mntfd = -1;
+        DBG("Can't create mount thread");
+        if(!Conf.RunModel){
+            close(mntfd);
+            mntfd = -1;
+        }
         return FALSE;
     }
     DBG("Mount opened, thread started");
@@ -347,19 +680,29 @@ int openMount(const char *path, int speed){
 
 // close all opened serial devices and quit threads
 void closeSerial(){
+    // TODO: close devices in "model" mode too!
+    if(Conf.RunModel) return;
     if(mntfd > -1){
-        DBG("Kill mount thread");
+        DBG("Cancel mount thread");
         pthread_cancel(mntthread);
-        DBG("close fd");
+        DBG("join mount thread");
+        pthread_join(mntthread, NULL);
+        DBG("close mount fd");
         close(mntfd);
         mntfd = -1;
     }
-    if(encfd > -1){
-        DBG("Kill encoder thread");
+    if(encfd[0] > -1){
+        DBG("Cancel encoder thread");
         pthread_cancel(encthread);
-        DBG("close fd");
-        close(encfd);
-        encfd = -1;
+        DBG("join encoder thread");
+        pthread_join(encthread, NULL);
+        DBG("close encoder's fd");
+        close(encfd[0]);
+        encfd[0] = -1;
+        if(Conf.SepEncoder == 2 && encfd[1] > -1){
+            close(encfd[1]);
+            encfd[1] = -1;
+        }
     }
 }
 
@@ -369,32 +712,45 @@ mcc_errcodes_t getMD(mountdata_t  *d){
     pthread_mutex_lock(&datamutex);
     *d = mountdata;
     pthread_mutex_unlock(&datamutex);
+    //DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val);
+    //DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL));
     return MCC_E_OK;
 }
 
+void setStat(axis_status_t Xstate, axis_status_t Ystate){
+    DBG("set x/y state to %d/%d", Xstate, Ystate);
+    pthread_mutex_lock(&datamutex);
+    mountdata.Xstate = Xstate;
+    mountdata.Ystate = Ystate;
+    pthread_mutex_unlock(&datamutex);
+}
+
 // write-read without locking mutex (to be used inside other functions)
 static int wr(const data_t *out, data_t *in, int needeol){
-    if((!out && !in) || mntfd < 0) return FALSE;
+    if((!out && !in) || mntfd < 0){
+        DBG("Wrong arguments or no mount fd");
+        return FALSE;
+    }
+    clrmntbuf();
     if(out){
         if(out->len != (size_t)write(mntfd, out->buf, out->len)){
             DBG("written bytes not equal to need");
             return FALSE;
         }
-        //DBG("Send to mount %zd bytes: %s", out->len, out->buf);
         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
     }
-    uint8_t buf[256];
-    data_t dumb = {.buf = buf, .maxlen = 256};
-    if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
+    if(!in) return TRUE;
     in->len = 0;
     for(size_t i = 0; i < in->maxlen; ++i){
         int b = getmntbyte();
         if(b < 0) break; // nothing to read -> go out
         in->buf[in->len++] = (uint8_t) b;
     }
+    while(getmntbyte() > -1);
     return TRUE;
 }
 
@@ -405,6 +761,7 @@ 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;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 1);
     pthread_mutex_unlock(&mntmutex);
@@ -412,6 +769,7 @@ int MountWriteRead(const data_t *out, data_t *in){
 }
 // send binary data - without EOL
 int MountWriteReadRaw(const data_t *out, data_t *in){
+    if(Conf.RunModel) return -1;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 0);
     pthread_mutex_unlock(&mntmutex);
@@ -435,42 +793,43 @@ static void loglcmd(SSlcmd *c){
 
 // send short/long binary command; return FALSE if failed
 static int bincmd(uint8_t *cmd, int len){
+    if(Conf.RunModel) return FALSE;
     static data_t *dscmd = NULL, *dlcmd = NULL;
     if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
     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};
+    //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");
+#ifdef EBUG
         logscmd((SSscmd*)cmd);
-        if(!wr(dscmd, &a, 1)) goto rtn;
+#endif
+        if(!wr(dscmd, NULL, 1)) goto rtn;
     }else if(len == sizeof(SSlcmd)){
         ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
         DBG("Long command");
+#ifdef EBUG
         loglcmd((SSlcmd*)cmd);
-        if(!wr(dlcmd, &a, 1)) goto rtn;
+#endif
+        if(!wr(dlcmd, NULL, 1)) goto rtn;
     }else{
         goto rtn;
     }
-    DBG("Write %d bytes and wait for ans", len);
     data_t d;
     d.buf = cmd;
     d.len = d.maxlen = len;
-    ret = wr(&d, &d, 0);
-#ifdef EBUG
-    if(len == sizeof(SSscmd)) logscmd((SSscmd*)cmd);
-    else loglcmd((SSlcmd*)cmd);
-#endif
+    ret = wr(&d, NULL, 0);
     DBG("%s", ret ? "SUCCESS" : "FAIL");
 rtn:
     pthread_mutex_unlock(&mntmutex);
     return ret;
 }
 
+// short, long and config text-binary commands
 // return TRUE if OK
 int cmdS(SSscmd *cmd){
     return bincmd((uint8_t *)cmd, sizeof(SSscmd));
@@ -480,6 +839,7 @@ int cmdL(SSlcmd *cmd){
 }
 // rw == 1 to write, 0 to read
 int cmdC(SSconfig *conf, int rw){
+    if(Conf.RunModel) return FALSE;
     static data_t *wcmd = NULL, *rcmd = NULL;
     int ret = FALSE;
     // dummy buffer to clear trash in input
@@ -493,16 +853,23 @@ int cmdC(SSconfig *conf, int rw){
     }else{ // read
         data_t d;
         d.buf = (uint8_t *) conf;
-        d.len = 0; d.maxlen = sizeof(SSconfig);
+        d.len = 0; d.maxlen = 0;
+        ret = wr(rcmd, &d, 1);
+        DBG("write command: %s", ret ? "TRUE" : "FALSE");
+        if(!ret) goto rtn;
+        // make a huge pause for stupid SSII
+        usleep(100000);
+        d.len = 0;  d.maxlen = sizeof(SSconfig);
         ret = wr(rcmd, &d, 1);
         DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
-        if(d.len != d.maxlen) return FALSE;
+        if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
         // simplest checksum
         uint16_t sum = 0;
         for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
         if(sum != conf->checksum){
             DBG("got sum: %u, need: %u", conf->checksum, sum);
-            return FALSE;
+            ret = FALSE;
+            goto rtn;
         }
     }
 rtn:

Auxiliary_utils/LibSidServo/serial.h

@@ -28,15 +28,17 @@
 // max error counter (when read() returns -1)
 #define MAX_ERR_CTR (100)
 
-double dtime();
 data_t *cmd2dat(const char *cmd);
 void data_free(data_t **x);
-int openEncoder(const char *path, int speed);
-int openMount(const char *path, int speed);
+int openEncoder();
+int openMount();
 void closeSerial();
 mcc_errcodes_t getMD(mountdata_t  *d);
+void setStat(axis_status_t Xstate, axis_status_t Ystate);
 int MountWriteRead(const data_t *out, data_t *in);
 int MountWriteReadRaw(const data_t *out, data_t *in);
 int cmdS(SSscmd *cmd);
 int cmdL(SSlcmd *cmd);
 int cmdC(SSconfig *conf, int rw);
+void getXspeed();
+void getYspeed();

Auxiliary_utils/LibSidServo/sidservo.h

@@ -16,6 +16,11 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * This file contains all need for external usage
+ */
+
+
 #pragma once
 
 #ifdef __cplusplus
@@ -27,6 +32,14 @@ extern "C"
 #include <stdint.h>
 #include <sys/time.h>
 
+// minimal serial speed of mount device
+#define MOUNT_BAUDRATE_MIN      (1200)
+// max speed interval, seconds
+#define MCC_CONF_MAX_SPEEDINT   (2.)
+// minimal speed interval in parts of EncoderReqInterval
+#define MCC_CONF_MIN_SPEEDC     (3.)
+
+
 // error codes
 typedef enum{
     MCC_E_OK = 0,       // all OK
@@ -35,22 +48,55 @@ typedef enum{
     MCC_E_ENCODERDEV,   // encoder device error or can't open
     MCC_E_MOUNTDEV,     // mount device error or can't open
     MCC_E_FAILED,       // failed to run command - protocol error
+    MCC_E_AMOUNT        // Just amount of errors
 } mcc_errcodes_t;
 
 typedef struct{
-    char*   MountDevPath;       // path to mount device
-    int     MountDevSpeed;      // serial speed
-    char*   EncoderDevPath;     // path to encoder device
-    int     EncoderDevSpeed;    // serial speed
-    int     SepEncoder;         // ==1 if encoder works as separate serial device
-    double  MountReqInterval;   // maximal interval between subsequent mount requests (seconds)
-    ;
+    double P, I, D;
+} PIDpar_t;
+
+typedef struct{
+    char*   MountDevPath;           // path to mount device
+    int     MountDevSpeed;          // serial speed
+    char*   EncoderDevPath;         // path to encoder device
+    int     EncoderDevSpeed;        // serial speed
+    int     SepEncoder;             // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
+    char*   EncoderXDevPath;        // paths to new controller devices
+    char*   EncoderYDevPath;
+    double  EncodersDisagreement;   // acceptable disagreement between motor and axis encoders
+    double  MountReqInterval;       // interval between subsequent mount requests (seconds)
+    double  EncoderReqInterval;     // interval between subsequent encoder requests (seconds)
+    double  EncoderSpeedInterval;   // interval between speed calculations
+    int     RunModel;               // == 1 if you want to use model instead of real mount
+    double  PIDMaxDt;               // maximal PID refresh time interval (if larger all old data will be cleared)
+    double  PIDRefreshDt;           // normal PID refresh interval
+    double  PIDCycleDt;             // PID I cycle time (analog of "RC" for PID on opamps)
+    PIDpar_t XPIDC;                 // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
+    PIDpar_t XPIDV;
+    PIDpar_t YPIDC;
+    PIDpar_t YPIDV;
+    double  MaxPointingErr;         // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
+    double  MaxFinePointingErr;     // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
+    double  MaxGuidingErr;          // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
+    int     XEncZero;               // encoders' zero position
+    int     YEncZero;
 } conf_t;
 
-// coordinates in degrees: X, Y and time when they were reached
+// coordinates/speeds in degrees or d/s: X, Y
 typedef struct{
-    double X; double Y; struct timeval msrtime;
-} coords_t;
+    double X; double Y;
+} coordpair_t;
+
+// coordinate/speed and time of last measurement
+typedef struct{
+    double val;
+    struct timespec t;
+} coordval_t;
+
+typedef struct{
+    coordval_t X;
+    coordval_t Y;
+} coordval_pair_t;
 
 // data to read/write
 typedef struct{
@@ -77,8 +123,8 @@ typedef struct{
     /* If 1, we are in computerless Slew and Track mode
        (no clutches; use handpad to slew; must be in Drag and Track mode too) */
     uint8_t slewtrack :1;
-    uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
-    uint8_t digin :3; // Digital input from radio handpad receiver
+    uint8_t digin_sens :1;  // Digital input from radio handpad receiver, or RA PEC Sensor sync
+    uint8_t digin :3;       // Digital input from radio handpad receiver
 } ybits_t;
 
 typedef struct{
@@ -89,25 +135,30 @@ typedef struct{
     uint16_t ain1;
 } extradata_t;
 
+typedef enum{
+    AXIS_STOPPED,
+    AXIS_SLEWING,
+    AXIS_POINTING,
+    AXIS_GUIDING,
+    AXIS_ERROR,
+} axis_status_t;
+
 typedef struct{
-    coords_t motposition;
-    coords_t encposition;
-    coords_t lastmotposition;
+    axis_status_t Xstate;
+    axis_status_t Ystate;
+    coordval_t motXposition;
+    coordval_t motYposition;
+    coordval_t encXposition;
+    coordval_t encYposition;
+    coordval_t encXspeed; // once per <config> s
+    coordval_t encYspeed;
     uint8_t keypad;
     extradata_t extradata;
     uint32_t millis;
     double temperature;
     double voltage;
-    int32_t XmotRaw;
-    int32_t YmotRaw;
-    int32_t XencRaw;
-    int32_t YencRaw;
 } mountdata_t;
 
-typedef struct{
-    ;
-} mountstat_t;
-
 typedef struct{
     double Xmot;        // 0  X motor position (rad)
     double Xspeed;      // 4  X speed (rad/s)
@@ -129,7 +180,7 @@ typedef struct{
     double Yatime;      // 28
 } long_command_t; // long command
 
-// hardware axe configuration
+// hardware axis configuration
 typedef struct{
     double accel;       // Default Acceleration, rad/s^2
     double backlash;    // Backlash (???)
@@ -140,13 +191,16 @@ typedef struct{
     double outplimit;   // Output Limit, percent (0..100)
     double currlimit;   // Current Limit (A)
     double intlimit;    // Integral Limit (???)
-} __attribute__((packed)) axe_config_t;
+    // these params are taken from mount by text commands (don't save negative values - better save these marks in xybits
+    double motor_stepsperrev;// encoder's steps per revolution: motor and axis
+    double axis_stepsperrev; // negative sign of these values means reverse direction
+} __attribute__((packed)) axis_config_t;
 
 // hardware configuration
 typedef struct{
-    axe_config_t Xconf;
+    axis_config_t Xconf;
     xbits_t xbits;
-    axe_config_t Yconf;
+    axis_config_t Yconf;
     ybits_t ybits;
     uint8_t address;
     double eqrate;      // Equatorial Rate (???)
@@ -166,27 +220,42 @@ typedef struct{
     uint32_t baudrate;  // Baud Rate (baud)
     double locsdeg;     // Local Search Degrees (rad)
     double locsspeed;   // Local Search Speed (rad/s)
-    double backlspd;    // Backlash speed (???)
+    double backlspd;    // Backlash speed (rad/s)
 } hardware_configuration_t;
 
+/* flags for slew function
+typedef struct{
+    uint32_t slewNguide : 1; // ==1 to guide after slewing
+} slewflags_t;
+*/
 // mount class
 typedef struct{
+    // TODO: on init/quit clear all XY-bits to default`
     mcc_errcodes_t  (*init)(conf_t *c); // init device
     void            (*quit)(); // deinit
     mcc_errcodes_t  (*getMountData)(mountdata_t *d); // get last data
-    mcc_errcodes_t  (*moveTo)(const double *X, const double *Y); // move to given position ans stop
-    mcc_errcodes_t  (*moveWspeed)(const coords_t *target, const  coords_t *speed); // move with given max speed
-    mcc_errcodes_t  (*setSpeed)(const double *X, const double *Y); // set speed
+//    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
+    mcc_errcodes_t  (*correctTo)(const coordval_pair_t *target);
+    mcc_errcodes_t  (*moveTo)(const coordpair_t *target); // move to given position and stop
+    mcc_errcodes_t  (*moveWspeed)(const coordpair_t *target, const  coordpair_t *speed); // move with given max speed
+    mcc_errcodes_t  (*setSpeed)(const coordpair_t *tagspeed); // set speed
     mcc_errcodes_t  (*stop)(); // stop
     mcc_errcodes_t  (*emergStop)(); // emergency stop
     mcc_errcodes_t  (*shortCmd)(short_command_t *cmd); // send/get short command
     mcc_errcodes_t  (*longCmd)(long_command_t *cmd); // send/get long command
     mcc_errcodes_t  (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
+    mcc_errcodes_t  (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
+    int             (*currentT)(struct timespec *t); // current time
+    double          (*timeFromStart)(); // amount of seconds from last init
+    double          (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times
+    double          (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time
+    mcc_errcodes_t  (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
+    mcc_errcodes_t  (*getMinSpeed)(coordpair_t *v); // minimal -//-
+    mcc_errcodes_t  (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
 } mount_t;
 
 extern mount_t Mount;
 
-
 #ifdef __cplusplus
 }
 #endif

Auxiliary_utils/LibSidServo/ssii.c

@@ -18,12 +18,17 @@
 
 #include <ctype.h>
 #include <inttypes.h>
+#include <stdio.h>
 #include <string.h>
+#include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
 #include "serial.h"
 #include "ssii.h"
 
+int X_ENC_ZERO, Y_ENC_ZERO;
+double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.;
+
 uint16_t SScalcChecksum(uint8_t *buf, int len){
     uint16_t checksum = 0;
     for(int i = 0; i < len; i++){
@@ -35,32 +40,50 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
     return checksum;
 }
 
+// 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){
+    if(*prev == INT32_MAX){
+        *stat = AXIS_STOPPED;
+        DBG("START");
+    }else if(*stat != AXIS_STOPPED){
+        if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
+            *stat = AXIS_STOPPED;
+            DBG("AXIS stopped");
+        }
+    }else if(*prev != cur){
+        DBG("AXIS moving");
+        *nstopped = 0;
+    }
+    *prev = cur;
+}
+// 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);
+    chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
+}
+
 /**
  * @brief SSconvstat - convert stat from SSII format to human
- * @param status (i) - just read data
- * @param mountdata (o) - output
+ * @param s (i) - just read data
+ * @param m (o) - output
+ * @param t - measurement time
  */
-void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){
-    if(!s || !m || !tdat) return;
-/*
-#ifdef EBUG
-    static double t0 = -1.;
-    if(t0 < 0.) t0 = dtime();
-#endif
-    DBG("Convert, t=%g", dtime()-t0);
-*/
-    m->motposition.X = X_MOT2RAD(s->Xmot);
-    m->motposition.Y = Y_MOT2RAD(s->Ymot);
-    m->motposition.msrtime = *tdat;
+void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
+    if(!s || !m || !t) return;
+    m->motXposition.val = X_MOT2RAD(s->Xmot);
+    m->motYposition.val = Y_MOT2RAD(s->Ymot);
+    ChkStopped(s, m);
+    m->motXposition.t = m->motYposition.t = *t;
     // fill encoder data from here, as there's no separate enc thread
     if(!Conf.SepEncoder){
-        m->encposition.X = X_ENC2RAD(s->Xenc);
-        m->encposition.Y = Y_ENC2RAD(s->Yenc);
-        m->encposition.msrtime = *tdat;
+        m->encXposition.val = Xenc2rad(s->Xenc);
+        DBG("encx: %g", m->encXposition.val);
+        m->encYposition.val = Yenc2rad(s->Yenc);
+        m->encXposition.t = m->encYposition.t = *t;
+        getXspeed(); getYspeed();
     }
-    m->lastmotposition.X = X_MOT2RAD(s->XLast);
-    m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
-    m->lastmotposition.msrtime = *tdat;
     m->keypad = s->keypad;
     m->extradata.ExtraBits = s->ExtraBits;
     m->extradata.ain0 = s->ain0;
@@ -86,7 +109,7 @@ int SStextcmd(const char *cmd, data_t *answer){
     data_t d;
     d.buf = (uint8_t*) cmd;
     d.len = d.maxlen = strlen(cmd);
-    DBG("send %zd bytes: %s", d.len, d.buf);
+    //DBG("send %zd bytes: %s", d.len, d.buf);
     return MountWriteRead(&d, answer);
 }
 
@@ -99,7 +122,7 @@ int SSrawcmd(const char *cmd, data_t *answer){
     data_t d;
     d.buf = (uint8_t*) cmd;
     d.len = d.maxlen = strlen(cmd);
-    DBG("send %zd bytes: %s", d.len, d.buf);
+    //DBG("send %zd bytes: %s", d.len, d.buf);
     return MountWriteReadRaw(&d, answer);
 }
 
@@ -153,3 +176,50 @@ int SSstop(int emerg){
     return TRUE;
 }
 
+// update motors' positions due to encoders'
+mcc_errcodes_t updateMotorPos(){
+    mountdata_t md = {0};
+    if(Conf.RunModel) return MCC_E_OK;
+    double t0 = timefromstart(), t = 0.;
+    struct timespec curt;
+    DBG("start @ %g", t0);
+    do{
+        t = timefromstart();
+        if(!curtime(&curt)){
+            usleep(10000);
+            continue;
+        }
+        //DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
+        if(MCC_E_OK == getMD(&md)){
+            if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
+                DBG("Just started? t-t0 = %g!", t - t0);
+                usleep(10000);
+                continue;
+            }
+            if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
+            DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
+            mcc_errcodes_t OK = MCC_E_OK;
+            if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
+                DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
+                DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
+                if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
+                    DBG("Xpos sync failed!");
+                    OK = MCC_E_FAILED;
+                }else DBG("Xpos sync OK, Dt=%g", t - t0);
+            }
+            if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
+                DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
+                if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
+                    DBG("Ypos sync failed!");
+                    OK = MCC_E_FAILED;
+                }else DBG("Ypos sync OK, Dt=%g", t - t0);
+            }
+            if(MCC_E_OK == OK){
+                DBG("Encoders synced");
+                return OK;
+            }
+        }
+        DBG("NO DATA; dt = %g", t - t0);
+    }while(t - t0 < 2.);
+    return MCC_E_FATAL;
+}

Auxiliary_utils/LibSidServo/ssii.h

@@ -16,6 +16,10 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * This file contains stuff for sidereal-servo specific protocol
+ */
+
 #pragma once
 
 #include <math.h>
@@ -165,44 +169,76 @@
 // X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits
 #define CMD_GETSTATTEXT "\r"
 
+// Loop freq
+#define SITECH_LOOP_FREQUENCY   (1953.)
 
+// amount of consequent same coordinates to detect stop
+#define MOTOR_STOPPED_CNT       (19)
 
-// steps per revolution
-#define X_MOT_STEPSPERREV   (3325952.)
-#define Y_MOT_STEPSPERREV   (4394960.)
+// replace macros with global variables inited when config read
+extern int X_ENC_ZERO, Y_ENC_ZERO;
+extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
 
-// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y
-#define X_SPEED_MAX         (0.17453)
-#define Y_SPEED_MAX         (0.13963)
-
-// motor position to radians and back
-#define X_MOT2RAD(n)    (2.*M_PI * ((double)n) / X_MOT_STEPSPERREV)
-#define Y_MOT2RAD(n)    (2.*M_PI * ((double)n) / Y_MOT_STEPSPERREV)
-#define X_RAD2MOT(r)    ((int32_t)((r) / 2./M_PI * X_MOT_STEPSPERREV))
-#define Y_RAD2MOT(r)    ((int32_t)((r) / 2./M_PI * Y_MOT_STEPSPERREV))
-// motor speed in rad/s and back
-#define X_MOTSPD2RS(n)  (X_MOT2RAD(n)/65536.*1953.)
-#define X_RS2MOTSPD(r)  ((int32_t)(X_RAD2MOT(r)*65536./1953.))
-#define Y_MOTSPD2RS(n)  (Y_MOT2RAD(n)/65536.*1953.)
-#define Y_RS2MOTSPD(r)  ((int32_t)(Y_RAD2MOT(r)*65536./1953.))
-// motor acceleration -//-
-#define X_MOTACC2RS(n)  (X_MOT2RAD(n)/65536.*1953.*1953.)
-#define X_RS2MOTACC(r)  ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.))
-#define Y_MOTACC2RS(n)  (Y_MOT2RAD(n)/65536.*1953.*1953.)
-#define Y_RS2MOTACC(r)  ((int32_t)(Y_RAD2MOT(r)*65536./1953./1953.))
-
-// adder time to seconds vice versa
-#define ADDER2S(a)  ((a)*1953.)
-#define S2ADDER(s)  ((s)/1953.)
+// TODO: take it from settings?
+// steps per revolution (SSI - x4 - for SSI)
+// -> hwconf.Xconf.mot/enc_stepsperrev
+//#define X_MOT_STEPSPERREV_SSI   (13312000.)
+// 13312000 / 4 = 3328000
+//#define X_MOT_STEPSPERREV   (3328000.)
+//#define Y_MOT_STEPSPERREV_SSI (17578668.)
+// 17578668 / 4 = 4394667
+//#define Y_MOT_STEPSPERREV   (4394667.)
 
 // encoder per revolution
-#define X_ENC_STEPSPERREV   (67108864.)
-#define Y_ENC_STEPSPERREV   (67108864.)
+//#define X_ENC_STEPSPERREV   (67108864.)
+//#define Y_ENC_STEPSPERREV   (67108864.)
+// encoder zero position
+// -> conf.XEncZero/YEncZero
+//#define X_ENC_ZERO          (61245239)
+//#define Y_ENC_ZERO          (36999830)
+// encoder reversed (no: +1) -> sign of ...stepsperrev
+//#define X_ENC_SIGN          (-1.)
+//#define Y_ENC_SIGN          (-1.)
+
+
 // encoder position to radians and back
-#define X_ENC2RAD(n)    (2.*M_PI * ((double)n) / X_ENC_STEPSPERREV)
-#define Y_ENC2RAD(n)    (2.*M_PI * ((double)n) / Y_ENC_STEPSPERREV)
-#define X_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
-#define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
+#define Xenc2rad(n)    ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
+#define Yenc2rad(n)    ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
+#define Xrad2enc(r)    ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
+#define Yrad2enc(r)    ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
+
+// convert angle in radians to +-pi
+static inline __attribute__((always_inline)) double ang2half(double ang){
+    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){
+    if(ang < 0.) ang += 2.*M_PI;
+    else if(ang > 2.*M_PI) ang -= 2.*M_PI;
+    return ang;
+}
+
+// motor position to radians and back
+#define X_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
+#define Y_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
+#define X_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
+#define Y_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
+// motor speed in rad/s and back
+#define X_MOTSPD2RS(n)  (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
+#define Y_MOTSPD2RS(n)  (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
+#define X_RS2MOTSPD(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
+#define Y_RS2MOTSPD(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
+// motor acceleration -//-
+#define X_MOTACC2RS(n)  (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
+#define Y_MOTACC2RS(n)  (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
+#define X_RS2MOTACC(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
+#define Y_RS2MOTACC(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
+
+// adder time to seconds vice versa
+#define ADDER2S(a)  ((a) / (SITECH_LOOP_FREQUENCY))
+#define S2ADDER(s)  ((s) * (SITECH_LOOP_FREQUENCY))
 
 // encoder's tolerance (ticks)
 #define YencTOL    (25.)
@@ -303,10 +339,10 @@ typedef struct{
 } __attribute__((packed)) SSconfig;
 
 uint16_t SScalcChecksum(uint8_t *buf, int len);
-void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat);
+void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
 int SStextcmd(const char *cmd, data_t *answer);
 int SSrawcmd(const char *cmd, data_t *answer);
 int SSgetint(const char *cmd, int64_t *ans);
 int SSsetterI(const char *cmd, int32_t ival);
 int SSstop(int emerg);
-int SSshortCmd(SSscmd *cmd);
+mcc_errcodes_t updateMotorPos();

Auxiliary_utils/PCS_create/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.30)
 set(PROJ PCS_create)
 set(MINOR_VERSION "0")
 set(MID_VERSION "1")

Auxiliary_utils/PCS_create/PCS.c

@@ -321,7 +321,7 @@ static void printheader(){
 }
 
 int main(int argc, char **argv) {
-    initial_setup();
+    sl_init();
     G = parse_args(argc, argv);
     if(G->pressure < 0.) ERRX("Pressure should be greater than zero");
     if(G->temperature < -100. || G->temperature > 100.) ERRX("Temperature over the range -100..+100");

Auxiliary_utils/PCS_create/cmdlnopts.c

@@ -40,7 +40,7 @@ glob_pars const Gdefault = {
  * Define command line options by filling structure:
  *  name    has_arg flag    val     type        argptr          help
 */
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
     // set 1 to param despite of its repeating number:
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        _("show this help")},
     {"10m",     NO_ARGS,    NULL,   't',    arg_int,    APTR(&G.for10m),    _("make output suitable for 10-micron mount")},
@@ -68,10 +68,10 @@ static myoption cmdlnopts[] = {
 glob_pars *parse_args(int argc, char **argv){
     void *ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr);
     // format of help: "Usage: progname [args]\n"
-    change_helpstring(_("Version: " PACKAGE_VERSION "\nUsage: %s [args] FITS_files\nMake PCS list for equatorial mount\n\tWhere args are:\n"));
+    sl_helpstring(_("Version: " PACKAGE_VERSION "\nUsage: %s [args] FITS_files\nMake PCS list for equatorial mount\n\tWhere args are:\n"));
     // parse arguments
-    parseargs(&argc, &argv, cmdlnopts);
-    if(help) showhelp(-1, cmdlnopts);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(help) sl_showhelp(-1, cmdlnopts);
     G.nfiles = argc;
     G.infiles = MALLOC(char*, argc);
     for(int i = 0; i < argc; i++){

Auxiliary_utils/SILA_inverter/Makefile

@@ -0,0 +1,59 @@
+# run `make DEF=...` to add extra defines
+PROGRAM := invertercmd
+LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
+LDFLAGS += -lusefull_macros
+SRCS := $(wildcard *.c)
+DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
+OBJDIR := mk
+CFLAGS += -O2 -Wall -Wextra -Wno-trampolines
+OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
+DEPS := $(OBJS:.o=.d)
+TARGFILE := $(OBJDIR)/TARGET
+CC = gcc
+#TARGET := RELEASE
+
+ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
+	TARGET := $(file < $(TARGFILE))
+else
+	TARGET := RELEASE
+endif
+
+ifeq ($(TARGET), DEBUG)
+	.DEFAULT_GOAL := debug
+endif
+
+release: CFLAGS += -flto
+release: LDFLAGS += -flto
+release: $(PROGRAM)
+
+debug: CFLAGS += -DEBUG -Werror
+debug: TARGET := DEBUG
+debug: $(PROGRAM)
+
+$(TARGFILE): $(OBJDIR)
+	@echo -e "\t\tTARGET: $(TARGET)"
+	@echo "$(TARGET)" > $(TARGFILE)
+
+$(PROGRAM) : $(TARGFILE) $(OBJS)
+	@echo -e "\t\tLD $(PROGRAM)"
+	$(CC) $(LDFLAGS) $(OBJS) -o $(PROGRAM)
+
+$(OBJDIR):
+	@mkdir $(OBJDIR)
+
+ifneq ($(MAKECMDGOALS),clean)
+-include $(DEPS)
+endif
+
+$(OBJDIR)/%.o: %.c
+	@echo -e "\t\tCC $<"
+	$(CC) -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ $<
+
+clean:
+	@echo -e "\t\tCLEAN"
+	@rm -rf $(OBJDIR) 2>/dev/null || true
+
+xclean: clean
+	@rm -f $(PROGRAM)
+
+.PHONY: clean xclean

Auxiliary_utils/SILA_inverter/Readme.md

@@ -0,0 +1 @@
+Serial communications with SILA inverters

Auxiliary_utils/SILA_inverter/getparams.c

@@ -0,0 +1,414 @@
+/*
+ * Copyright 2025 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 <ctype.h>
+#include <stdio.h>
+#include <string.h>
+#include <usefull_macros.h>
+
+static sl_tty_t *dev = NULL;
+static const char *wro = "(wrong parameter)";
+
+typedef struct{
+    int help;
+    char *path;
+    char *customcmd;
+    char *getstatus;
+    int baudrate;
+    int status;
+    int settershelp;
+} globopts;
+
+typedef struct{
+    int all;
+    int help;
+    int rating;
+    int flag;
+    int status;
+    int mode;
+    int warning;
+    int deflt;
+    int bateq;
+} statusinfo;
+
+typedef void (*parsefn)(const char *answer);
+
+static globopts G = {
+    .path = "/dev/ttyS0",
+    .baudrate = 2400,
+};
+
+static statusinfo S = {0};
+
+static sl_option_t opts[] = {
+    {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
+    {"devpath", NEED_ARG,   NULL,   'd',    arg_string, APTR(&G.path),      "path to device (default: /dev/ttyS0)"},
+    {"baudrate",NEED_ARG,   NULL,   'b',    arg_int,    APTR(&G.baudrate),  "baudrate (default: 2400)"},
+    {"cmd",     NEED_ARG,   NULL,   'c',    arg_string, APTR(&G.customcmd), "custom command to send"},
+    {"status",  NEED_ARG,   NULL,   's',    arg_string, APTR(&G.getstatus), "get status: comma-separated options (type 'help' for options)"},
+    {"helpsetters",NO_ARGS, NULL,   0,      arg_int,    APTR(&G.settershelp),"show help about setters"},
+    end_option
+};
+
+static sl_suboption_t sopts[] = {
+    {"all",     NO_ARGS,    arg_int,    APTR(&S.all)},
+    {"help",    NO_ARGS,    arg_int,    APTR(&S.help)},
+    {"rating",  NO_ARGS,    arg_int,    APTR(&S.rating)},
+    {"flag",    NO_ARGS,    arg_int,    APTR(&S.flag)},
+    {"status",  NO_ARGS,    arg_int,    APTR(&S.status)},
+    {"mode",    NO_ARGS,    arg_int,    APTR(&S.mode)},
+    {"warning", NO_ARGS,    arg_int,    APTR(&S.warning)},
+    {"default", NO_ARGS,    arg_int,    APTR(&S.deflt)},
+    {"bateq",   NO_ARGS,    arg_int,    APTR(&S.bateq)},
+    end_suboption
+};
+
+static void gettershelp(){
+    fprintf(stderr, "Status parameters:\n");
+    fprintf(stderr, "all - show all information available\n");
+    fprintf(stderr, "rating - device rating information (QPIRI)\n");
+    fprintf(stderr, "flag - device flag status (QFLAG)\n");
+    fprintf(stderr, "status - device general status parameters (QPIGS)\n");
+    fprintf(stderr, "mode - device mode (QMOD)\n");
+    fprintf(stderr, "warning - warning status (QPIWS)\n");
+    fprintf(stderr, "default - default settings (QDI)\n");
+    fprintf(stderr, "bateq - battery equalization parameters (QBEQI)\n");
+//  fprintf(stderr, "\n");
+}
+
+static uint8_t *cal_crc(const char *cmd, int len){
+    static uint8_t CRC[4]; // 0 - hi, 1 - low, four bytes for quick zeroing
+    if(!cmd || len < 1) return 0;
+    const uint16_t crc_table[16] = {
+        0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
+        0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef
+    };
+    *((uint32_t*)CRC) = 0;
+    uint16_t crc = 0;
+    uint8_t *ptr = (uint8_t*)cmd;
+    while(len--){
+        uint8_t da = ((uint8_t)(crc >> 8)) >> 4;
+        crc <<= 4;
+        crc ^= crc_table[da ^ (*ptr >> 4)];
+        da = ((uint8_t)(crc >> 8)) >> 4;
+        crc <<= 4;
+        crc ^= crc_table[da ^ (*ptr & 0x0f)];
+        ptr++;
+    }
+    uint8_t bCRCLow = crc;
+    uint8_t bCRCHign= (uint8_t)(crc >> 8);
+    if(bCRCLow==0x28 || bCRCLow==0x0d || bCRCLow==0x0a) ++bCRCLow;
+    if(bCRCHign==0x28 || bCRCHign==0x0d || bCRCHign==0x0a) ++bCRCHign;
+    CRC[0] = bCRCHign; CRC[1] = bCRCLow; CRC[2] = '\r';
+    DBG("CRC: 0x%02X 0x%02X 0x%02X", CRC[0], CRC[1], CRC[2]);
+    return CRC;
+}
+
+static int sendcmd(const char *cmd){
+    if(!cmd || !*cmd || !dev) return 0;
+    int len = strlen(cmd);
+    uint8_t *CRC = cal_crc(cmd, len);
+#ifdef EBUG
+    printf("COMMAND: ");
+    for(int i = 0; i < len; ++i) printf("0x%02X ", cmd[i]);
+    printf("\n");
+#endif
+    if(sl_tty_write(dev->comfd, cmd, len)){
+        WARN("Can't write command");
+        return 0;
+    }
+    if(sl_tty_write(dev->comfd, (const char*) CRC, 3)){
+        WARN("Can't write CRC & STREND");
+        return 0;
+    }
+    DBG("Command %s sent", cmd);
+    return 1;
+}
+
+static char *rd(){
+    if(!dev) return NULL;
+    int got = sl_tty_read(dev);
+    if(got < 0) ERR("Can't read");
+    DBG("got %d bytes, buflen: %zd", got, dev->buflen);
+    if(dev->buflen < 3) return NULL;
+    uint8_t *CRC = cal_crc(dev->buf, dev->buflen - 3);
+    DBG("GOT CRC: 0x%02X 0x%02X", (uint8_t)dev->buf[dev->buflen-3], (uint8_t)dev->buf[dev->buflen-2]);
+    if(CRC[0] != (uint8_t)dev->buf[dev->buflen-3] || CRC[1] != (uint8_t)dev->buf[dev->buflen-2]){
+        WARNX("Bad CRC");
+        return NULL;
+    }
+    char *r = dev->buf + 1;
+    dev->buf[dev->buflen-3] = 0;
+    return r;
+}
+// 230.0 13.0 230.0 50.0 13.0 3000 3000 24.0 23.0 21.0 28.2 27.0 0 25 50 0 0 2 - 01 1 0 27.0 0 0
+// b      c    d     e   f      h    i   j1  k1    j2   k2   l   o1 p1q0 o2p2q2r s  t u  v   w x
+static void ratingparsing(const char *str){
+    float b, c, d, e, f, j1, k1, j2, k2, l, v;
+    int h, i, o1, p1, q, o2, p2, q2, s, t, u, w, x;
+    char r;
+    int N = sscanf(str, "%f %f %f %f %f %d %d %f %f %f %f %f %d %d %d %d %d %d %c %d %d %d %f %d %d",
+        &b, &c, &d, &e, &f, &h, &i, &j1, &k1, &j2, &k2, &l, &o1, &p1, &q, &o2, &p2, &q2, &r, &s, &t,
+                   &u, &v, &w, &x);
+    if(N != 25){ WARNX("Got not full answer (%d instead of 25): '%s'", N, str); return; }
+    printf("Grid rating voltage: %g\nGrid rating current: %g\nAC optuput rating voltage: %g\n", b, c, d);
+    printf("AC output rating frequency: %g\nAC output rating current: %g\nAC output rating apparent power: %d\n", e, f, h);
+    printf("AC output rating active power: %d\nBattery rating voltage: %g\nBattery recharge voltage: %g\n", i, j1, k1);
+    printf("Battery undervoltage: %g\nBattery bulk voltage: %g\nBattery float voltage: %g\n", j2, k2, l);
+    static const char *types[] = {"AGM", "Flooded", "User"};
+    printf("Battery type: %s\n", (o1 > -1 && o1 < 3) ? types[o1] : wro);
+    printf("Current max AC charging current: %d\nCurrent max charging current: %d\n", p1, q);
+    printf("Input voltage range: %s\n", o2 ? "UPS" : "Appliance");
+    static const char *oprio[] = {"Utility", "Solar", "SBU"};
+    printf("Output source priority: %s\n", (p2 > -1 && p2 < 3) ? oprio[p2] : wro);
+    static const char *sprio[] = {"Utility", "Solar", "Solar+Utility", "Only solar charging"};
+    printf("Charger source priority: %s\n", (q2 > -1 && q2 < 4) ? sprio[q2] : wro);
+    printf("Parallel max num: %c\n", r);
+    printf("Machine type: ");
+    switch(s){
+        case 0: printf("Grid tie"); break;
+        case 1: printf("Off grid"); break;
+        case 10: printf("Hybrid"); break;
+        default: printf("%s", wro);
+    }
+    printf("\nTopology: %s\n", (t) ? "transformer" : "transformerless");
+    printf("Output mode: %d\nBattery redischarge voltage: %g\n", u, v);
+    printf("PV OK condition for parallel: %s\n", (w) ? "Only all connected" : "At least one connected");
+    printf("PV power balance: %s\n", (x) ? "Sum of powers" : "Max charged current");
+}
+
+static const char *DEflags = "abjkuvxyz";
+static const char *DEmeanings[] = {
+    "buzzer", "bypass", "power saving", "LCD display escape 1min", "overload restart",
+    "over temperature restart", "backlight on", "alarm on interrupt", "fault code record",
+    NULL
+};
+
+static void flagparsing(const char *str){
+    char c;
+    int first = 0;
+    while((c = *str++)){
+        if(c == 'D'){ green("\nDISABLED: "); first = 1; }
+        else if(c == 'E'){ red("ENABLED: "); first = 1; }
+        else{
+            const char *field = "unknown";
+            for(int i = 0; DEflags[i]; ++i){
+                if(DEflags[i] == c){ field = DEmeanings[i]; break; }
+            }
+            printf("%s%s", (first) ? "" : ", ", field);
+            first = 0;
+        }
+    }
+    printf("\n");
+}
+
+/**
+ * @brief showflags - display bitflags
+ * @param flags - string with flags ('1' / '0')
+ * @param meaning - array with meaning of each flag, starting from H bit, i.e. meaning[0] is lesser bit
+ * @param nfields - strlen of flags (or less), H first
+ */
+static void showflags(const char *flags, const char **meaning, int nfields){
+    for(int i = 0; i < nfields; ++i){
+        if(!meaning[i]) continue;
+        printf("\t%s: ", meaning[i]);
+        if(flags[i] == '1') red("on/yes\n");
+        else green("off/no\n");
+    }
+}
+
+// 230.0 50.0 232.0 50.0 0000 0000 000 409 26.99 000 100 0489 0000 000.0 00.00 00000 10011101 00 03 00000 100
+// b      c    d     e    f    g    h   i   j     k   o   t    e1    u    w      p     x       [unknown shit]
+static void statusparsing(const char *str){
+    float b, c, d, e, j, u, w;
+    int f, g, h, i, k, o, t, e1, p, S, H, I;
+    char x[9], T[4];
+    int N = sscanf(str, "%f %f %f %f %d %d %d %d %f %d %d %d %d %f %f %d %8s %d %d %d %3s",
+                    &b, &c, &d, &e, &f, &g, &h, &i, &j, &k, &o, &t, &e1, &u, &w, &p, x, &S, &H, &I, T);
+    DBG("N=%d", N);
+    if(N >= 17){
+        printf("Grid voltage: %g\nGrid frequency: %g\nAC output voltage: %g\nAC output frequency: %g\n",
+               b, c, d, e);
+        printf("AC output apparent power: %d\nAC output active power: %d\nOutput load percent: %d\n", f, g, h);
+        printf("Bus voltage: %d\nBattery voltage: %g\nBattery charging current: %d\nBattery capacity: %d\n", i, j, k, o);
+        printf("Inverter heat sink temperature: %d\nPV input current for battery: %d\nPV input voltage 1: %g\n", t, e1, u);
+        printf("Battery voltage from SCC: %g\nBattery discharge current: %d\nDevice status:\n", w, p);
+        static const char *sf[] = {"SBU priority version", "configuration changed", "SCC firmware updated",
+                                "Load status", "Steady batt voltage while charging", "Charging", "SCC charging", "AC charging"};
+        showflags(x, sf, 8);
+        if(N > 17){
+            printf("Battery offset for fans on: %d\nEEPROM version: %d\nPV charging power: %d\n", S, H, I);
+            printf("Inverter status:\n");
+            static const char *is[] = {"Charging to floating mode", "Switch", "Dustproof installed"};
+            showflags(T, is, 3);
+        }
+    }else WARNX("Get not full answer: %d instead of 17", N);
+}
+
+static void modeparsing(const char *str){
+    printf("Device mode: ");
+    switch(*str){
+        case 'B': printf("Battery"); break;
+        case 'F': printf("Fault"); break;
+        case 'H': printf("Power saving"); break;
+        case 'L': printf("Line"); break;
+        case 'P': printf("Power on"); break;
+        case 'S': printf("Standby"); break;
+        default: printf("Unknown");
+    }
+    printf("\n");
+}
+
+static void warningparsing(const char *str){
+    int l = strlen(str);
+    if(l < 32) WARNX("Non-full status! Data could be wrong");
+    static const char *wmsgs[] = {
+        NULL, "Inverter fault", "Bus over", "Bus under", "Bus soft fail", "Line fail", "OPV short",
+        "Inv voltage too low", "Inv voltage too high", "Over temperature", "Fan locked", "Battery voltage high",
+        "Battery low alarm", "Overcharge", "Battery under shutdown", "Battery derating", "Overload", "EEPROM fault",
+        "Inverter overcurrent", "Inverter soft fail", "Self test fail", "OP DC voltage over", "Bat open",
+        "Current sensor fail", "Battery short", "Power limit", "PV voltage high", "MPPT overload fault",
+        "MPPT overload warning", "Battery too low to charge", NULL, NULL
+    };
+    printf("Warning status:\n");
+    showflags(str, wmsgs, l);
+
+}
+
+static const char *endis(int val){
+    return (val) ? COLOR_RED "enable" COLOR_OLD : COLOR_GREEN "disable" COLOR_OLD;
+}
+
+// 230.0 50.0 0025 21.0 27.0 28.2 23.0 50 0 0 2 0 1 0 0 0 1 1 1 0 1 0 27.0 0 0 ()
+//  b     c     d   e    f    g    h    i j k l m n o p q r s t u v w y    x z  a
+static void defaultparsing(const char *str){
+    float b, c, e, f, g, h, y;
+    int d, i, j, k, l, m, n, o, p, q, r, s, t, u, v, w, x, z, a;
+    int N = sscanf(str, "%f %f %d %f %f %f %f %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %f %d %d %d",
+                   &b, &c, &d, &e, &f, &g, &h, &i, &j, &k, &l, &m, &n, &o, &p, &q, &r, &s, &t, &u, &v, &w, &y, &x, &z, &a);
+    if(N < 25){WARNX("Wrong data format: %d fields instead of 25", N); return;}
+    printf("AC output voltage: %g\nAC output frequency: %g\nMax AC charging current: %d\n", b, c, d);
+    printf("Battery undervoltage: %g\nCharging float voltage: %g\nCharging bulk voltage: %g\n", e, f, g);
+    printf("Battery default recharge voltage: %g\nMax charging current: %d\nAC input voltage range: %s\n", h, i, (j) ? "UPS" : "appliance");
+    printf("Output source priority: %s\nCharger source priority: %s\n", (k) ? "solar first": "utility first", (l) ? "solar first" : "utility first");
+    printf("Battery type: %s\nBuzzer: %s\nPover saving: %s\n", (m) ? "other" : "AGM", endis(n), endis(o));
+    printf("Overload restart: %s\nOver temperature restart: %s\n", endis(p), endis(q));
+    printf("Backlight: %s\nAlarm on interrupt: %s\nFault code record: %s\n", endis(r), endis(s), endis(t));
+    printf("Overload bypass: %s\nLCD timeout escape: %s\nOutput mode: %d\n", endis(u), endis(v), w);
+    printf("Battery re-discharge voltage: %g\nPV OK condition for parallel: %s\n", y, (x) ? "all" : "any");
+    printf("PV power balance: %s\n", (z) ? "?" : "PV max current is charged current");
+    if(N == 26) printf("Max charging time @ CV stage: %s\n", (a) ? "?" : "automatically");
+}
+
+// 0 060 030 050 030 29.20 000 120 0 0000
+// b  c   d   e   f   g     h   i  j  k
+static void equparsing(const char *str){
+    float g;
+    int b, c, d, e, f, h, i, j, k;
+    int N = sscanf(str, "%d %d %d %d %d %f%d %d %d %d", &b, &c, &d, &e, &f, &g, &h, &i, &j, &k);
+    if(N != 10){ WARNX("Not enougn parameters: got %d instead of 10\n", N); return; }
+    printf("Equalization: %s\n", endis(b));
+    printf("Eq. time: %d minutes\nEq. period: %d days\nEq. max current: %d\n", c, d, e);
+    printf("Eq. voltage: %g\nEq. over time: %d minutes\nEq. active status: %d\n", g, i, j);
+}
+
+static void printpar(const char *str){
+    printf("%s\n", str);
+}
+
+static void runparsing(const char *cmd, parsefn f){
+    if(sendcmd(cmd)){
+        char *got = rd();
+        if(got && *got){
+            f(got);
+            printf("\n");
+        } else red("Can't get data\n\n");
+    }
+}
+
+static void showstatus(){
+    green("\nModel name: "); runparsing("QMN", printpar);
+    if(S.all || S.rating) runparsing("QPIRI", ratingparsing);
+    if(S.all || S.flag) runparsing("QFLAG", flagparsing);
+    if(S.all || S.status) runparsing("QPIGS", statusparsing);
+    if(S.all || S.mode) runparsing("QMOD", modeparsing);
+    if(S.all || S.warning) runparsing("QPIWS", warningparsing);
+    if(S.all || S.deflt) runparsing("QDI", defaultparsing);
+    if(S.all || S.bateq) runparsing("QBEQI", equparsing);
+}
+
+static void showsettershelp(){
+    printf("\n\n");
+    red("Be carefull with setters! Think twice before changing something!!!\n\n");
+    printf("Here are setters...\n");
+    printf("PEx - enable status / DEx - disable status, where 'x':\n");
+    for(int i = 0; DEflags[i]; ++i) printf("\t%c - %s\n", DEflags[i], DEmeanings[i]);
+    printf("PF - set all control parameters to default\n");
+    printf("MCHGCx - max charging current (Amps)\n");
+    printf("MUCHGCx - utility max charging current\n");
+    printf("Fx - invertere output frequency\n");
+    printf("POPx - output source priority (0 - utility, 1 - solar, 2 - SBU)\n");
+    printf("PBCVx - battery re-charge voltage\n");
+    printf("PBDVx - battery re-discharge voltage\n");
+    printf("PCPx - inverter charging priority (0-3: utility first/solar first/solar+utility/solar only\n");
+    printf("PGRx - inverter grid voltage range (0 - appliance, 1 - UPS)\n");
+    printf("PBTx - battery type (0 - AGM, 1 - flooded)\n");
+    printf("PSDVx - battery cut-off voltage\n");
+    printf("PCVVx - CV (constant voltage) charging voltage\n");
+    printf("PBFTx - battery float charging voltage\n");
+    printf("PBEQEx - enable (1) or disable (0) battery equalization\n");
+    printf("PBEQT x - battery equalization time (minutes)\n");
+    printf("PBEQPx - battery equalization period (days)\n");
+    printf("PBEQVx - battery equalization voltage\n");
+    printf("PBEQOTx - battery equalization overtime (minutes)\n");
+    printf("PBEQAx - activate (1) or disactivate (0) battery equalization now\n");
+    printf("PCVTx - max charging time an CV stage\n");
+    printf("\n\nAnd some getters that aren't in `status` variants:\n");
+    printf("QID - inverter's serial\nQVFW - firmware version\nQMCHGCR - max charging currents available\n");
+    printf("QMUCHGCR - max utility charging currents available\n");
+    printf("\n\n");
+}
+
+int main(int argc, char **argv){
+    sl_init();
+    sl_parseargs(&argc, &argv, opts);
+    if(G.help) sl_showhelp(-1, opts);
+    if(!G.path || G.baudrate < 1) ERRX("Need device path and baudrate");
+    dev = sl_tty_new(G.path, G.baudrate, 128);
+    if(!dev) ERR("Can't init serial device");
+    dev = sl_tty_open(dev, 1);
+    if(!dev) ERR("Can't open %s", G.path);
+    if(sl_tty_tmout(100000)) WARNX("Can't set timeout");
+    DBG("bufsz: %zd", dev->bufsz);
+    if(G.getstatus){
+        if(!sl_get_suboption(G.getstatus, sopts) || S.help){
+            gettershelp();
+            return 1;
+        }
+        showstatus();
+    }
+    if(G.customcmd){
+        green("Try to send '%s'\n", G.customcmd);
+        if(sendcmd(G.customcmd)){
+            char *got = rd();
+            if(got) printf("Get data: '%s'\n", got);
+        }
+    }
+    sl_tty_close(&dev);
+    if(G.settershelp) showsettershelp();
+    return 0;
+}

Auxiliary_utils/Weather_chk/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.30)
 set(PROJ chkweather)
 set(MINOR_VERSION "1")
 set(MID_VERSION "0")

Auxiliary_utils/Weather_chk/cmdlnopts.c

@@ -40,7 +40,7 @@ glob_pars const Gdefault = {
  * Define command line options by filling structure:
  *  name    has_arg flag    val     type        argptr          help
 */
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
     // set 1 to param despite of its repeating number:
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        _("show this help")},
     {"speed",   NEED_ARG,   NULL,   's',    arg_int,    APTR(&G.speed),     _("baudrate (default: 9600)")},
@@ -59,10 +59,10 @@ static myoption cmdlnopts[] = {
 glob_pars *parse_args(int argc, char **argv){
     void *ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr);
     // format of help: "Usage: progname [args]\n"
-    change_helpstring(_("Usage: %s [args]\n\n\tWhere args are:\n"));
+    sl_helpstring(_("Usage: %s [args]\n\n\tWhere args are:\n"));
     // parse arguments
-    parseargs(&argc, &argv, cmdlnopts);
-    if(help) showhelp(-1, cmdlnopts);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(help) sl_showhelp(-1, cmdlnopts);
     if(argc > 0){
         WARNX("Wrong arguments:\n");
         for(int i = 0; i < argc; i++)

Auxiliary_utils/Weather_chk/main.c

@@ -51,28 +51,28 @@ static int getpar(char *string, double *Val, char *Name){
 
 int main(int argc, char **argv){
     glob_pars *G = NULL; // default parameters see in cmdlnopts.c
-    initial_setup();
+    sl_init();
     G = parse_args(argc, argv);
-    TTY_descr *dev = new_tty(G->ttyname, G->speed, 64);
+    sl_tty_t *dev = sl_tty_new(G->ttyname, G->speed, 64);
     if(!dev) return 1;
     size_t got, L = 0;
     char buff[BUFLEN], *ptr = buff;
     int errctr = 0;
     for(; errctr < ERRCTR_MAX; ++errctr){
-        if(!tty_open(dev, 1)){
+        if(!sl_tty_open(dev, 1)){
             sleep(1);
             continue;
         }
-        while(read_tty(dev)); // clear buffer
-        if(write_tty(dev->comfd, "?U\r\n", 3)){
+        while(sl_tty_read(dev)); // clear buffer
+        if(sl_tty_write(dev->comfd, "?U\r\n", 3)){
             WARNX("write_tty()");
             continue;
         }
-        double t0 = dtime();
-        while(dtime() - t0 < 10.){ // timeout - 10s
-            got = read_tty(dev);
+        double t0 = sl_dtime();
+        while(sl_dtime() - t0 < 10.){ // timeout - 10s
+            got = sl_tty_read(dev);
             if(got == 0) continue;
-            t0 = dtime();
+            t0 = sl_dtime();
             if(L + got > BUFLEN - 1) break;
             L += got;
             buff[L] = 0;
@@ -92,8 +92,8 @@ int main(int argc, char **argv){
             continue;
         }else break;
     }
-    while(read_tty(dev));
-    close_tty(&dev);
+    while(sl_tty_read(dev));
+    sl_tty_close(&dev);
     if(errctr == ERRCTR_MAX){
         ERRX("No connection to meteostation");
     }

Auxiliary_utils/bash_scripts/AZpoint

@@ -4,7 +4,7 @@ A="90:00:00"
 H="45:00:00"
 
 function sendcmd(){
-	echo $1 | nc 192.168.70.33 10001 -q10
+	echo $1 | nc localhost 10001 -q10
 }
 
 sendcmd ":Sz${A}#"

Auxiliary_utils/bash_scripts/allsky_sixel.sh

@@ -0,0 +1,20 @@
+#!/bin/bash
+
+#
+# $1 - scaling factor (in percents) of the image
+#
+
+scale=50
+if [ $# -gt 0 ]; then
+     scale=$1
+fi
+
+unset http_proxy
+
+clear
+while [[ 1 ]]; do
+    tput cup 0 0
+    curl -s http://zarch.sao.ru/webcam/mirat_allsky.cgi | magick - -colors 256 +dither -normalize -resize $scale% sixel:- 
+#    sleep 30s
+    sleep 5s
+done

Auxiliary_utils/bash_scripts/allsky_sixel_weather.sh

@@ -0,0 +1,114 @@
+#!/bin/bash
+
+#
+# $1 - scaling factor (in percents) of the image
+#
+
+function get_val {
+  echo $(echo $1 | cut -d "=" -f 2 | cut -d "." -f 1)
+}
+
+white_col="\e[97m"
+red_col="\e[1m\e[31m"
+end_col="\e[0m"
+last_row=0
+
+scale=50
+if [ $# -gt 0 ]; then
+     scale=$1
+fi
+
+unset http_proxy
+
+im_sleep=20 # in secs
+info_sleep=180 # in secs
+
+n_info=$((info_sleep/im_sleep))
+
+
+
+clear
+while [[ 1 ]]; do
+    # weather info
+    m_old=($(curl 192.168.70.33:12345 2>/dev/null)// / )
+    rain=${m_old[0]}
+    clouds=${m_old[1]}
+    temp=${m_old[2]}
+
+    m_new=($(curl localhost:3333/stat3600 2>/dev/null | sed 's/[\x01-\x1F\x7F]//g')// / )
+    windmax=${m_new[0]}
+    m_new=($(curl localhost:3333 2>/dev/null | sed 's/[\x01-\x1F\x7F]//g')// / )
+    wind=${m_new[0]}
+    humi=${m_new[4]}
+
+    rain_col=$white_col
+    clouds_col=$white_col
+    humi_col=$white_col
+    wind_col=$white_col
+    wind_max_col=$white_col
+
+    let rain_flag=$(get_val $rain)
+    if [[ $rain_flag -eq 1 ]]; then
+        rain_col=$red_col
+    fi
+
+    let clouds_val=$(get_val $clouds)
+    if [[ $clouds_val -le 1500 ]]; then
+        clouds_col=$red_col
+
+    fi
+
+    let humi_val=$(get_val $humi)
+    if [[ $humi_val -ge 90 ]]; then
+        humi_col=$red_col
+    fi
+
+    let wind_val=$(get_val $wind)
+    if [[ $wind_val -ge 10 ]]; then
+        wind_col=$red_col
+    fi
+
+    let wind_max_val=$(get_val $windmax)
+    if [[ $wind_max_val -ge 10 ]]; then
+        wind_max_col=$red_col
+    fi
+
+    ncols=`tput cols`
+    start_col=$((ncols-25))
+
+    tput cup $last_row $start_col
+#    echo -e "\e[4m`date`:\e[0m"
+    echo -e "\e[4m`date +'%F %T'`:\e[0m"
+
+    ((++last_row))
+
+    tput cup $last_row $start_col
+    echo -e "$clouds_col$clouds $rain_col($rain)$end_col"
+
+    ((++last_row))
+
+    tput cup $last_row $start_col
+#    echo -e "$temp $humi_col($humi)$end_col"
+    temp_val=`printf "%.1f" ${temp#*=}`
+    echo -e "Temp=$temp_val, ${humi_col}Hum=$humi_val%$end_col"
+
+    ((++last_row))
+
+    tput cup $last_row $start_col
+#    echo -e "$wind_col$wind $wind_max_col($windmax @hour)$end_col\n"
+    wind_max_val=`printf "%.1f" ${windmax#*=}`
+    echo -e "$wind_col$wind $wind_max_col(Max=$wind_max_val/hour)$end_col\n"
+
+    IFS='[;' read -p $'\e[6n' -d R -rs _ last_row col _
+
+    # allsky image
+
+    for i in `seq $n_info`; do
+        tput cup 0 0
+        curl -s http://zarch.sao.ru/webcam/omea_allsky.cgi  | magick - -colors 256 -normalize +dither -resize $scale% sixel:- 
+#        curl -s http://zarch.sao.ru/webcam/mirat_allsky.cgi | magick - -colors 256 -resize $scale% sixel:- 
+        sleep ${im_sleep}s
+    done
+
+
+done

Auxiliary_utils/bash_scripts/copy_and_xz.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+#
+# $1 - directory to be copied
+#
+
+RDIR="/home/obs/robotel1_2025"
+
+# copy non-FITS files
+tar c --exclude='*.fit' --exclude='*shit*' $1 | ssh obs@roboserv "tar x -C  $RDIR"
+#tar c --exclude='*.fit' --exclude='*shit*' $1 | ssh obs@roboserv "tar x -C  /home/obs/robotel1_2023"
+
+# copy FITS files and XZ-ing it on remote server
+CMD='sh -c "xz -6e -T0 - > $TAR_FILENAME.xz"'
+tar c $1/*.fit | ssh obs@roboserv "cd  $RDIR; tar x --to-command='$CMD'"
+#tar c $1/*.fit | ssh obs@roboserv "cd  /home/obs/robotel1_2023; tar x --to-command='$CMD'"
+
+ssh obs@roboserv "cd $RDIR;  tar c $1 | ssh data@robostorage 'tar x -C  /mnt/ARCHIVE/ROBOTEL1/'"

Auxiliary_utils/bash_scripts/park_telescope_1

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+A="01:48:38"
+H="01:13:29"
+
+function sendcmd(){
+	echo $1 | nc localhost 10001 -q10 || (echo "Can't connect" >&2; exit 1)
+}
+
+#lower limit is 0
+sendcmd ":So0#"
+sendcmd ":Sz${A}#"
+sendcmd ":Sa${H}#"
+sendcmd ":MA#" # || exit 1
+
+errctr=0
+
+while true; do
+	sleep 2
+	ANS=$(sendcmd ":Gstat#")
+	if [ "x$ANS" == "x" ]; then
+		[[ $((++errctr)) < 5 ]] && continue
+		echo "No connection to server" >&2
+		exit 1
+	fi
+	echo $ANS
+	[ $ANS == "0#" -o $ANS == "7#" ] && break
+done
+
+# stop tracking
+sendcmd ":AL#"
+
+echo -e "\n\nTelescope parked\n"

Auxiliary_utils/bash_scripts/park_telescope_2

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+A="00:00:00"
+H="2:00:00"
+
+function sendcmd(){
+	echo $1 | nc localhost 10001 -q10 || (echo "Can't connect" >&2; exit 1)
+}
+
+#lower limit is 0
+sendcmd ":So0#"
+sendcmd ":Sz${A}#"
+sendcmd ":Sa${H}#"
+sendcmd ":MA#" # || exit 1
+
+errctr=0
+
+while true; do
+	sleep 2
+	ANS=$(sendcmd ":Gstat#")
+	if [ "x$ANS" == "x" ]; then
+		[[ $((++errctr)) < 5 ]] && continue
+		echo "No connection to server" >&2
+		exit 1
+	fi
+	echo $ANS
+	[ $ANS == "0#" -o $ANS == "7#" ] && break
+done
+
+# stop tracking
+sendcmd ":AL#"
+
+echo -e "\n\nTelescope parked\n"

Auxiliary_utils/bash_scripts/run_full.new_foc

@@ -0,0 +1,165 @@
+#!/bin/bash
+RA="17:17:08.86"
+DEC="+67:57:11.4"
+OBJ="GALEX171708.5"
+EXPTIME=31000
+FLATTIME=40000
+OBS="Fatkhullin T.A."
+BADWEATHER=1300
+DATEEND=$(sunrise 14)
+# focus each N seconds
+FOCUST=7200
+
+FNO=1
+FDATE=0
+
+#
+# NOTE: THIS IS A NEW VERSION OF THE OBSERVATION SCRIPT!
+# CHAGLELOG: 
+#   Oct 2024: replace focussing algorithm (T. Fatkhullin)
+#             At the night start the first focussing run
+#             uses of two-step algorithm:
+#               1) rough focus astimation along full season-to-season
+#                  focus range (as it was implemented in the old obs. script)
+#               2) precise focussing along narrow range computed from
+#                  previous rough estimation
+#             Next focussing runs compute range from the current focus value.
+#             (new function 'focustel_new')
+#
+#  May 2025: rewrite detection of running this script process (T. Fatkhullin)
+#            (use of '-c' commandline option for 'pgrep' command)
+
+function focustel_new(){
+    rm focus*.fit 2>/dev/null
+    echo "Focussing..."
+
+    let fno=$1
+    if [[ $fno -gt 1 ]]; then
+        let curr_foc=`fli_control | tail -n 5 | head -n 1 | cut -d "=" -f 2`
+        let lowf=$curr_foc-1500
+        let highf=$curr_foc+1500
+        focus_seq_FLI.py -v -c focus$(printf "%02d" $1).jpg $lowf $highf 500
+        
+        if [[ $? -ne 0 ]]; then
+            echo -e "\nFOCUSSING SCRIPT RETURNED: $?"
+            echo -e "SOMETHING WAS WRONG IN FOCUSSING SEQUENCE!!! SET FOCUS TO PREVIOUS VALUE!!\n"
+            fli_control -g $curr_foc
+        fi
+    else
+        focus_seq_FLI.py --guess -v -N 7 -c focus$(printf "%02d" $1).jpg 47000 60000 500
+    fi 
+
+    FDATE=$(date +%s)
+}
+
+function sendcmd(){
+    echo $1 | nc 192.168.70.33 10001 -q10
+}
+
+function point_tel(){
+	touch lastpointing
+	send_coords -r $1 -d $2
+}
+
+#c=$(pgrep run_full | wc -l)
+# $ will run another run_full, so c=2 for single run
+#if [[ $c -gt 1 ]]; then
+#    echo "Another process is running; exiting"
+#    exit 1
+#fi
+
+if [[ $(pgrep -c run_full) -gt 1 ]]; then
+    echo "Another process is running! Exit!"
+    exit 1
+fi
+
+export http_proxy=""
+
+# set lower limit to 5degr
+send_command2mount ":So5#"
+
+echo "Time diff: $(($DATEEND-$(date +%s)))"
+if [ $(($DATEEND-$(date +%s))) -lt 3600 ]; then
+	echo "There's less an hour for observation!"
+	exit 2
+fi
+
+if [ $(($DATEEND-$(date +%s))) -gt 53200 ]; then
+	echo "There's more than 12 hours till closing, check script data!"
+	exit 3
+fi
+
+#STARTobs
+
+#send_coords
+#echo "Wait a little"
+#sleep 10
+
+echo "GoTo object: ${RA} ${DEC}"
+send_coords -r${RA} -d${DEC}
+
+echo "Start taking object"
+
+badweather=0
+while true; do
+	now=$(date +%s)
+	ANS=$(curl localhost:55555/status 2>/dev/null)
+	echo "Dome status: $ANS"
+	if [ $ANS != "opened" ]; then
+		echo "Closed"
+		curl localhost:55555/weather 2>/dev/null > DomeClosed
+		break;
+	fi
+	chkweather ${BADWEATHER} > lastweather && badweather=0 || badweather=$((badweather+1))
+	[ $badweather -gt 5 ] && break
+	[ -f stopobs ] && break
+	[ -f exitjob ] && exit 0
+	if [ "$now" -lt "$DATEEND" ]; then
+		est=$(sendcmd ":Gmte#"|sed -e 's/^0*//' -e 's/#//')
+		echo -e "\n\n\n\n\nEstimated time to flip: $est minutes"
+		if [[ ("x$est" == "x") || ($est -lt 3) ]]; then
+			point_tel "${RA}" "${DEC}"
+			continue
+		fi
+		ST=$(send_coords | awk '{print $4}')
+		[ "x$ST" == "x" ] && break
+		if [ $ST -ne "0" ]; then
+			point_tel "${RA}" "${DEC}"
+			continue
+		else
+			[ $(($(date +%s) - $FDATE)) -gt $FOCUST ] && focustel_new $((FNO++))
+#			[ $(($(date +%s) - $FDATE)) -gt $FOCUST ] && focustel $((FNO++))
+			preflash
+			fli_control -r /tmp/10micron.fitsheader -x $EXPTIME -N "${OBS}" -O "${OBJ}" "$OBJ"
+		fi
+	else
+		break
+	fi
+done
+
+echo "Dome closed @ $(date)" >> closed
+curl localhost:55555/close
+
+relay_manage -s1
+park_telescope
+
+for x in $(seq 1 10); do
+	preflash
+	fli_control -r /tmp/10micron.fitsheader -x1 -N "${OBS}" -O "bias" -d bias
+	preflash
+	fli_control -r /tmp/10micron.fitsheader -x $EXPTIME -N "${OBS}" -O "dark" -d dark
+	preflash
+	fli_control -r /tmp/10micron.fitsheader -n3 -x $FLATTIME -N "${OBS}" -O "flat" flat
+done
+
+relay_manage -r1
+echo "Closed @ $(date)" >> closed
+
+STOPobs || true
+
+DIR=$(basename $PWD)
+echo "TAR: $DIR"
+cd ..
+./copy_and_xz.sh $DIR
+echo "Archived, end"
+echo "$DIR archived @ $(date)"  >> archived

Auxiliary_utils/bash_scripts/setup_obs.sh

@@ -0,0 +1,44 @@
+#!/bin/bash
+
+#
+# The script creates working directory,
+# copies 'run' script into it and
+# edits observer name according to 
+# the its argument
+#
+# Working directory name is formed from
+# date of the script running and is computed
+# as follows:
+#    now - 12hours
+# i.e. the day starts from 12h not from 0h!
+#
+# $1 - observer name
+#
+
+if [[ $# -eq 0 ]]; then
+    obs_name="Fatkhullin T.A."
+else
+    obs_name=$1
+fi
+
+# now - 12h
+let now12=`date +%s`-12*3600
+
+# working directory
+wdir=/DATA/FITS/`date -d @$now12 +%y.%m.%d`
+
+echo -n "Creating working directory: $wdir ..."
+if [[ -d $wdir ]]; then
+    echo -e "\tFAILED! The directory already exists! Exit!"
+    exit 1
+else
+    echo -e "\tOK!"
+    mkdir $wdir
+    cd $wdir
+fi
+
+cp ../run_full.new_foc .
+
+# replace observer name
+sed -i "0,/^OBS=\".*\"/ s//OBS=\"${obs_name}\"/" run_full.new_foc
+#sed -i "0,/^OBS=\"[a-zA-Z \.]*\"/ s//OBS=\"${obs_name}\"/" run_full.new_foc

Auxiliary_utils/bash_scripts/video2dome1-2.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+mpv -vo sixel --really-quiet=yes "rtsp://viewer:view25@192.168.70.25:554/axis-media/media.amp?videocodec=h264&resolution=640x480"

Auxiliary_utils/calc_points/cmdlnopts.c

@@ -41,7 +41,7 @@ static glob_pars G = {
  * Define command line options by filling structure:
  *  name        has_arg     flag    val     type        argptr              help
 */
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
 // common options
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        "show this help"},
     {"delimeter",NEED_ARG,  NULL,   'd',    arg_string, APTR(&G.delimeter), "coordinates delimeter string (default: ':')"},
@@ -71,10 +71,10 @@ glob_pars *parse_args(int argc, char **argv){
     char helpstring[1024], *hptr = helpstring;
     snprintf(hptr, hlen, "Usage: %%s [args]\n\n\tWhere args are:\n");
     // format of help: "Usage: progname [args]\n"
-    change_helpstring(helpstring);
+    sl_helpstring(helpstring);
     // parse arguments
-    parseargs(&argc, &argv, cmdlnopts);
-    if(help) showhelp(-1, cmdlnopts);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(help) sl_showhelp(-1, cmdlnopts);
     if(argc > 0){
         G.rest_pars_num = argc;
         G.rest_pars = MALLOC(char *, argc);

Auxiliary_utils/calc_points/main.c

@@ -100,7 +100,7 @@ static void savepoints(FILE *f, point *pts, int N, char *delim, int mask){
 }
 
 int main(int argc, char **argv){
-    initial_setup();
+    sl_init();
     glob_pars *G = parse_args(argc, argv);
     FILE *f = NULL;
     if(G->outfile){

Auxiliary_utils/focussing/main.c

@@ -32,7 +32,7 @@ typedef struct{
 
 static glob_pars G = {.tolerance = 10.};
 
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
     // common options
     {"help",    NO_ARGS,    NULL,   'h',    arg_none,   APTR(&G.help),      _("show this help")},
     {"infile",  NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.input),     _("input file name")},
@@ -158,11 +158,11 @@ static double calcfocus(double coeffs[3]){
 
 int main(int argc, char **argv){
     char helpstring[256];
-    initial_setup();
+    sl_init();
     snprintf(helpstring, 255, "Usage: `cat file | %%s` or with args; file format \"x y\\n..\"\n\tArgs:\n");
-    change_helpstring(helpstring);
-    parseargs(&argc, &argv, cmdlnopts);
-    if(G.help) showhelp(-1, cmdlnopts);
+    sl_helpstring(helpstring);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(G.help) sl_showhelp(-1, cmdlnopts);
     if(G.tolerance <= 0.) ERRX("Tolerance should be > 0");
     FILE *f = stdin;
     if(G.input){

Auxiliary_utils/libastrotools/examples/coordstest.c

@@ -36,7 +36,7 @@ static const char *radtodeg(double r){
 
 
 int main(){
-    initial_setup();
+    sl_init();
     at_MJD_t mjd;
     if(!at_get_MJDu(time(NULL), &mjd)) ERRX("at_get_MJDu");
     printf("MJD=%g; TAI=%g/%g, TT=%g/%g, UTC=%g/%g\n", mjd.MJD, mjd.tai1, mjd.tai2, mjd.tt1, mjd.tt2, mjd.utc1, mjd.utc2);

Auxiliary_utils/libastrotools/examples/transform.c

@@ -56,7 +56,7 @@ static parameters G = {
     .py = -10000.
 };
 
-static myoption cmdlnopts[] = {
+static sl_option_t cmdlnopts[] = {
     {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
     {"obsplace",    NO_ARGS,    NULL,   'O',    arg_int,    APTR(&G.obsplace),  "input RA/Dec is observed place"},
     {"JD",          NEED_ARG,   NULL,   'J',    arg_double, APTR(&G.JD),        "Julian date"},
@@ -77,9 +77,9 @@ static myoption cmdlnopts[] = {
 
 
 int main(int argc, char **argv){
-    initial_setup();
-    parseargs(&argc, &argv, cmdlnopts);
-    if(G.help) showhelp(-1, cmdlnopts);
+    sl_init();
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(G.help) sl_showhelp(-1, cmdlnopts);
     at_MJD_t MJD;
     G.ra *= ERFA_DD2R;
     G.dec *= ERFA_DD2R;

Daemons/10micron_stellarium/main.c

@@ -226,6 +226,7 @@ int proc_data(uint8_t *data, ssize_t len){
     // convert RA/DEC to hours/degrees
     double tagRA = RA2HRS(ra), tagDec = DEC2DEG(dec);
     DBG("RA: %u (%g), DEC: %d (%g)", ra, tagRA, dec, tagDec);
+    putlog("RA: %u (%g degr), DEC: %d (%g degr)", ra, tagRA, dec, tagDec);
     // check RA/DEC
     horizCrds hnow; // without refraction
     polarCrds p2000, pnow;
@@ -236,7 +237,7 @@ int proc_data(uint8_t *data, ssize_t len){
         WARNX("Can't convert coordinates to Jnow");
         return 0;
     }
-#ifdef EBUG
+/*
     int i[4], j[4]; char pm, pm1;
     eraA2af(2, hnow.az, &pm, i);
     eraA2af(2, hnow.zd, &pm1, j);
@@ -245,10 +246,10 @@ int proc_data(uint8_t *data, ssize_t len){
         pm1,j[0],j[1],j[2],j[3]);
     eraA2af(2, M_PI_2 - hnow.zd, &pm, i);
     DBG("h: %c%02d %02d %02d.%2.d", pm, i[0],i[1],i[2],i[3]);
-#endif
+*/
     if(hnow.zd > 80.*ERFA_DD2R){
-        WARNX("Z > 80degr, stop telescope");
-        putlog("Z>80 - stop!");
+        WARNX("Z > 80degr (%g), stop telescope", hnow.zd * ERFA_DR2D);
+        putlog("Z=%.1f > 80 - stop!", hnow.zd * ERFA_DR2D);
         stop_telescope();
         return 0;
     }

Daemons/Readme.md

@@ -8,3 +8,5 @@ Different daemons & tools
 - *netsocket* - scripts for management of network 220V-socket
 - *send_coordinates* - get/send coordinates to 10-micron mount through stellarium daemon
 - *teldaemon* - open/close Astrosib-500 scope covers by network query
+- *weatherdaemon* - weather daemon for old meteostation
+- *weatherdaemon_newmeteo* - daemon for new (chinese) meteostation

Daemons/domedaemon-astrosib/server.c

@@ -180,9 +180,10 @@ static void toomuch(int fd){
     LOGWARN("Client fd=%d tried to connect after MAX reached", fd);
 }
 // new connections handler
-static void connected(sl_sock_t *c){
+static int connected(sl_sock_t *c){
     if(c->type == SOCKT_UNIX) LOGMSG("New client fd=%d connected", c->fd);
     else LOGMSG("New client fd=%d, IP=%s connected", c->fd, c->IP);
+    return TRUE;
 }
 // disconnected handler
 static void disconnected(sl_sock_t *c){
@@ -196,12 +197,12 @@ void server_run(sl_socktype_e type, const char *node, sl_tty_t *serial){
         ERRX("server_run(): wrong parameters");
     }
     dome_serialdev(serial);
-    sl_sock_changemaxclients(5);
-    sl_sock_maxclhandler(toomuch);
-    sl_sock_connhandler(connected);
-    sl_sock_dischandler(disconnected);
     s = sl_sock_run_server(type, node, -1, handlers);
     if(!s) ERRX("Can't create socket and/or run threads");
+    sl_sock_changemaxclients(s, 5);
+    sl_sock_maxclhandler(s, toomuch);
+    sl_sock_connhandler(s, connected);
+    sl_sock_dischandler(s, disconnected);
     while(s && s->connected){
         if(!s->rthread){
             LOGERR("Server handlers thread is dead");

Daemons/netdaemon/cmdlnopts.c

@@ -54,7 +54,7 @@ glob_pars const Gdefault = {
  * Define command line options by filling structure:
  *  name        has_arg     flag    val     type        argptr              help
 */
-myoption cmdlnopts[] = {
+sl_option_t cmdlnopts[] = {
 // common options
     {"help",    NO_ARGS,    NULL,   'h',    arg_int,    APTR(&help),        _("show this help")},
     {"device",  NEED_ARG,   NULL,   'i',    arg_string, APTR(&G.device),    _("serial device name (default: none)")},
@@ -79,10 +79,10 @@ glob_pars *parse_args(int argc, char **argv){
     void *ptr;
     ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr);
     // format of help: "Usage: progname [args]\n"
-    change_helpstring("Usage: %s [args]\n\n\tWhere args are:\n");
+    sl_helpstring("Usage: %s [args]\n\n\tWhere args are:\n");
     // parse arguments
-    parseargs(&argc, &argv, cmdlnopts);
-    if(help) showhelp(-1, cmdlnopts);
+    sl_parseargs(&argc, &argv, cmdlnopts);
+    if(help) sl_showhelp(-1, cmdlnopts);
     if(argc > 0){
         G.rest_pars_num = argc;
         G.rest_pars = calloc(argc, sizeof(char*));