diff --git a/PID.c b/PID.c index 6084a0a..458b247 100644 --- a/PID.c +++ b/PID.c @@ -122,7 +122,7 @@ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_ DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI); switch(axis->state){ case AXIS_SLEWING: - if(fe < Conf.MaxPointingErr){ + if(fe < Conf.MaxFinePointingErr){ axis->state = AXIS_POINTING; DBG("--> Pointing"); }else{ @@ -216,6 +216,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ setStat(xstate, ystate); } coordpair_t endpoint; +#if 0 // allow at least PIDMaxDt moving with target speed double dv = fabs(tagspeed.X - m.encXspeed.val); double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed @@ -227,6 +228,16 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ + Conf.PIDMaxDt * tagspeed.Y + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.; endpoint.Y = m.encYposition.val + Ysign * adder; +#endif + // allow 10s moving but not more than 10deg and not less than 1deg + double adder = fabs(tagspeed.X) * 10.; + if(adder > 0.17453) adder = 0.17453; + else if(adder < 0.017453) adder = 0.017453; + endpoint.X = m.encXposition.val + Xsign * adder; + adder = fabs(tagspeed.Y) * 10.; + if(adder > 0.17453) adder = 0.17453; + else if(adder < 0.017453) adder = 0.017453; + endpoint.Y = m.encYposition.val + Ysign * adder; DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.); return Mount.moveWspeed(&endpoint, &tagspeed); } diff --git a/main.c b/main.c index ab2ac00..827e420 100644 --- a/main.c +++ b/main.c @@ -311,10 +311,6 @@ static mcc_errcodes_t move2(const coordpair_t *target){ cmd.Ymot = target->Y; cmd.Xspeed = Xlimits.max.speed; cmd.Yspeed = Ylimits.max.speed; - /*mcc_errcodes_t r = shortcmd(&cmd); - if(r != MCC_E_OK) return r; - setslewingstate(); - return MCC_E_OK;*/ return shortcmd(&cmd); } @@ -351,10 +347,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed cmd.Ymot = target->Y; cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y; - mcc_errcodes_t r = shortcmd(&cmd); - if(r != MCC_E_OK) return r; - setslewingstate(); - return MCC_E_OK; + return shortcmd(&cmd); } /** diff --git a/serial.c b/serial.c index b8e4665..d22916e 100644 --- a/serial.c +++ b/serial.c @@ -55,6 +55,8 @@ static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder read mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings +static volatile int GlobExit = 0; + // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -178,16 +180,16 @@ static int readmntdata(uint8_t *buffer, int maxlen){ return -1; } if(!buffer || maxlen < 1) return 0; - DBG("ask for %d bytes", maxlen); + //DBG("ask for %d bytes", maxlen); int got = 0; fd_set rfds; struct timeval tv = mnt1Rtmout; do{ FD_ZERO(&rfds); FD_SET(mntfd, &rfds); - DBG("select"); + //DBG("select"); int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); - DBG("returned %d", retval); + //DBG("returned %d", retval); if(retval < 0){ if(errno == EINTR) continue; DBG("Error in select()"); @@ -244,7 +246,7 @@ static void *encoderthread1(void _U_ *u){ uint8_t databuf[ENC_DATALEN]; int wridx = 0, errctr = 0; struct timespec tcur; - while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ + while(encfd[0] > -1 && errctr < MAX_ERR_CTR && !GlobExit){ int b = getencbyte(); if(b == -2) ++errctr; if(b < 0) continue; @@ -402,27 +404,27 @@ static void *encoderthread2(void _U_ *u){ // Kalman filtering kalman3_predict(&kf[i]); kalman3_update(&kf[i], pos); - DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", - pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); mountdata.encXposition.val = kf[i].x[0]; 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(); - mountdata.encXspeed.val = kf[i].x[1]; - mountdata.encXspeed.t = mountdata.encXposition.t; + getXspeed(); + //mountdata.encXspeed.val = kf[i].x[1]; + //mountdata.encXspeed.t = mountdata.encXposition.t; }else{ pos = Yenc2rad(pos); kalman3_predict(&kf[i]); kalman3_update(&kf[i], pos); - DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", - pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); mountdata.encYposition.val = kf[i].x[0]; curtime(&mountdata.encYposition.t); - //getYspeed(); - mountdata.encYspeed.val = kf[i].x[1]; - mountdata.encYspeed.t = mountdata.encYposition.t; + getYspeed(); + //mountdata.encYspeed.val = kf[i].x[1]; + //mountdata.encYspeed.t = mountdata.encYposition.t; } pthread_mutex_unlock(&datamutex); } @@ -435,7 +437,7 @@ static void *encoderthread2(void _U_ *u){ } } if(got == 2) errctr = 0; - }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR); + }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR && !GlobExit); DBG("\n\nEXIT: ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ @@ -491,7 +493,7 @@ static void *mountthread(void _U_ *u){ if(Conf.RunModel){ double Xprev = NAN, Yprev = NAN; // previous coordinates int xcnt = 0, ycnt = 0; - while(1){ + while(!GlobExit){ coordpair_t c; movestate_t xst, yst; // now change data @@ -529,7 +531,7 @@ static void *mountthread(void _U_ *u){ // cmd to send data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); if(!cmd_getstat) goto failed; - while(mntfd > -1 && errctr < MAX_ERR_CTR){ + while(mntfd > -1 && errctr < MAX_ERR_CTR && !GlobExit){ // read data to status struct timespec tcur; if(!curtime(&tcur)) continue; @@ -671,14 +673,17 @@ create_thread: // close all opened serial devices and quit threads void closeSerial(){ - DBG("CLOSE all devices"); + GlobExit = 1; + DBG("Give 100ms to proper close"); + usleep(100000); + DBG("Force closed all devices"); if(mntfd > -1){ DBG("Cancel mount thread"); pthread_cancel(mntthread); DBG("join mount thread"); pthread_join(mntthread, NULL); DBG("close mount fd"); - close(mntfd); + if(mntfd > -1) close(mntfd); mntfd = -1; } if(encfd[0] > -1){ @@ -687,13 +692,14 @@ void closeSerial(){ DBG("join encoder thread"); pthread_join(encthread, NULL); DBG("close encoder's fd"); - close(encfd[0]); + if(encfd[0] > -1) close(encfd[0]); encfd[0] = -1; if(Conf.SepEncoder == 2 && encfd[1] > -1){ close(encfd[1]); encfd[1] = -1; } } + GlobExit = 0; } // get fresh encoder information @@ -721,10 +727,10 @@ static int wr(const data_t *out, data_t *in, int needeol){ DBG("Wrong arguments or no mount fd"); return FALSE; } - DBG("clrbuf"); + //DBG("clrbuf"); clrmntbuf(); if(out){ - DBG("write %zd bytes (%s)", out->len, out->buf); + //DBG("write %zd bytes (%s)", out->len, out->buf); if(out->len != (size_t)write(mntfd, out->buf, out->len)){ DBG("written bytes not equal to need"); return FALSE; @@ -755,11 +761,11 @@ static int wr(const data_t *out, data_t *in, int needeol){ */ int MountWriteRead(const data_t *out, data_t *in){ if(Conf.RunModel) return FALSE; - double t0 = timefromstart(); + //double t0 = timefromstart(); pthread_mutex_lock(&mntmutex); int ret = wr(out, in, 1); pthread_mutex_unlock(&mntmutex); - DBG("Got %gus", (timefromstart()-t0)*1e6); + //DBG("Got %gus", (timefromstart()-t0)*1e6); return ret; } // send binary data - without EOL @@ -771,7 +777,7 @@ int MountWriteReadRaw(const data_t *out, data_t *in){ return ret; } -#ifdef EBUG +#if 0 static void logscmd(SSscmd *c){ printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed); printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits); @@ -794,20 +800,17 @@ static int bincmd(uint8_t *cmd, int len){ if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD); int ret = FALSE; pthread_mutex_lock(&mntmutex); - // dummy buffer to clear trash in input - //char ans[300]; - //data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; if(len == sizeof(SSscmd)){ ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); - DBG("Short command"); -#ifdef EBUG + //DBG("Short command"); +#if 0 logscmd((SSscmd*)cmd); #endif if(!wr(dscmd, NULL, 1)) goto rtn; }else if(len == sizeof(SSlcmd)){ ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); - DBG("Long command"); -#ifdef EBUG + // DBG("Long command"); +#if 0 loglcmd((SSlcmd*)cmd); #endif if(!wr(dlcmd, NULL, 1)) goto rtn; @@ -822,8 +825,11 @@ static int bincmd(uint8_t *cmd, int len){ ret = wr(&d, &in, 0); DBG("%s", ret ? "SUCCESS" : "FAIL"); if(ret){ - DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d", - ans.Xmot, ans.Ymot, ans.XLast, ans.YLast); + SSscmd *sc = (SSscmd*)cmd; + mountdata.Xtarget = sc->Xmot; + mountdata.Ytarget = sc->Ymot; + DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d; Xtag/Ytag: %d/%d", + ans.Xmot, ans.Ymot, ans.XLast, ans.YLast, mountdata.Xtarget, mountdata.Ytarget); } rtn: pthread_mutex_unlock(&mntmutex); diff --git a/servo_real.conf b/servo_real.conf new file mode 100644 index 0000000..a09fb0e --- /dev/null +++ b/servo_real.conf @@ -0,0 +1,23 @@ +MountDevPath=/dev/ttyUSB0 +MountDevSpeed=19200 +EncoderDevSpeed=1000000 +MountReqInterval=0.1 +EncoderReqInterval=0.001 +SepEncoder=2 +EncoderXDevPath=/dev/encoder_X0 +EncoderYDevPath=/dev/encoder_Y0 +EncoderSpeedInterval=0.05 +RunModel=0 +# telescope is in "pointing state" when coordinate error less than MaxFinePointingErr and goes to "slewing state" +# when this error greater than MaxPointingErr +MaxPointingErr = 0.3490658504 # "pointing zone" - 20 degr +MaxFinePointingErr = 0.1745329252 # "guiding zone" - 10 degr +MaxGuidingErr = 4.8481368e-6 # "on target zone" - 1 arcsec +XPIDVP=0.9 +XPIDVI=0.0005 +XPIDVD=0.0 +YPIDVP=0.5 +YPIDVI=0.005 +YPIDVD=0. +XEncZero=36627112 +YEncZero=36067741 diff --git a/sidservo.h b/sidservo.h index ba4b8e5..bd50257 100644 --- a/sidservo.h +++ b/sidservo.h @@ -158,6 +158,9 @@ typedef struct{ uint32_t millis; double temperature; double voltage; + // target X/Y position by last `short` or `long` command + int32_t Xtarget; // in SidServo's counts + int32_t Ytarget; // -//- } mountdata_t; typedef struct{ diff --git a/ssii.c b/ssii.c index 87ec05c..e7c81cc 100644 --- a/ssii.c +++ b/ssii.c @@ -26,7 +26,7 @@ #include "serial.h" #include "ssii.h" -int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; +int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; // will be filled later from config // defaults until read from controller double X_MOT_STEPSPERREV = 13312000., Y_MOT_STEPSPERREV = 17578668., @@ -45,12 +45,13 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ } // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! -static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){ +static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){ if(*prev == INT32_MAX){ stat = AXIS_STOPPED; DBG("START"); - }else if(stat == AXIS_GONNASTOP){ // check stop + }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target if(*prev == cur){ + DBG("Test for stop, nstopped=%d", *nstopped); if(++(*nstopped) > MOTOR_STOPPED_CNT){ stat = AXIS_STOPPED; DBG("AXIS stopped"); @@ -68,8 +69,8 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){ static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state axis_status_t Xstat, Ystat; - Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate); - Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate); + Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate); + Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate); if(Xstat != m->Xstate || Ystat != m->Ystate){ DBG("Status changed"); setStat(Xstat, Ystat);