diff --git a/LibSidServo/TODO b/LibSidServo/TODO new file mode 100644 index 0000000..c636b16 --- /dev/null +++ b/LibSidServo/TODO @@ -0,0 +1,11 @@ +1. WTF after closing? + +closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 484): close fd + +closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 489): Kill encoder thread + +closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd +*** bit out of range 0 - FD_SETSIZE on fd_set ***: terminated +Aborted + + diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index db731ea..1969a0f 100644 --- a/LibSidServo/examples/conf.c +++ b/LibSidServo/examples/conf.c @@ -29,7 +29,8 @@ static conf_t Config = { .EncoderDevSpeed = 153000, .MountReqInterval = 0.1, .EncoderReqInterval = 0.05, - .SepEncoder = 2 + .SepEncoder = 2, + .EncoderSpeedInterval = 0.1, }; static sl_option_t opts[] = { @@ -42,6 +43,7 @@ static sl_option_t opts[] = { {"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"}, {"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"}, {"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"}, + {"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"}, end_option }; diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 604d1ca..a7d9ef6 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -23,6 +23,42 @@ #include "dump.h" #include "simpleconv.h" +#if 0 +// amount of elements used for encoders' data filtering +#define NFILT (10) + +static double filterK[NFILT]; +static double lastvals[2][NFILT] = {0}; +static int need2buildFilter = 1; + +static void buildFilter(){ + filterK[NFILT-1] = 1.; + double sum = 1.; + for(int i = NFILT-2; i > -1; --i){ + filterK[i] = (filterK[i+1] + 1.) * 1.1; + sum += filterK[i]; + } + for(int i = 0; i < NFILT; ++i) filterK[i] /= sum; +} + +static double filter(double val, int idx){ + if(need2buildFilter){ + buildFilter(); + need2buildFilter = 0; + } + static int ctr[2] = {0}; + for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1]; + lastvals[idx][0] = val; + double r = 0.; + if(ctr[idx] < NFILT){ + ++ctr[idx]; + return val; + } + for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i]; + return r; +} +#endif + /** * @brief logmnt - log mount data into file * @param fcoords - file to dump @@ -33,16 +69,18 @@ void logmnt(FILE *fcoords, mountdata_t *m){ //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) millis T V\n"); + fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); return; } - if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6; - double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6 - t0; + 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 %10u %6.1f %4.1f\n", - t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y), - RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y), - m->millis, m->temperature, m->voltage); + 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), + RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val), + RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val), + m->millis); fflush(fcoords); } @@ -64,21 +102,24 @@ void dumpmoving(FILE *fcoords, double t, int N){ WARNX("Can't get mount data"); LOGWARN("Can't get mount data"); } - uint32_t millis = mdata.encposition.msrtime.tv_usec; + uint32_t mdmillis = mdata.millis; + double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.; int ctr = -1; - double xlast = mdata.motposition.X, ylast = mdata.motposition.Y; - double t0 = sl_dtime(); - //DBG("millis = %u", millis); - while(sl_dtime() - t0 < t && ctr < N){ + double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val; + double t0 = Mount.currentT(); + while(Mount.currentT() - t0 < t && ctr < N){ usleep(1000); if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} - if(mdata.encposition.msrtime.tv_usec == millis) continue; - //DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y); - millis = mdata.encposition.msrtime.tv_usec; + double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.; + if(tmsr == enct) continue; + enct = tmsr; if(fcoords) logmnt(fcoords, &mdata); - if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ - xlast = mdata.motposition.X; - ylast = mdata.motposition.Y; + if(mdata.millis == mdmillis) continue; + DBG("ctr=%d", ctr); + mdmillis = mdata.millis; + if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ + xlast = mdata.motXposition.val; + ylast = mdata.motYposition.val; ctr = 0; }else ++ctr; } @@ -98,9 +139,9 @@ void waitmoving(int N){ if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(mdata.millis == millis) continue; millis = mdata.millis; - if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ - xlast = mdata.motposition.X; - ylast = mdata.motposition.Y; + if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ + xlast = mdata.motXposition.val; + ylast = mdata.motYposition.val; ctr = 0; }else ++ctr; } @@ -112,7 +153,7 @@ void waitmoving(int N){ * @param Y (o) - encoder position (or NULL) * @return FALSE if failed */ -int getPos(coords_t *mot, coords_t *enc){ +int getPos(coordval_pair_t *mot, coordval_pair_t *enc){ mountdata_t mdata = {0}; int errcnt = 0; do{ @@ -126,16 +167,22 @@ int getPos(coords_t *mot, coords_t *enc){ WARNX("Can't read mount status"); return FALSE; } - if(mot) *mot = mdata.motposition; - if(enc) *enc = mdata.encposition; + if(mot){ + mot->X = mdata.motXposition; + mot->Y = mdata.motYposition; + } + if(enc){ + enc->X = mdata.encXposition; + enc->Y = mdata.encYposition; + } return TRUE; } // check current position and go to 0 if non-zero void chk0(int ncycles){ - coords_t M; + coordval_pair_t M; if(!getPos(&M, NULL)) signals(2); - if(M.X || M.Y){ + if(M.X.val || M.Y.val){ WARNX("Mount position isn't @ zero; moving"); double zero = 0.; Mount.moveTo(&zero, &zero); diff --git a/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h index 414473c..728dd27 100644 --- a/LibSidServo/examples/dump.h +++ b/LibSidServo/examples/dump.h @@ -25,5 +25,5 @@ void logmnt(FILE *fcoords, mountdata_t *m); void dumpmoving(FILE *fcoords, double t, int N); void waitmoving(int N); -int getPos(coords_t *mot, coords_t *enc); +int getPos(coordval_pair_t *mot, coordval_pair_t *enc); void chk0(int ncycles); diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index 323e310..ee7f33f 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -85,18 +85,18 @@ int main(int argc, char **argv){ LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed); LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed); if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices"); - coords_t M; + coordval_pair_t M; if(!getPos(&M, NULL)) ERRX("Can't get current position"); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y; + double tagx = DEG2RAD(45.) + M.X.val, tagy = DEG2RAD(45.) + M.Y.val; if(MCC_E_OK != Mount.moveTo(&tagx, &tagy)) ERRX("Can't move to 45, 45"); dumpmoving(fcoords, 30., G.Ncycles); - Mount.moveTo(&M.X, &M.Y); + Mount.moveTo(&M.X.val, &M.Y.val); dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; diff --git a/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c new file mode 100644 index 0000000..006aabd --- /dev/null +++ b/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -0,0 +1,170 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// move telescope to target using short command and force it to track mode + +#include +#include +#include +#include +#include +#include +#include + +#include "conf.h" +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int Ncycles; + int relative; + double reqint; + char *coordsoutput; + char *conffile; + char *axis; +} parameters; + +static parameters G = { + .Ncycles = 40, + .reqint = -1., + .axis = "X", +}; + +static FILE *fcoords = NULL; + +static coords_t M; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"}, + {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// return TRUE if motor position is reached +- 0.01 degrees +#define XYcount (DEG2RAD(0.01)) +static int Wait(double tag){ + mountdata_t mdata; + red("Wait for %g degrees\n", RAD2DEG(tag)); + int errcnt = 0; + double sign = 0.; + uint32_t millis = 0; + double curpos = 0.; + do{ + if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; + else{ + errcnt = 0; + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(*G.axis == 'X') curpos = mdata.motposition.X; + else curpos = mdata.motposition.Y; + if(sign == 0.) sign = (curpos > tag) ? 1. : -1.; + //printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos), + // RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.); + } + }while(sign*(curpos - tag) > XYcount && errcnt < 10); + if(errcnt >= 10){ + WARNX("Too much errors"); + return FALSE; + } + green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag)); + fflush(stdout); + return TRUE; +} + +// move X/Y to 40 degr with given speed until given coord +static void move(double target, double limit, double speed){ + green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed); + short_command_t cmd = {0}; + if(*G.axis == 'X' || *G.axis == 'B'){ + cmd.Xmot = DEG2RAD(target) + M.X; + cmd.Xspeed = DEG2RAD(speed); + limit = DEG2RAD(limit) + M.X; + } + if(*G.axis == 'Y' || *G.axis == 'B'){ + cmd.Ymot = DEG2RAD(target) + M.Y; + cmd.Yspeed = DEG2RAD(speed); + if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y; + } + if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); + if(!Wait(limit)) signals(9); +} + + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){ + WARNX("\"Axis\" should be X, Y or B"); + return 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + if(G.reqint > 0.) Config->MountReqInterval = G.reqint; + if(MCC_E_OK != Mount.init(Config)){ + WARNX("Can't init devices"); + return 1; + } + if(!getPos(&M, NULL)) ERRX("Can't get current position"); + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + // move to X=40 degr with different speeds + pthread_t dthr; + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + // goto 10 degr with 2d/s and try to track for 8 seconds + move(10., 10.+2./60., 2.); + // be sure to move @ 0,0 + Mount.moveTo(&M.X, &M.Y); + // wait moving ends + pthread_join(dthr, NULL); + signals(0); + return 0; +} diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index 6081f68..bfb22ae 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -49,7 +49,7 @@ static parameters G = { static FILE *fcoords = NULL; -static coords_t M; +static coordval_pair_t M; static sl_option_t cmdlnopts[] = { {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, @@ -92,8 +92,8 @@ static int Wait(double tag){ errcnt = 0; if(mdata.millis == millis) continue; millis = mdata.millis; - if(*G.axis == 'X') curpos = mdata.motposition.X; - else curpos = mdata.motposition.Y; + if(*G.axis == 'X') curpos = mdata.motXposition.val; + else curpos = mdata.motYposition.val; if(sign == 0.) sign = (curpos > tag) ? 1. : -1.; //printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos), // RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.); @@ -110,22 +110,20 @@ static int Wait(double tag){ // move X/Y to 40 degr with given speed until given coord static void move(double target, double limit, double speed){ -#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0) green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed); short_command_t cmd = {0}; if(*G.axis == 'X' || *G.axis == 'B'){ - cmd.Xmot = DEG2RAD(target) + M.X; + cmd.Xmot = DEG2RAD(target) + M.X.val; cmd.Xspeed = DEG2RAD(speed); - limit = DEG2RAD(limit) + M.X; + limit = DEG2RAD(limit) + M.X.val; } if(*G.axis == 'Y' || *G.axis == 'B'){ - cmd.Ymot = DEG2RAD(target) + M.Y; + cmd.Ymot = DEG2RAD(target) + M.Y.val; cmd.Yspeed = DEG2RAD(speed); - if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y; + if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val; } - SCMD(); + if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); if(!Wait(limit)) signals(9); -#undef SCMD } @@ -169,11 +167,10 @@ int main(int argc, char **argv){ move(10., 3., 15./60.); // and go back with 7deg/s move(0., 0., 7.); - // be sure to move @ 0,0 - Mount.moveTo(&M.X, &M.Y); + // be sure to move @ starting position + Mount.moveTo(&M.X.val, &M.Y.val); // wait moving ends pthread_join(dthr, NULL); -#undef SCMD signals(0); return 0; } diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index 73c50e4..8425995 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/LibSidServo/examples/dumpswing.c @@ -83,18 +83,18 @@ void waithalf(double t){ uint32_t millis = 0; double xlast = 0., ylast = 0.; while(ctr < 5){ - if(sl_dtime() >= t) return; + if(Mount.currentT() >= t) return; usleep(1000); if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(mdata.millis == millis) continue; millis = mdata.millis; - if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ - DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); - xlast = mdata.motposition.X; - ylast = mdata.motposition.Y; + if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ + DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); + xlast = mdata.motXposition.val; + ylast = mdata.motYposition.val; ctr = 0; }else{ - DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); + DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); ++ctr; } } @@ -145,21 +145,21 @@ int main(int argc, char **argv){ }else{ tagX = 0.; tagY = DEG2RAD(G.amplitude); } - double t = sl_dtime(), t0 = t; + double t = Mount.currentT(), t0 = t; double divide = 2., rtagX = -tagX, rtagY = -tagY; for(int i = 0; i < G.Nswings; ++i){ Mount.moveTo(&tagX, &tagY); - DBG("CMD: %g", sl_dtime()-t0); + DBG("CMD: %g", Mount.currentT()-t0); t += G.period / divide; divide = 1.; waithalf(t); DBG("Moved to +, t=%g", t-t0); - DBG("CMD: %g", sl_dtime()-t0); + DBG("CMD: %g", Mount.currentT()-t0); Mount.moveTo(&rtagX, &rtagY); t += G.period; waithalf(t); DBG("Moved to -, t=%g", t-t0); - DBG("CMD: %g", sl_dtime()-t0); + DBG("CMD: %g", Mount.currentT()-t0); } double zero = 0.; // be sure to move @ 0,0 diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 7c4f843..24255aa 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -40,7 +40,7 @@ typedef struct{ } parameters; static parameters G = { - .Ncycles = 40, + .Ncycles = 10, .X = NAN, .Y = NAN, }; @@ -88,7 +88,7 @@ int main(int _U_ argc, char _U_ **argv){ return 1; } if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); - coords_t M; + coordval_pair_t M; if(!getPos(&M, NULL)) ERRX("Can't get current position"); if(G.coordsoutput){ if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); @@ -100,20 +100,20 @@ int main(int _U_ argc, char _U_ **argv){ logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); } - printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); if(isnan(G.X) && isnan(G.Y)) goto out; double *xtag = NULL, *ytag = NULL, xr, yr; double _7deg = RAD2DEG(7.); if(!isnan(G.X)){ xr = DEG2RAD(G.X); - if(G.relative) xr += M.X; + if(G.relative) xr += M.X.val; xtag = &xr; // set max speed Mount.setSpeed(&_7deg, NULL); } if(!isnan(G.Y)){ yr = DEG2RAD(G.Y); - if(G.relative) yr += M.Y; + if(G.relative) yr += M.Y.val; ytag = &yr; Mount.setSpeed(NULL, &_7deg); } @@ -126,12 +126,14 @@ int main(int _U_ argc, char _U_ **argv){ sleep(1); waitmoving(G.Ncycles); if(!getPos(&M, NULL)) WARNX("Can't get current position"); - else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); } out: + DBG("JOIN"); if(G.coordsoutput) pthread_join(dthr, NULL); + DBG("QUIT"); if(G.wait){ - if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); Mount.quit(); } return 0; diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 00dacd2..621f6f5 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -89,20 +89,21 @@ static void *dumping(void _U_ *u){ // calculate static void runtraectory(traectory_fn tfn){ if(!tfn) return; - coords_t telXY, traectXY; - double t0 = sl_dtime(); - uint32_t susec_last = 0; + coordval_pair_t telXY; + coordpair_t traectXY; + double t0 = Mount.currentT(); + double tlast = 0.; while(1){ if(!telpos(&telXY)){ WARNX("No next telescope position"); return; } - if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind - susec_last = telXY.msrtime.tv_usec; - double t = sl_dtime(); - if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break; + if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind + tlast = (telXY.X.t + telXY.Y.t) / 2.; + double t = Mount.currentT(); + if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break; if(!traectory_point(&traectXY, t)) break; - DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y)); + DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); } WARNX("No next traectory point"); } @@ -133,7 +134,7 @@ int main(int argc, char **argv){ print_tr_names(); return 1; } - coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; + coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; if(!init_traectory(tfn, &c)){ ERRX("Can't init traectory"); return 1; diff --git a/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf index d37e959..dfe0a25 100644 --- a/LibSidServo/examples/servo.conf +++ b/LibSidServo/examples/servo.conf @@ -4,5 +4,6 @@ EncoderXDevPath=/dev/encoder_X0 EncoderYDevPath=/dev/encoder_Y0 MountReqInterval=0.05 SepEncoder=2 -EncoderReqInterval=0.01 +EncoderReqInterval=0.001 EncoderDevSpeed=1000000 +EncoderSpeedInterval=0.05 diff --git a/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c index decd645..1977880 100644 --- a/LibSidServo/examples/traectories.c +++ b/LibSidServo/examples/traectories.c @@ -28,12 +28,12 @@ static traectory_fn cur_traectory = NULL; // starting point of traectory -static coords_t XYstart = {0}; +static coordpair_t XYstart = {0}; static double tstart = 0.; // convert Xe/Ye to approximate motor coordinates: // Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have // Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0 -static coords_t XYcor = {0}; +static coordval_pair_t XYcor = {0}; /** * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders @@ -41,20 +41,20 @@ static coords_t XYcor = {0}; * @param XY0 - starting point * @return FALSE if failed */ -int init_traectory(traectory_fn f, coords_t *XY0){ +int init_traectory(traectory_fn f, coordpair_t *XY0){ if(!f || !XY0) return FALSE; cur_traectory = f; XYstart = *XY0; - tstart = sl_dtime(); + tstart = Mount.currentT(); mountdata_t mdata; int ntries = 0; for(; ntries < 10; ++ntries){ if(MCC_E_OK == Mount.getMountData(&mdata)) break; } if(ntries == 10) return FALSE; - XYcor.X = mdata.motposition.X - mdata.encposition.X; - XYcor.Y = mdata.motposition.X + mdata.encposition.Y; - DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y)); + XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val; + XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val; + DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val)); return TRUE; } @@ -64,12 +64,10 @@ int init_traectory(traectory_fn f, coords_t *XY0){ * @param t - UNIX-time of event * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180) */ -int traectory_point(coords_t *nextpt, double t){ +int traectory_point(coordpair_t *nextpt, double t){ if(t < 0. || !cur_traectory) return FALSE; - coords_t pt; + coordpair_t pt; if(!cur_traectory(&pt, t)) return FALSE; - pt.msrtime.tv_sec = floor(t); - pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec; if(nextpt) *nextpt = pt; if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; return TRUE; @@ -77,24 +75,25 @@ int traectory_point(coords_t *nextpt, double t){ // current telescope position according to starting motor coordinates // @return FALSE if failed to get current coordinates -int telpos(coords_t *curpos){ +int telpos(coordval_pair_t *curpos){ mountdata_t mdata; int ntries = 0; for(; ntries < 10; ++ntries){ if(MCC_E_OK == Mount.getMountData(&mdata)) break; } if(ntries == 10) return FALSE; - coords_t pt; - pt.X = XYcor.X + mdata.encposition.X; - pt.Y = XYcor.Y + mdata.encposition.Y; - pt.msrtime = mdata.encposition.msrtime; + coordval_pair_t pt; + pt.X.val = XYcor.X.val + mdata.encXposition.val; + pt.Y.val = XYcor.Y.val + mdata.encYposition.val; + pt.X.t = mdata.encXposition.t; + pt.Y.t = mdata.encYposition.t; if(curpos) *curpos = pt; return TRUE; } // X=X0+1'/s, Y=Y0+15''/s -int Linear(coords_t *nextpt, double t){ - coords_t pt; +int Linear(coordpair_t *nextpt, double t){ + coordpair_t pt; pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart); pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); if(nextpt) *nextpt = pt; @@ -102,8 +101,8 @@ int Linear(coords_t *nextpt, double t){ } // X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi) -int SinCos(coords_t *nextpt, double t){ - coords_t pt; +int SinCos(coordpair_t *nextpt, double t){ + coordpair_t pt; pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI); pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI); if(nextpt) *nextpt = pt; diff --git a/LibSidServo/examples/traectories.h b/LibSidServo/examples/traectories.h index cfd9b2c..513eae6 100644 --- a/LibSidServo/examples/traectories.h +++ b/LibSidServo/examples/traectories.h @@ -21,12 +21,12 @@ #include "sidservo.h" // traectory -typedef int (*traectory_fn)(coords_t *, double); +typedef int (*traectory_fn)(coordpair_t *, double); -int init_traectory(traectory_fn f, coords_t *XY0); +int init_traectory(traectory_fn f, coordpair_t *XY0); traectory_fn traectory_by_name(const char *name); void print_tr_names(); -int traectory_point(coords_t *nextpt, double t); -int telpos(coords_t *curpos); -int Linear(coords_t *nextpt, double t); -int SinCos(coords_t *nextpt, double t); +int traectory_point(coordpair_t *nextpt, double t); +int telpos(coordval_pair_t *curpos); +int Linear(coordpair_t *nextpt, double t); +int SinCos(coordpair_t *nextpt, double t); diff --git a/LibSidServo/libsidservo.config b/LibSidServo/libsidservo.config index 33039ab..a8b7161 100644 --- a/LibSidServo/libsidservo.config +++ b/LibSidServo/libsidservo.config @@ -1,6 +1,6 @@ // Add predefined macros for your project here. For example: // #define THE_ANSWER 42 #define EBUG -#define _POSIX_C_SOURCE +// #define _POSIX_C_SOURCE 111 #define PACKAGE_VERSION "0.0.1" diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 4edd319..1b41ef1 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -13,8 +13,8 @@ ProjectExplorer.Project.EditorSettings + true true - false true Cpp @@ -94,6 +94,7 @@ ProjectExplorer.Project.Target.0 Desktop + true Desktop Desktop {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} @@ -135,6 +136,38 @@ По умолчанию GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 1 diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 18d928c..3d32e92 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -6,6 +6,7 @@ examples/conf.h examples/dump.c examples/dump.h examples/dumpmoving.c +examples/dumpmoving_dragNtrack.c examples/dumpmoving_scmd.c examples/dumpswing.c examples/goto.c diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 2d3a7aa..2a80709 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -66,6 +66,10 @@ static mcc_errcodes_t init(conf_t *c){ DBG("Bad value of MountReqInterval"); ret = MCC_E_BADFORMAT; } + if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ + DBG("Wrong speed interval"); + ret = MCC_E_BADFORMAT; + } uint8_t buf[1024]; data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; // read input data as there may be some trash on start @@ -93,6 +97,12 @@ static int chkYs(double s){ return TRUE; } +static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ + (void)target; + (void)flags; + return MCC_E_FAILED; +} + /** * @brief move2 - simple move to given point and stop @@ -144,7 +154,7 @@ static mcc_errcodes_t setspeed(const double *X, const double *Y){ * @param speed (i) - speed or NULL * @return */ -static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){ +static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ if(!target && !speed) return MCC_E_BADFORMAT; if(!target) return setspeed(&speed->X, &speed->Y); if(!speed) return move2(&target->X, &target->Y); @@ -351,6 +361,7 @@ mount_t Mount = { .init = init, .quit = quit, .getMountData = getMD, + .slewTo = slew2, .moveTo = move2, .moveWspeed = move2s, .setSpeed = setspeed, @@ -360,4 +371,5 @@ mount_t Mount = { .longCmd = longcmd, .getHWconfig = get_hwconf, .saveHWconfig = write_hwconf, + .currentT = dtime, }; diff --git a/LibSidServo/movingfilter.c b/LibSidServo/movingfilter.c- similarity index 100% rename from LibSidServo/movingfilter.c rename to LibSidServo/movingfilter.c- diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index ab6b8cf..62fa57b 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -37,13 +37,15 @@ static int encfd[2] = {-1, -1}, mntfd = -1; // main mount data static mountdata_t mountdata = {0}; +// last encoders time and last encoders data - for speed measurement +static coordval_t lastXenc = {0}, lastYenc = {0}; // mutexes for RW operations with mount device and data static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, datamutex = 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 +// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` static struct timeval encRtmout = {0}, mntRtmout = {0}; // encoders raw data typedef struct __attribute__((packed)){ @@ -64,7 +66,13 @@ double dtime(){ t = tv.tv_sec + ((double)tv.tv_usec)/1e6; return t; } - +#if 0 +double tv2d(struct timeval *tv){ + if(!tv) return 0.; + double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6; + return t; +} +#endif #if 0 // init start time static void gttime(){ @@ -75,12 +83,26 @@ static void gttime(){ } #endif +// calculate current X/Y speeds +static void getXspeed(double t){ + mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t); + mountdata.encXspeed.t = (lastXenc.t + t) / 2.; + lastXenc.val = mountdata.encXposition.val; + lastXenc.t = t; +} +static void getYspeed(double t){ + mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t); + mountdata.encYspeed.t = (lastYenc.t + t) / 2.; + lastYenc.val = mountdata.encYposition.val; + lastYenc.t = t; +} + /** - * @brief parse_encbuf - check encoder buffer (for separate encoder) and fill fresh data + * @brief parse_encbuf - check encoder buffer (for encoder data based on SSII proto) and fill fresh data * @param databuf - input buffer with 13 bytes of data - * @param nexttime - time when databuf[0] got + * @param t - time when databuf[0] got */ -static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ +static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ enc_t *edata = (enc_t*) databuf; /* #ifdef EBUG @@ -115,9 +137,13 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ return; } pthread_mutex_lock(&datamutex); - mountdata.encposition.X = X_ENC2RAD(edata->encX); - mountdata.encposition.Y = Y_ENC2RAD(edata->encY); - mountdata.encposition.msrtime = *tv; + mountdata.encXposition.val = X_ENC2RAD(edata->encX); + mountdata.encYposition.val = Y_ENC2RAD(edata->encY); + DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); + mountdata.encXposition.t = t; + mountdata.encYposition.t = t; + if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t); + if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t); 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); } @@ -126,10 +152,10 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ * @brief getencval - get uint64_t data from encoder * @param fd - encoder fd * @param val - value read - * @param tv - measurement time + * @param t - measurement time * @return amount of data read or 0 if problem */ -static int getencval(int fd, double *val, struct timeval *tv){ +static int getencval(int fd, double *val, double *t){ if(fd < 0) return FALSE; char buf[128]; int got = 0, Lmax = 127; @@ -152,7 +178,7 @@ static int getencval(int fd, double *val, struct timeval *tv){ buf[got] = 0; } else continue; if(strchr(buf, '\n')) break; - }while(Lmax && dtime() - t0 > Conf.EncoderReqInterval); + }while(Lmax && dtime() - t0 < Conf.EncoderReqInterval); if(got == 0) return 0; // WTF? char *estr = strrchr(buf, '\n'); if(!estr) return 0; @@ -167,7 +193,7 @@ static int getencval(int fd, double *val, struct timeval *tv){ return 0; // wrong number } if(val) *val = (double) data; - if(tv) gettimeofday(tv, NULL); + if(t) *t = t0; return got; } // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected @@ -175,11 +201,10 @@ static int getencbyte(){ if(encfd[0] < 0) return -1; uint8_t byte = 0; fd_set rfds; - struct timeval tv; do{ FD_ZERO(&rfds); FD_SET(encfd[0], &rfds); - tv = encRtmout; + struct timeval tv = encRtmout; int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv); if(!retval) break; if(retval < 0){ @@ -199,7 +224,6 @@ static int getmntbyte(){ if(mntfd < 0) return -1; uint8_t byte; fd_set rfds; - struct timeval tv; /* ssize_t l = read(mntfd, &byte, 1); //DBG("MNT read=%zd byte=0x%X", l, byte); if(l == 0) return -1; @@ -208,7 +232,7 @@ static int getmntbyte(){ do{ FD_ZERO(&rfds); FD_SET(mntfd, &rfds); - tv = mntRtmout; + struct timeval tv = mntRtmout; int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); if(retval < 0){ if(errno == EINTR) continue; @@ -231,7 +255,7 @@ static void *encoderthread1(void _U_ *u){ if(Conf.SepEncoder != 1) return NULL; uint8_t databuf[ENC_DATALEN]; int wridx = 0, errctr = 0; - struct timeval tv; + double t = 0.; while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ int b = getencbyte(); if(b == -2) ++errctr; @@ -242,12 +266,12 @@ static void *encoderthread1(void _U_ *u){ if((uint8_t)b == ENC_MAGICK){ // DBG("Got magic -> start filling packet"); databuf[wridx++] = (uint8_t) b; - gettimeofday(&tv, NULL); + t = dtime(); } continue; }else databuf[wridx++] = (uint8_t) b; if(wridx == ENC_DATALEN){ - parse_encbuf(databuf, &tv); + parse_encbuf(databuf, t); wridx = 0; } } @@ -258,30 +282,47 @@ static void *encoderthread1(void _U_ *u){ return NULL; } +// 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"); int errctr = 0; double t0 = dtime(); const char *req = "next\n"; + int need2ask = 0; // need or not to ask encoder for new data while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ - if(5 != write(encfd[0], req, 5)) ++errctr; - if(5 != write(encfd[1], req, 5)) ++errctr; - double v; - struct timeval tv; - if(getencval(encfd[0], &v, &tv)){ - mountdata.encposition.X = X_ENC2RAD(v); - mountdata.encposition.msrtime = tv; - if(getencval(encfd[1], &v, &tv)){ - mountdata.encposition.Y = Y_ENC2RAD(v); - mountdata.encposition.msrtime = tv; + if(need2ask){ + if(5 != write(encfd[0], req, 5)) { ++errctr; continue; } + else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; } + } + double v, t; + if(getencval(encfd[0], &v, &t)){ + mountdata.encXposition.val = X_ENC2RAD(v); + //DBG("encX(%g) = %g", t, mountdata.encXposition.val); + mountdata.encXposition.t = t; + if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t); + if(getencval(encfd[1], &v, &t)){ + mountdata.encYposition.val = Y_ENC2RAD(v); + //DBG("encY(%g) = %g", t, mountdata.encYposition.val); + mountdata.encYposition.t = t; + if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t); errctr = 0; - } else ++errctr; - } else ++errctr; + need2ask = 0; + } else { + if(need2ask) ++errctr; + else need2ask = 1; + continue; + } + } else { + if(need2ask) ++errctr; + else need2ask = 1; + continue; + } while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); } //DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval); t0 = dtime(); } + DBG("ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ close(encfd[i]); @@ -325,8 +366,7 @@ static void *mountthread(void _U_ *u){ */ while(mntfd > -1 && errctr < MAX_ERR_CTR){ // read data to status - struct timeval tgot; - if(0 != gettimeofday(&tgot, NULL)) continue; + double tgot = dtime(); if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ #ifdef EBUG DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); @@ -342,7 +382,7 @@ static void *mountthread(void _U_ *u){ errctr = 0; pthread_mutex_lock(&datamutex); // now change data - SSconvstat(status, &mountdata, &tgot); + SSconvstat(status, &mountdata, tgot); pthread_mutex_unlock(&datamutex); // allow writing & getters //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 8e01bfb..1a893f8 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -27,6 +27,11 @@ extern "C" #include #include +// max speed interval, seconds +#define MCC_CONF_MAX_SPEEDINT (2.) +// minimal speed interval in parts of EncoderReqInterval +#define MCC_CONF_MIN_SPEEDC (3.) + // error codes typedef enum{ MCC_E_OK = 0, // all OK @@ -47,13 +52,24 @@ typedef struct{ char* EncoderYDevPath; double MountReqInterval; // interval between subsequent mount requests (seconds) double EncoderReqInterval; // interval between subsequent encoder requests (seconds) - ; + double EncoderSpeedInterval;// interval between speed calculations } conf_t; -// coordinates in degrees: X, Y and time when they were reached +// coordinates/speeds in degrees or d/s: X, Y typedef struct{ - double X; double Y; struct timeval msrtime; -} coords_t; + double X; double Y; +} coordpair_t; + +// coordinate/speed and time of last measurement +typedef struct{ + double val; + double t; +} coordval_t; + +typedef struct{ + coordval_t X; + coordval_t Y; +} coordval_pair_t; // data to read/write typedef struct{ @@ -80,8 +96,8 @@ typedef struct{ /* If 1, we are in computerless Slew and Track mode (no clutches; use handpad to slew; must be in Drag and Track mode too) */ uint8_t slewtrack :1; - uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync - uint8_t digin :3; // Digital input from radio handpad receiver + uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync + uint8_t digin :3; // Digital input from radio handpad receiver } ybits_t; typedef struct{ @@ -93,9 +109,13 @@ typedef struct{ } extradata_t; typedef struct{ - coords_t motposition; - coords_t encposition; - coords_t lastmotposition; + coordval_t motXposition; + coordval_t motYposition; + coordval_t encXposition; + coordval_t encYposition; + // TODO: add speedX/Y + coordval_t encXspeed; // once per s + coordval_t encYspeed; uint8_t keypad; extradata_t extradata; uint32_t millis; @@ -168,13 +188,22 @@ typedef struct{ double backlspd; // Backlash speed (rad/s) } hardware_configuration_t; +// flags for slew function +typedef struct{ + uint32_t slewNguide : 1; // ==1 to gude after slewing +} slewflags_t; + // mount class typedef struct{ + // TODO: on init/quit clear all XY-bits to default` mcc_errcodes_t (*init)(conf_t *c); // init device void (*quit)(); // deinit mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data + // TODO: change (or add flags) switching slew-and-stop and slew-and-track + // add mount state: stop/slew/guide + mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); mcc_errcodes_t (*moveTo)(const double *X, const double *Y); // move to given position ans stop - mcc_errcodes_t (*moveWspeed)(const coords_t *target, const coords_t *speed); // move with given max speed + mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed mcc_errcodes_t (*setSpeed)(const double *X, const double *Y); // set speed mcc_errcodes_t (*stop)(); // stop mcc_errcodes_t (*emergStop)(); // emergency stop @@ -182,6 +211,7 @@ typedef struct{ mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration + double (*currentT)(); // current time } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 6a04241..30345d5 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -37,11 +37,12 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ /** * @brief SSconvstat - convert stat from SSII format to human - * @param status (i) - just read data - * @param mountdata (o) - output + * @param s (i) - just read data + * @param m (o) - output + * @param t - measurement time */ -void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){ - if(!s || !m || !tdat) return; +void SSconvstat(const SSstat *s, mountdata_t *m, double t){ + if(!s || !m) return; /* #ifdef EBUG static double t0 = -1.; @@ -49,18 +50,18 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){ #endif DBG("Convert, t=%g", dtime()-t0); */ - m->motposition.X = X_MOT2RAD(s->Xmot); - m->motposition.Y = Y_MOT2RAD(s->Ymot); - m->motposition.msrtime = *tdat; + m->motXposition.val = X_MOT2RAD(s->Xmot); + m->motYposition.val = Y_MOT2RAD(s->Ymot); + m->motXposition.t = m->motYposition.t = t; // fill encoder data from here, as there's no separate enc thread if(!Conf.SepEncoder){ - m->encposition.X = X_ENC2RAD(s->Xenc); - m->encposition.Y = Y_ENC2RAD(s->Yenc); - m->encposition.msrtime = *tdat; + m->encXposition.val = X_ENC2RAD(s->Xenc); + m->encYposition.val = Y_ENC2RAD(s->Yenc); + m->encXposition.t = m->encYposition.t = t; } - m->lastmotposition.X = X_MOT2RAD(s->XLast); - m->lastmotposition.Y = Y_MOT2RAD(s->YLast); - m->lastmotposition.msrtime = *tdat; + //m->lastmotposition.X = X_MOT2RAD(s->XLast); + //m->lastmotposition.Y = Y_MOT2RAD(s->YLast); + //m->lastmotposition.msrtime = *tdat; m->keypad = s->keypad; m->extradata.ExtraBits = s->ExtraBits; m->extradata.ain0 = s->ain0; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 181113d..8ea4e5f 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -169,7 +169,9 @@ #define SITECH_LOOP_FREQUENCY (1953.) // steps per revolution +// 13312000 / 4 = 3328000 #define X_MOT_STEPSPERREV (3325952.) +// 17578668 / 4 = 4394667 #define Y_MOT_STEPSPERREV (4394960.) // maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y @@ -199,9 +201,12 @@ // encoder per revolution #define X_ENC_STEPSPERREV (67108864.) #define Y_ENC_STEPSPERREV (67108864.) +// encoder zero position +#define X_ENC_ZERO (46033555) +#define Y_ENC_ZERO (36674010) // encoder position to radians and back -#define X_ENC2RAD(n) (2.*M_PI * ((double)n) / X_ENC_STEPSPERREV) -#define Y_ENC2RAD(n) (2.*M_PI * ((double)n) / Y_ENC_STEPSPERREV) +#define X_ENC2RAD(n) (2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) (2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) #define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) #define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) @@ -304,7 +309,7 @@ typedef struct{ } __attribute__((packed)) SSconfig; uint16_t SScalcChecksum(uint8_t *buf, int len); -void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat); +void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t); int SStextcmd(const char *cmd, data_t *answer); int SSrawcmd(const char *cmd, data_t *answer); int SSgetint(const char *cmd, int64_t *ans);