fixed bug in PID

LibSidServo/PID.c

@@ -29,6 +29,7 @@ PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
     if(!gain || Iarrsz < 3) return NULL;
     PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
     pid->gain = *gain;
+    DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
     pid->pidIarrSize = Iarrsz;
     pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
     return pid;
@@ -141,9 +142,9 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
     double dt = tagpos->t - pid->prevT;
     if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
     pid->prevT = tagpos->t;
-    //DBG("CALC PID (er=%g, dt=%g)", error, dt);
+    DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state);
     double tagspeed = pid_calculate(pid, error, dt);
-    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
+    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based
     return tagspeed; // coordinate-based
 }
 
@@ -176,13 +177,15 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
     axis.position = m.encXposition;
     axis.speed = m.encXspeed;
     tagspeed.X = getspeed(&target->X, &pidX, &axis);
-    if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
+    if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X;
+    if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
     axis_status_t xstate = axis.state;
     axis.state = m.Ystate;
     axis.position = m.encYposition;
     axis.speed = m.encYspeed;
     tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
-    if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
+    if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y;
+    if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
     axis_status_t ystate = axis.state;
     if(m.Xstate != xstate || m.Ystate != ystate){
         DBG("State changed");

LibSidServo/examples/dump.c

@@ -67,17 +67,13 @@ static double filter(double val, int idx){
 void logmnt(FILE *fcoords, mountdata_t *m){
     if(!fcoords) return;
     //DBG("LOG %s", m ? "data" : "header");
-    static double t0 = -1.;
     if(!m){ // write header
         fprintf(fcoords, "#     time    Xmot(deg)   Ymot(deg) Xenc(deg)  Yenc(deg)   VX(d/s)    VY(d/s)     millis\n");
         return;
     }
-    double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
-    if(t0 < 0.) t0 = tnow;
-    double t = tnow - t0;
     // write data
     fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
-            t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
+            m->motXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
             RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
             RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
             m->millis);

LibSidServo/examples/scmd_traectory.c

@@ -107,7 +107,7 @@ static void runtraectory(traectory_fn tfn){
             return;
         }
         if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
-        DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
+        DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t);
         tlastX = telXY.X.t; tlastY = telXY.Y.t;
         double t = Mount.currentT();
         if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
@@ -119,9 +119,9 @@ static void runtraectory(traectory_fn tfn){
         else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
         if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
         else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
-        DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
+        //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
         DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
-        DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
+        //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
         if(errlog)
             fprintf(errlog, "%10.4g  %10.4g  %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
         if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");

LibSidServo/examples/traectories.c

@@ -76,6 +76,7 @@ int telpos(coordval_pair_t *curpos){
     }
     if(ntries == 10) return FALSE;
     coordval_pair_t pt;
+    //DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
     pt.X.val = mdata.encXposition.val;
     pt.Y.val = mdata.encYposition.val;
     pt.X.t = mdata.encXposition.t;

LibSidServo/main.c

@@ -75,10 +75,8 @@ static void quit(){
     DBG("Exit");
 }
 
-void getModData(mountdata_t *mountdata){
+void getModData(coordval_pair_t *c){
-    if(!mountdata || !Xmodel || !Ymodel) return;
+    if(!c || !Xmodel || !Ymodel) return;
-    static double oldmt = -100.; // old `millis measurement` time
-    static uint32_t oldmillis = 0;
     double tnow = nanotime();
     moveparam_t Xp, Yp;
     movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
@@ -86,14 +84,9 @@ void getModData(mountdata_t *mountdata){
     if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
     movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
     if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
-    mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
+    c->X.t = c->Y.t = tnow;
-    mountdata->motXposition.val = mountdata->encXposition.val  = Xp.coord;
+    c->X.val = Xp.coord;
-    mountdata->motYposition.val  = mountdata->encYposition.val  = Yp.coord;
+    c->Y.val =  Yp.coord;
-    getXspeed(); getYspeed();
-    if(tnow - oldmt > Conf.MountReqInterval){
-        oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3);
-        oldmt = tnow;
-    }else mountdata->millis = oldmillis;
 }
 
 /**

LibSidServo/main.h

@@ -28,7 +28,7 @@
 
 extern conf_t Conf;
 double nanotime();
-void getModData(mountdata_t *mountdata);
+void getModData(coordval_pair_t *c);
 typedef struct{
     double *x, *t, *t2, *xt; // arrays of coord/time and multiply
     double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply

LibSidServo/movingmodel.c

@@ -60,7 +60,7 @@ movemodel_t *model_init(limits_t *l){
 
 int model_move2(movemodel_t *model, moveparam_t *target, double t){
     if(!target || !model) return FALSE;
-    DBG("MOVE to %g at speed %g", target->coord, target->speed);
+    //DBG("MOVE to %g at speed %g", target->coord, target->speed);
     // only positive velocity
     if(target->speed < 0.) target->speed = -target->speed;
     // don't mind about acceleration - user cannot set it now

LibSidServo/ramp.c

@@ -120,7 +120,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
         }
     }else{
         // if we are here, we have the worst case: change speed direction
-        DBG("Hardest case: change speed direction");
+       // DBG("Hardest case: change speed direction");
         // now we should calculate coordinate at which model stops and biuld new trapezium from that point
         double x0 = m->curparams.coord, v0 = m->curparams.speed;
         double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
@@ -132,7 +132,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
         m->Times[STAGE_ACCEL] = t;
         m->Params[STAGE_ACCEL].coord = x0;
         m->Params[STAGE_ACCEL].speed = v0;
-        DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
+       // DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
         return;
     }
     m->state = ST_MOVE;
@@ -210,11 +210,10 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
     if(m->movingstage == STAGE_STOPPED){
         m->curparams.coord = m->Params[STAGE_STOPPED].coord;
         pthread_mutex_unlock(&m->mutex);
-        DBG("REACHED STOPping stage @ t=%g", t);
+       /* DBG("REACHED STOPping stage @ t=%g", t);
         for(int s = STAGE_STOPPED; s >= 0; --s){
             DBG("T[%d]=%g, ", s, m->Times[s]);
-        }
+        }*/
-        fflush(stdout);
         emstop(m, t);
         goto ret;
     }

LibSidServo/serial.c

@@ -64,11 +64,13 @@ void getXspeed(){
         ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
         if(!ls) return;
     }
+    pthread_mutex_lock(&datamutex);
     double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
     if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
         mountdata.encXspeed.val = speed;
         mountdata.encXspeed.t = mountdata.encXposition.t;
     }
+    pthread_mutex_unlock(&datamutex);
     //DBG("Xspeed=%g", mountdata.encXspeed.val);
 #if 0
     mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
@@ -83,11 +85,13 @@ void getYspeed(){
         ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
         if(!ls) return;
     }
+    pthread_mutex_lock(&datamutex);
     double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
     if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
         mountdata.encYspeed.val = speed;
         mountdata.encYspeed.t = mountdata.encYposition.t;
     }
+    pthread_mutex_unlock(&datamutex);
 #if 0
     mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
     mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
@@ -325,15 +329,19 @@ static void *encoderthread2(void _U_ *u){
         }
         double v, t;
         if(getencval(encfd[0], &v, &t)){
+            pthread_mutex_lock(&datamutex);
             mountdata.encXposition.val = X_ENC2RAD(v);
             //DBG("encX(%g) = %g", t, mountdata.encXposition.val);
             mountdata.encXposition.t = t;
+            pthread_mutex_unlock(&datamutex);
             //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
             getXspeed();
             if(getencval(encfd[1], &v, &t)){
+                pthread_mutex_lock(&datamutex);
                 mountdata.encYposition.val = Y_ENC2RAD(v);
                 //DBG("encY(%g) = %g", t, mountdata.encYposition.val);
                 mountdata.encYposition.t = t;
+                pthread_mutex_unlock(&datamutex);
                 //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
                 getYspeed();
                 errctr = 0;
@@ -384,12 +392,25 @@ static void *mountthread(void _U_ *u){
     uint8_t buf[2*sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
     bzero(&mountdata, sizeof(mountdata));
-    if(Conf.RunModel) while(1){
-        pthread_mutex_lock(&datamutex);
-        // now change data
-        getModData(&mountdata);
-        pthread_mutex_unlock(&datamutex);
     double t0 = nanotime();
+    static double oldmt = -100.; // old `millis measurement` time
+    static uint32_t oldmillis = 0;
+    if(Conf.RunModel) while(1){
+        coordval_pair_t c;
+        // now change data
+        getModData(&c);
+        pthread_mutex_lock(&datamutex);
+        double tnow = c.X.t;
+        mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow;
+        mountdata.motXposition.val = mountdata.encXposition.val  = c.X.val;
+        mountdata.motYposition.val  = mountdata.encYposition.val  = c.Y.val;
+        //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
+        if(tnow - oldmt > Conf.MountReqInterval){
+            oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3);
+            oldmt = tnow;
+        }else mountdata.millis = oldmillis;
+        pthread_mutex_unlock(&datamutex);
+        getXspeed(); getYspeed();
         while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
         t0 = nanotime();
     }

LibSidServo/sidservo.h

@@ -56,9 +56,10 @@ extern "C"
 // normal PID refresh interval
 #define MCC_PID_REFRESH_DT      (0.1)
 // boundary conditions for axis state: "slewing/pointing/guiding"
-// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees
 //#define MCC_MAX_POINTING_ERR    (0.20943951)
-#define MCC_MAX_POINTING_ERR    (0.08726646)
+//#define MCC_MAX_POINTING_ERR    (0.08726646)
+#define MCC_MAX_POINTING_ERR    (0.13962634)
 // if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
 #define MCC_MAX_GUIDING_ERR     (0.026179939)
 // if error less than this value we suppose that target is captured and guiding is good: 0.1''

LibSidServo/ssii.c

@@ -18,6 +18,7 @@
 
 #include <ctype.h>
 #include <inttypes.h>
+#include <stdio.h>
 #include <string.h>
 #include <unistd.h>