Fixed some troubles

.qtcreator/libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
+<!-- Written by QtCreator 18.0.0, 2026-03-13T12:23:05. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

examples/scmd_traectory.c

@@ -146,10 +146,12 @@ int main(int argc, char **argv){
             ERRX("Can't open error log %s", G.errlog);
         else
             fprintf(errlog, "#    time      Xerr''      Yerr''   // target - real\n");
+        setbuf(errlog, NULL);
     }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
+        setbuf(fcoords, NULL);
     }else fcoords = stdout;
     Config = readServoConf(G.conffile);
     if(!Config || G.dumpconf){
@@ -178,7 +180,7 @@ int main(int argc, char **argv){
     signal(SIGINT, signals);  // ctrl+C - quit
     signal(SIGQUIT, signals); // ctrl+\ - quit
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-    chk0(G.Ncycles);
+//    chk0(G.Ncycles);
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
     ;

libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
+<!-- Written by QtCreator 18.0.0, 2026-03-13T12:23:05. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

main.c

@@ -202,6 +202,7 @@ static mcc_errcodes_t init(conf_t *c){
         if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
         return MCC_E_OK;
     }
+    DBG("Try to open mount device");
     if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
@@ -209,33 +210,53 @@ static mcc_errcodes_t init(conf_t *c){
         DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
         ret = MCC_E_MOUNTDEV;
     }
-    if(Conf.SepEncoder){
-        if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
-            DBG("Define encoder device path");
-            ret = MCC_E_BADFORMAT;
-        }else if(!openEncoder()){
-            DBG("Can't open encoder device");
-            ret = MCC_E_ENCODERDEV;
-        }
-    }
     // TODO: read hardware configuration on init
     if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
         DBG("Wrong speed interval");
         ret = MCC_E_BADFORMAT;
     }
-    if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
     if(ret != MCC_E_OK) return ret;
+    DBG("Exit ACM, exit manual mode");
+    SSrawcmd(CMD_EXITACM, NULL);
+    SStextcmd(CMD_AUTOX, NULL);
+    SStextcmd(CMD_AUTOY, NULL);
     // read HW config to update constants
     hardware_configuration_t HW;
-    if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
+    DBG("Read hardware configuration");
+    ret = MCC_E_FAILED;
+    for(int i = 0; i < MAX_ERR_CTR; ++i){
+        DBG("TRY %d..", i);
+        ret = get_hwconf(&HW);
+        if(ret == MCC_E_OK) break;
+    }
+    if(MCC_E_OK != ret) return ret;
     // make a pause for actual encoder's values
+    DBG("Check encoders");
+    if(Conf.SepEncoder){
+        if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
+            DBG("Define encoder device path");
+            ret = MCC_E_BADFORMAT;
+        }else{
+            ret = MCC_E_ENCODERDEV;
+            for(int i = 0; i < MAX_ERR_CTR; ++i){
+                if(openEncoder()){
+                    ret = MCC_E_OK;
+                    break;
+                }
+            }
+        }
+    }
+    if(MCC_E_OK != ret) return ret;
     double t0 = timefromstart();
-    while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
+    DBG("Wait for first encoders' measurement");
+    while(timefromstart() - t0 < Conf.EncoderReqInterval * 15.) usleep(1000);
+    DBG("Update motor position");
     mcc_errcodes_t e = updateMotorPos();
     // and refresh data after updating
     DBG("Wait for next mount reading");
     t0 = timefromstart();
-    while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
+    while(timefromstart() - t0 < Conf.MountReqInterval * 5.) usleep(1000);
+    DBG("ALL READY!");
     return e;
 }
 

serial.c

@@ -49,7 +49,11 @@ static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
 // encoders thread and mount thread
 static pthread_t encthread, mntthread;
 // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
-static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
+// this values will be modified later
+static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder reading timeout
+    mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading
+    mntRtmout =  {.tv_sec = 0, .tv_usec = 50000}; // next readings
+
 // encoders raw data
 typedef struct __attribute__((packed)){
     uint8_t magick;
@@ -231,43 +235,60 @@ static int getencbyte(){
     }while(1);
     return (int)byte;
 }
-// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
+
-static int getmntbyte(){
+/**
-    if(mntfd < 0) return -1;
+ * @brief readmntdata - read data
-    uint8_t byte;
+ * @param buffer - input buffer
+ * @param maxlen - maximal buffer length
+ * @return amount of bytes read or -1 in case of error
+ */
+static int readmntdata(uint8_t *buffer, int maxlen){
+    if(mntfd < 0){
+        DBG("mntfd non opened");
+        return -1;
+    }
+    if(!buffer || maxlen < 1) return 0;
+    DBG("ask for %d bytes", maxlen);
+    int got = 0;
     fd_set rfds;
-   /* ssize_t l = read(mntfd, &byte, 1);
+    struct timeval tv = mnt1Rtmout;
-    //DBG("MNT read=%zd byte=0x%X", l, byte);
-    if(l == 0) return -1;
-    if(l != 1) return -2; // disconnected ??
-    return (int) byte;*/
     do{
         FD_ZERO(&rfds);
         FD_SET(mntfd, &rfds);
-        struct timeval tv = mntRtmout;
+        DBG("select");
         int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
+        DBG("returned %d", retval);
         if(retval < 0){
             if(errno == EINTR) continue;
             DBG("Error in select()");
             return -1;
         }
-        //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
         if(FD_ISSET(mntfd, &rfds)){
-            ssize_t l = read(mntfd, &byte, 1);
+            ssize_t l = read(mntfd, buffer, maxlen);
-            //DBG("MNT read=%zd byte=0x%X", l, byte);
+            if(l == 0){
-            if(l != 1){
+                DBG("read ZERO");
+                continue;
+            }
+            if(l < 0){
                 DBG("Mount disconnected?");
                 return -2; // disconnected ??
             }
+            buffer += l;
+            maxlen -= l;
+            got += l;
+        }else{
+            DBG("no new data after %d bytes (%s)", got, buffer - got);
             break;
-        } else return -1;
-    }while(1);
-    return (int)byte;
         }
+        tv = mntRtmout;
+    }while(maxlen);
+    return got;
+}
+
 // clear data from input buffer
 static void clrmntbuf(){
     if(mntfd < 0) return;
-    uint8_t byte;
+    uint8_t bytes[256];
     fd_set rfds;
     do{
         FD_ZERO(&rfds);
@@ -280,8 +301,9 @@ static void clrmntbuf(){
             break;
         }
         if(FD_ISSET(mntfd, &rfds)){
-            ssize_t l = read(mntfd, &byte, 1);
+            ssize_t l = read(mntfd, &bytes, 256);
-            if(l != 1) break;
+            if(l < 1) break;
+            DBG("clr got %zd bytes: %s", l, bytes);
         }else break;
     }while(1);
 }
@@ -426,15 +448,19 @@ static void *encoderthread2(void _U_ *u){
             if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
                 if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
                     pthread_mutex_lock(&datamutex);
+                    double pos = (double)msrlast[i];
+                    //DBG("pos[%d]=%g", i, pos);
+                    ;;
+                    //DBG("Got med: %g", pos);
                     if(i == 0){
-                        mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
+                        mountdata.encXposition.val = Xenc2rad(pos);
                         curtime(&mountdata.encXposition.t);
                         /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
                             msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
                             X_ENC_ZERO, X_ENC_STEPSPERREV);*/
                         getXspeed();
                     }else{
-                        mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
+                        mountdata.encYposition.val = Yenc2rad(pos);
                         curtime(&mountdata.encYposition.t);
                         getYspeed();
                     }
@@ -496,7 +522,7 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t
 // main mount thread
 static void *mountthread(void _U_ *u){
     int errctr = 0;
-    uint8_t buf[2*sizeof(SSstat)];
+    uint8_t buf[sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
     bzero(&mountdata, sizeof(mountdata));
     double t0 = timefromstart(), tstart = t0, tcur = t0;
@@ -616,15 +642,18 @@ static int ttyopen(const char *path, speed_t speed){
 
 // return FALSE if failed
 int openEncoder(){
+    // TODO: open real devices in "model" mode too!
     if(Conf.RunModel) return TRUE;
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
+    /*
+    encRtmout.tv_sec = 0;
+    encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
+*/
     if(Conf.SepEncoder == 1){ // only one device
         DBG("One device");
         if(encfd[0] > -1) close(encfd[0]);
         encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
         if(encfd[0] < 0) return FALSE;
-        encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
         if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
             close(encfd[0]);
             encfd[0] = -1;
@@ -638,8 +667,6 @@ int openEncoder(){
             encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
             if(encfd[i] < 0) return FALSE;
         }
-        encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
         if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
             for(int i = 0; i < 2; ++i){
                 close(encfd[i]);
@@ -654,6 +681,7 @@ int openEncoder(){
 
 // return FALSE if failed
 int openMount(){
+    // TODO: open real devices in "model" mode too!
     if(Conf.RunModel) goto create_thread;
     if(mntfd > -1) close(mntfd);
     DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
@@ -661,16 +689,13 @@ int openMount(){
     if(mntfd < 0) return FALSE;
     DBG("mntfd=%d", mntfd);
     // clear buffer
-    while(getmntbyte() > -1);
+    clrmntbuf();
-    /*int g = write(mntfd, "XXS\r", 4);
+    /*
-    DBG("Written %d", g);
+    mnt1Rtmout.tv_sec = 0;
-    uint8_t buf[100];
+    mnt1Rtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
-    do{
-        ssize_t l = read(mntfd, buf, 100);
-        DBG("got %zd", l);
-    }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
+    mntRtmout.tv_usec = mnt1Rtmout.tv_usec / 50;
+*/
 create_thread:
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
         DBG("Can't create mount thread");
@@ -686,8 +711,7 @@ create_thread:
 
 // close all opened serial devices and quit threads
 void closeSerial(){
-    // TODO: close devices in "model" mode too!
+    DBG("CLOSE all devices");
-    if(Conf.RunModel) return;
     if(mntfd > -1){
         DBG("Cancel mount thread");
         pthread_cancel(mntthread);
@@ -737,26 +761,29 @@ static int wr(const data_t *out, data_t *in, int needeol){
         DBG("Wrong arguments or no mount fd");
         return FALSE;
     }
+    DBG("clrbuf");
     clrmntbuf();
     if(out){
+        DBG("write %zd bytes (%s)", out->len, out->buf);
         if(out->len != (size_t)write(mntfd, out->buf, out->len)){
             DBG("written bytes not equal to need");
             return FALSE;
         }
+        //DBG("eol, mntfd=%d", mntfd);
         if(needeol){
             int g = write(mntfd, "\r", 1); // add EOL
             (void) g;
         }
-        usleep(50000); // add little pause so that the idiot has time to swallow
+        //usleep(50000); // add little pause so that the idiot has time to swallow
     }
-    if(!in) return TRUE;
+    if(!in || in->maxlen < 1) return TRUE;
+    int got = readmntdata(in->buf, in->maxlen);
+    if(got < 0){
+        DBG("Error reading mount data!");
         in->len = 0;
-    for(size_t i = 0; i < in->maxlen; ++i){
+        return FALSE;
-        int b = getmntbyte();
-        if(b < 0) break; // nothing to read -> go out
-        in->buf[in->len++] = (uint8_t) b;
     }
-    while(getmntbyte() > -1);
+    in->len = got;
     return TRUE;
 }
 
@@ -767,15 +794,17 @@ static int wr(const data_t *out, data_t *in, int needeol){
  * @return FALSE if failed
  */
 int MountWriteRead(const data_t *out, data_t *in){
-    if(Conf.RunModel) return -1;
+    if(Conf.RunModel) return FALSE;
+    double t0 = timefromstart();
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 1);
     pthread_mutex_unlock(&mntmutex);
+    DBG("Got %gus", (timefromstart()-t0)*1e6);
     return ret;
 }
 // send binary data - without EOL
 int MountWriteReadRaw(const data_t *out, data_t *in){
-    if(Conf.RunModel) return -1;
+    if(Conf.RunModel) return FALSE;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 0);
     pthread_mutex_unlock(&mntmutex);

ssii.c

@@ -82,6 +82,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
     m->motXposition.t = m->motYposition.t = *t;
     // fill encoder data from here, as there's no separate enc thread
     if(!Conf.SepEncoder){
+        DBG("ENCODER from SSII");
         m->encXposition.val = Xenc2rad(s->Xenc);
         DBG("encx: %g", m->encXposition.val);
         m->encYposition.val = Yenc2rad(s->Yenc);