From 7634856967fa1d0918e706edb1dbde585d29f0a2 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Wed, 23 Jul 2025 00:01:33 +0300 Subject: [PATCH] refactoring --- Auxiliary_utils/LibSidServo/TODO | 11 + Auxiliary_utils/LibSidServo/examples/conf.c | 13 +- Auxiliary_utils/LibSidServo/examples/dump.c | 99 +++++-- Auxiliary_utils/LibSidServo/examples/dump.h | 2 +- .../LibSidServo/examples/dumpmoving.c | 6 +- .../examples/dumpmoving_dragNtrack.c | 170 ++++++++++++ .../LibSidServo/examples/dumpmoving_scmd.c | 46 ++-- .../LibSidServo/examples/dumpswing.c | 20 +- Auxiliary_utils/LibSidServo/examples/goto.c | 20 +- .../LibSidServo/examples/scmd_traectory.c | 19 +- .../LibSidServo/examples/servo.conf | 3 +- .../LibSidServo/examples/traectories.c | 39 ++- .../LibSidServo/examples/traectories.h | 12 +- .../LibSidServo/libsidservo.config | 2 +- .../LibSidServo/libsidservo.creator.user | 37 ++- Auxiliary_utils/LibSidServo/libsidservo.files | 1 + Auxiliary_utils/LibSidServo/main.c | 156 +++++++++-- .../{movingfilter.c => movingfilter.c-} | 0 Auxiliary_utils/LibSidServo/serial.c | 242 ++++++++++++++---- Auxiliary_utils/LibSidServo/serial.h | 4 +- Auxiliary_utils/LibSidServo/sidservo.h | 64 +++-- Auxiliary_utils/LibSidServo/ssii.c | 27 +- Auxiliary_utils/LibSidServo/ssii.h | 42 +-- 23 files changed, 801 insertions(+), 234 deletions(-) create mode 100644 Auxiliary_utils/LibSidServo/TODO create mode 100644 Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c rename Auxiliary_utils/LibSidServo/{movingfilter.c => movingfilter.c-} (100%) diff --git a/Auxiliary_utils/LibSidServo/TODO b/Auxiliary_utils/LibSidServo/TODO new file mode 100644 index 0000000..c636b16 --- /dev/null +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/conf.c b/Auxiliary_utils/LibSidServo/examples/conf.c index 428bec4..1969a0f 100644 --- a/Auxiliary_utils/LibSidServo/examples/conf.c +++ b/Auxiliary_utils/LibSidServo/examples/conf.c @@ -24,10 +24,13 @@ static conf_t Config = { .MountDevPath = "/dev/ttyUSB0", .MountDevSpeed = 19200, - .EncoderDevPath = "/dev/ttyUSB1", + .EncoderXDevPath = "/dev/encoderX0", + .EncoderYDevPath = "/dev/encoderY0", .EncoderDevSpeed = 153000, .MountReqInterval = 0.1, - .SepEncoder = 1 + .EncoderReqInterval = 0.05, + .SepEncoder = 2, + .EncoderSpeedInterval = 0.1, }; static sl_option_t opts[] = { @@ -36,7 +39,11 @@ static sl_option_t opts[] = { {"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"}, {"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"}, {"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"}, - {"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device"}, + {"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"}, + {"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/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c index 604d1ca..a7d9ef6 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/dump.h b/Auxiliary_utils/LibSidServo/examples/dump.h index 414473c..728dd27 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.h +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/dumpmoving.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving.c index 323e310..ee7f33f 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c new file mode 100644 index 0000000..006aabd --- /dev/null +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c index ec9a029..bfb22ae 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c +++ b/Auxiliary_utils/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"}, @@ -77,8 +77,8 @@ static void *dumping(void _U_ *u){ return NULL; } -// return TRUE if motor position is reached +- 0.1 degrees -#define XYcount (DEG2RAD(0.1)) +// 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)); @@ -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.); @@ -104,27 +104,26 @@ static int Wait(double tag){ 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){ -#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); - 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 } @@ -160,21 +159,18 @@ int main(int argc, char **argv){ pthread_t dthr; logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); - // goto 1 degr with 1'/s - move(10., 1., 1./60.); - // goto 2 degr with 2'/s - move(10., 2., 2./60.); - // goto 3 degr with 5'/s - move(10., 3., 5./60.); - // goto 4 degr with 10'/s - move(10., 4., 10./60.); - // and go back with 5deg/s - move(0., 0., 5.); - // be sure to move @ 0,0 - Mount.moveTo(&M.X, &M.Y); + // goto 30' with 5'/s + move(10., 30./60., 5./60.); + // goto 1' with 10'/s + move(10., 1., 10./60.); + // goto 3degr with 15'/s + move(10., 3., 15./60.); + // and go back with 7deg/s + move(0., 0., 7.); + // 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/Auxiliary_utils/LibSidServo/examples/dumpswing.c b/Auxiliary_utils/LibSidServo/examples/dumpswing.c index 73c50e4..8425995 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpswing.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c index 3da2b9a..24255aa 100644 --- a/Auxiliary_utils/LibSidServo/examples/goto.c +++ b/Auxiliary_utils/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,18 +100,22 @@ 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); } printf("Moving to "); if(xtag) printf("X=%gdeg ", G.X); @@ -122,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/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c index 00dacd2..621f6f5 100644 --- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/servo.conf b/Auxiliary_utils/LibSidServo/examples/servo.conf index d37e959..dfe0a25 100644 --- a/Auxiliary_utils/LibSidServo/examples/servo.conf +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/traectories.c b/Auxiliary_utils/LibSidServo/examples/traectories.c index decd645..1977880 100644 --- a/Auxiliary_utils/LibSidServo/examples/traectories.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/examples/traectories.h b/Auxiliary_utils/LibSidServo/examples/traectories.h index cfd9b2c..513eae6 100644 --- a/Auxiliary_utils/LibSidServo/examples/traectories.h +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/libsidservo.config b/Auxiliary_utils/LibSidServo/libsidservo.config index 33039ab..a8b7161 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.config +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index dff1f3d..1b41ef1 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files index 18d928c..3d32e92 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.files +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index 788ad86..2a80709 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -49,16 +49,16 @@ static mcc_errcodes_t init(conf_t *c){ if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; - }else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){ + }else if(!openMount()){ DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed); ret = MCC_E_MOUNTDEV; } if(Conf.SepEncoder){ - if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){ - DBG("Define encoder device path and speed"); + if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ + DBG("Define encoder device path"); ret = MCC_E_BADFORMAT; - }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){ - DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed); + }else if(!openEncoder()){ + DBG("Can't open encoder device"); ret = MCC_E_ENCODERDEV; } } @@ -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); @@ -152,10 +162,10 @@ static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){ return MCC_E_BADFORMAT; char buf[128]; int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X); - snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTX, tag, CMD_MOTXYS, spd); + snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTX, tag, CMD_MOTXYS, spd); if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y); - snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTY, tag, CMD_MOTXYS, spd); + snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTY, tag, CMD_MOTXYS, spd); if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; return MCC_E_OK; } @@ -230,20 +240,119 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){ return MCC_E_OK; } -mcc_errcodes_t get_hwconf(hardware_configuration_t *c){ - if(!c) return MCC_E_BADFORMAT; - SSconfig conf; - if(!cmdC(&conf, FALSE)) return MCC_E_FAILED; - // and bored transformations - DBG("Xacc=%u", conf.Xconf.accel); - DBG("Yacc=%u", conf.Yconf.accel); - c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel); - DBG("cacc: %g", c->Xconf.accel); - c->Xconf.backlash = conf.Xconf.backlash; - // ... - c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel); - c->Xconf.backlash = conf.Xconf.backlash; - // ... +static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ + if(!hwConfig) return MCC_E_BADFORMAT; + SSconfig config; + if(!cmdC(&config, FALSE)) return MCC_E_FAILED; + // Convert acceleration (ticks per loop^2 to rad/s^2) + hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel); + hwConfig->Yconf.accel = Y_MOTACC2RS(config.Yconf.accel); + // Convert backlash (ticks to radians) + hwConfig->Xconf.backlash = X_MOT2RAD(config.Xconf.backlash); + hwConfig->Yconf.backlash = Y_MOT2RAD(config.Yconf.backlash); + // Convert error limit (ticks to radians) + hwConfig->Xconf.errlimit = X_MOT2RAD(config.Xconf.errlimit); + hwConfig->Yconf.errlimit = Y_MOT2RAD(config.Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + hwConfig->Xconf.propgain = (double)config.Xconf.propgain; + hwConfig->Yconf.propgain = (double)config.Yconf.propgain; + hwConfig->Xconf.intgain = (double)config.Xconf.intgain; + hwConfig->Yconf.intgain = (double)config.Yconf.intgain; + hwConfig->Xconf.derivgain = (double)config.Xconf.derivgain; + hwConfig->Yconf.derivgain = (double)config.Yconf.derivgain; + // Output limit is a percentage (0-100) + hwConfig->Xconf.outplimit = (double)config.Xconf.outplimit / 255.0 * 100.0; + hwConfig->Yconf.outplimit = (double)config.Yconf.outplimit / 255.0 * 100.0; + // Current limit in amps + hwConfig->Xconf.currlimit = (double)config.Xconf.currlimit / 100.0; + hwConfig->Yconf.currlimit = (double)config.Yconf.currlimit / 100.0; + // Integral limit is unitless + hwConfig->Xconf.intlimit = (double)config.Xconf.intlimit; + hwConfig->Yconf.intlimit = (double)config.Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + hwConfig->xbits = config.xbits; + hwConfig->ybits = config.ybits; + // Copy address + hwConfig->address = config.address; + + // TODO: What to do with eqrate, eqadj and trackgoal? + + // Convert latitude (degrees * 100 to radians) + hwConfig->latitude = (double)config.latitude / 100.0 * M_PI / 180.0; + // Copy ticks per revolution + hwConfig->Xsetpr = config.Xsetpr; + hwConfig->Ysetpr = config.Ysetpr; + hwConfig->Xmetpr = config.Xmetpr; + hwConfig->Ymetpr = config.Ymetpr; + // Convert slew rates (ticks per loop to rad/s) + hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); + hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); + // Convert pan rates (ticks per loop to rad/s) + hwConfig->Xpanrate = X_MOTSPD2RS(config.Xpanrate); + hwConfig->Ypanrate = Y_MOTSPD2RS(config.Ypanrate); + // Convert guide rates (ticks per loop to rad/s) + hwConfig->Xguiderate = X_MOTSPD2RS(config.Xguiderate); + hwConfig->Yguiderate = Y_MOTSPD2RS(config.Yguiderate); + // copy baudrate + hwConfig->baudrate = (uint32_t) config.baudrate; + // Convert local search degrees (degrees * 100 to radians) + hwConfig->locsdeg = (double)config.locsdeg / 100.0 * M_PI / 180.0; + // Convert local search speed (arcsec per second to rad/s) + hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0); + // Convert backlash speed (ticks per loop to rad/s) + hwConfig->backlspd = X_MOTSPD2RS(config.backlspd); + return MCC_E_OK; +} + +static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ + SSconfig config; + // Convert acceleration (rad/s^2 to ticks per loop^2) + config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel); + config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel); + // Convert backlash (radians to ticks) + config.Xconf.backlash = X_RAD2MOT(hwConfig->Xconf.backlash); + config.Yconf.backlash = Y_RAD2MOT(hwConfig->Yconf.backlash); + // Convert error limit (radians to ticks) + config.Xconf.errlimit = X_RAD2MOT(hwConfig->Xconf.errlimit); + config.Yconf.errlimit = Y_RAD2MOT(hwConfig->Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + config.Xconf.propgain = (uint16_t)hwConfig->Xconf.propgain; + config.Yconf.propgain = (uint16_t)hwConfig->Yconf.propgain; + config.Xconf.intgain = (uint16_t)hwConfig->Xconf.intgain; + config.Yconf.intgain = (uint16_t)hwConfig->Yconf.intgain; + config.Xconf.derivgain = (uint16_t)hwConfig->Xconf.derivgain; + config.Yconf.derivgain = (uint16_t)hwConfig->Yconf.derivgain; + // Output limit is a percentage (0-100), so convert back to 0-255 + config.Xconf.outplimit = (uint8_t)(hwConfig->Xconf.outplimit / 100.0 * 255.0); + config.Yconf.outplimit = (uint8_t)(hwConfig->Yconf.outplimit / 100.0 * 255.0); + // Current limit is in amps (convert back to *100) + config.Xconf.currlimit = (uint16_t)(hwConfig->Xconf.currlimit * 100.0); + config.Yconf.currlimit = (uint16_t)(hwConfig->Yconf.currlimit * 100.0); + // Integral limit is unitless, so no conversion needed + config.Xconf.intlimit = (uint16_t)hwConfig->Xconf.intlimit; + config.Yconf.intlimit = (uint16_t)hwConfig->Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + config.xbits = hwConfig->xbits; + config.ybits = hwConfig->ybits; + // Convert latitude (radians to degrees * 100) + config.latitude = (uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0); + // Convert slew rates (rad/s to ticks per loop) + config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate); + config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate); + // Convert pan rates (rad/s to ticks per loop) + config.Xpanrate = X_RS2MOTSPD(hwConfig->Xpanrate); + config.Ypanrate = Y_RS2MOTSPD(hwConfig->Ypanrate); + // Convert guide rates (rad/s to ticks per loop) + config.Xguiderate = X_RS2MOTSPD(hwConfig->Xguiderate); + config.Yguiderate = Y_RS2MOTSPD(hwConfig->Yguiderate); + // Convert local search degrees (radians to degrees * 100) + config.locsdeg = (uint32_t)(hwConfig->locsdeg * 180.0 / M_PI * 100.0); + // Convert local search speed (rad/s to arcsec per second) + config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI); + // Convert backlash speed (rad/s to ticks per loop) + config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); + // TODO - next + (void) config; return MCC_E_OK; } @@ -252,6 +361,7 @@ mount_t Mount = { .init = init, .quit = quit, .getMountData = getMD, + .slewTo = slew2, .moveTo = move2, .moveWspeed = move2s, .setSpeed = setspeed, @@ -260,4 +370,6 @@ mount_t Mount = { .shortCmd = shortcmd, .longCmd = longcmd, .getHWconfig = get_hwconf, + .saveHWconfig = write_hwconf, + .currentT = dtime, }; diff --git a/Auxiliary_utils/LibSidServo/movingfilter.c b/Auxiliary_utils/LibSidServo/movingfilter.c- similarity index 100% rename from Auxiliary_utils/LibSidServo/movingfilter.c rename to Auxiliary_utils/LibSidServo/movingfilter.c- diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index 3f4b823..62fa57b 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -34,16 +34,18 @@ #include "serial.h" // serial devices FD -static int encfd = -1, mntfd = -1; +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,31 +137,82 @@ 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); } +/** + * @brief getencval - get uint64_t data from encoder + * @param fd - encoder fd + * @param val - value read + * @param t - measurement time + * @return amount of data read or 0 if problem + */ +static int getencval(int fd, double *val, double *t){ + if(fd < 0) return FALSE; + char buf[128]; + int got = 0, Lmax = 127; + double t0 = dtime(); + do{ + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + struct timeval tv = encRtmout; + int retval = select(fd + 1, &rfds, NULL, NULL, &tv); + if(!retval) continue; + if(retval < 0){ + if(errno == EINTR) continue; + return 0; + } + if(FD_ISSET(fd, &rfds)){ + ssize_t l = read(fd, &buf[got], Lmax); + if(l < 1) return 0; // disconnected ?? + got += l; Lmax -= l; + buf[got] = 0; + } else continue; + if(strchr(buf, '\n')) break; + }while(Lmax && dtime() - t0 < Conf.EncoderReqInterval); + if(got == 0) return 0; // WTF? + char *estr = strrchr(buf, '\n'); + if(!estr) return 0; + *estr = 0; + char *bgn = strrchr(buf, '\n'); + if(bgn) ++bgn; + else bgn = buf; + char *eptr; + long data = strtol(bgn, &eptr, 10); + if(eptr != estr){ + DBG("NAN"); + return 0; // wrong number + } + if(val) *val = (double) data; + 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 static int getencbyte(){ - if(encfd < 0) return -1; + if(encfd[0] < 0) return -1; uint8_t byte = 0; fd_set rfds; - struct timeval tv; do{ FD_ZERO(&rfds); - FD_SET(encfd, &rfds); - tv = encRtmout; - int retval = select(encfd + 1, &rfds, NULL, NULL, &tv); + FD_SET(encfd[0], &rfds); + struct timeval tv = encRtmout; + int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv); if(!retval) break; if(retval < 0){ if(errno == EINTR) continue; return -1; } - if(FD_ISSET(encfd, &rfds)){ - ssize_t l = read(encfd, &byte, 1); + if(FD_ISSET(encfd[0], &rfds)){ + ssize_t l = read(encfd[0], &byte, 1); if(l != 1) return -2; // disconnected ?? break; } else return -1; @@ -151,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; @@ -160,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; @@ -179,11 +251,12 @@ static int getmntbyte(){ } // main encoder thread (for separate encoder): read next data and make parsing -static void *encoderthread(void _U_ *u){ +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; - while(encfd > -1 && errctr < MAX_ERR_CTR){ + double t = 0.; + while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ int b = getencbyte(); if(b == -2) ++errctr; if(b < 0) continue; @@ -193,18 +266,68 @@ static void *encoderthread(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; } } - if(encfd > -1){ - close(encfd); - encfd = -1; + if(encfd[0] > -1){ + close(encfd[0]); + encfd[0] = -1; + } + 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(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; + 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]); + encfd[i] = -1; + } } return NULL; } @@ -243,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); @@ -260,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); @@ -293,7 +415,7 @@ static int ttyopen(const char *path, speed_t speed){ //tty.c_cc[VMIN] = 0; // non-canonical mode //tty.c_cc[VTIME] = 5; if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; } - DBG("Check speed"); + DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed); if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; } // try to set exclusive if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");} @@ -301,27 +423,47 @@ static int ttyopen(const char *path, speed_t speed){ } // return FALSE if failed -int openEncoder(const char *path, int speed){ +int openEncoder(){ if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent - if(encfd > -1) close(encfd); - encfd = ttyopen(path, (speed_t) speed); - if(encfd < 0) return FALSE; - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 200000000 / speed; // 20 bytes - if(pthread_create(&encthread, NULL, encoderthread, NULL)){ - close(encfd); - encfd = -1; - return FALSE; - } + 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 = 200000000 / Conf.EncoderDevSpeed; // 20 bytes + if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ + close(encfd[0]); + encfd[0] = -1; + return FALSE; + } + }else if(Conf.SepEncoder == 2){ + DBG("Two devices!"); + const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath}; + for(int i = 0; i < 2; ++i){ + if(encfd[i] > -1) close(encfd[i]); + encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed); + if(encfd[i] < 0) return FALSE; + } + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 1000; // 1ms + if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ + for(int i = 0; i < 2; ++i){ + close(encfd[i]); + encfd[i] = -1; + } + return FALSE; + } + }else return FALSE; DBG("Encoder opened, thread started"); return TRUE; } // return FALSE if failed -int openMount(const char *path, int speed){ +int openMount(){ if(mntfd > -1) close(mntfd); - DBG("Open mount %s @ %d", path, speed); - mntfd = ttyopen(path, (speed_t) speed); + DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); + mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed); if(mntfd < 0) return FALSE; DBG("mntfd=%d", mntfd); // clear buffer @@ -334,7 +476,7 @@ int openMount(const char *path, int speed){ DBG("got %zd", l); }while(1);*/ mntRtmout.tv_sec = 0; - mntRtmout.tv_usec = 500000000 / speed; // 50 bytes + mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes if(pthread_create(&mntthread, NULL, mountthread, NULL)){ DBG("Can't create thread"); close(mntfd); @@ -354,12 +496,16 @@ void closeSerial(){ close(mntfd); mntfd = -1; } - if(encfd > -1){ + if(encfd[0] > -1){ DBG("Kill encoder thread"); pthread_cancel(encthread); DBG("close fd"); - close(encfd); - encfd = -1; + close(encfd[0]); + encfd[0] = -1; + if(Conf.SepEncoder == 2){ + close(encfd[1]); + encfd[1] = -1; + } } } diff --git a/Auxiliary_utils/LibSidServo/serial.h b/Auxiliary_utils/LibSidServo/serial.h index aa2604c..95b5e8d 100644 --- a/Auxiliary_utils/LibSidServo/serial.h +++ b/Auxiliary_utils/LibSidServo/serial.h @@ -31,8 +31,8 @@ double dtime(); data_t *cmd2dat(const char *cmd); void data_free(data_t **x); -int openEncoder(const char *path, int speed); -int openMount(const char *path, int speed); +int openEncoder(); +int openMount(); void closeSerial(); mcc_errcodes_t getMD(mountdata_t *d); int MountWriteRead(const data_t *out, data_t *in); diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index 5f85d50..1a893f8 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/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 @@ -42,15 +47,29 @@ typedef struct{ int MountDevSpeed; // serial speed char* EncoderDevPath; // path to encoder device int EncoderDevSpeed; // serial speed - int SepEncoder; // ==1 if encoder works as separate serial device - double MountReqInterval; // maximal interval between subsequent mount requests (seconds) - ; + int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices + char* EncoderXDevPath; // paths to new controller devices + 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{ @@ -77,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{ @@ -90,18 +109,18 @@ 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; double temperature; double voltage; - int32_t XmotRaw; - int32_t YmotRaw; - int32_t XencRaw; - int32_t YencRaw; } mountdata_t; typedef struct{ @@ -166,22 +185,33 @@ typedef struct{ uint32_t baudrate; // Baud Rate (baud) double locsdeg; // Local Search Degrees (rad) double locsspeed; // Local Search Speed (rad/s) - double backlspd; // Backlash speed (???) + 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 mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command 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/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index 6a04241..30345d5 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/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/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index 866f78f..8ea4e5f 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -165,10 +165,13 @@ // X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits #define CMD_GETSTATTEXT "\r" - +// Loop freq +#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 @@ -176,31 +179,34 @@ #define Y_SPEED_MAX (0.13963) // motor position to radians and back -#define X_MOT2RAD(n) (2.*M_PI * ((double)n) / X_MOT_STEPSPERREV) -#define Y_MOT2RAD(n) (2.*M_PI * ((double)n) / Y_MOT_STEPSPERREV) -#define X_RAD2MOT(r) ((int32_t)((r) / 2./M_PI * X_MOT_STEPSPERREV)) -#define Y_RAD2MOT(r) ((int32_t)((r) / 2./M_PI * Y_MOT_STEPSPERREV)) +#define X_MOT2RAD(n) (2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) +#define Y_MOT2RAD(n) (2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV) +#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV)) +#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV)) // motor speed in rad/s and back -#define X_MOTSPD2RS(n) (X_MOT2RAD(n)/65536.*1953.) -#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r)*65536./1953.)) -#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) -#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.)) +#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) // motor acceleration -//- -#define X_MOTACC2RS(n) (X_MOT2RAD(n)/65536.*1953.*1953.) -#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.)) -#define Y_MOTACC2RS(n) (Y_MOT2RAD(n)/65536.*1953.*1953.) -#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953./1953.)) +#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) +#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) +#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) +#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) // adder time to seconds vice versa -#define ADDER2S(a) ((a)*1953.) -#define S2ADDER(s) ((s)/1953.) +#define ADDER2S(a) ((a) * SITECH_LOOP_FREQUENCY) +#define S2ADDER(s) ((s) / SITECH_LOOP_FREQUENCY) // 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)) @@ -303,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);