less squares 4 speed + fixed some bugs (but found more)

LibSidServo/ssii.c

@@ -78,6 +78,7 @@ 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->keypad = s->keypad;
     m->extradata.ExtraBits = s->ExtraBits;
@@ -164,9 +165,7 @@ int SSstop(int emerg){
     const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
     const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
     for(; i < 10; ++i){
-        DBG("t1: %g", nanotime());
         if(!SStextcmd(cmdx, NULL)) continue;
-        DBG("t2: %g", nanotime());
         if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
@@ -176,6 +175,7 @@ int SSstop(int emerg){
 // update motors' positions due to encoders'
 mcc_errcodes_t updateMotorPos(){
     mountdata_t md = {0};
+    if(Conf.RunModel) return MCC_E_OK;
     double t0 = nanotime(), t = 0.;
     DBG("start @ %g", t0);
     do{