diff --git a/Auxiliary_utils/LibSidServo/TODO b/Auxiliary_utils/LibSidServo/TODO index 98925fa..1d2dcb7 100644 --- a/Auxiliary_utils/LibSidServo/TODO +++ b/Auxiliary_utils/LibSidServo/TODO @@ -1,3 +1,3 @@ -1. PID: slew2 -2. add model & config "model ON" - +fix encoders opening for several tries +encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request) +Read HW config even in model mode diff --git a/Auxiliary_utils/LibSidServo/examples/SSIIconf.c b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c index d399116..7980878 100644 --- a/Auxiliary_utils/LibSidServo/examples/SSIIconf.c +++ b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c @@ -34,7 +34,9 @@ typedef struct{ static hardware_configuration_t HW = {0}; -static parameters G = {0}; +static parameters G = { + .conffile = "servo.conf", +}; static sl_option_t cmdlnopts[] = { {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, @@ -53,7 +55,7 @@ static sl_option_t confopts[] = { static void dumpaxis(char axis, axis_config_t *c){ #define STRUCTPAR(p) (c)->p -#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0) +#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0) #define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0) DUMPD(accel); DUMPD(backlash); @@ -64,6 +66,8 @@ static void dumpaxis(char axis, axis_config_t *c){ DUMP(outplimit); DUMP(currlimit); DUMP(intlimit); + DUMP(motor_stepsperrev); + DUMP(axis_stepsperrev); #undef DUMP #undef DUMPD } diff --git a/Auxiliary_utils/LibSidServo/examples/conf.c b/Auxiliary_utils/LibSidServo/examples/conf.c index 9db5981..8a77322 100644 --- a/Auxiliary_utils/LibSidServo/examples/conf.c +++ b/Auxiliary_utils/LibSidServo/examples/conf.c @@ -83,6 +83,8 @@ static sl_option_t opts[] = { {"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"}, {"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"}, {"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"}, + {"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"}, + {"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"}, // {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""}, end_option }; @@ -110,3 +112,17 @@ void dumpConf(){ void confHelp(){ sl_conf_showhelp(-1, opts); } + +const char* errcodes[MCC_E_AMOUNT] = { + [MCC_E_OK] = "OK", + [MCC_E_FATAL] = "Fatal error", + [MCC_E_BADFORMAT] = "Wrong data format", + [MCC_E_ENCODERDEV] = "Encoder error", + [MCC_E_MOUNTDEV] = "Mount error", + [MCC_E_FAILED] = "Failed to run" +}; +// return string with error code +const char *EcodeStr(mcc_errcodes_t e){ + if(e >= MCC_E_AMOUNT) return "Wrong error code"; + return errcodes[e]; +} diff --git a/Auxiliary_utils/LibSidServo/examples/conf.h b/Auxiliary_utils/LibSidServo/examples/conf.h index f15d4ab..062528e 100644 --- a/Auxiliary_utils/LibSidServo/examples/conf.h +++ b/Auxiliary_utils/LibSidServo/examples/conf.h @@ -25,3 +25,4 @@ void confHelp(); conf_t *readServoConf(const char *filename); void dumpConf(); +const char *EcodeStr(mcc_errcodes_t e); diff --git a/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c index 6c5cadc..b17deab 100644 --- a/Auxiliary_utils/LibSidServo/examples/goto.c +++ b/Auxiliary_utils/LibSidServo/examples/goto.c @@ -120,7 +120,11 @@ int main(int _U_ argc, char _U_ **argv){ } printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y); tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y); - Mount.moveTo(&tag); + mcc_errcodes_t e = Mount.moveTo(&tag); + if(MCC_E_OK != e){ + WARNX("Cant go to given coordinates: %s\n", EcodeStr(e)); + goto out; + } if(G.wait){ sleep(1); waitmoving(G.Ncycles); @@ -132,7 +136,9 @@ out: 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.val), RAD2DEG(M.Y.val)); + usleep(250000); // pause to refresh coordinates + if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val), + RAD2DEG(E.X.val), RAD2DEG(E.Y.val)); Mount.quit(); } return 0; diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index d12d482..e6b41cb 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} ProjectExplorer.Project.ActiveTarget @@ -40,9 +40,9 @@ 1 0 false - false + true false - 1 + 0 true true 0 @@ -51,10 +51,10 @@ false 1 true - true + false true *.md, *.MD, Makefile - true + false true true @@ -79,7 +79,7 @@ true true Builtin.DefaultTidyAndClazy - 4 + 8 true @@ -96,12 +96,12 @@ true Desktop Desktop - {91347f2c-5221-46a7-80b1-0a054ca02f79} + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} 0 0 0 - /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + /tmp/robo5/mountcontrol.git/LibSidServo @@ -133,7 +133,7 @@ false - По умолчанию + Default GenericProjectManager.GenericBuildConfiguration 0 0 @@ -168,6 +168,7 @@ true true + %{RunConfig:Executable:Path} 1 @@ -203,6 +204,7 @@ true true + %{RunConfig:Executable:Path} 1 diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index b56811b..a6f04b0 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -25,6 +25,7 @@ #include #include #include +#include #include "main.h" #include "movingmodel.h" @@ -53,6 +54,7 @@ limits_t .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}} ; static mcc_errcodes_t shortcmd(short_command_t *cmd); +static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig); /** * @brief curtime - monotonic time from first run @@ -87,6 +89,7 @@ static int initstarttime(){ curtime(&t0); return TRUE; } + // return difference (in seconds) between time1 and time0 double timediff(const struct timespec *time1, const struct timespec *time0){ if(!time1 || !time0) return -1.; @@ -220,7 +223,18 @@ static mcc_errcodes_t init(conf_t *c){ } if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; - return updateMotorPos(); + // read HW config to update constants + hardware_configuration_t HW; + if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; + // make a pause for actual encoder's values + double t0 = timefromstart(); + while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000); + mcc_errcodes_t e = updateMotorPos(); + // and refresh data after updating + DBG("Wait for next mount reading"); + t0 = timefromstart(); + while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000); + return e; } // check coordinates (rad) and speeds (rad/s); return FALSE if failed @@ -242,7 +256,7 @@ static int chkYs(double s){ return TRUE; } -// set SLEWING state if axis was stopped later +// set SLEWING state if axis was stopped static void setslewingstate(){ //FNAME(); mountdata_t d; @@ -258,19 +272,6 @@ static void setslewingstate(){ }else DBG("CAN't GET MOUNT DATA!"); } -/* -static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ - (void)target; - (void)flags; - //if(Conf.RunModel) return ... ; - if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; - //... - setStat(AXIS_SLEWING, AXIS_SLEWING); - //... - return MCC_E_FAILED; -} -*/ - /** * @brief move2 - simple move to given point and stop * @param X - new X coordinate (radians: -pi..pi) or NULL @@ -287,10 +288,11 @@ static mcc_errcodes_t move2(const coordpair_t *target){ cmd.Ymot = target->Y; cmd.Xspeed = Xlimits.max.speed; cmd.Yspeed = Ylimits.max.speed; - mcc_errcodes_t r = shortcmd(&cmd); + /*mcc_errcodes_t r = shortcmd(&cmd); if(r != MCC_E_OK) return r; setslewingstate(); - return MCC_E_OK; + return MCC_E_OK;*/ + return shortcmd(&cmd); } /** @@ -427,6 +429,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ if(!hwConfig) return MCC_E_BADFORMAT; if(Conf.RunModel) return MCC_E_FAILED; SSconfig config; + DBG("Read HW configuration"); 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); @@ -466,8 +469,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ // Copy ticks per revolution hwConfig->Xsetpr = __bswap_32(config.Xsetpr); hwConfig->Ysetpr = __bswap_32(config.Ysetpr); - hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less - hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4; + hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less + hwConfig->Ymetpr = __bswap_32(config.Ymetpr); // Convert slew rates (ticks per loop to rad/s) hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); @@ -485,6 +488,30 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ 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); + // now read text commands + int64_t i64; + double Xticks, Yticks; + DBG("SERIAL"); + // motor's encoder ticks per rev + if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED; + Xticks = ((double) i64); // divide by 4 as these values stored ??? + if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED; + Yticks = ((double) i64); + X_ENC_ZERO = Conf.XEncZero; + Y_ENC_ZERO = Conf.YEncZero; + DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev); + X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks; + Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks; + DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV); + // axis encoder ticks per rev + if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED; + Xticks = (double) i64; + if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED; + Yticks = (double) i64; + DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev); + X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks; + Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks; + DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV); return MCC_E_OK; } @@ -538,8 +565,9 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); config.Xsetpr = __bswap_32(hwConfig->Xsetpr); config.Ysetpr = __bswap_32(hwConfig->Ysetpr); - config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4); - config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4); + config.Xmetpr = __bswap_32(hwConfig->Xmetpr); + config.Ymetpr = __bswap_32(hwConfig->Ymetpr); + // todo - also write text params // TODO - next (void) config; return MCC_E_OK; diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index 5e040f5..f4cc227 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -48,7 +48,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, // encoders thread and mount thread static pthread_t encthread, mntthread; // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` -static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -131,8 +131,8 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ return; } pthread_mutex_lock(&datamutex); - mountdata.encXposition.val = X_ENC2RAD(edata->encX); - mountdata.encYposition.val = Y_ENC2RAD(edata->encY); + mountdata.encXposition.val = Xenc2rad(edata->encX); + mountdata.encYposition.val = Yenc2rad(edata->encY); DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); mountdata.encXposition.t = *t; mountdata.encYposition.t = *t; @@ -151,32 +151,52 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ * @return amount of data read or 0 if problem */ static int getencval(int fd, double *val, struct timespec *t){ - if(fd < 0) return FALSE; + if(fd < 0){ + DBG("Encoder fd < 0!"); + return FALSE; + } char buf[128]; int got = 0, Lmax = 127; double t0 = timefromstart(); + //DBG("start: %.6g", t0); 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){ + //DBG("select()==0 - timeout, %.6g", timefromstart()); + break; + } if(retval < 0){ - if(errno == EINTR) continue; + if(errno == EINTR){ + DBG("EINTR"); + continue; + } + DBG("select() < 0"); return 0; } if(FD_ISSET(fd, &rfds)){ ssize_t l = read(fd, &buf[got], Lmax); - if(l < 1) return 0; // disconnected ?? + if(l < 1){ + DBG("read() < 0"); + return 0; // disconnected ?? + } got += l; Lmax -= l; buf[got] = 0; } else continue; - if(strchr(buf, '\n')) break; - }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval); - if(got == 0) return 0; // WTF? + if(buf[got-1] == '\n') break; // got EOL as last symbol + }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.); + if(got == 0){ + //DBG("No data from encoder, tfs=%.6g", timefromstart()); + return 0; + } char *estr = strrchr(buf, '\n'); - if(!estr) return 0; + if(!estr){ + DBG("No EOL"); + return 0; + } *estr = 0; char *bgn = strrchr(buf, '\n'); if(bgn) ++bgn; @@ -188,7 +208,7 @@ static int getencval(int fd, double *val, struct timespec *t){ return 0; // wrong number } if(val) *val = (double) data; - if(t){ if(!curtime(t)) return 0; } + if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} return got; } // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected @@ -312,40 +332,56 @@ static void *encoderthread2(void _U_ *u){ int need2ask = 0; // need or not to ask encoder for new data while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ struct timespec t; - if(!curtime(&t)) continue; if(need2ask){ + //DBG("ASK for new data, tfs=%.6g", timefromstart()); if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } + //DBG("OK"); + need2ask = 0; + usleep(100); + } + if(!curtime(&t)){ + DBG("Where is time?"); + continue; } double v; if(getencval(encfd[0], &v, &t)){ pthread_mutex_lock(&datamutex); - mountdata.encXposition.val = X_ENC2RAD(v); + mountdata.encXposition.val = Xenc2rad(v); mountdata.encXposition.t = t; pthread_mutex_unlock(&datamutex); + //DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart()); getXspeed(); if(getencval(encfd[1], &v, &t)){ pthread_mutex_lock(&datamutex); - mountdata.encYposition.val = Y_ENC2RAD(v); + mountdata.encYposition.val = Yenc2rad(v); mountdata.encYposition.t = t; pthread_mutex_unlock(&datamutex); + //DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV); getYspeed(); errctr = 0; need2ask = 0; - } else { - if(need2ask) ++errctr; + //DBG("tgot=%.6g", timefromstart()); + while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } + t0 = timefromstart(); + }else{ + DBG("NO Y DATA!!!"); + /*if(need2ask) ++errctr; else need2ask = 1; - continue; + continue;*/ + ++errctr; + need2ask = 1; } - } else { - if(need2ask) ++errctr; + }else{ // no data - ask for new + //DBG("Need new, tfs=%.6g", timefromstart()); + /*if(need2ask) ++errctr; else need2ask = 1; - continue; + continue;*/ + ++errctr; + need2ask = 1; } - while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } - t0 = timefromstart(); } - DBG("ERRCTR=%d", errctr); + DBG("\n\nEXIT: ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ close(encfd[i]); @@ -480,8 +516,15 @@ static int ttyopen(const char *path, speed_t speed){ int fd = -1; struct termios2 tty; DBG("Try to open %s @ %d", path, speed); - if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1; - if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; } + if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){ + DBG("Can't open device %s: %s", path, strerror(errno)); + return -1; + } + if(ioctl(fd, TCGETS2, &tty)){ + DBG("Can't read TTY settings"); + close(fd); + return -1; + } tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG) tty.c_iflag = 0; // don't do any changes in input stream tty.c_oflag = 0; // don't do any changes in output stream @@ -490,7 +533,11 @@ static int ttyopen(const char *path, speed_t speed){ tty.c_ospeed = speed; //tty.c_cc[VMIN] = 0; // non-canonical mode //tty.c_cc[VTIME] = 5; - if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; } + if(ioctl(fd, TCSETS2, &tty)){ + DBG("Can't set TTY settings"); + close(fd); + return -1; + } 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 @@ -523,7 +570,7 @@ int openEncoder(){ if(encfd[i] < 0) return FALSE; } encRtmout.tv_sec = 0; - encRtmout.tv_usec = 1000; // 1ms + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ for(int i = 0; i < 2; ++i){ close(encfd[i]); @@ -602,6 +649,8 @@ mcc_errcodes_t getMD(mountdata_t *d){ pthread_mutex_lock(&datamutex); *d = mountdata; pthread_mutex_unlock(&datamutex); + //DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val); + //DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL)); return MCC_E_OK; } @@ -629,6 +678,7 @@ static int wr(const data_t *out, data_t *in, int needeol){ int g = write(mntfd, "\r", 1); // add EOL (void) g; } + usleep(50000); // add little pause so that the idiot has time to swallow } if(!in) return TRUE; in->len = 0; @@ -740,7 +790,13 @@ int cmdC(SSconfig *conf, int rw){ }else{ // read data_t d; d.buf = (uint8_t *) conf; - d.len = 0; d.maxlen = sizeof(SSconfig); + d.len = 0; d.maxlen = 0; + ret = wr(rcmd, &d, 1); + DBG("write command: %s", ret ? "TRUE" : "FALSE"); + if(!ret) goto rtn; + // make a huge pause for stupid SSII + usleep(100000); + d.len = 0; d.maxlen = sizeof(SSconfig); ret = wr(rcmd, &d, 1); DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen); if(d.len != d.maxlen){ ret = FALSE; goto rtn; } diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index d075289..ed4feab 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -48,6 +48,7 @@ typedef enum{ MCC_E_ENCODERDEV, // encoder device error or can't open MCC_E_MOUNTDEV, // mount device error or can't open MCC_E_FAILED, // failed to run command - protocol error + MCC_E_AMOUNT // Just amount of errors } mcc_errcodes_t; typedef struct{ @@ -74,9 +75,11 @@ typedef struct{ PIDpar_t XPIDV; PIDpar_t YPIDC; PIDpar_t YPIDV; - double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees - double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg - double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1'' + double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees + double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg + double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1'' + int XEncZero; // encoders' zero position + int YEncZero; } conf_t; // coordinates/speeds in degrees or d/s: X, Y @@ -188,6 +191,9 @@ typedef struct{ double outplimit; // Output Limit, percent (0..100) double currlimit; // Current Limit (A) double intlimit; // Integral Limit (???) + // these params are taken from mount by text commands (don't save negative values - better save these marks in xybits + double motor_stepsperrev;// encoder's steps per revolution: motor and axis + double axis_stepsperrev; // negative sign of these values means reverse direction } __attribute__((packed)) axis_config_t; // hardware configuration diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index 6e5c89e..2c88e52 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -26,6 +26,9 @@ #include "serial.h" #include "ssii.h" +int X_ENC_ZERO, Y_ENC_ZERO; +double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.; + uint16_t SScalcChecksum(uint8_t *buf, int len){ uint16_t checksum = 0; for(int i = 0; i < len; i++){ @@ -75,8 +78,9 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){ m->motXposition.t = m->motYposition.t = *t; // fill encoder data from here, as there's no separate enc thread if(!Conf.SepEncoder){ - m->encXposition.val = X_ENC2RAD(s->Xenc); - m->encYposition.val = Y_ENC2RAD(s->Yenc); + m->encXposition.val = Xenc2rad(s->Xenc); + DBG("encx: %g", m->encXposition.val); + m->encYposition.val = Yenc2rad(s->Yenc); m->encXposition.t = m->encYposition.t = *t; getXspeed(); getYspeed(); } @@ -185,9 +189,10 @@ mcc_errcodes_t updateMotorPos(){ usleep(10000); continue; } + //DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV); if(MCC_E_OK == getMD(&md)){ if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){ - DBG("Just started, t-t0 = %g!", t - t0); + DBG("Just started? t-t0 = %g!", t - t0); usleep(10000); continue; } @@ -195,14 +200,15 @@ mcc_errcodes_t updateMotorPos(){ DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec); mcc_errcodes_t OK = MCC_E_OK; if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){ - DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); + DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val); + DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val)); if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ DBG("Xpos sync failed!"); OK = MCC_E_FAILED; }else DBG("Xpos sync OK, Dt=%g", t - t0); } if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){ - DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); + DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val); if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ DBG("Ypos sync failed!"); OK = MCC_E_FAILED; diff --git a/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index abd8007..39d65a0 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -175,62 +175,70 @@ // amount of consequent same coordinates to detect stop #define MOTOR_STOPPED_CNT (19) +// replace macros with global variables inited when config read +extern int X_ENC_ZERO, Y_ENC_ZERO; +extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV; + // TODO: take it from settings? // steps per revolution (SSI - x4 - for SSI) -#define X_MOT_STEPSPERREV_SSI (13312000.) +// -> hwconf.Xconf.mot/enc_stepsperrev +//#define X_MOT_STEPSPERREV_SSI (13312000.) // 13312000 / 4 = 3328000 -#define X_MOT_STEPSPERREV (3328000.) -#define Y_MOT_STEPSPERREV_SSI (17578668.) +//#define X_MOT_STEPSPERREV (3328000.) +//#define Y_MOT_STEPSPERREV_SSI (17578668.) // 17578668 / 4 = 4394667 -#define Y_MOT_STEPSPERREV (4394667.) +//#define Y_MOT_STEPSPERREV (4394667.) // encoder per revolution -#define X_ENC_STEPSPERREV (67108864.) -#define Y_ENC_STEPSPERREV (67108864.) +//#define X_ENC_STEPSPERREV (67108864.) +//#define Y_ENC_STEPSPERREV (67108864.) // encoder zero position -#define X_ENC_ZERO (61245239) -#define Y_ENC_ZERO (36999830) -// encoder reversed (no: +1) -#define X_ENC_SIGN (-1.) -#define Y_ENC_SIGN (-1.) +// -> conf.XEncZero/YEncZero +//#define X_ENC_ZERO (61245239) +//#define Y_ENC_ZERO (36999830) +// encoder reversed (no: +1) -> sign of ...stepsperrev +//#define X_ENC_SIGN (-1.) +//#define Y_ENC_SIGN (-1.) + + // encoder position to radians and back -#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV) -#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 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)) +#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV)) +#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV)) +#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV))) +#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV))) // convert angle in radians to +-pi -static inline double ang2half(double ang){ +static inline __attribute__((always_inline)) double ang2half(double ang){ if(ang < -M_PI) ang += 2.*M_PI; else if(ang > M_PI) ang -= 2.*M_PI; return ang; } // convert to only positive: 0..2pi -static inline double ang2full(double ang){ +static inline __attribute__((always_inline)) double ang2full(double ang){ if(ang < 0.) ang += 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI; return ang; } // motor position to radians and back -#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) -#define Y_MOT2RAD(n) ang2half(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) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV)) +#define Y_MOT2RAD(n) ang2half(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. * 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)) +#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. * 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)) +#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) / SITECH_LOOP_FREQUENCY) -#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) +#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY)) +#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY)) // encoder's tolerance (ticks) #define YencTOL (25.)