From f6edc12b011eb53c2051bf3cc0c1a193dc79fde9 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Sat, 1 Nov 2025 15:02:11 +0300 Subject: [PATCH] fixed time --- Auxiliary_utils/LibSidServo/PID.c | 11 +++-- Auxiliary_utils/LibSidServo/examples/conf.c | 2 +- Auxiliary_utils/LibSidServo/examples/dump.c | 16 ++++--- Auxiliary_utils/LibSidServo/examples/dump.h | 1 + .../examples/dumpmoving_dragNtrack.c | 2 +- .../LibSidServo/examples/dumpswing.c | 28 +++++++---- Auxiliary_utils/LibSidServo/examples/goto.c | 8 ++-- .../LibSidServo/examples/scmd_traectory.c | 20 ++++---- .../LibSidServo/examples/servo.conf | 2 +- .../LibSidServo/examples/traectories.c | 1 + .../LibSidServo/libsidservo.creator.user | 18 +++---- Auxiliary_utils/LibSidServo/main.c | 30 +++++------- Auxiliary_utils/LibSidServo/main.h | 3 +- Auxiliary_utils/LibSidServo/movingmodel.c | 5 ++ Auxiliary_utils/LibSidServo/ramp.c | 15 +++--- Auxiliary_utils/LibSidServo/serial.c | 47 ++++++++++++++++--- Auxiliary_utils/LibSidServo/sidservo.h | 5 +- Auxiliary_utils/LibSidServo/ssii.c | 1 + 18 files changed, 139 insertions(+), 76 deletions(-) diff --git a/Auxiliary_utils/LibSidServo/PID.c b/Auxiliary_utils/LibSidServo/PID.c index 8afe031..4a3e874 100644 --- a/Auxiliary_utils/LibSidServo/PID.c +++ b/Auxiliary_utils/LibSidServo/PID.c @@ -29,6 +29,7 @@ PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){ if(!gain || Iarrsz < 3) return NULL; PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); pid->gain = *gain; + DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D); pid->pidIarrSize = Iarrsz; pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); return pid; @@ -141,9 +142,9 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t double dt = tagpos->t - pid->prevT; if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME; pid->prevT = tagpos->t; - //DBG("CALC PID (er=%g, dt=%g)", error, dt); + DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state); double tagspeed = pid_calculate(pid, error, dt); - if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based + if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based return tagspeed; // coordinate-based } @@ -176,13 +177,15 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi axis.position = m.encXposition; axis.speed = m.encXspeed; tagspeed.X = getspeed(&target->X, &pidX, &axis); - if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED; + if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X; + if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED; axis_status_t xstate = axis.state; axis.state = m.Ystate; axis.position = m.encYposition; axis.speed = m.encYspeed; tagspeed.Y = getspeed(&target->Y, &pidY, &axis); - if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED; + if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y; + if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED; axis_status_t ystate = axis.state; if(m.Xstate != xstate || m.Ystate != ystate){ DBG("State changed"); diff --git a/Auxiliary_utils/LibSidServo/examples/conf.c b/Auxiliary_utils/LibSidServo/examples/conf.c index 1271a21..5439925 100644 --- a/Auxiliary_utils/LibSidServo/examples/conf.c +++ b/Auxiliary_utils/LibSidServo/examples/conf.c @@ -52,7 +52,7 @@ static sl_option_t opts[] = { {"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)"}, {"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)"}, + {"SepEncoder", NEED_ARG, 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"}, diff --git a/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c index 27fe123..6bb6620 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.c +++ b/Auxiliary_utils/LibSidServo/examples/dump.c @@ -23,6 +23,9 @@ #include "dump.h" #include "simpleconv.h" +// starting dump time (to conform different logs) +static double dumpT0 = -1.; + #if 0 // amount of elements used for encoders' data filtering #define NFILT (10) @@ -59,6 +62,10 @@ static double filter(double val, int idx){ } #endif +// return starting time of dump +double dumpt0(){ return dumpT0; } + + /** * @brief logmnt - log mount data into file * @param fcoords - file to dump @@ -67,17 +74,13 @@ static double filter(double val, int idx){ void logmnt(FILE *fcoords, mountdata_t *m){ if(!fcoords) return; //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) VX(d/s) VY(d/s) millis\n"); return; - } - double tnow = (m->encXposition.t + m->encYposition.t) / 2.; - if(t0 < 0.) t0 = tnow; - double t = tnow - t0; + }else if(dumpT0 < 0.) dumpT0 = m->encXposition.t; // write data 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), + m->encXposition.t - dumpT0, 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); @@ -143,6 +146,7 @@ void waitmoving(int N){ if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ xlast = mdata.motXposition.val; ylast = mdata.motYposition.val; + //DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast)); ctr = 0; }else ++ctr; } diff --git a/Auxiliary_utils/LibSidServo/examples/dump.h b/Auxiliary_utils/LibSidServo/examples/dump.h index 728dd27..0089539 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.h +++ b/Auxiliary_utils/LibSidServo/examples/dump.h @@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N); void waitmoving(int N); int getPos(coordval_pair_t *mot, coordval_pair_t *enc); void chk0(int ncycles); +double dumpt0(); diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c index 67dcf71..487d826 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -216,7 +216,7 @@ int main(int argc, char **argv){ sleep(5); // return to zero and wait green("Return 2 zero and wait\n"); - if(!return2zero()) ERRX("Can't return"); + if(MCC_E_OK != return2zero()) ERRX("Can't return"); Wait(0., 0); Wait(0., 1); // wait moving ends diff --git a/Auxiliary_utils/LibSidServo/examples/dumpswing.c b/Auxiliary_utils/LibSidServo/examples/dumpswing.c index 2facc3d..01002ab 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpswing.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpswing.c @@ -110,16 +110,28 @@ int main(int argc, char **argv){ return 1; } if(G.coordsoutput){ - if(!(fcoords = fopen(G.coordsoutput, "w"))) - ERRX("Can't open %s", G.coordsoutput); + if(!(fcoords = fopen(G.coordsoutput, "w"))){ + WARNX("Can't open %s", G.coordsoutput); + return 1; + } }else fcoords = stdout; - if(G.Ncycles < 7) ERRX("Ncycles should be >7"); + if(G.Ncycles < 2){ + WARNX("Ncycles should be >2"); + return 1; + } double absamp = fabs(G.amplitude); - if(absamp < 0.01 || absamp > 45.) - ERRX("Amplitude should be from 0.01 to 45 degrees"); - if(G.period < 0.1 || G.period > 900.) - ERRX("Period should be from 0.1 to 900s"); - if(G.Nswings < 1) ERRX("Nswings should be more than 0"); + if(absamp < 0.01 || absamp > 45.){ + WARNX("Amplitude should be from 0.01 to 45 degrees"); + return 1; + } + if(G.period < 0.1 || G.period > 900.){ + WARNX("Period should be from 0.1 to 900s"); + return 1; + } + if(G.Nswings < 1){ + WARNX("Nswings should be more than 0"); + return 1; + } conf_t *Config = readServoConf(G.conffile); if(!Config){ dumpConf(); diff --git a/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c index 1038926..15c0796 100644 --- a/Auxiliary_utils/LibSidServo/examples/goto.c +++ b/Auxiliary_utils/LibSidServo/examples/goto.c @@ -61,12 +61,13 @@ static FILE* fcoords = NULL; static pthread_t dthr; void signals(int sig){ - pthread_cancel(dthr); if(sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } + DBG("Quit"); Mount.quit(); + DBG("close"); if(fcoords) fclose(fcoords); exit(sig); } @@ -90,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){ if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); coordval_pair_t M, E; if(!getPos(&M, &E)) ERRX("Can't get current position"); + DBG("xt: %g, x: %g", M.X.t, M.X.val); if(G.coordsoutput){ - if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); + if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n"); G.wait = 1; - } - if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); logmnt(fcoords, NULL); diff --git a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c index 5acf339..3d0e81c 100644 --- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c +++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c @@ -75,7 +75,6 @@ static sl_option_t cmdlnopts[] = { }; void signals(int sig){ - pthread_cancel(dthr); if(sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); @@ -99,7 +98,7 @@ static void runtraectory(traectory_fn tfn){ coordval_pair_t target; coordpair_t traectXY, endpoint; endpoint.X = G.Xmax, endpoint.Y = G.Ymax; - double t0 = Mount.currentT(), tlast = 0.; + double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT(); double tlastX = 0., tlastY = 0.; while(1){ if(!telpos(&telXY)){ @@ -107,10 +106,10 @@ static void runtraectory(traectory_fn tfn){ return; } if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind - DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t); + DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t); tlastX = telXY.X.t; tlastY = telXY.Y.t; double t = Mount.currentT(); - if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break; + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break; if(!traectory_point(&traectXY, t)) break; target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.t = target.Y.t = t; @@ -119,11 +118,14 @@ static void runtraectory(traectory_fn tfn){ else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax; if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax; else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax; - DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); - DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); - DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); - if(errlog) - fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + if(t0 < 0.) t0 = dumpt0(); + else{ + //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); + DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); + if(errlog) + fprintf(errlog, "%10.4f %10.4f %10.4f\n", telXY.X.t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + } if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!"); while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50); tlast = t; diff --git a/Auxiliary_utils/LibSidServo/examples/servo.conf b/Auxiliary_utils/LibSidServo/examples/servo.conf index 220926e..9e7d3d4 100644 --- a/Auxiliary_utils/LibSidServo/examples/servo.conf +++ b/Auxiliary_utils/LibSidServo/examples/servo.conf @@ -1,4 +1,4 @@ -Current configuration: +# Current configuration MountDevPath=/dev/ttyUSB0 MountDevSpeed=19200 EncoderDevPath=(null) diff --git a/Auxiliary_utils/LibSidServo/examples/traectories.c b/Auxiliary_utils/LibSidServo/examples/traectories.c index ebc31cf..1baac90 100644 --- a/Auxiliary_utils/LibSidServo/examples/traectories.c +++ b/Auxiliary_utils/LibSidServo/examples/traectories.c @@ -76,6 +76,7 @@ int telpos(coordval_pair_t *curpos){ } if(ntries == 10) return FALSE; coordval_pair_t pt; + //DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t); pt.X.val = mdata.encXposition.val; pt.Y.val = mdata.encYposition.val; pt.X.t = mdata.encXposition.t; diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index 73de00a..c62ce51 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -110,8 +110,8 @@ GenericProjectManager.GenericMakeStep 1 - Сборка - Сборка + Build + Build ProjectExplorer.BuildSteps.Build @@ -123,8 +123,8 @@ GenericProjectManager.GenericMakeStep 1 - Очистка - Очистка + Clean + Clean ProjectExplorer.BuildSteps.Clean 2 @@ -139,8 +139,8 @@ 0 - Развёртывание - Развёртывание + Deploy + Deploy ProjectExplorer.BuildSteps.Deploy 1 @@ -173,8 +173,8 @@ 0 - Развёртывание - Развёртывание + Deploy + Deploy ProjectExplorer.BuildSteps.Deploy 1 diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index 6927a4b..d6d267c 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -51,17 +51,18 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd); * @return time in seconds */ double nanotime(){ - static struct timespec *start = NULL; + //static struct timespec *start = NULL; struct timespec now; - if(!start){ + /*if(!start){ start = malloc(sizeof(struct timespec)); if(!start) return -1.; if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.; - } + }*/ if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.; - double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; + /*double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; double sd = (double)now.tv_sec - (double)start->tv_sec; - return sd + nd; + return sd + nd;*/ + return (double)now.tv_sec + (double)now.tv_nsec * 1e-9; } /** @@ -75,10 +76,8 @@ static void quit(){ DBG("Exit"); } -void getModData(mountdata_t *mountdata){ - if(!mountdata || !Xmodel || !Ymodel) return; - static double oldmt = -100.; // old `millis measurement` time - static uint32_t oldmillis = 0; +void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst){ + if(!c || !Xmodel || !Ymodel) return; double tnow = nanotime(); moveparam_t Xp, Yp; movestate_t Xst = Xmodel->get_state(Xmodel, &Xp); @@ -86,14 +85,11 @@ void getModData(mountdata_t *mountdata){ 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); - 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; - getXspeed(); getYspeed(); - if(tnow - oldmt > Conf.MountReqInterval){ - oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3); - oldmt = tnow; - }else mountdata->millis = oldmillis; + c->X.t = c->Y.t = tnow; + c->X.val = Xp.coord; + c->Y.val = Yp.coord; + if(xst) *xst = Xst; + if(yst) *yst = Yst; } /** diff --git a/Auxiliary_utils/LibSidServo/main.h b/Auxiliary_utils/LibSidServo/main.h index 0eb9fe2..c8ae4c9 100644 --- a/Auxiliary_utils/LibSidServo/main.h +++ b/Auxiliary_utils/LibSidServo/main.h @@ -24,11 +24,12 @@ #include +#include "movingmodel.h" #include "sidservo.h" extern conf_t Conf; double nanotime(); -void getModData(mountdata_t *mountdata); +void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst); 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 diff --git a/Auxiliary_utils/LibSidServo/movingmodel.c b/Auxiliary_utils/LibSidServo/movingmodel.c index 83936ac..0472459 100644 --- a/Auxiliary_utils/LibSidServo/movingmodel.c +++ b/Auxiliary_utils/LibSidServo/movingmodel.c @@ -63,6 +63,11 @@ int model_move2(movemodel_t *model, moveparam_t *target, double t){ DBG("MOVE to %g at speed %g", target->coord, target->speed); // only positive velocity if(target->speed < 0.) target->speed = -target->speed; + if(fabs(target->speed) < model->Min.speed){ + DBG("STOP"); + model->stop(model, t); + return TRUE; + } // don't mind about acceleration - user cannot set it now return model->calculate(model, target, t); } diff --git a/Auxiliary_utils/LibSidServo/ramp.c b/Auxiliary_utils/LibSidServo/ramp.c index 6e88293..b9f4728 100644 --- a/Auxiliary_utils/LibSidServo/ramp.c +++ b/Auxiliary_utils/LibSidServo/ramp.c @@ -23,12 +23,14 @@ #include "main.h" #include "ramp.h" -/* + #ifdef EBUG #undef DBG #define DBG(...) +#undef FNAME +#define FNAME() #endif -*/ + static double coord_tolerance = COORD_TOLERANCE_DEFAULT; static void emstop(movemodel_t *m, double _U_ t){ @@ -120,7 +122,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){ } }else{ // if we are here, we have the worst case: change speed direction - DBG("Hardest case: change speed direction"); + // DBG("Hardest case: change speed direction"); // now we should calculate coordinate at which model stops and biuld new trapezium from that point double x0 = m->curparams.coord, v0 = m->curparams.speed; double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a; @@ -132,7 +134,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){ m->Times[STAGE_ACCEL] = t; m->Params[STAGE_ACCEL].coord = x0; m->Params[STAGE_ACCEL].speed = v0; - DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0); + // DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0); return; } m->state = ST_MOVE; @@ -210,11 +212,10 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){ if(m->movingstage == STAGE_STOPPED){ m->curparams.coord = m->Params[STAGE_STOPPED].coord; pthread_mutex_unlock(&m->mutex); - DBG("REACHED STOPping stage @ t=%g", t); + /* DBG("REACHED STOPping stage @ t=%g", t); for(int s = STAGE_STOPPED; s >= 0; --s){ DBG("T[%d]=%g, ", s, m->Times[s]); - } - fflush(stdout); + }*/ emstop(m, t); goto ret; } diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index 8e7f82b..5b5c410 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -59,16 +59,20 @@ typedef struct __attribute__((packed)){ // calculate current X/Y speeds void getXspeed(){ + static double t0 = -1.; // time of start - eliminate problem of very large times in squares + if(t0 < 0.) t0 = mountdata.encXposition.t; 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); + pthread_mutex_lock(&datamutex); + double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0); if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; } + pthread_mutex_unlock(&datamutex); //DBG("Xspeed=%g", mountdata.encXspeed.val); #if 0 mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t); @@ -78,16 +82,20 @@ void getXspeed(){ #endif } void getYspeed(){ + static double t0 = -1.; // time of start - eliminate problem of very large times in squares + if(t0 < 0.) t0 = mountdata.encXposition.t; 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); + pthread_mutex_lock(&datamutex); + double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0); if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ mountdata.encYspeed.val = speed; mountdata.encYspeed.t = mountdata.encYposition.t; } + pthread_mutex_unlock(&datamutex); #if 0 mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t); mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.; @@ -325,15 +333,19 @@ static void *encoderthread2(void _U_ *u){ } double v, t; if(getencval(encfd[0], &v, &t)){ + pthread_mutex_lock(&datamutex); mountdata.encXposition.val = X_ENC2RAD(v); //DBG("encX(%g) = %g", t, mountdata.encXposition.val); mountdata.encXposition.t = t; + pthread_mutex_unlock(&datamutex); //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); getXspeed(); if(getencval(encfd[1], &v, &t)){ + pthread_mutex_lock(&datamutex); mountdata.encYposition.val = Y_ENC2RAD(v); //DBG("encY(%g) = %g", t, mountdata.encYposition.val); mountdata.encYposition.t = t; + pthread_mutex_unlock(&datamutex); //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); getYspeed(); errctr = 0; @@ -384,12 +396,35 @@ static void *mountthread(void _U_ *u){ uint8_t buf[2*sizeof(SSstat)]; SSstat *status = (SSstat*) buf; bzero(&mountdata, sizeof(mountdata)); + double t0 = nanotime(), tstart = t0; + static double oldmt = -100.; // old `millis measurement` time + static uint32_t oldmillis = 0; if(Conf.RunModel) while(1){ - pthread_mutex_lock(&datamutex); + coordval_pair_t c; + movestate_t xst, yst; // now change data - getModData(&mountdata); + getModData(&c, &xst, &yst); + pthread_mutex_lock(&datamutex); + double tnow = c.X.t; + mountdata.encXposition.t = mountdata.encYposition.t = tnow; + mountdata.encXposition.val = c.X.val; + mountdata.encYposition.val = c.Y.val; + //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); + if(tnow - oldmt > Conf.MountReqInterval){ + oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3); + mountdata.motYposition.t = mountdata.motXposition.t = tnow; + if(xst == ST_MOVE) + mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motXposition.val = c.X.val; + if(yst == ST_MOVE) + mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motYposition.val = c.Y.val; + oldmt = tnow; + }else mountdata.millis = oldmillis; pthread_mutex_unlock(&datamutex); - double t0 = nanotime(); + getXspeed(); getYspeed(); while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); t0 = nanotime(); } @@ -572,7 +607,7 @@ void closeSerial(){ DBG("close encoder's fd"); close(encfd[0]); encfd[0] = -1; - if(Conf.SepEncoder == 2){ + if(Conf.SepEncoder == 2 && encfd[1] > -1){ close(encfd[1]); encfd[1] = -1; } diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index e43bacf..a3dc44c 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -56,9 +56,10 @@ extern "C" // normal PID refresh interval #define MCC_PID_REFRESH_DT (0.1) // boundary conditions for axis state: "slewing/pointing/guiding" -// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees +// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees //#define MCC_MAX_POINTING_ERR (0.20943951) -#define MCC_MAX_POINTING_ERR (0.08726646) +//#define MCC_MAX_POINTING_ERR (0.08726646) +#define MCC_MAX_POINTING_ERR (0.13962634) // if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg #define MCC_MAX_GUIDING_ERR (0.026179939) // if error less than this value we suppose that target is captured and guiding is good: 0.1'' diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index 51ed8b1..cb4526a 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -18,6 +18,7 @@ #include #include +#include #include #include