diff --git a/LibSidServo/.qtcreator/libsidservo.creator.user b/LibSidServo/.qtcreator/libsidservo.creator.user index 58c6b84..186d416 100644 --- a/LibSidServo/.qtcreator/libsidservo.creator.user +++ b/LibSidServo/.qtcreator/libsidservo.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} ProjectExplorer.Project.ActiveTarget @@ -40,9 +40,9 @@ 1 0 false - true + false false - 0 + 1 true true 0 @@ -51,10 +51,10 @@ false 1 true - false + true true *.md, *.MD, Makefile - false + true true true @@ -79,7 +79,7 @@ true true Builtin.DefaultTidyAndClazy - 8 + 4 true @@ -96,12 +96,12 @@ true Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /tmp/robo5/mountcontrol.git/LibSidServo + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions @@ -133,7 +133,7 @@ false - Default + По умолчанию GenericProjectManager.GenericBuildConfiguration 0 0 @@ -168,7 +168,6 @@ true true - %{RunConfig:Executable:Path} 1 @@ -204,7 +203,6 @@ true true - %{RunConfig:Executable:Path} 1 diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 58c6b84..186d416 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} ProjectExplorer.Project.ActiveTarget @@ -40,9 +40,9 @@ 1 0 false - true + false false - 0 + 1 true true 0 @@ -51,10 +51,10 @@ false 1 true - false + true true *.md, *.MD, Makefile - false + true true true @@ -79,7 +79,7 @@ true true Builtin.DefaultTidyAndClazy - 8 + 4 true @@ -96,12 +96,12 @@ true Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /tmp/robo5/mountcontrol.git/LibSidServo + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions @@ -133,7 +133,7 @@ false - Default + По умолчанию GenericProjectManager.GenericBuildConfiguration 0 0 @@ -168,7 +168,6 @@ true true - %{RunConfig:Executable:Path} 1 @@ -204,7 +203,6 @@ true true - %{RunConfig:Executable:Path} 1 diff --git a/LibSidServo/main.c b/LibSidServo/main.c index a6f04b0..f2d731d 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -161,6 +161,8 @@ double LS_calc_slope(less_square_t *l, double x, double t){ if(!l) return 0.; size_t idx = l->idx; double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx]; + /*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt, + l->xsum, l->tsum, l->t2sum, l->xtsum);*/ double t2 = t * t, xt = x * t; l->x[idx] = x; l->t2[idx] = t2; l->t[idx] = t; l->xt[idx] = xt; @@ -172,9 +174,9 @@ double LS_calc_slope(less_square_t *l, double x, double t){ l->xtsum += xt - oldxt; double n = (double)l->arraysz; double denominator = n * l->t2sum - l->tsum * l->tsum; - //DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator); if(fabs(denominator) < 1e-7) return 0.; double numerator = n * l->xtsum - l->xsum * l->tsum; + //DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator); // point: (sum_x - slope * sum_t) / n; return (numerator / denominator); } diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 32c5edd..fc57cd6 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -71,7 +71,6 @@ void getXspeed(){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; } - //DBG("Xspeed=%g", mountdata.encXspeed.val); } void getYspeed(){ static less_square_t *ls = NULL; @@ -328,9 +327,12 @@ typedef struct{ // write to buffer next data portion; return FALSE in case of error static int readstrings(buf_t *buf, int fd){ - FNAME(); if(!buf){DBG("Empty buffer"); return FALSE;} int L = XYBUFSZ - buf->len; + if(L < 0){ + DBG("buf not initialized!"); + buf->len = 0; + } if(L == 0){ DBG("buffer overfull: %d!", buf->len); char *lastn = strrchr(buf->buf, '\n'); @@ -344,6 +346,7 @@ static int readstrings(buf_t *buf, int fd){ }else buf->len = 0; L = XYBUFSZ - buf->len; } + //DBG("read %d bytes from %d", L, fd); int got = read(fd, &buf->buf[buf->len], L); if(got < 0){ DBG("read()"); @@ -351,13 +354,13 @@ static int readstrings(buf_t *buf, int fd){ }else if(got == 0){ DBG("NO data"); return TRUE; } buf->len += got; buf->buf[buf->len] = 0; - DBG("buf[%d]: %s", buf->len, buf->buf); + //DBG("buf[%d]: %s", buf->len, buf->buf); return TRUE; } // return TRUE if got, FALSE if no data found static int getdata(buf_t *buf, long *out){ - if(!buf || buf->len < 1) return FALSE; + if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE; // read record between last '\n' and previous (or start of string) char *last = &buf->buf[buf->len - 1]; //DBG("buf: _%s_", buf->buf); @@ -378,7 +381,7 @@ static int getdata(buf_t *buf, long *out){ // try to write '\n' asking new data portion; return FALSE if failed static int asknext(int fd){ - FNAME(); + //FNAME(); if(fd < 0) return FALSE; int i = 0; for(; i < 5; ++i){ @@ -399,7 +402,7 @@ static void *encoderthread2(void _U_ *u){ pfds[0].fd = encfd[0]; pfds[0].events = POLLIN; pfds[1].fd = encfd[1]; pfds[1].events = POLLIN; double t0[2], tstart; - buf_t strbuf[2]; + buf_t strbuf[2] = {0}; long msrlast[2]; // last encoder data double mtlast[2]; // last measurement time asknext(encfd[0]); asknext(encfd[1]); @@ -426,6 +429,9 @@ static void *encoderthread2(void _U_ *u){ if(i == 0){ mountdata.encXposition.val = Xenc2rad((double)msrlast[i]); 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]); diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 2c88e52..6e6f3f9 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -26,8 +26,12 @@ #include "serial.h" #include "ssii.h" -int X_ENC_ZERO, Y_ENC_ZERO; -double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.; +int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; +// defaults until read from controller +double X_MOT_STEPSPERREV = 13312000., + Y_MOT_STEPSPERREV = 17578668., + X_ENC_STEPSPERREV = 67108864., + Y_ENC_STEPSPERREV = 67108864.; uint16_t SScalcChecksum(uint8_t *buf, int len){ uint16_t checksum = 0; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 39d65a0..0748369 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -209,12 +209,14 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE // convert angle in radians to +-pi static inline __attribute__((always_inline)) double ang2half(double ang){ + ang = fmod(ang, 2.*M_PI); if(ang < -M_PI) ang += 2.*M_PI; else if(ang > M_PI) ang -= 2.*M_PI; return ang; } // convert to only positive: 0..2pi static inline __attribute__((always_inline)) double ang2full(double ang){ + ang = fmod(ang, 2.*M_PI); if(ang < 0.) ang += 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI; return ang;