From 9fbd858086300dbccbebb180b289738531d324fc Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Thu, 31 Jul 2025 17:03:15 +0300 Subject: [PATCH] less squares 4 speed + fixed some bugs (but found more) --- LibSidServo/examples/conf.c | 1 + LibSidServo/examples/dump.c | 3 +- LibSidServo/examples/dumpmoving_dragNtrack.c | 26 +- LibSidServo/examples/dumpswing.c | 12 +- LibSidServo/libsidservo.config | 5 +- LibSidServo/libsidservo.creator.user | 40 +-- LibSidServo/libsidservo.files | 6 +- LibSidServo/main.c | 121 +++++++--- LibSidServo/main.h | 10 + LibSidServo/movingmodel.c | 18 +- LibSidServo/movingmodel.h | 36 ++- LibSidServo/ramp.c | 242 ++++++++++--------- LibSidServo/serial.c | 136 +++++++++-- LibSidServo/serial.h | 2 + LibSidServo/ssii.c | 4 +- 15 files changed, 428 insertions(+), 234 deletions(-) diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 1969a0f..31662d1 100644 --- a/LibSidServo/examples/conf.c +++ b/LibSidServo/examples/conf.c @@ -44,6 +44,7 @@ static sl_option_t opts[] = { {"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"}, + {"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"}, end_option }; diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 5b818f6..62019aa 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){ enct = tmsr; if(fcoords) logmnt(fcoords, &mdata); if(mdata.millis == mdmillis) continue; - //DBG("ctr=%d", ctr); + //DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val); mdmillis = mdata.millis; if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ xlast = mdata.motXposition.val; @@ -123,6 +123,7 @@ void dumpmoving(FILE *fcoords, double t, int N){ ctr = 0; }else ++ctr; } + DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0); } /** diff --git a/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c index 326ec5e..67dcf71 100644 --- a/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -120,16 +120,13 @@ static coordpair_t lastTag = {0}, lastSpeed = {0}; // slew to given position and start tracking // pos/speed in deg and deg/s -static mcc_errcodes_t gotos(coordpair_t *target, coordpair_t *speed){ +static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){ short_command_t cmd = {0}; DBG("Try to move to (%g, %g) with speed (%g, %g)", target->X, target->Y, speed->X, speed->Y); - target->X = DEG2RAD(target->X); - target->Y = DEG2RAD(target->Y); - speed->X = DEG2RAD(speed->X); - speed->Y = DEG2RAD(speed->Y); - cmd.Xmot = target->X; cmd.Ymot = target->Y; - cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y; + cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y); + cmd.Xspeed = DEG2RAD(speed->X); + cmd.Yspeed = DEG2RAD(speed->Y); lastTag = *target; lastSpeed = *speed; /*cmd.xychange = 1; @@ -142,7 +139,8 @@ static mcc_errcodes_t return2zero(){ short_command_t cmd = {0}; DBG("Try to move to zero"); cmd.Xmot = 0.; cmd.Ymot = 0.; - cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.); + cmd.Xspeed = MCC_MAX_X_SPEED; + cmd.Yspeed = MCC_MAX_Y_SPEED; /*cmd.xychange = 1; cmd.XBits = 100; cmd.YBits = 20;*/ @@ -151,11 +149,11 @@ static mcc_errcodes_t return2zero(){ static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){ long_command_t cmd = {0}; - cmd.Xspeed = lastSpeed.X; - cmd.Yspeed = lastSpeed.Y; - cmd.Xmot = lastTag.X; - cmd.Ymot = lastTag.Y; - cmd.Xadder = adder->X; cmd.Yadder = adder->Y; + cmd.Xspeed = DEG2RAD(lastSpeed.X); + cmd.Yspeed = DEG2RAD(lastSpeed.Y); + cmd.Xmot = DEG2RAD(lastTag.X); + cmd.Ymot = DEG2RAD(lastTag.Y); + cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y); cmd.Xatime = time->X; cmd.Yatime = time->Y; return Mount.longCmd(&cmd); } @@ -218,7 +216,7 @@ int main(int argc, char **argv){ sleep(5); // return to zero and wait green("Return 2 zero and wait\n"); - return2zero(); + if(!return2zero()) ERRX("Can't return"); Wait(0., 0); Wait(0., 1); // wait moving ends diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index da65bef..c023b11 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/LibSidServo/examples/dumpswing.c @@ -89,12 +89,12 @@ void waithalf(double t){ if(mdata.millis == millis) continue; millis = mdata.millis; if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ - DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); + //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.motYposition.val)); + //DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); ++ctr; } } @@ -162,9 +162,13 @@ int main(int argc, char **argv){ DBG("Moved to -, t=%g", t-t0); DBG("CMD: %g", Mount.currentT()-t0); } - tag = (coordpair_t){.X = 0., .Y = 0.}; + green("Move to zero @ %g\n", Mount.currentT()); + tag = (coordpair_t){0}; // be sure to move @ 0,0 - Mount.moveTo(&tag); + if(MCC_E_OK != Mount.moveTo(&tag)){ + Mount.emergStop(); + Mount.moveTo(&tag); + } // wait moving ends pthread_join(dthr, NULL); #undef SCMD diff --git a/LibSidServo/libsidservo.config b/LibSidServo/libsidservo.config index 128fef3..2a32b68 100644 --- a/LibSidServo/libsidservo.config +++ b/LibSidServo/libsidservo.config @@ -1,6 +1,7 @@ // Add predefined macros for your project here. For example: // #define THE_ANSWER 42 #define EBUG -#define _POSIX_C_SOURCE 111 +#define _POSIX_C_SOURCE 11111111 #define PACKAGE_VERSION "0.0.1" - +#define _XOPEN_SOURCE 666 +#define _DEFAULT_SOURCE diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 7ee670a..73de00a 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} ProjectExplorer.Project.ActiveTarget @@ -40,9 +40,9 @@ 1 0 false - true + false false - 0 + 1 true true 0 @@ -51,10 +51,10 @@ false 1 true - false + true true *.md, *.MD, Makefile - false + true true true @@ -75,13 +75,11 @@ 0 true - - true true true Builtin.DefaultTidyAndClazy - 8 + 4 true @@ -97,12 +95,12 @@ true Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /Big/Data/00__Small_tel/C-sources/erfa + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions @@ -112,8 +110,8 @@ GenericProjectManager.GenericMakeStep 1 - Build - Build + Сборка + Сборка ProjectExplorer.BuildSteps.Build @@ -125,8 +123,8 @@ GenericProjectManager.GenericMakeStep 1 - Clean - Clean + Очистка + Очистка ProjectExplorer.BuildSteps.Clean 2 @@ -141,8 +139,8 @@ 0 - Deploy - Deploy + Развёртывание + Развёртывание ProjectExplorer.BuildSteps.Deploy 1 @@ -154,7 +152,9 @@ true true + 0 true + 2 @@ -173,8 +173,8 @@ 0 - Deploy - Deploy + Развёртывание + Развёртывание ProjectExplorer.BuildSteps.Deploy 1 @@ -186,7 +186,9 @@ true true + 0 true + 2 diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 3d32e92..7b0b53e 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -1,5 +1,4 @@ CMakeLists.txt -dbg.h examples/SSIIconf.c examples/conf.c examples/conf.h @@ -18,6 +17,11 @@ serial.c examples/CMakeLists.txt examples/traectories.c examples/traectories.h +main.h +movingmodel.c +movingmodel.h +ramp.c +ramp.h serial.h ssii.c ssii.h diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 147eb3f..534e036 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -37,10 +37,10 @@ static movemodel_t *Xmodel, *Ymodel; // radians, rad/sec, rad/sec^2 static limits_t Xlimits = { - .min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}}, Ylimits = { - .min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}} ; static mcc_errcodes_t shortcmd(short_command_t *cmd); @@ -76,22 +76,73 @@ static void quit(){ void getModData(mountdata_t *mountdata){ if(!mountdata || !Xmodel || !Ymodel) return; + static double oldmt = -100.; // old `millis measurement` time + static uint32_t oldmillis = 0; double tnow = nanotime(); moveparam_t Xp, Yp; - movestate_t Xst = Xmodel->get_state(&Xp); - if(Xst == ST_MOVE) Xst = Xmodel->proc_move(&Xp, tnow); - movestate_t Yst = Ymodel->get_state(&Yp); - if(Yst == ST_MOVE) Yst = Ymodel->proc_move(&Yp, tnow); + movestate_t Xst = Xmodel->get_state(Xmodel, &Xp); + //DBG("Xstate = %d", Xst); + if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow); + movestate_t Yst = Ymodel->get_state(Ymodel, &Yp); + if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow); bzero(mountdata, sizeof(mountdata_t)); mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow; mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord; mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord; - mountdata->encXspeed.t = mountdata->encYspeed.t = tnow; - mountdata->encXspeed.val = Xp.speed; - mountdata->encYspeed.val = Yp.speed; - mountdata->millis = (uint32_t)(tnow * 1e3); + getXspeed(); getYspeed(); + if(tnow - oldmt > Conf.MountReqInterval){ + oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3); + oldmt = tnow; + }else mountdata->millis = oldmillis; } +/** + * less square calculations of speed + */ +less_square_t *LS_init(size_t Ndata){ + if(Ndata < 5){ + DBG("Ndata=%zd - TOO SMALL", Ndata); + return NULL; + } + DBG("Init less squares: %zd", Ndata); + less_square_t *l = calloc(1, sizeof(less_square_t)); + l->x = calloc(Ndata, sizeof(double)); + l->t2 = calloc(Ndata, sizeof(double)); + l->t = calloc(Ndata, sizeof(double)); + l->xt = calloc(Ndata, sizeof(double)); + l->arraysz = Ndata; + return l; +} +void LS_delete(less_square_t **l){ + if(!l || !*l) return; + free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt); + free(*l); + *l = NULL; +} +// add next data portion and calculate current slope +double LS_calc_slope(less_square_t *l, double x, double t){ + if(!l) return 0.; + size_t idx = l->idx; + double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx]; + double t2 = t * t, xt = x * t; + l->x[idx] = x; l->t2[idx] = t2; + l->t[idx] = t; l->xt[idx] = xt; + ++idx; + l->idx = (idx >= l->arraysz) ? 0 : idx; + l->xsum += x - oldx; + l->t2sum += t2 - oldt2; + l->tsum += t - oldt; + l->xtsum += xt - oldxt; + double n = (double)l->arraysz; + double denominator = n * l->t2sum - l->tsum * l->tsum; + //DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator); + if(fabs(denominator) < 1e-7) return 0.; + double numerator = n * l->xtsum - l->xsum * l->tsum; + // point: (sum_x - slope * sum_t) / n; + return (numerator / denominator); +} + + /** * @brief init - open serial devices and do other job * @param c - initial configuration @@ -105,7 +156,7 @@ static mcc_errcodes_t init(conf_t *c){ Xmodel = model_init(&Xlimits); Ymodel = model_init(&Ylimits); if(Conf.RunModel){ - if(!Xmodel || !Ymodel) return MCC_E_FAILED; + if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; return MCC_E_OK; } if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ @@ -132,10 +183,9 @@ static mcc_errcodes_t init(conf_t *c){ 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 - if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED; + //uint8_t buf[1024]; + //data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; + if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; return updateMotorPos(); } @@ -180,14 +230,6 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ static mcc_errcodes_t move2(const coordpair_t *target){ if(!target) return MCC_E_BADFORMAT; if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; - if(Conf.RunModel){ - double curt = nanotime(); - moveparam_t param = {0}; - param.coord = target->X; param.speed = MCC_MAX_X_SPEED; - if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; - param.coord = target->Y; param.speed = MCC_MAX_Y_SPEED; - if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; - } if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; short_command_t cmd = {0}; DBG("x,y: %g, %g", target->X, target->Y); @@ -227,14 +269,14 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed if(!target || !speed) return MCC_E_BADFORMAT; if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; - if(Conf.RunModel){ + /* if(Conf.RunModel){ double curt = nanotime(); moveparam_t param = {0}; param.coord = target->X; param.speed = speed->X; if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; param.coord = target->Y; param.speed = speed->Y; if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; - } + }*/ if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; short_command_t cmd = {0}; cmd.Xmot = target->X; @@ -254,8 +296,8 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed static mcc_errcodes_t emstop(){ if(Conf.RunModel){ double curt = nanotime(); - Xmodel->emergency_stop(curt); - Ymodel->emergency_stop(curt); + Xmodel->emergency_stop(Xmodel, curt); + Ymodel->emergency_stop(Ymodel, curt); return MCC_E_OK; } if(!SSstop(TRUE)) return MCC_E_FAILED; @@ -265,8 +307,8 @@ static mcc_errcodes_t emstop(){ static mcc_errcodes_t stop(){ if(Conf.RunModel){ double curt = nanotime(); - Xmodel->stop(curt); - Ymodel->stop(curt); + Xmodel->stop(Xmodel, curt); + Ymodel->stop(Ymodel,curt); return MCC_E_OK; } if(!SSstop(FALSE)) return MCC_E_FAILED; @@ -280,7 +322,15 @@ static mcc_errcodes_t stop(){ */ static mcc_errcodes_t shortcmd(short_command_t *cmd){ if(!cmd) return MCC_E_BADFORMAT; - if(Conf.RunModel) return MCC_E_FAILED; + if(Conf.RunModel){ + double curt = nanotime(); + moveparam_t param = {0}; + param.coord = cmd->Xmot; param.speed = cmd->Xspeed; + if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; + param.coord = cmd->Ymot; param.speed = cmd->Yspeed; + if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; + return MCC_E_OK; + } SSscmd s = {0}; DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot); s.Xmot = X_RAD2MOT(cmd->Xmot); @@ -302,7 +352,15 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){ */ static mcc_errcodes_t longcmd(long_command_t *cmd){ if(!cmd) return MCC_E_BADFORMAT; - if(Conf.RunModel) return MCC_E_FAILED; + if(Conf.RunModel){ + double curt = nanotime(); + moveparam_t param = {0}; + param.coord = cmd->Xmot; param.speed = cmd->Xspeed; + if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; + param.coord = cmd->Ymot; param.speed = cmd->Yspeed; + if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; + return MCC_E_OK; + } SSlcmd l = {0}; l.Xmot = X_RAD2MOT(cmd->Xmot); l.Ymot = Y_RAD2MOT(cmd->Ymot); @@ -455,3 +513,4 @@ mount_t Mount = { .saveHWconfig = write_hwconf, .currentT = nanotime, }; + diff --git a/LibSidServo/main.h b/LibSidServo/main.h index badc8b9..e642f1a 100644 --- a/LibSidServo/main.h +++ b/LibSidServo/main.h @@ -29,6 +29,16 @@ extern conf_t Conf; double nanotime(); void getModData(mountdata_t *mountdata); +typedef struct{ + double *x, *t, *t2, *xt; // arrays of coord/time and multiply + double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply + size_t idx; // index of current data in array + size_t arraysz; // size of arrays +} less_square_t; + +less_square_t *LS_init(size_t Ndata); +void LS_delete(less_square_t **ls); +double LS_calc_slope(less_square_t *l, double x, double t); // unused arguments of functions #define _U_ __attribute__((__unused__)) diff --git a/LibSidServo/movingmodel.c b/LibSidServo/movingmodel.c index 0bfa9c0..c1c61e8 100644 --- a/LibSidServo/movingmodel.c +++ b/LibSidServo/movingmodel.c @@ -36,9 +36,8 @@ static void chkminmax(double *min, double *max){ movemodel_t *model_init(limits_t *l){ if(!l) return FALSE; - movemodel_t *model = calloc(1, sizeof(movemodel_t)); - memcpy(model, &trapez, sizeof(movemodel_t)); - if(!model->init_limits) goto fail; + movemodel_t *m = calloc(1, sizeof(movemodel_t)); + *m = trapez; moveparam_t *max = &l->max, *min = &l->min; if(min->speed < 0.) min->speed = -min->speed; if(max->speed < 0.) max->speed = -max->speed; @@ -47,10 +46,13 @@ movemodel_t *model_init(limits_t *l){ chkminmax(&min->coord, &max->coord); chkminmax(&min->speed, &max->speed); chkminmax(&min->accel, &max->accel); - if(!model->init_limits(l)) return NULL; - return model; -fail: - free(model); return NULL; + m->Min = l->min; + m->Max = l->max; + m->movingstage = STAGE_STOPPED; + m->state = ST_STOP; + pthread_mutex_init(&m->mutex, NULL); + DBG("model inited"); + return m; } int model_move2(movemodel_t *model, moveparam_t *target, double t){ @@ -59,5 +61,5 @@ int model_move2(movemodel_t *model, moveparam_t *target, double t){ // only positive velocity if(target->speed < 0.) target->speed = -target->speed; // don't mind about acceleration - user cannot set it now - return model->calculate(target, t); + return model->calculate(model, target, t); } diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h index 0c0fab3..1c70982 100644 --- a/LibSidServo/movingmodel.h +++ b/LibSidServo/movingmodel.h @@ -17,12 +17,13 @@ */ #pragma once +#include #include "sidservo.h" // tolerance, time ticks -#define COORD_TOLERANCE_DEFAULT (0.01) -#define COORD_TOLERANCE_MIN (0.0001) +#define COORD_TOLERANCE_DEFAULT (1e-6) +#define COORD_TOLERANCE_MIN (1e-8) #define COORD_TOLERANCE_MAX (10.) #define TIME_TICK_DEFAULT (0.0001) #define TIME_TICK_MIN (1e-9) @@ -46,14 +47,29 @@ typedef struct{ double acceleration; } limits_t; -typedef struct{ - int (*init_limits)(limits_t *lim); // init values of limits, jerk - int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t - movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t - movestate_t (*get_state)(moveparam_t *cur); // get current moving state - void (*stop)(double t); // stop by ramp - void (*emergency_stop)(double t); // stop with highest acceleration - double (*stoppenanotime)(); // time when moving will ends +typedef enum{ + STAGE_ACCEL, // start from zero speed and accelerate to Max speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +typedef struct movemodel{ + moveparam_t Min; + moveparam_t Max; + movingstage_t movingstage; + movestate_t state; + double Times[STAGE_AMOUNT]; + moveparam_t Params[STAGE_AMOUNT]; + moveparam_t curparams; // init values of limits, jerk + int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state + void (*stop)(struct movemodel *m, double t); // stop by ramp + void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration + double (*stoppedtime)(struct movemodel *m); // time when moving will ends + pthread_mutex_t mutex; } movemodel_t; movemodel_t *model_init(limits_t *l); diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c index efa209c..f4b74b4 100644 --- a/LibSidServo/ramp.c +++ b/LibSidServo/ramp.c @@ -25,53 +25,38 @@ #include "ramp.h" static double coord_tolerance = COORD_TOLERANCE_DEFAULT; -static movestate_t state = ST_STOP; -static moveparam_t Min, Max; -typedef enum{ - STAGE_ACCEL, // start from zero speed and accelerate to Max speed - STAGE_MAXSPEED, // go with target speed - STAGE_DECEL, // go from target speed to zero - STAGE_STOPPED, // stop - STAGE_AMOUNT -} movingstage_t; - -static movingstage_t movingstage = STAGE_STOPPED; -static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts -static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage -static moveparam_t curparams = {0}; // current coordinate/speed/acceleration - -static int initlims(limits_t *lim){ - if(!lim) return FALSE; - Min = lim->min; - Max = lim->max; - return TRUE; +static void emstop(movemodel_t *m, double _U_ t){ + FNAME(); + pthread_mutex_lock(&m->mutex); + m->curparams.accel = 0.; + m->curparams.speed = 0.; + bzero(m->Times, sizeof(double) * STAGE_AMOUNT); + bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT); + m->state = ST_STOP; + m->movingstage = STAGE_STOPPED; + pthread_mutex_unlock(&m->mutex); } -static void emstop(double _U_ t){ - curparams.accel = 0.; - curparams.speed = 0.; - bzero(Times, sizeof(Times)); - bzero(Params, sizeof(Params)); - state = ST_STOP; - movingstage = STAGE_STOPPED; -} - -static void stop(double t){ - if(state == ST_STOP || movingstage == STAGE_STOPPED) return; - movingstage = STAGE_DECEL; - state = ST_MOVE; - Times[STAGE_DECEL] = t; - Params[STAGE_DECEL].speed = curparams.speed; - if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel; - else Params[STAGE_DECEL].accel = Max.accel; - Params[STAGE_DECEL].coord = curparams.coord; +static void stop(movemodel_t *m, double t){ + FNAME(); + pthread_mutex_lock(&m->mutex); + if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret; + m->movingstage = STAGE_DECEL; + m->state = ST_MOVE; + m->Times[STAGE_DECEL] = t; + m->Params[STAGE_DECEL].speed = m->curparams.speed; + if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel; + else m->Params[STAGE_DECEL].accel = m->Max.accel; + m->Params[STAGE_DECEL].coord = m->curparams.coord; // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2 - Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel; + m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel; // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2 - double dt = Times[STAGE_STOPPED] - t; - Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt + - Params[STAGE_DECEL].accel * dt * dt / 2.; + double dt = m->Times[STAGE_STOPPED] - t; + m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt + + m->Params[STAGE_DECEL].accel * dt * dt / 2.; +ret: + pthread_mutex_unlock(&m->mutex); } /** @@ -80,141 +65,166 @@ static void stop(double t){ * @param t - current time value * @return FALSE if can't move with given parameters */ -static int calc(moveparam_t *x, double t){ +static int calc(movemodel_t *m, moveparam_t *x, double t){ + DBG("coord/speed: %g/%g", x->coord, x->speed); if(!x) return FALSE; - if(x->coord < Min.coord || x->coord > Max.coord) return FALSE; - if(x->speed < Min.speed || x->speed > Max.speed) return FALSE; - double Dx = fabs(x->coord - curparams.coord); // full distance - double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds + pthread_mutex_lock(&m->mutex); + int ret = FALSE; + if(x->coord < m->Min.coord || x->coord > m->Max.coord){ + DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord); + goto ret; + } + if(x->speed < m->Min.speed || x->speed > m->Max.speed){ + DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed); + goto ret; + } + double Dx = fabs(x->coord - m->curparams.coord); // full distance + double sign = (x->coord > m->curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds // we have two variants: with or without stage with constant speed - double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed + double dt23 = x->speed / m->Max.accel; // time of deceleration stage for given speed double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs) DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23); double setspeed = x->speed; // new max speed (we can change it if need) double dt01, dx01; // we'll fill them depending on starting conditions - Times[0] = t; - Params[0].speed = curparams.speed; - Params[0].coord = curparams.coord; + m->Times[0] = t; + m->Params[0].speed = m->curparams.speed; + m->Params[0].coord = m->curparams.coord; - double curspeed = fabs(curparams.speed); - double dt0s = curspeed / Max.accel; // time of stopping phase + double curspeed = fabs(m->curparams.speed); + double dt0s = curspeed / m->Max.accel; // time of stopping phase double dx0s = curspeed * dt0s / 2.; // distance - DBG("dt0s=%g, dx0s=%g", dt0s, dx0s); - if(dx0s > Dx){ + DBG("dt0s=%g, dx0s=%g, curspeed=%g", dt0s, dx0s, curspeed); + // TODO: fix this! Target should stop and after thar reach given coordinate!!! + if(dx0s - Dx > coord_tolerance){ DBG("distance too short"); - return FALSE; + goto ret; } if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target DBG("Distance good to just stop"); - stop(t); - return TRUE; + pthread_mutex_unlock(&m->mutex); + stop(m, t); + ret = TRUE; + goto ret; } - if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign + if(m->curparams.speed * sign < 0. || m->state == ST_STOP){ // we should change speed sign // after stop we will have full profile double dxs3 = Dx - dx0s; - double newspeed = sqrt(Max.accel * dxs3); + double newspeed = sqrt(m->Max.accel * dxs3); if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed DBG("dxs3=%g, setspeed=%g", dxs3, setspeed); - dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; - Params[0].accel = sign * Max.accel; - if(state == ST_STOP) dx01 = setspeed * dt01 / 2.; - else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed); + dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel; + m->Params[0].accel = sign * m->Max.accel; + if(m->state == ST_STOP) dx01 = setspeed * dt01 / 2.; + else dx01 = dt01 * (dt01 / 2. * m->Max.accel - curspeed); DBG("dx01=%g, dt01=%g", dx01, dt01); }else{ // increase or decrease speed without stopping phase - dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; - double a = sign * Max.accel; - if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;} + dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel; + double a = sign * m->Max.accel; + if(sign * m->curparams.speed < 0.){DBG("change direction"); a = -a;} else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;} //double a = (curspeed > setspeed) ? -Max.accel : Max.accel; - dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; + dx01 = sign*(curspeed * dt01 + a * dt01 * dt01 / 2.); DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); if(dx01 + dx23 > Dx){ // calculate max speed - setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.); + setspeed = sqrt(m->Max.accel * Dx - curspeed * curspeed / 2.); if(setspeed < curspeed){ - setspeed = curparams.speed; + setspeed = m->curparams.speed; dt01 = 0.; dx01 = 0.; - Params[0].accel = 0.; + m->Params[0].accel = 0.; }else{ - Params[0].accel = a; - dt01 = fabs(setspeed - curspeed) / Max.accel; - dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.; + m->Params[0].accel = a; + dt01 = fabs(setspeed - curspeed) / m->Max.accel; + dx01 = curspeed * dt01 + m->Max.accel * dt01 * dt01 / 2.; } - }else Params[0].accel = a; + }else m->Params[0].accel = a; } - if(setspeed < Min.speed){ - DBG("New speed should be too small"); - return FALSE; + if(setspeed < m->Min.speed){ + DBG("New speed (%g) should be too small (<%g)", setspeed, m->Min.speed); + goto ret; } - moveparam_t *p = &Params[STAGE_MAXSPEED]; + moveparam_t *p = &m->Params[STAGE_MAXSPEED]; p->accel = 0.; p->speed = sign * setspeed; - p->coord = curparams.coord + dx01 * sign; - Times[STAGE_MAXSPEED] = Times[0] + dt01; - dt23 = setspeed / Max.accel; + p->coord = m->curparams.coord + dx01 * sign; + m->Times[STAGE_MAXSPEED] = m->Times[0] + dt01; + dt23 = setspeed / m->Max.accel; dx23 = setspeed * dt23 / 2.; + DBG("setspeed=%g, dt23=%g, tx23=%g", setspeed, dt23, dx23); // calculate dx12 and dt12 double dx12 = Dx - dx01 - dx23; if(dx12 < -coord_tolerance){ DBG("Oops, WTF dx12=%g?", dx12); - return FALSE; + goto ret; } double dt12 = dx12 / setspeed; - p = &Params[STAGE_DECEL]; - p->accel = -sign * Max.accel; + p = &m->Params[STAGE_DECEL]; + p->accel = -sign * m->Max.accel; p->speed = sign * setspeed; - p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12; - Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12; - p = &Params[STAGE_STOPPED]; + p->coord = m->Params[STAGE_MAXSPEED].coord + sign * dx12; + m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12; + p = &m->Params[STAGE_STOPPED]; p->accel = 0.; p->speed = 0.; p->coord = x->coord; - Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23; + m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23; for(int i = 0; i < 4; ++i) DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i, - Times[i], Params[i].coord, Params[i].speed, Params[i].accel); - state = ST_MOVE; - movingstage = STAGE_ACCEL; - return TRUE; + m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel); + m->state = ST_MOVE; + m->movingstage = STAGE_ACCEL; + ret = TRUE; +ret: + pthread_mutex_unlock(&m->mutex); + return ret; } -static movestate_t proc(moveparam_t *next, double t){ - if(state == ST_STOP) goto ret; +static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){ + pthread_mutex_lock(&m->mutex); + if(m->state == ST_STOP) goto ret; for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){ - if(Times[s] <= t){ // check time for current stage - movingstage = s; + if(m->Times[s] <= t){ // check time for current stage + m->movingstage = s; break; } } - if(movingstage == STAGE_STOPPED){ - curparams.coord = Params[STAGE_STOPPED].coord; - emstop(t); + if(m->movingstage == STAGE_STOPPED){ + m->curparams.coord = m->Params[STAGE_STOPPED].coord; + pthread_mutex_unlock(&m->mutex); + emstop(m, t); goto ret; } // calculate current parameters - double dt = t - Times[movingstage]; - double a = Params[movingstage].accel; - double v0 = Params[movingstage].speed; - double x0 = Params[movingstage].coord; - curparams.accel = a; - curparams.speed = v0 + a * dt; - curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; + double dt = t - m->Times[m->movingstage]; + double a = m->Params[m->movingstage].accel; + double v0 = m->Params[m->movingstage].speed; + double x0 = m->Params[m->movingstage].coord; + m->curparams.accel = a; + m->curparams.speed = v0 + a * dt; + m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; ret: - if(next) *next = curparams; - return state; + if(next) *next = m->curparams; + movestate_t st = m->state; + pthread_mutex_unlock(&m->mutex); + return st; } -static movestate_t getst(moveparam_t *cur){ - if(cur) *cur = curparams; - return state; +static movestate_t getst(movemodel_t *m, moveparam_t *cur){ + pthread_mutex_lock(&m->mutex); + if(cur) *cur = m->curparams; + movestate_t st = m->state; + pthread_mutex_unlock(&m->mutex); + return st; } -static double gettstop(){ - return Times[STAGE_STOPPED]; +static double gettstop(movemodel_t *m){ + pthread_mutex_lock(&m->mutex); + double r = m->Times[STAGE_STOPPED]; + pthread_mutex_unlock(&m->mutex); + return r; } movemodel_t trapez = { - .init_limits = initlims, .stop = stop, .emergency_stop = emstop, .get_state = getst, .calculate = calc, .proc_move = proc, - .stoppenanotime = gettstop, + .stoppedtime = gettstop, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index dc3918c..74eaf61 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -33,13 +33,14 @@ #include "main.h" #include "movingmodel.h" #include "serial.h" +#include "ssii.h" // serial devices FD 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}; +//static coordval_t lastXenc = {0}, lastYenc = {0}; // mutexes for RW operations with mount device and data static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, @@ -57,17 +58,42 @@ typedef struct __attribute__((packed)){ } enc_t; // calculate current X/Y speeds -static void getXspeed(double t){ +void getXspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t); + if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ + mountdata.encXspeed.val = speed; + mountdata.encXspeed.t = mountdata.encXposition.t; + } + //DBG("Xspeed=%g", mountdata.encXspeed.val); +#if 0 mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t); - mountdata.encXspeed.t = (lastXenc.t + t) / 2.; + mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.; lastXenc.val = mountdata.encXposition.val; lastXenc.t = t; +#endif } -static void getYspeed(double t){ +void getYspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t); + if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ + mountdata.encYspeed.val = speed; + mountdata.encYspeed.t = mountdata.encYposition.t; + } +#if 0 mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t); - mountdata.encYspeed.t = (lastYenc.t + t) / 2.; + mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.; lastYenc.val = mountdata.encYposition.val; lastYenc.t = t; +#endif } /** @@ -115,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ 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); + //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); + //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); + getXspeed(); getYspeed(); 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); } @@ -222,6 +249,31 @@ static int getmntbyte(){ }while(1); return (int)byte; } +// clear data from input buffer +static void clrmntbuf(){ + if(mntfd < 0) return; + uint8_t byte; + fd_set rfds; + //double t0 = nanotime(); + //int n = 0; + do{ + FD_ZERO(&rfds); + FD_SET(mntfd, &rfds); + struct timeval tv = {.tv_sec=0, .tv_usec=10}; + int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); + if(retval < 0){ + if(errno == EINTR) continue; + DBG("Error in select()"); + break; + } + if(FD_ISSET(mntfd, &rfds)){ + ssize_t l = read(mntfd, &byte, 1); + if(l != 1) break; + //++n; + } else break; + }while(1); + //DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n); +} // main encoder thread (for separate encoder): read next data and make parsing static void *encoderthread1(void _U_ *u){ @@ -273,12 +325,14 @@ static void *encoderthread2(void _U_ *u){ 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(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); + getXspeed(); 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); + //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); + getYspeed(); errctr = 0; need2ask = 0; } else { @@ -326,13 +380,14 @@ static void *mountthread(void _U_ *u){ int errctr = 0; uint8_t buf[2*sizeof(SSstat)]; SSstat *status = (SSstat*) buf; + bzero(&mountdata, sizeof(mountdata)); if(Conf.RunModel) while(1){ pthread_mutex_lock(&datamutex); // now change data getModData(&mountdata); pthread_mutex_unlock(&datamutex); double t0 = nanotime(); - while(nanotime() - t0 < Conf.MountReqInterval) usleep(500); + while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); t0 = nanotime(); } // data to get @@ -342,8 +397,32 @@ static void *mountthread(void _U_ *u){ if(!cmd_getstat) goto failed; while(mntfd > -1 && errctr < MAX_ERR_CTR){ // read data to status - double tgot = nanotime(), t0 = tgot; - //DBG("tgot=%g", tgot); + double t0 = nanotime(); +#if 0 +// 127 milliseconds to get answer on X/Y commands!!! + int64_t ans; + int ctr = 0; + if(SSgetint(CMD_MOTX, &ans)){ + pthread_mutex_lock(&datamutex); + mountdata.motXposition.t = tgot; + mountdata.motXposition.val = X_MOT2RAD(ans); + pthread_mutex_unlock(&datamutex); + ++ctr; + } + tgot = nanotime(); + if(SSgetint(CMD_MOTY, &ans)){ + pthread_mutex_lock(&datamutex); + mountdata.motXposition.t = tgot; + mountdata.motXposition.val = X_MOT2RAD(ans); + pthread_mutex_unlock(&datamutex); + ++ctr; + } + if(ctr == 2){ + mountdata.millis = (uint32_t)(1e3 * tgot); + DBG("Got both coords; millis=%d", mountdata.millis); + } +#endif + // 80 milliseconds to get answer on GETSTAT 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); @@ -359,11 +438,12 @@ static void *mountthread(void _U_ *u){ errctr = 0; pthread_mutex_lock(&datamutex); // now change data - SSconvstat(status, &mountdata, tgot); + SSconvstat(status, &mountdata, t0); pthread_mutex_unlock(&datamutex); + //DBG("GOT FULL stat by %g", nanotime() - t0); // allow writing & getters do{ - usleep(5000); + usleep(500); }while(nanotime() - t0 < Conf.MountReqInterval); t0 = nanotime(); } @@ -439,7 +519,7 @@ int openEncoder(){ // return FALSE if failed int openMount(){ - if(Conf.RunModel) return TRUE; + if(Conf.RunModel) goto create_thread; if(mntfd > -1) close(mntfd); DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed); @@ -456,10 +536,13 @@ int openMount(){ }while(1);*/ mntRtmout.tv_sec = 0; mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed +create_thread: if(pthread_create(&mntthread, NULL, mountthread, NULL)){ - DBG("Can't create thread"); - close(mntfd); - mntfd = -1; + DBG("Can't create mount thread"); + if(!Conf.RunModel){ + close(mntfd); + mntfd = -1; + } return FALSE; } DBG("Mount opened, thread started"); @@ -512,6 +595,7 @@ void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){ // write-read without locking mutex (to be used inside other functions) static int wr(const data_t *out, data_t *in, int needeol){ if((!out && !in) || mntfd < 0) return FALSE; + clrmntbuf(); //double t0 = nanotime(); if(out){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){ @@ -525,9 +609,10 @@ static int wr(const data_t *out, data_t *in, int needeol){ } } //DBG("sent by %g", nanotime() - t0); - uint8_t buf[256]; - data_t dumb = {.buf = buf, .maxlen = 256}; - if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash + //uint8_t buf[256]; + //data_t dumb = {.buf = buf, .maxlen = 256}; + if(!in) return TRUE; + //if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash in->len = 0; for(size_t i = 0; i < in->maxlen; ++i){ int b = getmntbyte(); @@ -585,26 +670,25 @@ static int bincmd(uint8_t *cmd, int len){ int ret = FALSE; pthread_mutex_lock(&mntmutex); // dummy buffer to clear trash in input - char ans[300]; - data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; + //char ans[300]; + //data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; if(len == sizeof(SSscmd)){ ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); DBG("Short command"); #ifdef EBUG logscmd((SSscmd*)cmd); #endif - if(!wr(dscmd, &a, 1)) goto rtn; + if(!wr(dscmd, NULL, 1)) goto rtn; }else if(len == sizeof(SSlcmd)){ ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); DBG("Long command"); #ifdef EBUG loglcmd((SSlcmd*)cmd); #endif - if(!wr(dlcmd, &a, 1)) goto rtn; + if(!wr(dlcmd, NULL, 1)) goto rtn; }else{ goto rtn; } - DBG("Write %d bytes and wait for ans", len); data_t d; d.buf = cmd; d.len = d.maxlen = len; diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index 47390b7..dd4a95d 100644 --- a/LibSidServo/serial.h +++ b/LibSidServo/serial.h @@ -40,3 +40,5 @@ int MountWriteReadRaw(const data_t *out, data_t *in); int cmdS(SSscmd *cmd); int cmdL(SSlcmd *cmd); int cmdC(SSconfig *conf, int rw); +void getXspeed(); +void getYspeed(); diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 5cd6a73..ca293b8 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -78,6 +78,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){ m->encXposition.val = X_ENC2RAD(s->Xenc); m->encYposition.val = Y_ENC2RAD(s->Yenc); m->encXposition.t = m->encYposition.t = t; + getXspeed(); getYspeed(); } m->keypad = s->keypad; m->extradata.ExtraBits = s->ExtraBits; @@ -164,9 +165,7 @@ int SSstop(int emerg){ const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX; const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY; for(; i < 10; ++i){ - DBG("t1: %g", nanotime()); if(!SStextcmd(cmdx, NULL)) continue; - DBG("t2: %g", nanotime()); if(SStextcmd(cmdy, NULL)) break; } if(i == 10) return FALSE; @@ -176,6 +175,7 @@ int SSstop(int emerg){ // update motors' positions due to encoders' mcc_errcodes_t updateMotorPos(){ mountdata_t md = {0}; + if(Conf.RunModel) return MCC_E_OK; double t0 = nanotime(), t = 0.; DBG("start @ %g", t0); do{