some work done

Auxiliary_utils/LibSidServo/TODO

@@ -1,3 +1,3 @@
-1. PID: slew2
+fix encoders opening for several tries
-2. add model & config "model ON"
+encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request)
-
+Read HW config even in model mode

Auxiliary_utils/LibSidServo/examples/SSIIconf.c

@@ -34,7 +34,9 @@ typedef struct{
 
 static hardware_configuration_t HW = {0};
 
-static parameters G = {0};
+static parameters G = {
+    .conffile = "servo.conf",
+};
 
 static sl_option_t cmdlnopts[] = {
     {"help",        NO_ARGS,    NULL,   'h',    arg_int,    APTR(&G.help),      "show this help"},
@@ -53,7 +55,7 @@ static sl_option_t confopts[] = {
 
 static void dumpaxis(char axis, axis_config_t *c){
 #define STRUCTPAR(p)    (c)->p
-#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
+#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0)
 #define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
     DUMPD(accel);
     DUMPD(backlash);
@@ -64,6 +66,8 @@ static void dumpaxis(char axis, axis_config_t *c){
     DUMP(outplimit);
     DUMP(currlimit);
     DUMP(intlimit);
+    DUMP(motor_stepsperrev);
+    DUMP(axis_stepsperrev);
 #undef DUMP
 #undef DUMPD
 }

Auxiliary_utils/LibSidServo/examples/conf.c

@@ -83,6 +83,8 @@ static sl_option_t opts[] = {
     {"MaxPointingErr",  NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MaxPointingErr),   "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
     {"MaxFinePointingErr",NEED_ARG, NULL,   0,  arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
     {"MaxGuidingErr",   NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MaxGuidingErr),    "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
+    {"XEncZero",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.XEncZero),         "X axis encoder approximate zero position"},
+    {"YEncZero",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.YEncZero),         "Y axis encoder approximate zero position"},
     // {"",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.), ""},
     end_option
 };
@@ -110,3 +112,17 @@ void dumpConf(){
 void confHelp(){
     sl_conf_showhelp(-1, opts);
 }
+
+const char* errcodes[MCC_E_AMOUNT] = {
+    [MCC_E_OK] = "OK",
+    [MCC_E_FATAL] = "Fatal error",
+    [MCC_E_BADFORMAT] = "Wrong data format",
+    [MCC_E_ENCODERDEV] = "Encoder error",
+    [MCC_E_MOUNTDEV] = "Mount error",
+    [MCC_E_FAILED] = "Failed to run"
+};
+// return string with error code
+const char *EcodeStr(mcc_errcodes_t e){
+    if(e >= MCC_E_AMOUNT) return "Wrong error code";
+    return errcodes[e];
+}

Auxiliary_utils/LibSidServo/examples/conf.h

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

Auxiliary_utils/LibSidServo/examples/goto.c

@@ -120,7 +120,11 @@ int main(int _U_ argc, char _U_ **argv){
     }
     printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
     tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
-    Mount.moveTo(&tag);
+    mcc_errcodes_t e = Mount.moveTo(&tag);
+    if(MCC_E_OK != e){
+        WARNX("Cant go to given coordinates: %s\n", EcodeStr(e));
+        goto out;
+    }
     if(G.wait){
         sleep(1);
         waitmoving(G.Ncycles);
@@ -132,7 +136,9 @@ out:
     if(G.coordsoutput) pthread_join(dthr, NULL);
     DBG("QUIT");
     if(G.wait){
-        if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
+        usleep(250000); // pause to refresh coordinates
+        if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val),
+            RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
         Mount.quit();
     }
     return 0;

Auxiliary_utils/LibSidServo/libsidservo.creator.user

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

Auxiliary_utils/LibSidServo/main.c

@@ -25,6 +25,7 @@
 #include <time.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <unistd.h>
 
 #include "main.h"
 #include "movingmodel.h"
@@ -53,6 +54,7 @@ limits_t
         .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
 ;
 static mcc_errcodes_t shortcmd(short_command_t *cmd);
+static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
 
 /**
  * @brief curtime - monotonic time from first run
@@ -87,6 +89,7 @@ static int initstarttime(){
     curtime(&t0);
     return TRUE;
 }
+
 // return difference (in seconds) between time1 and time0
 double timediff(const struct timespec *time1, const struct timespec *time0){
     if(!time1 || !time0) return -1.;
@@ -220,7 +223,18 @@ static mcc_errcodes_t init(conf_t *c){
     }
     if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
     if(ret != MCC_E_OK) return ret;
-    return updateMotorPos();
+    // read HW config to update constants
+    hardware_configuration_t HW;
+    if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
+    // make a pause for actual encoder's values
+    double t0 = timefromstart();
+    while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
+    mcc_errcodes_t e = updateMotorPos();
+    // and refresh data after updating
+    DBG("Wait for next mount reading");
+    t0 = timefromstart();
+    while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
+    return e;
 }
 
 // check coordinates (rad) and speeds (rad/s); return FALSE if failed
@@ -242,7 +256,7 @@ static int chkYs(double s){
     return TRUE;
 }
 
-// set SLEWING state if axis was stopped later
+// set SLEWING state if axis was stopped
 static void setslewingstate(){
     //FNAME();
     mountdata_t d;
@@ -258,19 +272,6 @@ static void setslewingstate(){
     }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(AXIS_SLEWING, AXIS_SLEWING);
-    //...
-    return MCC_E_FAILED;
-}
-*/
-
 /**
  * @brief move2 - simple move to given point and stop
  * @param X - new X coordinate (radians: -pi..pi) or NULL
@@ -287,10 +288,11 @@ static mcc_errcodes_t move2(const coordpair_t *target){
     cmd.Ymot = target->Y;
     cmd.Xspeed = Xlimits.max.speed;
     cmd.Yspeed = Ylimits.max.speed;
-    mcc_errcodes_t r = shortcmd(&cmd);
+    /*mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
     setslewingstate();
-    return MCC_E_OK;
+    return MCC_E_OK;*/
+    return shortcmd(&cmd);
 }
 
 /**
@@ -427,6 +429,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
     if(!hwConfig) return MCC_E_BADFORMAT;
     if(Conf.RunModel) return MCC_E_FAILED;
     SSconfig config;
+    DBG("Read HW configuration");
     if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
     // Convert acceleration (ticks per loop^2 to rad/s^2)
     hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
@@ -466,8 +469,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) / 4; // as documentation said, real ticks are 4 times less
+    hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
-    hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
+    hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
     // Convert slew rates (ticks per loop to rad/s)
     hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
     hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@@ -485,6 +488,30 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
     hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
     // Convert backlash speed (ticks per loop to rad/s)
     hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
+    // now read text commands
+    int64_t i64;
+    double Xticks, Yticks;
+    DBG("SERIAL");
+    // motor's encoder ticks per rev
+    if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED;
+    Xticks = ((double) i64); // divide by 4 as these values stored ???
+    if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED;
+    Yticks = ((double) i64);
+    X_ENC_ZERO = Conf.XEncZero;
+    Y_ENC_ZERO = Conf.YEncZero;
+    DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
+    X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
+    Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
+    DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
+    // axis encoder ticks per rev
+    if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
+    Xticks = (double) i64;
+    if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
+    Yticks = (double) i64;
+    DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
+    X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
+    Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
+    DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
     return MCC_E_OK;
 }
 
@@ -538,8 +565,9 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
     config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
     config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
     config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
-    config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4);
+    config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
-    config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4);
+    config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
+    // todo - also write text params
     // TODO - next
     (void) config;
     return MCC_E_OK;

Auxiliary_utils/LibSidServo/serial.c

@@ -48,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 = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
+static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -131,8 +131,8 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
         return;
     }
     pthread_mutex_lock(&datamutex);
-    mountdata.encXposition.val = X_ENC2RAD(edata->encX);
+    mountdata.encXposition.val = Xenc2rad(edata->encX);
-    mountdata.encYposition.val = Y_ENC2RAD(edata->encY);
+    mountdata.encYposition.val = Yenc2rad(edata->encY);
     DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
     mountdata.encXposition.t = *t;
     mountdata.encYposition.t = *t;
@@ -151,32 +151,52 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
  * @return amount of data read or 0 if problem
  */
 static int getencval(int fd, double *val, struct timespec *t){
-    if(fd < 0) return FALSE;
+    if(fd < 0){
+        DBG("Encoder fd < 0!");
+        return FALSE;
+    }
     char buf[128];
     int got = 0, Lmax = 127;
     double t0 = timefromstart();
+    //DBG("start: %.6g", t0);
     do{
         fd_set rfds;
         FD_ZERO(&rfds);
         FD_SET(fd, &rfds);
         struct timeval tv = encRtmout;
         int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
-        if(!retval) continue;
+        if(!retval){
+            //DBG("select()==0 - timeout, %.6g", timefromstart());
+            break;
+        }
         if(retval < 0){
-            if(errno == EINTR) continue;
+            if(errno == EINTR){
+                DBG("EINTR");
+                continue;
+            }
+            DBG("select() < 0");
             return 0;
         }
         if(FD_ISSET(fd, &rfds)){
             ssize_t l = read(fd, &buf[got], Lmax);
-            if(l < 1) return 0; // disconnected ??
+            if(l < 1){
+                DBG("read() < 0");
+                return 0; // disconnected ??
+            }
             got += l; Lmax -= l;
             buf[got] = 0;
         } else continue;
-        if(strchr(buf, '\n')) break;
+        if(buf[got-1] == '\n') break; // got EOL as last symbol
-    }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval);
+    }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
-    if(got == 0) return 0; // WTF?
+    if(got == 0){
+        //DBG("No data from encoder, tfs=%.6g", timefromstart());
+        return 0;
+    }
     char *estr = strrchr(buf, '\n');
-    if(!estr) return 0;
+    if(!estr){
+        DBG("No EOL");
+        return 0;
+    }
     *estr = 0;
     char *bgn = strrchr(buf, '\n');
     if(bgn) ++bgn;
@@ -188,7 +208,7 @@ static int getencval(int fd, double *val, struct timespec *t){
         return 0; // wrong number
     }
     if(val) *val = (double) data;
-    if(t){ if(!curtime(t)) return 0; }
+    if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
     return got;
 }
 // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@@ -312,40 +332,56 @@ static void *encoderthread2(void _U_ *u){
     int need2ask = 0; // need or not to ask encoder for new data
     while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
         struct timespec t;
-        if(!curtime(&t)) continue;
         if(need2ask){
+            //DBG("ASK for new data, tfs=%.6g", timefromstart());
             if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
             else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
+            //DBG("OK");
+            need2ask = 0;
+            usleep(100);
+        }
+        if(!curtime(&t)){
+            DBG("Where is time?");
+            continue;
         }
         double v;
         if(getencval(encfd[0], &v, &t)){
             pthread_mutex_lock(&datamutex);
-            mountdata.encXposition.val = X_ENC2RAD(v);
+            mountdata.encXposition.val = Xenc2rad(v);
             mountdata.encXposition.t = t;
             pthread_mutex_unlock(&datamutex);
+            //DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart());
             getXspeed();
             if(getencval(encfd[1], &v, &t)){
                 pthread_mutex_lock(&datamutex);
-                mountdata.encYposition.val = Y_ENC2RAD(v);
+                mountdata.encYposition.val = Yenc2rad(v);
                 mountdata.encYposition.t = t;
                 pthread_mutex_unlock(&datamutex);
+                //DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV);
                 getYspeed();
                 errctr = 0;
                 need2ask = 0;
-            } else {
+                //DBG("tgot=%.6g", timefromstart());
-                if(need2ask) ++errctr;
+                while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); }
+                t0 = timefromstart();
+            }else{
+                DBG("NO Y DATA!!!");
+                /*if(need2ask) ++errctr;
                 else need2ask = 1;
-                continue;
+                continue;*/
+                ++errctr;
+                need2ask = 1;
             }
-        } else {
+        }else{ // no data - ask for new
-            if(need2ask) ++errctr;
+            //DBG("Need new, tfs=%.6g", timefromstart());
+            /*if(need2ask) ++errctr;
             else need2ask = 1;
-            continue;
+            continue;*/
+            ++errctr;
+            need2ask = 1;
         }
-        while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); }
-        t0 = timefromstart();
     }
-    DBG("ERRCTR=%d", errctr);
+    DBG("\n\nEXIT: ERRCTR=%d", errctr);
     for(int i = 0; i < 2; ++i){
         if(encfd[i] > -1){
             close(encfd[i]);
@@ -480,8 +516,15 @@ static int ttyopen(const char *path, speed_t speed){
     int fd = -1;
     struct termios2 tty;
     DBG("Try to open %s @ %d", path, speed);
-    if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
+    if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
-    if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
+        DBG("Can't open device %s: %s", path, strerror(errno));
+        return -1;
+    }
+    if(ioctl(fd, TCGETS2, &tty)){
+        DBG("Can't read TTY settings");
+        close(fd);
+        return -1;
+    }
     tty.c_lflag     = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
     tty.c_iflag     = 0; // don't do any changes in input stream
     tty.c_oflag     = 0; // don't do any changes in output stream
@@ -490,7 +533,11 @@ static int ttyopen(const char *path, speed_t speed){
     tty.c_ospeed = speed;
     //tty.c_cc[VMIN]  = 0;  // non-canonical mode
     //tty.c_cc[VTIME] = 5;
-    if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
+    if(ioctl(fd, TCSETS2, &tty)){
+        DBG("Can't set TTY settings");
+        close(fd);
+        return -1;
+    }
     DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
     if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
     // try to set exclusive
@@ -523,7 +570,7 @@ int openEncoder(){
             if(encfd[i] < 0) return FALSE;
         }
         encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 1000; // 1ms
+        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
         if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
             for(int i = 0; i < 2; ++i){
                 close(encfd[i]);
@@ -602,6 +649,8 @@ mcc_errcodes_t getMD(mountdata_t  *d){
     pthread_mutex_lock(&datamutex);
     *d = mountdata;
     pthread_mutex_unlock(&datamutex);
+    //DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val);
+    //DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL));
     return MCC_E_OK;
 }
 
@@ -629,6 +678,7 @@ static int wr(const data_t *out, data_t *in, int needeol){
             int g = write(mntfd, "\r", 1); // add EOL
             (void) g;
         }
+        usleep(50000); // add little pause so that the idiot has time to swallow
     }
     if(!in) return TRUE;
     in->len = 0;
@@ -740,7 +790,13 @@ int cmdC(SSconfig *conf, int rw){
     }else{ // read
         data_t d;
         d.buf = (uint8_t *) conf;
-        d.len = 0; d.maxlen = sizeof(SSconfig);
+        d.len = 0; d.maxlen = 0;
+        ret = wr(rcmd, &d, 1);
+        DBG("write command: %s", ret ? "TRUE" : "FALSE");
+        if(!ret) goto rtn;
+        // make a huge pause for stupid SSII
+        usleep(100000);
+        d.len = 0;  d.maxlen = sizeof(SSconfig);
         ret = wr(rcmd, &d, 1);
         DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
         if(d.len != d.maxlen){ ret = FALSE; goto rtn; }

Auxiliary_utils/LibSidServo/sidservo.h

@@ -48,6 +48,7 @@ typedef enum{
     MCC_E_ENCODERDEV,   // encoder device error or can't open
     MCC_E_MOUNTDEV,     // mount device error or can't open
     MCC_E_FAILED,       // failed to run command - protocol error
+    MCC_E_AMOUNT        // Just amount of errors
 } mcc_errcodes_t;
 
 typedef struct{
@@ -74,9 +75,11 @@ typedef struct{
     PIDpar_t XPIDV;
     PIDpar_t YPIDC;
     PIDpar_t YPIDV;
-    double MaxPointingErr;          // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
+    double  MaxPointingErr;         // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
-    double MaxFinePointingErr;      // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
+    double  MaxFinePointingErr;     // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
-    double MaxGuidingErr;           // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
+    double  MaxGuidingErr;          // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
+    int     XEncZero;               // encoders' zero position
+    int     YEncZero;
 } conf_t;
 
 // coordinates/speeds in degrees or d/s: X, Y
@@ -188,6 +191,9 @@ typedef struct{
     double outplimit;   // Output Limit, percent (0..100)
     double currlimit;   // Current Limit (A)
     double intlimit;    // Integral Limit (???)
+    // these params are taken from mount by text commands (don't save negative values - better save these marks in xybits
+    double motor_stepsperrev;// encoder's steps per revolution: motor and axis
+    double axis_stepsperrev; // negative sign of these values means reverse direction
 } __attribute__((packed)) axis_config_t;
 
 // hardware configuration

Auxiliary_utils/LibSidServo/ssii.c

@@ -26,6 +26,9 @@
 #include "serial.h"
 #include "ssii.h"
 
+int X_ENC_ZERO, Y_ENC_ZERO;
+double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.;
+
 uint16_t SScalcChecksum(uint8_t *buf, int len){
     uint16_t checksum = 0;
     for(int i = 0; i < len; i++){
@@ -75,8 +78,9 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
     m->motXposition.t = m->motYposition.t = *t;
     // fill encoder data from here, as there's no separate enc thread
     if(!Conf.SepEncoder){
-        m->encXposition.val = X_ENC2RAD(s->Xenc);
+        m->encXposition.val = Xenc2rad(s->Xenc);
-        m->encYposition.val = Y_ENC2RAD(s->Yenc);
+        DBG("encx: %g", m->encXposition.val);
+        m->encYposition.val = Yenc2rad(s->Yenc);
         m->encXposition.t = m->encYposition.t = *t;
         getXspeed(); getYspeed();
     }
@@ -185,9 +189,10 @@ mcc_errcodes_t updateMotorPos(){
             usleep(10000);
             continue;
         }
+        //DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
         if(MCC_E_OK == getMD(&md)){
             if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
-                DBG("Just started, t-t0 = %g!", t - t0);
+                DBG("Just started? t-t0 = %g!", t - t0);
                 usleep(10000);
                 continue;
             }
@@ -195,14 +200,15 @@ mcc_errcodes_t updateMotorPos(){
             DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
             mcc_errcodes_t OK = MCC_E_OK;
             if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
-                DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
+                DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
+                DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
                 if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
                     DBG("Xpos sync failed!");
                     OK = MCC_E_FAILED;
                 }else DBG("Xpos sync OK, Dt=%g", t - t0);
             }
             if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
-                DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
+                DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
                 if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
                     DBG("Ypos sync failed!");
                     OK = MCC_E_FAILED;

Auxiliary_utils/LibSidServo/ssii.h

@@ -175,62 +175,70 @@
 // amount of consequent same coordinates to detect stop
 #define MOTOR_STOPPED_CNT       (19)
 
+// replace macros with global variables inited when config read
+extern int X_ENC_ZERO, Y_ENC_ZERO;
+extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
+
 // TODO: take it from settings?
 // steps per revolution (SSI - x4 - for SSI)
-#define X_MOT_STEPSPERREV_SSI   (13312000.)
+// -> hwconf.Xconf.mot/enc_stepsperrev
+//#define X_MOT_STEPSPERREV_SSI   (13312000.)
 // 13312000 / 4 = 3328000
-#define X_MOT_STEPSPERREV   (3328000.)
+//#define X_MOT_STEPSPERREV   (3328000.)
-#define Y_MOT_STEPSPERREV_SSI (17578668.)
+//#define Y_MOT_STEPSPERREV_SSI (17578668.)
 // 17578668 / 4 = 4394667
-#define Y_MOT_STEPSPERREV   (4394667.)
+//#define Y_MOT_STEPSPERREV   (4394667.)
 
 // encoder per revolution
-#define X_ENC_STEPSPERREV   (67108864.)
+//#define X_ENC_STEPSPERREV   (67108864.)
-#define Y_ENC_STEPSPERREV   (67108864.)
+//#define Y_ENC_STEPSPERREV   (67108864.)
 // encoder zero position
-#define X_ENC_ZERO          (61245239)
+// -> conf.XEncZero/YEncZero
-#define Y_ENC_ZERO          (36999830)
+//#define X_ENC_ZERO          (61245239)
-// encoder reversed (no: +1)
+//#define Y_ENC_ZERO          (36999830)
-#define X_ENC_SIGN          (-1.)
+// encoder reversed (no: +1) -> sign of ...stepsperrev
-#define Y_ENC_SIGN          (-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 Xenc2rad(n)    ang2half(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 Yenc2rad(n)    ang2half(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 Xrad2enc(r)    ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
-#define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
+#define Yrad2enc(r)    ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
 
 // convert angle in radians to +-pi
-static inline double ang2half(double ang){
+static inline __attribute__((always_inline)) double ang2half(double ang){
     if(ang < -M_PI) ang += 2.*M_PI;
     else if(ang > M_PI) ang -= 2.*M_PI;
     return ang;
 }
 // convert to only positive: 0..2pi
-static inline double ang2full(double ang){
+static inline __attribute__((always_inline)) double ang2full(double ang){
     if(ang < 0.) ang += 2.*M_PI;
     else if(ang > 2.*M_PI) ang -= 2.*M_PI;
     return ang;
 }
 
 // motor position to radians and back
-#define X_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
+#define X_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
-#define Y_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
+#define Y_MOT2RAD(n)    ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
-#define X_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
+#define X_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
-#define Y_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
+#define Y_RAD2MOT(r)    ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
 // motor speed in rad/s and back
-#define X_MOTSPD2RS(n)  (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
+#define X_MOTSPD2RS(n)  (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
-#define Y_MOTSPD2RS(n)  (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
+#define Y_MOTSPD2RS(n)  (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
-#define X_RS2MOTSPD(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
+#define X_RS2MOTSPD(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
-#define Y_RS2MOTSPD(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
+#define Y_RS2MOTSPD(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
 // motor acceleration -//-
-#define X_MOTACC2RS(n)  (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
+#define X_MOTACC2RS(n)  (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
-#define Y_MOTACC2RS(n)  (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
+#define Y_MOTACC2RS(n)  (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
-#define X_RS2MOTACC(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
+#define X_RS2MOTACC(r)  ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
-#define Y_RS2MOTACC(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
+#define Y_RS2MOTACC(r)  ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
 
 // adder time to seconds vice versa
-#define ADDER2S(a)  ((a) / SITECH_LOOP_FREQUENCY)
+#define ADDER2S(a)  ((a) / (SITECH_LOOP_FREQUENCY))
-#define S2ADDER(s)  ((s) * SITECH_LOOP_FREQUENCY)
+#define S2ADDER(s)  ((s) * (SITECH_LOOP_FREQUENCY))
 
 // encoder's tolerance (ticks)
 #define YencTOL    (25.)