is moving model really works?

Auxiliary_utils/LibSidServo/PID.c

@@ -0,0 +1,193 @@
+/*
+ * 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;
+    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){
+    if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){
+        DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT);
+        return axis->speed.val; // data is too old or wrong
+    }
+    double error = tagpos->val - axis->position.val, fe = fabs(error);
+    PIDController_t *pid = NULL;
+    switch(axis->state){
+        case AXIS_SLEWING:
+            if(fe < MCC_MAX_POINTING_ERR){
+                axis->state = AXIS_POINTING;
+                DBG("--> Pointing");
+                pid = pidpair->PIDC;
+            }else{
+                DBG("Slewing...");
+                return -1.; // max speed for given axis
+            }
+            break;
+        case AXIS_POINTING:
+            if(fe < MCC_MAX_GUIDING_ERR){
+                axis->state = AXIS_GUIDING;
+                DBG("--> Guiding");
+                pid = pidpair->PIDV;
+            }else if(fe > MCC_MAX_POINTING_ERR){
+                DBG("--> Slewing");
+                axis->state = AXIS_SLEWING;
+                return -1.;
+            } else pid = pidpair->PIDC;
+            break;
+        case AXIS_GUIDING:
+            pid = pidpair->PIDV;
+            if(fe > MCC_MAX_GUIDING_ERR){
+                DBG("--> Pointing");
+                axis->state = AXIS_POINTING;
+                pid = pidpair->PIDC;
+            }else if(fe < MCC_MAX_ATTARGET_ERR){
+                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!!!!");
+            axis->state = AXIS_SLEWING;
+            return -1.;
+        case AXIS_ERROR:
+            DBG("Can't move from erroneous state");
+            return 0.;
+    }
+    if(!pid){
+        DBG("WTF? Where is a PID?");
+        return axis->speed.val;
+    }
+    if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
+        DBG("time diff too big: clear PID");
+        pid_clear(pid);
+    }
+    double dt = tagpos->t - pid->prevT;
+    if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
+    pid->prevT = tagpos->t;
+    //DBG("CALC PID (er=%g, dt=%g)", error, dt);
+    double tagspeed = pid_calculate(pid, error, dt);
+    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // 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, const coordpair_t *endpoint){
+    static PIDpair_t pidX = {0}, pidY = {0};
+    if(!pidX.PIDC){
+        pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidX.PIDC) return MCC_E_FATAL;
+        pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidX.PIDV) return MCC_E_FATAL;
+    }
+    if(!pidY.PIDC){
+        pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidY.PIDC) return MCC_E_FATAL;
+        pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidY.PIDV) return MCC_E_FATAL;
+    }
+    mountdata_t m;
+    coordpair_t tagspeed;
+    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(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_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(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
+    axis_status_t ystate = axis.state;
+    if(m.Xstate != xstate || m.Ystate != ystate){
+        DBG("State changed");
+        setStat(xstate, ystate);
+    }
+    DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
+    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
+    double 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, const coordpair_t *endpoint);

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/main.c

@@ -22,13 +22,18 @@
 #include <usefull_macros.h>
 
 #include "moving.h"
+#include "PID.h"
 
 // errors for states: slewing/pointing/guiding
-#define MAX_POINTING_ERR    (50.)
+// 10-degrees zone - Coordinate-driven PID
-#define MAX_GUIDING_ERR     (5.)
+#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;
 
@@ -48,93 +53,64 @@ typedef struct{
     double dTcorr;
     double Tend;
     double minerr;
-    double P, I, D;
+    double startcoord;
+    double error;
+    PIDpar_t gainC, gainV;
 } pars;
 
 static pars G = {
-    .ramptype = "t",
     .dTmon = 0.01,
     .dTcorr = 0.05,
     .Tend = 100.,
     .minerr = 0.1,
-    .P = 0.8,
+    .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 = 1e6, .speed = 1e3, .accel = 500.},
+    .max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
-    .jerk = 10.
 };
 
-typedef struct {
-    double kp, ki, kd;  // 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;
-
-static PIDController pid;
 
 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"},
     {"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)"},
-    {"prop",    NEED_ARG,   NULL,   'P',    arg_double, APTR(&G.P),             "P-coefficient of PID"},
+    {"propC",   NEED_ARG,   NULL,   'P',    arg_double, APTR(&G.gainC.P),       "P-coefficient of coordinate-driven PID"},
-    {"integ",   NEED_ARG,   NULL,   'I',    arg_double, APTR(&G.I),             "I-coefficient of PID"},
+    {"integC",  NEED_ARG,   NULL,   'I',    arg_double, APTR(&G.gainC.I),       "I-coefficient of coordinate-driven PID"},
-    {"diff",    NEED_ARG,   NULL,   'D',    arg_double, APTR(&G.D),             "D-coefficient of 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 target_coord(20.);
+    if(t > 20. && t < 30.) return 0.;
-    double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
+    //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;
 }
 
-/* P-only == oscillations
-static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
-    double error = targcoord - p->coord;
-    if(fabs(error) < G.minerr) return p->speed;
-    return p->speed + error / dt / 500.;
-}
-*/
-
-static void pid_init(PIDController *pid, double kp, double ki, double kd) {
-    pid->kp = fabs(kp);
-    pid->ki = fabs(ki);
-    pid->kd = fabs(kd);
-    pid->prev_error = 0.;
-    pid->integral = 0.;
-    pid->curIidx = 0;
-    pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
-    if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
-    pid->pidIarray = MALLOC(double, pid->pidIarrSize);
-}
-
-static void pid_clear(PIDController *pid){
-    if(!pid) return;
-    bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
-    pid->integral = 0.;
-    pid->prev_error = 0.;
-    pid->curIidx = 0;
-}
-
 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(&pid);
+                pid_clear(pidC);
                 state = Pointing;
                 green("--> Pointing\n");
+                pid = pidC;
             }else{
                 red("Slewing...\n");
                 return (error > 0.) ? limits.max.speed : -limits.max.speed;
@@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
             break;
         case Pointing:
             if(fe < MAX_GUIDING_ERR){
-                pid_clear(&pid);
+                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");
-                    //pid_clear(&pid);
+            }else printf("Current error: %g\n", fe);
-                    //return p->speed;
-            }
             break;
     }
-
+    if(!pid){
-    red("Calculate PID\n");
+        WARNX("where is PID?"); return p->speed;
-    double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
+    }
-    pid.pidIarray[pid.curIidx++] = oldi;
+    double tagspeed = pid_calculate(pid, error, dt);
-    if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
+    if(state == Guiding) return p->speed + tagspeed;
-    pid.integral += newi - oldi;
+    return tagspeed;
-    double derivative = (error - pid.prev_error) / dt;
-    pid.prev_error = error;
-    DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
-    double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
-    if(state == Pointing) add /= 3.;
-    else if(state == Guiding) add /= 7.;
-    DBG("ADD = %g; new speed = %g", add, p->speed + add);
-    if(state == Guiding) return p->speed + add  / dt / 10.;
-    return add / dt;
 }
-// ./moving  -l coords -P.5 -I.05 -D1.5
+// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
-// ./moving  -l coords -P1.3 -D1.6
+// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
 
 static void start_model(double Tend){
-    double T = 0., Tcorr = 0.;//, Tlast = 0.;
+    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;
@@ -215,6 +192,9 @@ static void start_model(double Tend){
                 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){
@@ -228,16 +208,15 @@ int main(int argc, char **argv){
     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.");
-    pid_init(&pid, G.P, G.I, G.D);
+    pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
-    fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
+    pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
-    ramptype_t ramp = RAMP_AMOUNT;
+    if(!pidC || !pidV) ERRX("Can't init PID regulators");
-    if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
+    model = init_moving(&limits);
-    else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
-    else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
-    else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
-    model = init_moving(ramp, &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

@@ -24,12 +24,9 @@
 
 #include "moving.h"
 #include "moving_private.h"
-#include "Dramp.h"
-#include "Sramp.h"
 #include "Tramp.h"
 
-//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+static movemodel_t *model = &trapez;
-static movemodel_t *model = NULL;
 double coord_tolerance = COORD_TOLERANCE_DEFAULT;
 double time_tick = TIME_TICK_DEFAULT;
 
@@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){
     *max = t;
 }
 
-movemodel_t *init_moving(ramptype_t type, limits_t *l){
+movemodel_t *init_moving(limits_t *l){
     if(!l) return FALSE;
-    switch(type){
-        case RAMP_DUMB:
-            model = &dumb;
-        break;
-        case RAMP_TRAPEZIUM:
-            model = &trapez;
-        break;
-        case RAMP_S:
-            model = &s_shaped;
-        break;
-        default:
-            return FALSE;
-    }
     if(!model->init_limits) return NULL;
     moveparam_t *max = &l->max, *min = &l->min;
     if(min->speed < 0.) min->speed = -min->speed;

Auxiliary_utils/LibSidServo/PID_test/moving.h

@@ -26,13 +26,6 @@
 #define TIME_TICK_MIN           (1e-9)
 #define TIME_TICK_MAX           (10.)
 
-typedef enum{
-    RAMP_DUMB,          // no ramp: infinite acceleration/deceleration
-    RAMP_TRAPEZIUM,     // trapezium ramp
-    RAMP_S,             // s-shaped ramp
-    RAMP_AMOUNT
-} ramptype_t;
-
 typedef enum{
     ST_STOP,            // stopped
     ST_MOVE,            // moving
@@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative
 typedef struct{
     moveparam_t min;
     moveparam_t max;
-    double jerk;
 } limits_t;
 
 typedef struct{
@@ -64,7 +56,7 @@ typedef struct{
 extern double coord_tolerance;
 
 double nanot();
-movemodel_t *init_moving(ramptype_t type, limits_t *l);
+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/examples/SSIIconf.c

@@ -51,10 +51,10 @@ static sl_option_t confopts[] = {
     end_option
 };
 
-static void dumpaxe(char axe, axe_config_t *c){
+static void dumpaxis(char axis, axis_config_t *c){
 #define STRUCTPAR(p)    (c)->p
-#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
+#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
-#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(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);
@@ -99,14 +99,15 @@ static void dumpHWconf(){
 #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 axe configuration:\n");
+    green("X axis configuration:\n");
-    dumpaxe('X', &HW.Xconf);
+    dumpaxis('X', &HW.Xconf);
     green("X bits:\n");
     dumpxbits(&HW.xbits);
-    green("Y axe configuration:\n");
+    green("Y axis configuration:\n");
-    dumpaxe('Y', &HW.Yconf);
+    dumpaxis('Y', &HW.Yconf);
     green("Y bits:\n");
     dumpybits(&HW.ybits);
+    green("Other:\n");
     printf("address=%d\n", HW.address);
     DUMP(eqrate);
     DUMP(eqadj);
@@ -142,10 +143,12 @@ 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){

Auxiliary_utils/LibSidServo/examples/conf.c

@@ -31,6 +31,18 @@ static conf_t Config = {
     .EncoderReqInterval = 0.05,
     .SepEncoder = 2,
     .EncoderSpeedInterval = 0.1,
+    .XPIDC.P = 0.8,
+    .XPIDC.I = 0.1,
+    .XPIDC.D = 0.3,
+    .XPIDV.P = 1.,
+    .XPIDV.I = 0.01,
+    .XPIDV.D = 0.2,
+    .YPIDC.P = 0.8,
+    .YPIDC.I = 0.1,
+    .YPIDC.D = 0.3,
+    .YPIDV.P = 0.5,
+    .YPIDV.I = 0.2,
+    .YPIDV.D = 0.5,
 };
 
 static sl_option_t opts[] = {
@@ -44,6 +56,19 @@ static sl_option_t opts[] = {
     {"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)"},
     {"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"},
+    {"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)"},
     end_option
 };
 

Auxiliary_utils/LibSidServo/examples/dump.c

@@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
         enct = tmsr;
         if(fcoords) logmnt(fcoords, &mdata);
         if(mdata.millis == mdmillis) continue;
-        //DBG("ctr=%d", ctr);
+        //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;
@@ -123,10 +123,11 @@ void dumpmoving(FILE *fcoords, double t, int N){
             ctr = 0;
         }else ++ctr;
     }
+    DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - 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){

Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c

@@ -120,16 +120,13 @@ 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(coordpair_t *target, coordpair_t *speed){
+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);
-    target->X = DEG2RAD(target->X);
+    cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
-    target->Y = DEG2RAD(target->Y);
+    cmd.Xspeed = DEG2RAD(speed->X);
-    speed->X = DEG2RAD(speed->X);
+    cmd.Yspeed = DEG2RAD(speed->Y);
-    speed->Y = DEG2RAD(speed->Y);
-    cmd.Xmot = target->X; cmd.Ymot = target->Y;
-    cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y;
     lastTag = *target;
     lastSpeed = *speed;
     /*cmd.xychange = 1;
@@ -142,7 +139,8 @@ static mcc_errcodes_t return2zero(){
     short_command_t cmd = {0};
     DBG("Try to move to zero");
     cmd.Xmot = 0.; cmd.Ymot = 0.;
-    cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.);
+    cmd.Xspeed = MCC_MAX_X_SPEED;
+    cmd.Yspeed = MCC_MAX_Y_SPEED;
     /*cmd.xychange = 1;
     cmd.XBits = 100;
     cmd.YBits = 20;*/
@@ -151,11 +149,11 @@ static mcc_errcodes_t return2zero(){
 
 static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
     long_command_t cmd = {0};
-    cmd.Xspeed = lastSpeed.X;
+    cmd.Xspeed = DEG2RAD(lastSpeed.X);
-    cmd.Yspeed = lastSpeed.Y;
+    cmd.Yspeed = DEG2RAD(lastSpeed.Y);
-    cmd.Xmot = lastTag.X;
+    cmd.Xmot = DEG2RAD(lastTag.X);
-    cmd.Ymot = lastTag.Y;
+    cmd.Ymot = DEG2RAD(lastTag.Y);
-    cmd.Xadder = adder->X; cmd.Yadder = adder->Y;
+    cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
     cmd.Xatime = time->X; cmd.Yatime = time->Y;
     return Mount.longCmd(&cmd);
 }
@@ -218,7 +216,7 @@ int main(int argc, char **argv){
     sleep(5);
     // return to zero and wait
     green("Return 2 zero and wait\n");
-    return2zero();
+    if(!return2zero()) ERRX("Can't return");
     Wait(0., 0);
     Wait(0., 1);
     // wait moving ends

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
@@ -89,12 +89,12 @@ void waithalf(double t){
         if(mdata.millis == millis) continue;
         millis = mdata.millis;
         if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
-            DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
+            //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.motYposition.val));
+            //DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
             ++ctr;
         }
     }
@@ -114,7 +114,8 @@ int main(int argc, char **argv){
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
     if(G.Ncycles < 7) ERRX("Ncycles should be >7");
-    if(G.amplitude < 0.01 || G.amplitude > 45.)
+    double absamp = fabs(G.amplitude);
+    if(absamp < 0.01 || absamp > 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");
@@ -162,9 +163,13 @@ int main(int argc, char **argv){
         DBG("Moved to -, t=%g", t-t0);
         DBG("CMD: %g", Mount.currentT()-t0);
     }
-    tag = (coordpair_t){.X = 0., .Y = 0.};
+    green("Move to zero @ %g\n", Mount.currentT());
+    tag = (coordpair_t){0};
     // be sure to move @ 0,0
-    Mount.moveTo(&tag);
+    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/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,12 @@ 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 FILE *fcoords = NULL, *errlog = NULL;
 static pthread_t dthr;
 static parameters G = {
     .Ncycles = 40,
@@ -61,12 +63,14 @@ 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)"},
+    {"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 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
 };
 
@@ -76,6 +80,8 @@ void signals(int sig){
         signal(sig, SIG_IGN);
         DBG("Get signal %d, quit.\n", sig);
     }
+    Mount.stop();
+    sleep(1);
     Mount.quit();
     if(fcoords) fclose(fcoords);
     exit(sig);
@@ -90,22 +96,39 @@ static void *dumping(void _U_ *u){
 static void runtraectory(traectory_fn tfn){
     if(!tfn) return;
     coordval_pair_t telXY;
-    coordpair_t traectXY;
+    coordval_pair_t target;
-    double t0 = Mount.currentT();
+    coordpair_t traectXY, endpoint;
-    double tlast = 0.;
+    endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
+    double t0 = Mount.currentT(), tlast = 0.;
+    double tlastX = 0., tlastY = 0.;
     while(1){
         if(!telpos(&telXY)){
             WARNX("No next telescope position");
             return;
         }
-        if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
+        if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
-        tlast = (telXY.X.t + telXY.Y.t) / 2.;
+        DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
+        tlastX = telXY.X.t; tlastY = telXY.Y.t;
         double t = Mount.currentT();
-        if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
+        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
         if(!traectory_point(&traectXY, t)) break;
-        DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+        target.X.val = traectXY.X; target.Y.val = traectXY.Y;
+        target.X.t = target.Y.t = t;
+        // check whether we should change direction
+        if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
+        else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
+        if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
+        else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
+        DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
+        DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, 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.4g  %10.4g  %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+        if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
+        while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
+        tlast = t;
     }
-    WARNX("No next traectory point");
+    WARNX("No next traectory point or emulation ends");
 }
 
 int main(int argc, char **argv){
@@ -118,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){
+    if(!Config || G.dumpconf){
         dumpConf();
         return 1;
     }

Auxiliary_utils/LibSidServo/examples/servo.conf

@@ -1,9 +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.001
-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 DEG2RAD(d)  ((d)/180.*M_PI)
-#define ASEC2RAD(d) (d/180.*M_PI/3600.)
+#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
-#define AMIN2RAD(d) (d/180.*M_PI/60.)
+#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
-#define RAD2DEG(r)  (r/M_PI*180.)
+#define RAD2DEG(r)  ((r)/M_PI*180.)
-#define RAD2ASEC(r) (r/M_PI*180.*3600.)
+#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
-#define RAD2AMIN(r) (r/M_PI*180.*60.)
+#define RAD2AMIN(r) ((r)/M_PI*180.*60.)
 

Auxiliary_utils/LibSidServo/examples/traectories.c

@@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL;
 // starting point of traectory
 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 coordval_pair_t XYcor = {0};
 
 /**
  * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@@ -52,9 +48,6 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
         if(MCC_E_OK == Mount.getMountData(&mdata)) break;
     }
     if(ntries == 10) return FALSE;
-    XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
-    XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
-    DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
     return TRUE;
 }
 
@@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){
     }
     if(ntries == 10) return FALSE;
     coordval_pair_t pt;
-    pt.X.val = XYcor.X.val + mdata.encXposition.val;
+    pt.X.val = mdata.encXposition.val;
-    pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
+    pt.Y.val = mdata.encYposition.val;
     pt.X.t = mdata.encXposition.t;
     pt.Y.t = mdata.encYposition.t;
     if(curpos) *curpos = pt;
@@ -94,7 +87,7 @@ int telpos(coordval_pair_t *curpos){
 // X=X0+1'/s, Y=Y0+15''/s
 int Linear(coordpair_t *nextpt, double t){
     coordpair_t pt;
-    pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
+    pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
     pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
     if(nextpt) *nextpt = pt;
     return TRUE;
@@ -103,7 +96,7 @@ int Linear(coordpair_t *nextpt, double t){
 // X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
 int SinCos(coordpair_t *nextpt, double t){
     coordpair_t pt;
-    pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
+    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;
@@ -116,8 +109,8 @@ typedef struct{
 } tr_names;
 
 static tr_names names[] = {
-    {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
+    {Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
-    {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
+    {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
     {NULL, NULL, NULL}
 };
 

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  111
+#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,10 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
+<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
-  <value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
+  <value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
  </data>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -40,9 +40,9 @@
    <value type="int" key="EditorConfiguration.PaddingMode">1</value>
    <value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
    <value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
-   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
    <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
-   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
    <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
    <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
    <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
@@ -51,10 +51,10 @@
    <value type="bool" key="EditorConfiguration.UseIndenter">false</value>
    <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
    <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
-   <value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
+   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
    <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
    <value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
-   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+   <value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
    <value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
    <value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
   </valuemap>
@@ -75,13 +75,11 @@
    <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>
     <value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
-    <value type="int" key="ClangTools.ParallelJobs">8</value>
+    <value type="int" key="ClangTools.ParallelJobs">4</value>
     <value type="bool" key="ClangTools.PreferConfigFile">true</value>
     <valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
     <valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
@@ -97,12 +95,12 @@
    <value type="bool" key="HasPerBcDcs">true</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
-   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
-    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/C-sources/erfa</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
       <valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
@@ -112,8 +110,8 @@
       <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.DefaultDisplayName">Сборка</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</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">
@@ -125,8 +123,8 @@
       <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.DefaultDisplayName">Очистка</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</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>
@@ -141,8 +139,8 @@
     <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.DefaultDisplayName">Развёртывание</value>
-      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</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>
@@ -154,7 +152,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"/>
@@ -173,8 +173,8 @@
    <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.DefaultDisplayName">Развёртывание</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</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>
@@ -186,7 +186,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"/>

Auxiliary_utils/LibSidServo/libsidservo.files

@@ -1,5 +1,6 @@
 CMakeLists.txt
-dbg.h
+PID.c
+PID.h
 examples/SSIIconf.c
 examples/conf.c
 examples/conf.h
@@ -18,6 +19,11 @@ serial.c
 examples/CMakeLists.txt
 examples/traectories.c
 examples/traectories.h
+main.h
+movingmodel.c
+movingmodel.h
+ramp.c
+ramp.h
 serial.h
 ssii.c
 ssii.h

Auxiliary_utils/LibSidServo/main.c

@@ -16,28 +16,133 @@
  * 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 "dbg.h"
+#include "main.h"
+#include "movingmodel.h"
 #include "serial.h"
 #include "ssii.h"
+#include "PID.h"
 
 conf_t Conf = {0};
-
+// parameters for model
+static movemodel_t *Xmodel, *Ymodel;
+// radians, rad/sec, rad/sec^2
+static limits_t
+    Xlimits = {
+        .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
+    Ylimits = {
+        .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
+;
 static mcc_errcodes_t shortcmd(short_command_t *cmd);
 
+/**
+ * @brief nanotime - monotonic time from first run
+ * @return time in seconds
+ */
+double nanotime(){
+    static struct timespec *start = NULL;
+    struct timespec now;
+    if(!start){
+        start = malloc(sizeof(struct timespec));
+        if(!start) return -1.;
+        if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
+    }
+    if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
+    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;
+}
+
 /**
  * @brief quit - close all opened and return to default state
  */
 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(mountdata_t *mountdata){
+    if(!mountdata || !Xmodel || !Ymodel) return;
+    static double oldmt = -100.; // old `millis measurement` time
+    static uint32_t oldmillis = 0;
+    double tnow = nanotime();
+    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);
+    mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
+    mountdata->motXposition.val = mountdata->encXposition.val  = Xp.coord;
+    mountdata->motYposition.val  = mountdata->encYposition.val  = Yp.coord;
+    getXspeed(); getYspeed();
+    if(tnow - oldmt > Conf.MountReqInterval){
+        oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3);
+        oldmt = tnow;
+    }else mountdata->millis = oldmillis;
+}
+
+/**
+ * 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
@@ -48,6 +153,12 @@ static mcc_errcodes_t init(conf_t *c){
     if(!c) return MCC_E_BADFORMAT;
     Conf = *c;
     mcc_errcodes_t ret = MCC_E_OK;
+    Xmodel = model_init(&Xlimits);
+    Ymodel = model_init(&Ylimits);
+    if(Conf.RunModel){
+        if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
+        return MCC_E_OK;
+    }
     if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
@@ -72,10 +183,9 @@ static mcc_errcodes_t init(conf_t *c){
         DBG("Wrong speed interval");
         ret = MCC_E_BADFORMAT;
     }
-    uint8_t buf[1024];
+    //uint8_t buf[1024];
-    data_t d = {.buf = buf, .len = 0, .maxlen = 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, NULL)) ret = MCC_E_FAILED;
-    if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
     if(ret != MCC_E_OK) return ret;
     return updateMotorPos();
 }
@@ -99,16 +209,34 @@ static int chkYs(double s){
     return TRUE;
 }
 
+// set SLEWING state if axis was stopped later
+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!");
+}
+
+/*
 static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
     (void)target;
     (void)flags;
+    //if(Conf.RunModel) return ... ;
     if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
     //...
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setStat(AXIS_SLEWING, AXIS_SLEWING);
     //...
     return MCC_E_FAILED;
 }
-
+*/
 
 /**
  * @brief move2 - simple move to given point and stop
@@ -128,7 +256,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
     cmd.Yspeed = MCC_MAX_Y_SPEED;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -140,6 +268,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
  */
 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);
@@ -165,7 +294,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
     cmd.Yspeed = speed->Y;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -174,11 +303,25 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
+    FNAME();
+    if(Conf.RunModel){
+        double curt = nanotime();
+        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 = nanotime();
+        Xmodel->stop(Xmodel, curt);
+        Ymodel->stop(Ymodel,curt);
+        return MCC_E_OK;
+    }
     if(!SSstop(FALSE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
@@ -190,6 +333,16 @@ 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 = nanotime();
+        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("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
     s.Xmot = X_RAD2MOT(cmd->Xmot);
@@ -201,16 +354,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;
+    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 = nanotime();
+        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);
@@ -221,11 +385,13 @@ 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;
+    setslewingstate();
     return MCC_E_OK;
 }
 
 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;
     if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
     // Convert acceleration (ticks per loop^2 to rad/s^2)
@@ -266,8 +432,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
     // Copy ticks per revolution
     hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
     hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
-    hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
+    hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less
-    hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
+    hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
     // Convert slew rates (ticks per loop to rad/s)
     hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
     hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@@ -290,6 +456,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
 
 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);
@@ -349,7 +516,7 @@ mount_t Mount = {
     .init = init,
     .quit = quit,
     .getMountData = getMD,
-    .slewTo = slew2,
+//    .slewTo = slew2,
     .moveTo = move2,
     .moveWspeed = move2s,
     .setSpeed = setspeed,
@@ -359,5 +526,7 @@ mount_t Mount = {
     .longCmd = longcmd,
     .getHWconfig = get_hwconf,
     .saveHWconfig = write_hwconf,
-    .currentT = dtime,
+    .currentT = nanotime,
+    .correctTo = correct2,
 };
+

Auxiliary_utils/LibSidServo/main.h

@@ -0,0 +1,80 @@
+/*
+ * 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 "sidservo.h"
+
+extern conf_t Conf;
+double nanotime();
+void getModData(mountdata_t *mountdata);
+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__))
+// break absent in `case`
+#define FALLTHRU    __attribute__ ((fallthrough))
+// and synonym for FALLTHRU
+#define NOBREAKHERE __attribute__ ((fallthrough))
+// 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/movingmodel.c

@@ -0,0 +1,68 @@
+/*
+ * 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 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(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/ramp.c

@@ -0,0 +1,258 @@
+/*
+ * 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(...)
+#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]);
+        }
+        fflush(stdout);
+        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

@@ -21,24 +21,26 @@
 #include <fcntl.h>
 #include <math.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[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};
+//static coordval_t lastXenc = {0}, lastYenc = {0};
 
 // mutexes for RW operations with mount device and data
 static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
@@ -46,7 +48,7 @@ static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
 // encoders thread and mount thread
 static pthread_t encthread, mntthread;
 // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
-static struct timeval encRtmout = {0}, mntRtmout = {0};
+static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -55,57 +57,43 @@ typedef struct __attribute__((packed)){
     uint8_t CRC[4];
 } enc_t;
 
-/**
- * @brief dtime - monotonic time from first run
- * @return
- */
-double dtime(){
-    struct timespec start_time = {0}, cur_time;
-    if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
-        clock_gettime(CLOCK_MONOTONIC, &start_time);
-    }
-    clock_gettime(CLOCK_MONOTONIC, &cur_time);
-    return ((double)(cur_time.tv_sec - start_time.tv_sec) +
-                     (cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
-}
-#if 0
-double dtime(){
-    double t;
-    struct timeval tv;
-    gettimeofday(&tv, NULL);
-    t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
-    return t;
-}
-#endif
-#if 0
-double tv2d(struct timeval *tv){
-    if(!tv) return 0.;
-    double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6;
-    return t;
-}
-#endif
-#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
-
 // calculate current X/Y speeds
-static void getXspeed(double t){
+void getXspeed(){
+    static less_square_t *ls = NULL;
+    if(!ls){
+        ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
+        if(!ls) return;
+    }
+    double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
+    if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
+        mountdata.encXspeed.val = speed;
+        mountdata.encXspeed.t = mountdata.encXposition.t;
+    }
+    //DBG("Xspeed=%g", mountdata.encXspeed.val);
+#if 0
     mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
-    mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
+    mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
     lastXenc.val = mountdata.encXposition.val;
     lastXenc.t = t;
+#endif
 }
-static void getYspeed(double t){
+void getYspeed(){
+    static less_square_t *ls = NULL;
+    if(!ls){
+        ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
+        if(!ls) return;
+    }
+    double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
+    if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
+        mountdata.encYspeed.val = speed;
+        mountdata.encYspeed.t = mountdata.encYposition.t;
+    }
+#if 0
     mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
-    mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
+    mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
     lastYenc.val = mountdata.encYposition.val;
     lastYenc.t = t;
+#endif
 }
 
 /**
@@ -153,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
     DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
     mountdata.encXposition.t = t;
     mountdata.encYposition.t = t;
-    if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
+    //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
-    if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
+    //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
+    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);
 }
@@ -170,7 +159,7 @@ static int getencval(int fd, double *val, double *t){
     if(fd < 0) return FALSE;
     char buf[128];
     int got = 0, Lmax = 127;
-    double t0 = dtime();
+    double t0 = nanotime();
     do{
         fd_set rfds;
         FD_ZERO(&rfds);
@@ -189,7 +178,7 @@ static int getencval(int fd, double *val, double *t){
             buf[got] = 0;
         } else continue;
         if(strchr(buf, '\n')) break;
-    }while(Lmax && dtime() - t0 < Conf.EncoderReqInterval);
+    }while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval);
     if(got == 0) return 0; // WTF?
     char *estr = strrchr(buf, '\n');
     if(!estr) return 0;
@@ -254,12 +243,40 @@ 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;
+    //double t0 = nanotime();
+    //int n = 0;
+    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;
+            //++n;
+        } else break;
+    }while(1);
+    //DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
+}
 
 // main encoder thread (for separate encoder): read next data and make parsing
 static void *encoderthread1(void _U_ *u){
@@ -277,7 +294,7 @@ static void *encoderthread1(void _U_ *u){
             if((uint8_t)b == ENC_MAGICK){
 //                DBG("Got magic -> start filling packet");
                 databuf[wridx++] = (uint8_t) b;
-                t = dtime();
+                t = nanotime();
             }
             continue;
         }else databuf[wridx++] = (uint8_t) b;
@@ -298,25 +315,27 @@ static void *encoderthread2(void _U_ *u){
     if(Conf.SepEncoder != 2) return NULL;
     DBG("Thread started");
     int errctr = 0;
-    double t0 = dtime();
+    double t0 = nanotime();
-    const char *req = "next\n";
+    const char *req = "\n";
     int need2ask = 0; // need or not to ask encoder for new data
     while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
         if(need2ask){
-            if(5 != write(encfd[0], req, 5)) { ++errctr; continue; }
+            if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
-            else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; }
+            else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
         }
         double v, t;
         if(getencval(encfd[0], &v, &t)){
             mountdata.encXposition.val = X_ENC2RAD(v);
             //DBG("encX(%g) = %g", t, mountdata.encXposition.val);
             mountdata.encXposition.t = t;
-            if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
+            //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
+            getXspeed();
             if(getencval(encfd[1], &v, &t)){
                 mountdata.encYposition.val = Y_ENC2RAD(v);
                 //DBG("encY(%g) = %g", t, mountdata.encYposition.val);
                 mountdata.encYposition.t = t;
-                if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
+                //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
+                getYspeed();
                 errctr = 0;
                 need2ask = 0;
             } else {
@@ -329,9 +348,9 @@ static void *encoderthread2(void _U_ *u){
             else need2ask = 1;
             continue;
         }
-        while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
+        while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); }
-        //DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
+        //DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval);
-        t0 = dtime();
+        t0 = nanotime();
     }
     DBG("ERRCTR=%d", errctr);
     for(int i = 0; i < 2; ++i){
@@ -364,20 +383,49 @@ static void *mountthread(void _U_ *u){
     int errctr = 0;
     uint8_t buf[2*sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
+    bzero(&mountdata, sizeof(mountdata));
+    if(Conf.RunModel) while(1){
+        pthread_mutex_lock(&datamutex);
+        // now change data
+        getModData(&mountdata);
+        pthread_mutex_unlock(&datamutex);
+        double t0 = nanotime();
+        while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
+        t0 = nanotime();
+    }
     // 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
-        double tgot = dtime();
+        double t0 = nanotime();
+#if 0
+// 127 milliseconds to get answer on X/Y commands!!!
+        int64_t ans;
+        int ctr = 0;
+        if(SSgetint(CMD_MOTX, &ans)){
+            pthread_mutex_lock(&datamutex);
+            mountdata.motXposition.t = tgot;
+            mountdata.motXposition.val = X_MOT2RAD(ans);
+            pthread_mutex_unlock(&datamutex);
+            ++ctr;
+        }
+        tgot = nanotime();
+        if(SSgetint(CMD_MOTY, &ans)){
+            pthread_mutex_lock(&datamutex);
+            mountdata.motXposition.t = tgot;
+            mountdata.motXposition.val = X_MOT2RAD(ans);
+            pthread_mutex_unlock(&datamutex);
+            ++ctr;
+        }
+        if(ctr == 2){
+            mountdata.millis = (uint32_t)(1e3 * tgot);
+            DBG("Got both coords; millis=%d", mountdata.millis);
+        }
+#endif
+        // 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);
@@ -393,13 +441,14 @@ static void *mountthread(void _U_ *u){
         errctr = 0;
         pthread_mutex_lock(&datamutex);
         // now change data
-        SSconvstat(status, &mountdata, tgot);
+        SSconvstat(status, &mountdata, t0);
         pthread_mutex_unlock(&datamutex);
+        //DBG("GOT FULL stat by %g", nanotime() - t0);
         // allow writing & getters
-        //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
+        do{
-        if(dtime() - t0 >= Conf.MountReqInterval) usleep(50);
+            usleep(500);
-        while(dtime() - t0 < Conf.MountReqInterval);
+        }while(nanotime() - t0 < Conf.MountReqInterval);
-        t0 = dtime();
+        t0 = nanotime();
     }
     data_free(&cmd_getstat);
 failed:
@@ -435,6 +484,7 @@ static int ttyopen(const char *path, speed_t speed){
 
 // return FALSE if failed
 int openEncoder(){
+    if(Conf.RunModel) return TRUE;
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
     if(Conf.SepEncoder == 1){ // only one device
         DBG("One device");
@@ -442,7 +492,7 @@ int openEncoder(){
         encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
         if(encfd[0] < 0) return FALSE;
         encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
+        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
         if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
             close(encfd[0]);
             encfd[0] = -1;
@@ -472,6 +522,7 @@ int openEncoder(){
 
 // return FALSE if failed
 int openMount(){
+    if(Conf.RunModel) goto create_thread;
     if(mntfd > -1) close(mntfd);
     DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
     mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
@@ -487,11 +538,14 @@ int openMount(){
         DBG("got %zd", l);
     }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 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");
+        DBG("Can't create mount thread");
-        close(mntfd);
+        if(!Conf.RunModel){
-        mntfd = -1;
+            close(mntfd);
+            mntfd = -1;
+        }
         return FALSE;
     }
     DBG("Mount opened, thread started");
@@ -500,21 +554,22 @@ int openMount(){
 
 // close all opened serial devices and quit threads
 void closeSerial(){
+    if(Conf.RunModel) return;
     if(mntfd > -1){
-        DBG("Kill mount thread");
+        DBG("Cancel mount thread");
         pthread_cancel(mntthread);
         DBG("join mount thread");
         pthread_join(mntthread, NULL);
-        DBG("close fd");
+        DBG("close mount fd");
         close(mntfd);
         mntfd = -1;
     }
     if(encfd[0] > -1){
-        DBG("Kill encoder thread");
+        DBG("Cancel encoder thread");
         pthread_cancel(encthread);
         DBG("join encoder thread");
         pthread_join(encthread, NULL);
-        DBG("close fd");
+        DBG("close encoder's fd");
         close(encfd[0]);
         encfd[0] = -1;
         if(Conf.SepEncoder == 2){
@@ -533,16 +588,22 @@ mcc_errcodes_t getMD(mountdata_t  *d){
     return MCC_E_OK;
 }
 
-void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
+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.Xstatus = Xstatus;
+    mountdata.Xstate = Xstate;
-    mountdata.Ystatus = Ystatus;
+    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();
+    //double t0 = nanotime();
     if(out){
         if(out->len != (size_t)write(mntfd, out->buf, out->len)){
             DBG("written bytes not equal to need");
@@ -554,16 +615,18 @@ static int wr(const data_t *out, data_t *in, int needeol){
             (void) g;
         }
     }
-    uint8_t buf[256];
+    //DBG("sent by %g", nanotime() - t0);
-    data_t dumb = {.buf = buf, .maxlen = 256};
+    //uint8_t buf[256];
-    if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
+    //data_t dumb = {.buf = buf, .maxlen = 256};
+    if(!in) return TRUE;
+    //if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
     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;
     }
-    //DBG("Clear trashing input");
+    //DBG("got %zd bytes by %g", in->len, nanotime() - t0);
     while(getmntbyte() > -1);
     return TRUE;
 }
@@ -575,6 +638,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);
@@ -582,6 +646,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);
@@ -605,32 +670,32 @@ 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];
+    //char ans[300];
-    data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
+    //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);
 #endif
-        if(!wr(dscmd, &a, 1)) goto rtn;
+        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);
 #endif
-        if(!wr(dlcmd, &a, 1)) goto rtn;
+        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;
@@ -641,6 +706,7 @@ rtn:
     return ret;
 }
 
+// short, long and config text-binary commands
 // return TRUE if OK
 int cmdS(SSscmd *cmd){
     return bincmd((uint8_t *)cmd, sizeof(SSscmd));
@@ -650,6 +716,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

Auxiliary_utils/LibSidServo/serial.h

@@ -28,16 +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();
 int openMount();
 void closeSerial();
 mcc_errcodes_t getMD(mountdata_t  *d);
-void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
+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,14 +32,37 @@ extern "C"
 #include <stdint.h>
 #include <sys/time.h>
 
+// acceptable position error - 0.1''
+#define MCC_POSITION_ERROR      (5e-7)
+// acceptable disagreement between motor and axis encoders - 2''
+#define MCC_ENCODERS_ERROR      (1e-7)
+
 // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
 #define MCC_MAX_X_SPEED         (0.174533)
 #define MCC_MAX_Y_SPEED         (0.139626)
+// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
+// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
+#define MCC_X_ACCELERATION      (0.219911)
+#define MCC_Y_ACCELERATION      (0.165806)
 
 // max speed interval, seconds
 #define MCC_CONF_MAX_SPEEDINT   (2.)
 // minimal speed interval in parts of EncoderReqInterval
 #define MCC_CONF_MIN_SPEEDC     (3.)
+// PID I cycle time (analog of "RC" for PID on opamps)
+#define MCC_PID_CYCLE_TIME      (5.)
+// maximal PID refresh time interval (if larger all old data will be cleared)
+#define MCC_PID_MAX_DT          (1.)
+// normal PID refresh interval
+#define MCC_PID_REFRESH_DT      (0.1)
+// boundary conditions for axis state: "slewing/pointing/guiding"
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
+//#define MCC_MAX_POINTING_ERR    (0.20943951)
+#define MCC_MAX_POINTING_ERR    (0.08726646)
+// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
+#define MCC_MAX_GUIDING_ERR     (0.026179939)
+// if error less than this value we suppose that target is captured and guiding is good: 0.1''
+#define MCC_MAX_ATTARGET_ERR    (4.8481368e-7)
 
 // error codes
 typedef enum{
@@ -47,16 +75,25 @@ typedef enum{
 } mcc_errcodes_t;
 
 typedef struct{
-    char*   MountDevPath;       // path to mount device
+    double P, I, D;
-    int     MountDevSpeed;      // serial speed
+} PIDpar_t;
-    char*   EncoderDevPath;     // path to encoder device
+
-    int     EncoderDevSpeed;    // serial speed
+typedef struct{
-    int     SepEncoder;         // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
+    char*   MountDevPath;           // path to mount device
-    char*   EncoderXDevPath;    // paths to new controller devices
+    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  MountReqInterval;   // interval between subsequent mount requests (seconds)
+    double  MountReqInterval;       // interval between subsequent mount requests (seconds)
-    double  EncoderReqInterval; // interval between subsequent encoder requests (seconds)
+    double  EncoderReqInterval;     // interval between subsequent encoder requests (seconds)
-    double  EncoderSpeedInterval;// interval between speed calculations
+    double  EncoderSpeedInterval;   // interval between speed calculations
+    int     RunModel;               // == 1 if you want to use model instead of real mount
+    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;
 } conf_t;
 
 // coordinates/speeds in degrees or d/s: X, Y
@@ -113,16 +150,16 @@ typedef struct{
 } extradata_t;
 
 typedef enum{
-    MNT_STOPPED,
+    AXIS_STOPPED,
-    MNT_SLEWING,
+    AXIS_SLEWING,
-    MNT_POINTING,
+    AXIS_POINTING,
-    MNT_GUIDING,
+    AXIS_GUIDING,
-    MNT_ERROR,
+    AXIS_ERROR,
-} mnt_status_t;
+} axis_status_t;
 
 typedef struct{
-    mnt_status_t Xstatus;
+    axis_status_t Xstate;
-    mnt_status_t Ystatus;
+    axis_status_t Ystate;
     coordval_t motXposition;
     coordval_t motYposition;
     coordval_t encXposition;
@@ -157,7 +194,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 (???)
@@ -168,13 +205,13 @@ typedef struct{
     double outplimit;   // Output Limit, percent (0..100)
     double currlimit;   // Current Limit (A)
     double intlimit;    // Integral Limit (???)
-} __attribute__((packed)) axe_config_t;
+} __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 (???)
@@ -197,19 +234,19 @@ typedef struct{
     double backlspd;    // Backlash speed (rad/s)
 } hardware_configuration_t;
 
-// flags for slew function
+/* 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  (*slewTo)(const coordpair_t *target, slewflags_t flags);
+//    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
-    mcc_errcodes_t  (*correctTo)(coordval_pair_t *target);
+    mcc_errcodes_t  (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
     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
@@ -224,7 +261,6 @@ typedef struct{
 
 extern mount_t Mount;
 
-
 #ifdef __cplusplus
 }
 #endif

Auxiliary_utils/LibSidServo/ssii.c

@@ -19,8 +19,9 @@
 #include <ctype.h>
 #include <inttypes.h>
 #include <string.h>
+#include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
 #include "serial.h"
 #include "ssii.h"
 
@@ -35,16 +36,18 @@ 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 axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
+static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
     if(*prev == INT32_MAX){
-        *stat = MNT_STOPPED;
+        *stat = AXIS_STOPPED;
-    }else if(*stat != MNT_STOPPED){
+        DBG("START");
-        if(*prev == cur){
+    }else if(*stat != AXIS_STOPPED){
-            if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
+        if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
+            *stat = AXIS_STOPPED;
+            DBG("AXIS stopped");
         }
     }else if(*prev != cur){
-        //*stat = MNT_SLEWING;
+        DBG("AXIS moving");
         *nstopped = 0;
     }
     *prev = cur;
@@ -53,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta
 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
-    axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
+    chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
-    axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
+    chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
 }
 
 /**
@@ -65,13 +68,6 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
  */
 void SSconvstat(const SSstat *s, mountdata_t *m, double t){
     if(!s || !m) return;
-/*
-#ifdef EBUG
-    static double t0 = -1.;
-    if(t0 < 0.) t0 = dtime();
-#endif
-    DBG("Convert, t=%g", dtime()-t0);
-*/
     m->motXposition.val = X_MOT2RAD(s->Xmot);
     m->motYposition.val = Y_MOT2RAD(s->Ymot);
     ChkStopped(s, m);
@@ -81,10 +77,8 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
         m->encXposition.val = X_ENC2RAD(s->Xenc);
         m->encYposition.val = Y_ENC2RAD(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;
@@ -110,7 +104,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);
 }
 
@@ -123,7 +117,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);
 }
 
@@ -180,22 +174,38 @@ int SSstop(int emerg){
 // update motors' positions due to encoders'
 mcc_errcodes_t updateMotorPos(){
     mountdata_t md = {0};
-    double t0 = dtime(), t = 0.;
+    if(Conf.RunModel) return MCC_E_OK;
+    double t0 = nanotime(), t = 0.;
     DBG("start @ %g", t0);
     do{
-        t = dtime();
+        t = nanotime();
         if(MCC_E_OK == getMD(&md)){
-            DBG("got");
+            if(md.encXposition.t == 0 || md.encYposition.t == 0){
-            if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
+                DBG("Just started, t-t0 = %g!", t - t0);
-                DBG("FIX motors position to encoders");
+                sleep(1);
-                int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
+                DBG("t-t0 = %g", nanotime() - t0);
-                if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
+                //usleep(10000);
-                    DBG("All OK");
+                continue;
-                    return MCC_E_OK;
+            }
-                }
+            DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
-            }else{
+            mcc_errcodes_t OK = MCC_E_OK;
-                DBG("on position");
+            if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
-                return MCC_E_OK;
+                DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, 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", nanotime() - t0);
+            }
+            if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
+                DBG("NEED to sync Y: motors=%g, axiss=%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", nanotime() - t0);
+            }
+            if(MCC_E_OK == OK){
+                DBG("Encoders synced");
+                return OK;
             }
         }
         DBG("NO DATA; dt = %g", t - t0);

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>
@@ -169,18 +173,32 @@
 #define SITECH_LOOP_FREQUENCY   (1953.)
 
 // amount of consequent same coordinates to detect stop
-#define MOTOR_STOPPED_CNT       (20)
+#define MOTOR_STOPPED_CNT       (4)
 
+// TODO: take it from settings?
 // steps per revolution (SSI - x4 - for SSI)
-// 13312000 / 4 = 3328000
 #define X_MOT_STEPSPERREV_SSI   (13312000.)
-//#define X_MOT_STEPSPERREV   (3325952.)
+// 13312000 / 4 = 3328000
 #define X_MOT_STEPSPERREV   (3328000.)
-// 17578668 / 4 = 4394667
 #define Y_MOT_STEPSPERREV_SSI (17578668.)
-//#define Y_MOT_STEPSPERREV   (4394960.)
+// 17578668 / 4 = 4394667
 #define Y_MOT_STEPSPERREV   (4394667.)
 
+// encoder per revolution
+#define X_ENC_STEPSPERREV   (67108864.)
+#define Y_ENC_STEPSPERREV   (67108864.)
+// encoder zero position
+#define X_ENC_ZERO          (61245239)
+#define Y_ENC_ZERO          (36999830)
+// encoder reversed (no: +1)
+#define X_ENC_SIGN          (-1.)
+#define Y_ENC_SIGN          (-1.)
+// encoder position to radians and back
+#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
+#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / 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))
+
 // convert angle in radians to +-pi
 static inline double ang2half(double ang){
     if(ang < -M_PI) ang += 2.*M_PI;
@@ -214,21 +232,6 @@ static inline double ang2full(double ang){
 #define ADDER2S(a)  ((a) / SITECH_LOOP_FREQUENCY)
 #define S2ADDER(s)  ((s) * SITECH_LOOP_FREQUENCY)
 
-// encoder per revolution
-#define X_ENC_STEPSPERREV   (67108864.)
-#define Y_ENC_STEPSPERREV   (67108864.)
-// encoder zero position
-#define X_ENC_ZERO          (61245239)
-#define Y_ENC_ZERO          (36999830)
-// encoder reversed (no: +1)
-#define X_ENC_SIGN          (-1.)
-#define Y_ENC_SIGN          (-1.)
-// encoder position to radians and back
-#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
-#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / 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))
-
 // encoder's tolerance (ticks)
 #define YencTOL    (25.)
 #define XencTOL    (25.)
@@ -334,5 +337,4 @@ 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();

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");
+        WARNX("Z > 80degr (%g), stop telescope", hnow.zd * ERFA_DR2D);
-        putlog("Z>80 - stop!");
+        putlog("Z=%.1f > 80 - stop!", hnow.zd * ERFA_DR2D);
         stop_telescope();
         return 0;
     }