some problems fixed; model isn't tested yet

LibSidServo/examples/SSIIconf.c

@@ -107,6 +107,7 @@ static void dumpHWconf(){
     dumpaxe('Y', &HW.Yconf);
     green("Y bits:\n");
     dumpybits(&HW.ybits);
+    green("Other:\n");
     printf("address=%d\n", HW.address);
     DUMP(eqrate);
     DUMP(eqadj);

LibSidServo/serial.c

@@ -47,7 +47,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;
@@ -326,8 +326,7 @@ static void *mountthread(void _U_ *u){
     int errctr = 0;
     uint8_t buf[2*sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
-    if(Conf.RunModel)
+    if(Conf.RunModel) while(1){
-    while(1){
         pthread_mutex_lock(&datamutex);
         // now change data
         getModData(&mountdata);
@@ -344,7 +343,7 @@ static void *mountthread(void _U_ *u){
     while(mntfd > -1 && errctr < MAX_ERR_CTR){
         // read data to status
         double tgot = nanotime(), t0 = tgot;
-        DBG("tgot=%g", tgot);
+        //DBG("tgot=%g", tgot);
         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);
@@ -363,7 +362,9 @@ static void *mountthread(void _U_ *u){
         SSconvstat(status, &mountdata, tgot);
         pthread_mutex_unlock(&datamutex);
         // allow writing & getters
-        while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
+        do{
+            usleep(5000);
+        }while(nanotime() - t0 < Conf.MountReqInterval);
         t0 = nanotime();
     }
     data_free(&cmd_getstat);
@@ -511,17 +512,19 @@ void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
 // 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;
+    //double t0 = nanotime();
     if(out){
         if(out->len != (size_t)write(mntfd, out->buf, out->len)){
             DBG("written bytes not equal to need");
             return FALSE;
         }
-        DBG("Send to mount %zd bytes: %s", out->len, out->buf);
+        //DBG("Send to mount %zd bytes: %s", out->len, out->buf);
         if(needeol){
             int g = write(mntfd, "\r", 1); // add EOL
             (void) g;
         }
     }
+    //DBG("sent by %g", nanotime() - t0);
     uint8_t buf[256];
     data_t dumb = {.buf = buf, .maxlen = 256};
     if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
@@ -531,9 +534,8 @@ static int wr(const data_t *out, data_t *in, int needeol){
         if(b < 0) break; // nothing to read -> go out
         in->buf[in->len++] = (uint8_t) b;
     }
-    DBG("got %zd bytes", in->len);
+    //DBG("got %zd bytes by %g", in->len, nanotime() - t0);
-    //DBG("Clear trashing input");
+    while(getmntbyte() > -1);
-    //while(getmntbyte() > -1);
     return TRUE;
 }
 

LibSidServo/ssii.c

@@ -40,11 +40,14 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
 static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
     if(*prev == INT32_MAX){
         *stat = MNT_STOPPED;
+        DBG("START");
     }else if(*stat != MNT_STOPPED){
-        if(*prev == cur){
+        if(*prev == cur  && ++(*nstopped) > MOTOR_STOPPED_CNT){
-            if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
+            *stat = MNT_STOPPED;
+            DBG("AXE stopped");
         }
     }else if(*prev != cur){
+        DBG("AXE moving");
         //*stat = MNT_SLEWING;
         *nstopped = 0;
     }
@@ -101,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);
 }
 
@@ -114,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);
 }
 
@@ -190,7 +193,7 @@ mcc_errcodes_t updateMotorPos(){
                 DBG("FIX motors position to encoders");
                 int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
                 if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
-                    DBG("All OK");
+                    DBG("All OK, Dt=%g", nanotime() - t0);
                     return MCC_E_OK;
                 }
             }else{

LibSidServo/ssii.h

@@ -173,7 +173,7 @@
 #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)