diff --git a/.qtcreator/libsidservo.creator.user b/.qtcreator/libsidservo.creator.user index 186d416..688a25e 100644 --- a/.qtcreator/libsidservo.creator.user +++ b/.qtcreator/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/examples/scmd_traectory.c b/examples/scmd_traectory.c index 67ad655..91af985 100644 --- a/examples/scmd_traectory.c +++ b/examples/scmd_traectory.c @@ -146,10 +146,12 @@ int main(int argc, char **argv){ ERRX("Can't open error log %s", G.errlog); else fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n"); + setbuf(errlog, NULL); } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); + setbuf(fcoords, NULL); }else fcoords = stdout; Config = readServoConf(G.conffile); if(!Config || G.dumpconf){ @@ -178,7 +180,7 @@ int main(int argc, char **argv){ signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - chk0(G.Ncycles); +// chk0(G.Ncycles); logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); ; diff --git a/libsidservo.creator.user b/libsidservo.creator.user index 186d416..688a25e 100644 --- a/libsidservo.creator.user +++ b/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/main.c b/main.c index f2d731d..ab2ac00 100644 --- a/main.c +++ b/main.c @@ -202,6 +202,7 @@ static mcc_errcodes_t init(conf_t *c){ if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; return MCC_E_OK; } + DBG("Try to open mount device"); if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; @@ -209,33 +210,53 @@ static mcc_errcodes_t init(conf_t *c){ DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed); ret = MCC_E_MOUNTDEV; } - if(Conf.SepEncoder){ - if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ - DBG("Define encoder device path"); - ret = MCC_E_BADFORMAT; - }else if(!openEncoder()){ - DBG("Can't open encoder device"); - ret = MCC_E_ENCODERDEV; - } - } // TODO: read hardware configuration on init if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ DBG("Wrong speed interval"); ret = MCC_E_BADFORMAT; } - if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; + DBG("Exit ACM, exit manual mode"); + SSrawcmd(CMD_EXITACM, NULL); + SStextcmd(CMD_AUTOX, NULL); + SStextcmd(CMD_AUTOY, NULL); // read HW config to update constants hardware_configuration_t HW; - if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; + DBG("Read hardware configuration"); + ret = MCC_E_FAILED; + for(int i = 0; i < MAX_ERR_CTR; ++i){ + DBG("TRY %d..", i); + ret = get_hwconf(&HW); + if(ret == MCC_E_OK) break; + } + if(MCC_E_OK != ret) return ret; // make a pause for actual encoder's values + DBG("Check encoders"); + if(Conf.SepEncoder){ + if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ + DBG("Define encoder device path"); + ret = MCC_E_BADFORMAT; + }else{ + ret = MCC_E_ENCODERDEV; + for(int i = 0; i < MAX_ERR_CTR; ++i){ + if(openEncoder()){ + ret = MCC_E_OK; + break; + } + } + } + } + if(MCC_E_OK != ret) return ret; double t0 = timefromstart(); - while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000); + DBG("Wait for first encoders' measurement"); + while(timefromstart() - t0 < Conf.EncoderReqInterval * 15.) usleep(1000); + DBG("Update motor position"); mcc_errcodes_t e = updateMotorPos(); // and refresh data after updating DBG("Wait for next mount reading"); t0 = timefromstart(); - while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000); + while(timefromstart() - t0 < Conf.MountReqInterval * 5.) usleep(1000); + DBG("ALL READY!"); return e; } diff --git a/serial.c b/serial.c index fc57cd6..25b0ab9 100644 --- a/serial.c +++ b/serial.c @@ -49,7 +49,11 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, // encoders thread and mount thread static pthread_t encthread, mntthread; // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` -static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; +// this values will be modified later +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder reading timeout + mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading + mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings + // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -231,43 +235,60 @@ static int getencbyte(){ }while(1); return (int)byte; } -// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected -static int getmntbyte(){ - if(mntfd < 0) return -1; - uint8_t byte; + +/** + * @brief readmntdata - read data + * @param buffer - input buffer + * @param maxlen - maximal buffer length + * @return amount of bytes read or -1 in case of error + */ +static int readmntdata(uint8_t *buffer, int maxlen){ + if(mntfd < 0){ + DBG("mntfd non opened"); + return -1; + } + if(!buffer || maxlen < 1) return 0; + DBG("ask for %d bytes", maxlen); + int got = 0; fd_set rfds; - /* ssize_t l = read(mntfd, &byte, 1); - //DBG("MNT read=%zd byte=0x%X", l, byte); - if(l == 0) return -1; - if(l != 1) return -2; // disconnected ?? - return (int) byte;*/ + struct timeval tv = mnt1Rtmout; do{ FD_ZERO(&rfds); FD_SET(mntfd, &rfds); - struct timeval tv = mntRtmout; + DBG("select"); int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); + DBG("returned %d", retval); if(retval < 0){ if(errno == EINTR) continue; DBG("Error in select()"); return -1; } - //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds)); if(FD_ISSET(mntfd, &rfds)){ - ssize_t l = read(mntfd, &byte, 1); - //DBG("MNT read=%zd byte=0x%X", l, byte); - if(l != 1){ + ssize_t l = read(mntfd, buffer, maxlen); + if(l == 0){ + DBG("read ZERO"); + continue; + } + if(l < 0){ DBG("Mount disconnected?"); return -2; // disconnected ?? } + buffer += l; + maxlen -= l; + got += l; + }else{ + DBG("no new data after %d bytes (%s)", got, buffer - got); break; - } else return -1; - }while(1); - return (int)byte; + } + tv = mntRtmout; + }while(maxlen); + return got; } + // clear data from input buffer static void clrmntbuf(){ if(mntfd < 0) return; - uint8_t byte; + uint8_t bytes[256]; fd_set rfds; do{ FD_ZERO(&rfds); @@ -280,9 +301,10 @@ static void clrmntbuf(){ break; } if(FD_ISSET(mntfd, &rfds)){ - ssize_t l = read(mntfd, &byte, 1); - if(l != 1) break; - } else break; + ssize_t l = read(mntfd, &bytes, 256); + if(l < 1) break; + DBG("clr got %zd bytes: %s", l, bytes); + }else break; }while(1); } @@ -426,15 +448,19 @@ static void *encoderthread2(void _U_ *u){ if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ pthread_mutex_lock(&datamutex); + double pos = (double)msrlast[i]; + //DBG("pos[%d]=%g", i, pos); + ;; + //DBG("Got med: %g", pos); if(i == 0){ - mountdata.encXposition.val = Xenc2rad((double)msrlast[i]); + mountdata.encXposition.val = Xenc2rad(pos); 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]); + mountdata.encYposition.val = Yenc2rad(pos); curtime(&mountdata.encYposition.t); getYspeed(); } @@ -496,7 +522,7 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t // main mount thread static void *mountthread(void _U_ *u){ int errctr = 0; - uint8_t buf[2*sizeof(SSstat)]; + uint8_t buf[sizeof(SSstat)]; SSstat *status = (SSstat*) buf; bzero(&mountdata, sizeof(mountdata)); double t0 = timefromstart(), tstart = t0, tcur = t0; @@ -616,15 +642,18 @@ static int ttyopen(const char *path, speed_t speed){ // return FALSE if failed int openEncoder(){ + // TODO: open real devices in "model" mode too! if(Conf.RunModel) return TRUE; if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent + /* + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes +*/ if(Conf.SepEncoder == 1){ // only one device DBG("One device"); if(encfd[0] > -1) close(encfd[0]); encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed); if(encfd[0] < 0) return FALSE; - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ close(encfd[0]); encfd[0] = -1; @@ -638,8 +667,6 @@ int openEncoder(){ encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed); if(encfd[i] < 0) return FALSE; } - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ for(int i = 0; i < 2; ++i){ close(encfd[i]); @@ -654,6 +681,7 @@ int openEncoder(){ // return FALSE if failed int openMount(){ + // TODO: open real devices in "model" mode too! if(Conf.RunModel) goto create_thread; if(mntfd > -1) close(mntfd); DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); @@ -661,16 +689,13 @@ int openMount(){ if(mntfd < 0) return FALSE; DBG("mntfd=%d", mntfd); // clear buffer - while(getmntbyte() > -1); - /*int g = write(mntfd, "XXS\r", 4); - DBG("Written %d", g); - uint8_t buf[100]; - do{ - ssize_t l = read(mntfd, buf, 100); - DBG("got %zd", l); - }while(1);*/ + clrmntbuf(); + /* + mnt1Rtmout.tv_sec = 0; + mnt1Rtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed mntRtmout.tv_sec = 0; - mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed + mntRtmout.tv_usec = mnt1Rtmout.tv_usec / 50; +*/ create_thread: if(pthread_create(&mntthread, NULL, mountthread, NULL)){ DBG("Can't create mount thread"); @@ -686,8 +711,7 @@ create_thread: // close all opened serial devices and quit threads void closeSerial(){ - // TODO: close devices in "model" mode too! - if(Conf.RunModel) return; + DBG("CLOSE all devices"); if(mntfd > -1){ DBG("Cancel mount thread"); pthread_cancel(mntthread); @@ -737,26 +761,29 @@ static int wr(const data_t *out, data_t *in, int needeol){ DBG("Wrong arguments or no mount fd"); return FALSE; } + DBG("clrbuf"); clrmntbuf(); if(out){ + 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; } + //DBG("eol, mntfd=%d", mntfd); if(needeol){ int g = write(mntfd, "\r", 1); // add EOL (void) g; } - usleep(50000); // add little pause so that the idiot has time to swallow + //usleep(50000); // add little pause so that the idiot has time to swallow } - if(!in) return TRUE; - in->len = 0; - for(size_t i = 0; i < in->maxlen; ++i){ - int b = getmntbyte(); - if(b < 0) break; // nothing to read -> go out - in->buf[in->len++] = (uint8_t) b; + if(!in || in->maxlen < 1) return TRUE; + int got = readmntdata(in->buf, in->maxlen); + if(got < 0){ + DBG("Error reading mount data!"); + in->len = 0; + return FALSE; } - while(getmntbyte() > -1); + in->len = got; return TRUE; } @@ -767,15 +794,17 @@ static int wr(const data_t *out, data_t *in, int needeol){ * @return FALSE if failed */ int MountWriteRead(const data_t *out, data_t *in){ - if(Conf.RunModel) return -1; + if(Conf.RunModel) return FALSE; + double t0 = timefromstart(); pthread_mutex_lock(&mntmutex); int ret = wr(out, in, 1); pthread_mutex_unlock(&mntmutex); + DBG("Got %gus", (timefromstart()-t0)*1e6); return ret; } // send binary data - without EOL int MountWriteReadRaw(const data_t *out, data_t *in){ - if(Conf.RunModel) return -1; + if(Conf.RunModel) return FALSE; pthread_mutex_lock(&mntmutex); int ret = wr(out, in, 0); pthread_mutex_unlock(&mntmutex); diff --git a/ssii.c b/ssii.c index 6e6f3f9..76d9a0b 100644 --- a/ssii.c +++ b/ssii.c @@ -82,6 +82,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){ m->motXposition.t = m->motYposition.t = *t; // fill encoder data from here, as there's no separate enc thread if(!Conf.SepEncoder){ + DBG("ENCODER from SSII"); m->encXposition.val = Xenc2rad(s->Xenc); DBG("encx: %g", m->encXposition.val); m->encYposition.val = Yenc2rad(s->Yenc);