diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 52d5200..f2d731d 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -47,10 +47,10 @@ static movemodel_t *Xmodel, *Ymodel; // accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2 limits_t Xlimits = { - .min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}}, Ylimits = { - .min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}} ; static mcc_errcodes_t shortcmd(short_command_t *cmd); @@ -93,7 +93,7 @@ static int initstarttime(){ // return difference (in seconds) between time1 and time0 double timediff(const struct timespec *time1, const struct timespec *time0){ if(!time1 || !time0) return -1.; - return (double)(time1->tv_sec - time0->tv_sec) + (double)(time1->tv_nsec - time0->tv_nsec) / 1e9; + return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9; } // difference between given time and last initstarttime() call double timediff0(const struct timespec *time1){ @@ -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); } @@ -319,8 +321,8 @@ static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){ */ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ if(!target || !speed) return MCC_E_BADFORMAT; - // if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; - // if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; + if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; // updateMotorPos() here can make a problem; TODO: remove? if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; short_command_t cmd = {0}; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index f4cc227..fc57cd6 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -64,15 +65,12 @@ void getXspeed(){ ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); if(!ls) return; } - pthread_mutex_lock(&datamutex); double dt = timediff0(&mountdata.encXposition.t); double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt); if(fabs(speed) < 1.5 * Xlimits.max.speed){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; } - pthread_mutex_unlock(&datamutex); - //DBG("Xspeed=%g", mountdata.encXspeed.val); } void getYspeed(){ static less_square_t *ls = NULL; @@ -80,14 +78,12 @@ void getYspeed(){ ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); if(!ls) return; } - pthread_mutex_lock(&datamutex); double dt = timediff0(&mountdata.encYposition.t); double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt); if(fabs(speed) < 1.5 * Ylimits.max.speed){ mountdata.encYspeed.val = speed; mountdata.encYspeed.t = mountdata.encYposition.t; } - pthread_mutex_unlock(&datamutex); } /** @@ -136,13 +132,12 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); mountdata.encXposition.t = *t; mountdata.encYposition.t = *t; - pthread_mutex_unlock(&datamutex); - //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); - //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); getXspeed(); getYspeed(); + pthread_mutex_unlock(&datamutex); //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI); } +#if 0 /** * @brief getencval - get uint64_t data from encoder * @param fd - encoder fd @@ -211,6 +206,8 @@ static int getencval(int fd, double *val, struct timespec *t){ if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} return got; } +#endif + // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected static int getencbyte(){ if(encfd[0] < 0) return -1; @@ -322,65 +319,137 @@ static void *encoderthread1(void _U_ *u){ return NULL; } +#define XYBUFSZ (128) +typedef struct{ + char buf[XYBUFSZ+1]; + int len; +} buf_t; + +// write to buffer next data portion; return FALSE in case of error +static int readstrings(buf_t *buf, int fd){ + 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'); + if(lastn){ + fprintf(stderr, "BUFOVR: _%s_", buf->buf); + ++lastn; + buf->len = XYBUFSZ - (lastn - buf->buf); + DBG("Memmove %d", buf->len); + memmove(lastn, buf->buf, buf->len); + buf->buf[buf->len] = 0; + }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()"); + return FALSE; + }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); + return TRUE; +} + +// return TRUE if got, FALSE if no data found +static int getdata(buf_t *buf, long *out){ + 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); + if(*last != '\n') return FALSE; + *last = 0; + //DBG("buf: _%s_", buf->buf); + char *prev = strrchr(buf->buf, '\n'); + if(!prev) prev = buf->buf; + else{ + fprintf(stderr, "MORETHANONE: _%s_", buf->buf); + ++prev; // after last '\n' + } + if(out) *out = atol(prev); + // clear buffer + buf->len = 0; + return TRUE; +} + +// try to write '\n' asking new data portion; return FALSE if failed +static int asknext(int fd){ + //FNAME(); + if(fd < 0) return FALSE; + int i = 0; + for(; i < 5; ++i){ + int l = write(fd, "\n", 1); + //DBG("l=%d", l); + if(1 == l) return TRUE; + usleep(100); + } + DBG("5 tries... failed!"); + return FALSE; +} + // main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0 static void *encoderthread2(void _U_ *u){ if(Conf.SepEncoder != 2) return NULL; DBG("Thread started"); + struct pollfd pfds[2]; + 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] = {0}; + long msrlast[2]; // last encoder data + double mtlast[2]; // last measurement time + asknext(encfd[0]); asknext(encfd[1]); + t0[0] = t0[1] = tstart = timefromstart(); int errctr = 0; - double t0 = timefromstart(); - const char *req = "\n"; - int need2ask = 0; // need or not to ask encoder for new data - while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ - struct timespec t; - if(need2ask){ - //DBG("ASK for new data, tfs=%.6g", timefromstart()); - if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } - else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } - //DBG("OK"); - need2ask = 0; - usleep(100); + do{ // main cycle + if(poll(pfds, 2, 0) < 0){ + DBG("poll()"); + break; } - if(!curtime(&t)){ - DBG("Where is time?"); - continue; - } - double v; - if(getencval(encfd[0], &v, &t)){ - pthread_mutex_lock(&datamutex); - mountdata.encXposition.val = Xenc2rad(v); - mountdata.encXposition.t = t; - pthread_mutex_unlock(&datamutex); - //DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart()); - getXspeed(); - if(getencval(encfd[1], &v, &t)){ - pthread_mutex_lock(&datamutex); - mountdata.encYposition.val = Yenc2rad(v); - mountdata.encYposition.t = t; - pthread_mutex_unlock(&datamutex); - //DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV); - getYspeed(); - errctr = 0; - need2ask = 0; - //DBG("tgot=%.6g", timefromstart()); - while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } - t0 = timefromstart(); - }else{ - DBG("NO Y DATA!!!"); - /*if(need2ask) ++errctr; - else need2ask = 1; - continue;*/ - ++errctr; - need2ask = 1; + int got = 0; + for(int i = 0; i < 2; ++i){ + if(pfds[i].revents && POLLIN){ + if(!readstrings(&strbuf[i], encfd[i])){ + ++errctr; + break; + } + } + double curt = timefromstart(); + if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt; + if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records + if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ + pthread_mutex_lock(&datamutex); + 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]); + curtime(&mountdata.encYposition.t); + getYspeed(); + } + pthread_mutex_unlock(&datamutex); + } + if(!asknext(encfd[i])){ + ++errctr; + break; + } + t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt; + ++got; } - }else{ // no data - ask for new - //DBG("Need new, tfs=%.6g", timefromstart()); - /*if(need2ask) ++errctr; - else need2ask = 1; - continue;*/ - ++errctr; - need2ask = 1; } - } + if(got == 2) errctr = 0; + }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR); DBG("\n\nEXIT: ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ @@ -445,26 +514,26 @@ static void *mountthread(void _U_ *u){ if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue; pthread_mutex_lock(&datamutex); mountdata.encXposition.t = mountdata.encYposition.t = tnow; - mountdata.encXposition.val = c.X; - mountdata.encYposition.val = c.Y; + mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error + mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6; //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); if(tcur - oldmt > Conf.MountReqInterval){ oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3); mountdata.motYposition.t = mountdata.motXposition.t = tnow; if(xst == ST_MOVE) mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.; - else - mountdata.motXposition.val = c.X; + //else + // mountdata.motXposition.val = c.X; if(yst == ST_MOVE) mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.; - else - mountdata.motYposition.val = c.Y; + //else + // mountdata.motYposition.val = c.Y; oldmt = tcur; }else mountdata.millis = oldmillis; chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate); chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate); - pthread_mutex_unlock(&datamutex); getXspeed(); getYspeed(); + pthread_mutex_unlock(&datamutex); while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50); t0 = timefromstart(); } 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;