diff --git a/LibSidServo/.qtcreator/libsidservo.creator.user b/LibSidServo/.qtcreator/libsidservo.creator.user new file mode 100644 index 0000000..230e00f --- /dev/null +++ b/LibSidServo/.qtcreator/libsidservo.creator.user @@ -0,0 +1,220 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /tmp/robo5/mountcontrol.git/LibSidServo + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + %{RunConfig:Executable:Path} + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + %{RunConfig:Executable:Path} + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/LibSidServo/.qtcreator/libsidservo.creator.user.cf63021 b/LibSidServo/.qtcreator/libsidservo.creator.user.cf63021 new file mode 100644 index 0000000..dfa9ebb --- /dev/null +++ b/LibSidServo/.qtcreator/libsidservo.creator.user.cf63021 @@ -0,0 +1,218 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/LibSidServo/TODO b/LibSidServo/TODO new file mode 100644 index 0000000..c367bdc --- /dev/null +++ b/LibSidServo/TODO @@ -0,0 +1,2 @@ +check angle conversions +Read HW config even in model mode diff --git a/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c index d399116..7980878 100644 --- a/LibSidServo/examples/SSIIconf.c +++ b/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/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 9db5981..f1f8705 100644 --- a/LibSidServo/examples/conf.c +++ b/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 }; diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index dfa9ebb..230e00f 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/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/LibSidServo/main.c b/LibSidServo/main.c index b56811b..165a53b 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -53,6 +53,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 +88,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,6 +222,9 @@ static mcc_errcodes_t init(conf_t *c){ } if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; + // read HW config to update constants + hardware_configuration_t HW; + if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; return updateMotorPos(); } @@ -258,19 +263,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 @@ -427,6 +419,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); @@ -485,6 +478,26 @@ 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) / 4.; // divide by 4 as these values stored + if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED; + Yticks = ((double) i64) / 4.; + X_ENC_ZERO = Conf.XEncZero; + Y_ENC_ZERO = Conf.YEncZero; + X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = (config.xbits.motrev) ? -Xticks : Xticks; + Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = (config.ybits.motrev) ? -Yticks : Yticks; + // 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; + X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks; + Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks; return MCC_E_OK; } @@ -540,6 +553,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ config.Ysetpr = __bswap_32(hwConfig->Ysetpr); config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4); config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4); + // todo - also write text params // TODO - next (void) config; return MCC_E_OK; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 5e040f5..ad4997e 100644 --- a/LibSidServo/serial.c +++ b/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 = 5000}, 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; @@ -320,13 +320,13 @@ static void *encoderthread2(void _U_ *u){ 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); 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); getYspeed(); @@ -740,7 +740,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/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index d075289..0ed9cd5 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -74,9 +74,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 +190,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/LibSidServo/ssii.c b/LibSidServo/ssii.c index 6e5c89e..23a6fb5 100644 --- a/LibSidServo/ssii.c +++ b/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,8 @@ 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); + m->encYposition.val = Yenc2rad(s->Yenc); m->encXposition.t = m->encYposition.t = *t; getXspeed(); getYspeed(); } diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index abd8007..1ba3441 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -175,29 +175,35 @@ // 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){