PID seems like almost working, but I have a great problem with model!

LibSidServo/serial.c

@@ -243,7 +243,10 @@ static int getmntbyte(){
         if(FD_ISSET(mntfd, &rfds)){
             ssize_t l = read(mntfd, &byte, 1);
             //DBG("MNT read=%zd byte=0x%X", l, byte);
-            if(l != 1) return -2; // disconnected ??
+            if(l != 1){
+                DBG("Mount disconnected?");
+                return -2; // disconnected ??
+            }
             break;
         } else return -1;
     }while(1);
@@ -586,6 +589,7 @@ mcc_errcodes_t getMD(mountdata_t  *d){
 }
 
 void setStat(axis_status_t Xstate, axis_status_t Ystate){
+    DBG("set x/y state to %d/%d", Xstate, Ystate);
     pthread_mutex_lock(&datamutex);
     mountdata.Xstate = Xstate;
     mountdata.Ystate = Ystate;
@@ -594,7 +598,10 @@ void setStat(axis_status_t Xstate, axis_status_t Ystate){
 
 // 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;
+    if((!out && !in) || mntfd < 0){
+        DBG("Wrong arguments or no mount fd");
+        return FALSE;
+    }
     clrmntbuf();
     //double t0 = nanotime();
     if(out){