.

fixed time
2025-12-06 02:35:14 +03:00 · 2025-11-01 15:39:55 +03:00 · 2025-11-01 15:02:11 +03:00
19 changed files with 141 additions and 76 deletions
--- a/Auxiliary_utils/LibSidServo/PID.c
+++ b/Auxiliary_utils/LibSidServo/PID.c
@ -29,6 +29,7 @@ PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
    if(!gain || Iarrsz < 3) return NULL;
    PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
    pid->gain = *gain;
+    DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
    pid->pidIarrSize = Iarrsz;
    pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
    return pid;
@ -141,9 +142,9 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
    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);
+    DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state);
    double tagspeed = pid_calculate(pid, error, dt);
-    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
+    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based
    return tagspeed; // coordinate-based
 }

@ -176,13 +177,15 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
    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;
+    if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X;
+    if(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;
+    if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y;
+    if(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");
--- a/Auxiliary_utils/LibSidServo/examples/conf.c
+++ b/Auxiliary_utils/LibSidServo/examples/conf.c
@ -52,7 +52,7 @@ static sl_option_t opts[] = {
    {"EncoderDevSpeed", NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.EncoderDevSpeed),  "serial speed of encoder device"},
    {"MountReqInterval",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
    {"EncoderReqInterval",NEED_ARG, NULL,   0,  arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
-    {"SepEncoder",      NO_ARGS,    NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device (1 - one device, 2 - two devices)"},
+    {"SepEncoder",      NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device (1 - one device, 2 - two devices)"},
    {"EncoderXDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderXDevPath),  "path to X encoder (/dev/encoderX0)"},
    {"EncoderYDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderYDevPath),  "path to Y encoder (/dev/encoderY0)"},
    {"EncoderSpeedInterval", NEED_ARG,NULL, 0,  arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
--- a/Auxiliary_utils/LibSidServo/examples/dump.c
+++ b/Auxiliary_utils/LibSidServo/examples/dump.c
@ -23,6 +23,9 @@
 #include "dump.h"
 #include "simpleconv.h"

+// starting dump time (to conform different logs)
+static double dumpT0 = -1.;
+
 #if 0
 // amount of elements used for encoders' data filtering
 #define NFILT	(10)
@ -59,6 +62,10 @@ static double filter(double val, int idx){
 }
 #endif

+// return starting time of dump
+double dumpt0(){ return dumpT0; }
+
+
 /**
 * @brief logmnt - log mount data into file
 * @param fcoords - file to dump
@ -67,17 +74,13 @@ static double filter(double val, int idx){
 void logmnt(FILE *fcoords, mountdata_t *m){
    if(!fcoords) return;
    //DBG("LOG %s", m ? "data" : "header");
-    static double t0 = -1.;
    if(!m){ // write header
        fprintf(fcoords, "#     time    Xmot(deg)   Ymot(deg) Xenc(deg)  Yenc(deg)   VX(d/s)    VY(d/s)     millis\n");
        return;
-    }
-    double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
-    if(t0 < 0.) t0 = tnow;
-    double t = tnow - t0;
+    }else if(dumpT0 < 0.) dumpT0 = m->encXposition.t;
    // write data
    fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
-            t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
+            m->encXposition.t - dumpT0, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
            RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
            RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
            m->millis);
@ -143,6 +146,7 @@ void waitmoving(int N){
        if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
            xlast = mdata.motXposition.val;
            ylast = mdata.motYposition.val;
+            //DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
            ctr = 0;
        }else ++ctr;
    }
--- a/Auxiliary_utils/LibSidServo/examples/dump.h
+++ b/Auxiliary_utils/LibSidServo/examples/dump.h
@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N);
 void waitmoving(int N);
 int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
 void chk0(int ncycles);
+double dumpt0();
--- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c
+++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c
@ -216,7 +216,7 @@ int main(int argc, char **argv){
    sleep(5);
    // return to zero and wait
    green("Return 2 zero and wait\n");
-    if(!return2zero()) ERRX("Can't return");
+    if(MCC_E_OK != return2zero()) ERRX("Can't return");
    Wait(0., 0);
    Wait(0., 1);
    // wait moving ends
--- a/Auxiliary_utils/LibSidServo/examples/dumpswing.c
+++ b/Auxiliary_utils/LibSidServo/examples/dumpswing.c
@ -110,16 +110,28 @@ int main(int argc, char **argv){
        return 1;
    }
    if(G.coordsoutput){
-        if(!(fcoords = fopen(G.coordsoutput, "w")))
-            ERRX("Can't open %s", G.coordsoutput);
+        if(!(fcoords = fopen(G.coordsoutput, "w"))){
+            WARNX("Can't open %s", G.coordsoutput);
+            return 1;
+        }
    }else fcoords = stdout;
-    if(G.Ncycles < 7) ERRX("Ncycles should be >7");
+    if(G.Ncycles < 2){
+        WARNX("Ncycles should be >2");
+        return 1;
+    }
    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");
-    if(G.Nswings < 1) ERRX("Nswings should be more than 0");
+    if(absamp < 0.01 || absamp > 45.){
+        WARNX("Amplitude should be from 0.01 to 45 degrees");
+        return 1;
+    }
+    if(G.period < 0.1 || G.period > 900.){
+        WARNX("Period should be from 0.1 to 900s");
+        return 1;
+    }
+    if(G.Nswings < 1){
+        WARNX("Nswings should be more than 0");
+        return 1;
+    }
    conf_t *Config = readServoConf(G.conffile);
    if(!Config){
        dumpConf();
--- a/Auxiliary_utils/LibSidServo/examples/goto.c
+++ b/Auxiliary_utils/LibSidServo/examples/goto.c
@ -61,12 +61,13 @@ static FILE* fcoords = NULL;
 static pthread_t dthr;

 void signals(int sig){
-    pthread_cancel(dthr);
    if(sig){
        signal(sig, SIG_IGN);
        DBG("Get signal %d, quit.\n", sig);
    }
+    DBG("Quit");
    Mount.quit();
+    DBG("close");
    if(fcoords) fclose(fcoords);
    exit(sig);
 }
@ -90,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){
    if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
    coordval_pair_t M, E;
    if(!getPos(&M, &E)) ERRX("Can't get current position");
+    DBG("xt: %g, x: %g", M.X.t, M.X.val);
    if(G.coordsoutput){
-        if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
+        if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
        G.wait = 1;
-    }
-    if(G.coordsoutput){
        if(!(fcoords = fopen(G.coordsoutput, "w")))
            ERRX("Can't open %s", G.coordsoutput);
        logmnt(fcoords, NULL);
--- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c
+++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c
@ -75,7 +75,6 @@ static sl_option_t cmdlnopts[] = {
 };

 void signals(int sig){
-    pthread_cancel(dthr);
    if(sig){
        signal(sig, SIG_IGN);
        DBG("Get signal %d, quit.\n", sig);
@ -99,7 +98,7 @@ static void runtraectory(traectory_fn tfn){
    coordval_pair_t target;
    coordpair_t traectXY, endpoint;
    endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
-    double t0 = Mount.currentT(), tlast = 0.;
+    double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT();
    double tlastX = 0., tlastY = 0.;
    while(1){
        if(!telpos(&telXY)){
@ -107,10 +106,10 @@ static void runtraectory(traectory_fn tfn){
            return;
        }
        if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
-        DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
+        DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t);
        tlastX = telXY.X.t; tlastY = telXY.Y.t;
        double t = Mount.currentT();
-        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
+        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
        if(!traectory_point(&traectXY, t)) break;
        target.X.val = traectXY.X; target.Y.val = traectXY.Y;
        target.X.t = target.Y.t = t;
@ -119,11 +118,14 @@ static void runtraectory(traectory_fn tfn){
        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));
+        if(t0 < 0.) t0 = dumpt0();
+        else{
+            //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));
+            //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));
+                fprintf(errlog, "%10.4f  %10.4f  %10.4f\n", telXY.X.t-t0, 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;
--- a/Auxiliary_utils/LibSidServo/examples/servo.conf
+++ b/Auxiliary_utils/LibSidServo/examples/servo.conf
@ -1,4 +1,4 @@
-Current configuration:
+# Current configuration
 MountDevPath=/dev/ttyUSB0
 MountDevSpeed=19200
 EncoderDevPath=(null)
--- a/Auxiliary_utils/LibSidServo/examples/traectories.c
+++ b/Auxiliary_utils/LibSidServo/examples/traectories.c
@ -76,6 +76,7 @@ int telpos(coordval_pair_t *curpos){
    }
    if(ntries == 10) return FALSE;
    coordval_pair_t pt;
+    //DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
    pt.X.val = mdata.encXposition.val;
    pt.Y.val = mdata.encYposition.val;
    pt.X.t = mdata.encXposition.t;
--- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user
+++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user
@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
+<!-- Written by QtCreator 17.0.2, 2025-11-01T14:58:43. -->
 <qtcreator>
 <data>
  <variable>EnvironmentId</variable>
@ -110,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">Сборка</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
    </valuemap>
    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -123,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">Очистка</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
    </valuemap>
    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@ -139,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">Развёртывание</value>
-      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
     </valuemap>
     <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -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">Развёртывание</value>
-     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
    </valuemap>
    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
--- a/Auxiliary_utils/LibSidServo/main.c
+++ b/Auxiliary_utils/LibSidServo/main.c
@ -51,17 +51,18 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd);
 * @return time in seconds
 */
 double nanotime(){
-    static struct timespec *start = NULL;
+    //static struct timespec *start = NULL;
    struct timespec now;
-    if(!start){
+    /*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 nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
    double sd = (double)now.tv_sec - (double)start->tv_sec;
-    return sd + nd;
+    return sd + nd;*/
+    return (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
 }

 /**
@ -75,10 +76,8 @@ static void quit(){
    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;
+void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst){
+    if(!c || !Xmodel || !Ymodel) return;
    double tnow = nanotime();
    moveparam_t Xp, Yp;
    movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
@ -86,14 +85,11 @@ void getModData(mountdata_t *mountdata){
    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;
+    c->X.t = c->Y.t = tnow;
+    c->X.val = Xp.coord;
+    c->Y.val =  Yp.coord;
+    if(xst) *xst = Xst;
+    if(yst) *yst = Yst;
 }

 /**
--- a/Auxiliary_utils/LibSidServo/main.h
+++ b/Auxiliary_utils/LibSidServo/main.h
@ -24,11 +24,12 @@

 #include <stdlib.h>

+#include "movingmodel.h"
 #include "sidservo.h"

 extern conf_t Conf;
 double nanotime();
-void getModData(mountdata_t *mountdata);
+void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst);
 typedef struct{
    double *x, *t, *t2, *xt; // arrays of coord/time and multiply
    double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
--- a/Auxiliary_utils/LibSidServo/movingmodel.c
+++ b/Auxiliary_utils/LibSidServo/movingmodel.c
@ -63,6 +63,11 @@ int model_move2(movemodel_t *model, moveparam_t *target, double t){
    DBG("MOVE to %g at speed %g", target->coord, target->speed);
    // only positive velocity
    if(target->speed < 0.) target->speed = -target->speed;
+    if(fabs(target->speed) < model->Min.speed){
+        DBG("STOP");
+        model->stop(model, t);
+        return TRUE;
+    }
    // don't mind about acceleration - user cannot set it now
    return model->calculate(model, target, t);
 }
--- a/Auxiliary_utils/LibSidServo/ramp.c
+++ b/Auxiliary_utils/LibSidServo/ramp.c
@ -23,12 +23,14 @@

 #include "main.h"
 #include "ramp.h"
-/*
+
 #ifdef EBUG
 #undef DBG
 #define DBG(...)
+#undef FNAME
+#define FNAME()
 #endif
-*/
+
 static double coord_tolerance = COORD_TOLERANCE_DEFAULT;

 static void emstop(movemodel_t *m, double _U_ t){
@ -120,7 +122,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
        }
    }else{
        // if we are here, we have the worst case: change speed direction
-        DBG("Hardest 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;
@ -132,7 +134,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
        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);
+       // DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
        return;
    }
    m->state = ST_MOVE;
@ -210,11 +212,10 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
    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);
+       /* 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;
    }
--- a/Auxiliary_utils/LibSidServo/serial.c
+++ b/Auxiliary_utils/LibSidServo/serial.c
@ -59,16 +59,20 @@ typedef struct __attribute__((packed)){

 // calculate current X/Y speeds
 void getXspeed(){
+    static double t0 = -1.; // time of start - eliminate problem of very large times in squares
+    if(t0 < 0.) t0 = mountdata.encXposition.t;
    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);
+    pthread_mutex_lock(&datamutex);
+    double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0);
    if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
        mountdata.encXspeed.val = speed;
        mountdata.encXspeed.t = mountdata.encXposition.t;
    }
+    pthread_mutex_unlock(&datamutex);
    //DBG("Xspeed=%g", mountdata.encXspeed.val);
 #if 0
    mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
@ -78,16 +82,20 @@ void getXspeed(){
 #endif
 }
 void getYspeed(){
+    static double t0 = -1.; // time of start - eliminate problem of very large times in squares
+    if(t0 < 0.) t0 = mountdata.encXposition.t;
    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);
+    pthread_mutex_lock(&datamutex);
+    double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0);
    if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
        mountdata.encYspeed.val = speed;
        mountdata.encYspeed.t = mountdata.encYposition.t;
    }
+    pthread_mutex_unlock(&datamutex);
 #if 0
    mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
    mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
@ -325,15 +333,19 @@ static void *encoderthread2(void _U_ *u){
        }
        double v, t;
        if(getencval(encfd[0], &v, &t)){
+            pthread_mutex_lock(&datamutex);
            mountdata.encXposition.val = X_ENC2RAD(v);
            //DBG("encX(%g) = %g", t, mountdata.encXposition.val);
            mountdata.encXposition.t = t;
+            pthread_mutex_unlock(&datamutex);
            //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
            getXspeed();
            if(getencval(encfd[1], &v, &t)){
+                pthread_mutex_lock(&datamutex);
                mountdata.encYposition.val = Y_ENC2RAD(v);
                //DBG("encY(%g) = %g", t, mountdata.encYposition.val);
                mountdata.encYposition.t = t;
+                pthread_mutex_unlock(&datamutex);
                //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
                getYspeed();
                errctr = 0;
@ -384,12 +396,35 @@ static void *mountthread(void _U_ *u){
    uint8_t buf[2*sizeof(SSstat)];
    SSstat *status = (SSstat*) buf;
    bzero(&mountdata, sizeof(mountdata));
+    double t0 = nanotime(), tstart = t0;
+    static double oldmt = -100.; // old `millis measurement` time
+    static uint32_t oldmillis = 0;
    if(Conf.RunModel) while(1){
-        pthread_mutex_lock(&datamutex);
+        coordval_pair_t c;
+        movestate_t xst, yst;
        // now change data
-        getModData(&mountdata);
+        getModData(&c, &xst, &yst);
+        pthread_mutex_lock(&datamutex);
+        double tnow = c.X.t;
+        mountdata.encXposition.t = mountdata.encYposition.t = tnow;
+        mountdata.encXposition.val = c.X.val;
+        mountdata.encYposition.val = c.Y.val;
+        //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
+        if(tnow - oldmt > Conf.MountReqInterval){
+            oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
+            mountdata.motYposition.t = mountdata.motXposition.t = tnow;
+            if(xst == ST_MOVE)
+                mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
+            else
+                mountdata.motXposition.val = c.X.val;
+            if(yst == ST_MOVE)
+                mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
+            else
+                mountdata.motYposition.val = c.Y.val;
+            oldmt = tnow;
+        }else mountdata.millis = oldmillis;
        pthread_mutex_unlock(&datamutex);
-        double t0 = nanotime();
+        getXspeed(); getYspeed();
        while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
        t0 = nanotime();
    }
@ -572,7 +607,7 @@ void closeSerial(){
        DBG("close encoder's fd");
        close(encfd[0]);
        encfd[0] = -1;
-        if(Conf.SepEncoder == 2){
+        if(Conf.SepEncoder == 2 && encfd[1] > -1){
            close(encfd[1]);
            encfd[1] = -1;
        }
--- a/Auxiliary_utils/LibSidServo/sidservo.h
+++ b/Auxiliary_utils/LibSidServo/sidservo.h
@ -56,9 +56,10 @@ extern "C"
 // 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
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees
 //#define MCC_MAX_POINTING_ERR    (0.20943951)
-#define MCC_MAX_POINTING_ERR    (0.08726646)
+//#define MCC_MAX_POINTING_ERR    (0.08726646)
+#define MCC_MAX_POINTING_ERR    (0.13962634)
 // 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''
--- a/Auxiliary_utils/LibSidServo/ssii.c
+++ b/Auxiliary_utils/LibSidServo/ssii.c
@ -18,6 +18,7 @@

 #include <ctype.h>
 #include <inttypes.h>
+#include <stdio.h>
 #include <string.h>
 #include <unistd.h>

--- a/Daemons/Readme.md
+++ b/Daemons/Readme.md
@ -8,3 +8,5 @@ Different daemons & tools
 - *netsocket* - scripts for management of network 220V-socket
 - *send_coordinates* - get/send coordinates to 10-micron mount through stellarium daemon
 - *teldaemon* - open/close Astrosib-500 scope covers by network query
+- *weatherdaemon* - weather daemon for old meteostation
+- *weatherdaemon_newmeteo* - daemon for new (chinese) meteostation
Author	SHA1	Message	Date
Edward Emelianov	2aa8502796	.	2025-11-01 15:39:55 +03:00
Edward Emelianov	f6edc12b01	fixed time	2025-11-01 15:02:11 +03:00