From 9f2e893f1a74b8ad60d41b55cf6c0b3dd5d1811d Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Mon, 23 Mar 2026 17:22:58 +0300 Subject: [PATCH] seems like PID works on real telescope --- Auxiliary_utils/LibSidServo/CMakeLists.txt | 10 +- Auxiliary_utils/LibSidServo/PID.c | 130 +++++--- Auxiliary_utils/LibSidServo/PID.h | 13 +- .../LibSidServo/examples/scmd_traectory.c | 31 +- .../LibSidServo/examples/traectories.c | 22 +- Auxiliary_utils/LibSidServo/kalman.c | 169 ++++++++++ Auxiliary_utils/LibSidServo/kalman.h | 34 ++ .../LibSidServo/libsidservo.creator.user | 22 +- Auxiliary_utils/LibSidServo/libsidservo.files | 2 + Auxiliary_utils/LibSidServo/main.c | 60 ++-- Auxiliary_utils/LibSidServo/serial.c | 309 +++++++++--------- Auxiliary_utils/LibSidServo/servo_real.conf | 23 ++ Auxiliary_utils/LibSidServo/sidservo.h | 12 +- Auxiliary_utils/LibSidServo/ssii.c | 39 ++- Auxiliary_utils/LibSidServo/ssii.h | 2 + 15 files changed, 598 insertions(+), 280 deletions(-) create mode 100644 Auxiliary_utils/LibSidServo/kalman.c create mode 100644 Auxiliary_utils/LibSidServo/kalman.h create mode 100644 Auxiliary_utils/LibSidServo/servo_real.conf diff --git a/Auxiliary_utils/LibSidServo/CMakeLists.txt b/Auxiliary_utils/LibSidServo/CMakeLists.txt index bdf2c99..04a2133 100644 --- a/Auxiliary_utils/LibSidServo/CMakeLists.txt +++ b/Auxiliary_utils/LibSidServo/CMakeLists.txt @@ -15,6 +15,8 @@ set(CMAKE_COLOR_MAKEFILE ON) option(DEBUG "Compile in debug mode" OFF) option(EXAMPLES "Compile also some examples" ON) +option(BUILD_SHARED "Build shared libarary" OFF) + # cmake -DDEBUG=on -> debugging if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") @@ -52,7 +54,13 @@ aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES) #list(APPEND ${PROJ}_LIBRARIES "-lfftw3_threads") # library -add_library(${PROJ} SHARED ${SOURCES}) + +if(BUILD_SHARED) + add_library(${PROJ} SHARED ${SOURCES}) +else() + add_library(${PROJ} STATIC ${SOURCES}) +endif() + # library header files set(LIBHEADER "sidservo.h") # -I diff --git a/Auxiliary_utils/LibSidServo/PID.c b/Auxiliary_utils/LibSidServo/PID.c index 3f642cd..458b247 100644 --- a/Auxiliary_utils/LibSidServo/PID.c +++ b/Auxiliary_utils/LibSidServo/PID.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include #include #include @@ -25,34 +26,70 @@ #include "PID.h" #include "serial.h" -PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){ +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double prev_tagpos; // previous target position + double integral; // Integral term + double *pidIarray; // array for Integral + struct timespec prevT; // time of previous correction + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +typedef struct{ + axis_status_t state; + coordval_t position; + coordval_t speed; +} axisdata_t; + +static 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)); + curtime(&pid->prevT); return pid; } // don't clear lastT! -void pid_clear(PIDController_t *pid){ +static void pid_clear(PIDController_t *pid){ if(!pid) return; DBG("CLEAR PID PARAMETERS"); bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); pid->integral = 0.; pid->prev_error = 0.; pid->curIidx = 0; + curtime(&pid->prevT); } - -void pid_delete(PIDController_t **pid){ +/* +static void pid_delete(PIDController_t **pid){ if(!pid || !*pid) return; if((*pid)->pidIarray) free((*pid)->pidIarray); free(*pid); *pid = NULL; -} +}*/ -double pid_calculate(PIDController_t *pid, double error, double dt){ +// calculate new motor speed +static double pid_calculate(PIDController_t *pid, double axispos, const coordval_t *target){ + double dtpid = timediff(&target->t, &pid->prevT); + if(dtpid < 0 || dtpid > Conf.PIDMaxDt){ + DBG("time diff too big: clear PID"); + pid_clear(pid); + pid->prev_tagpos = target->val; + return 0.; + } + double dt = timediff(&target->t, &pid->prevT); + if(dt < FLT_EPSILON){ + DBG("Target time in past"); + return 0.; + } + pid->prevT = target->t; + double error = target->val - axispos; + double tagspeed = (target->val - pid->prev_tagpos) / dt; + pid->prev_tagpos = target->val; // calculate flowing integral double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; //DBG("oldi/new: %g, %g", oldi, newi); @@ -61,43 +98,33 @@ double pid_calculate(PIDController_t *pid, double error, double dt){ pid->integral += newi - oldi; double derivative = (error - pid->prev_error) / dt; pid->prev_error = error; - double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; - DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); + DBG("pid pars: P=%g, I=%g, D=%f", pid->gain.P, pid->gain.I, pid->gain.D); + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative + tagspeed; + DBG("tagspeed=%g, P=%g, I=%g, D=%g; sum=%g", tagspeed, pid->gain.P * error, + pid->gain.I * pid->integral, pid->gain.D * derivative, sum); return sum; } -typedef struct{ - PIDController_t *PIDC; - PIDController_t *PIDV; -} PIDpair_t; - -typedef struct{ - axis_status_t state; - coordval_t position; - coordval_t speed; -} axisdata_t; /** * @brief process - Process PID for given axis * @param tagpos - given coordinate of target position * @param endpoint - endpoint for this coordinate * @param pid - pid itself - * @return calculated new speed or -1 for max speed + * @return calculated NEW SPEED or NAN for max speed */ -static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ +static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_t *axis){ double dt = timediff(&tagpos->t, &axis->position.t); if(dt < 0 || dt > Conf.PIDMaxDt){ DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt); return axis->speed.val; // data is too old or wrong } double error = tagpos->val - axis->position.val, fe = fabs(error); - DBG("error: %g", error); - PIDController_t *pid = NULL; + DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI); switch(axis->state){ case AXIS_SLEWING: - if(fe < Conf.MaxPointingErr){ + if(fe < Conf.MaxFinePointingErr){ axis->state = AXIS_POINTING; DBG("--> Pointing"); - pid = pidpair->PIDC; }else{ DBG("Slewing..."); return NAN; // max speed for given axis @@ -107,28 +134,26 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t if(fe < Conf.MaxFinePointingErr){ axis->state = AXIS_GUIDING; DBG("--> Guiding"); - pid = pidpair->PIDV; }else if(fe > Conf.MaxPointingErr){ DBG("--> Slewing"); axis->state = AXIS_SLEWING; return NAN; - } else pid = pidpair->PIDC; + } break; case AXIS_GUIDING: - pid = pidpair->PIDV; if(fe > Conf.MaxFinePointingErr){ DBG("--> Pointing"); axis->state = AXIS_POINTING; - pid = pidpair->PIDC; }else if(fe < Conf.MaxGuidingErr){ DBG("At target"); // TODO: we can point somehow that we are at target or introduce new axis state - }else DBG("Current error: %g", fe); + }else DBG("Current abs error: %g", fe); break; + case AXIS_GONNASTOP: case AXIS_STOPPED: // start pointing to target; will change speed next time DBG("AXIS STOPPED!!!! --> Slewing"); axis->state = AXIS_SLEWING; - return getspeed(tagpos, pidpair, axis); + return getspeed(tagpos, pid, axis); case AXIS_ERROR: DBG("Can't move from erroneous state"); return 0.; @@ -137,17 +162,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t DBG("WTF? Where is a PID?"); return axis->speed.val; } - double dtpid = timediff(&tagpos->t, &pid->prevT); - if(dtpid < 0 || dtpid > Conf.PIDMaxDt){ - DBG("time diff too big: clear PID"); - pid_clear(pid); - } - if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt; - pid->prevT = tagpos->t; - DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state); - double tagspeed = pid_calculate(pid, error, dtpid); - if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based - return tagspeed; // coordinate-based + return pid_calculate(pid, axis->position.val, tagpos); } /** @@ -157,18 +172,14 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t * @return error code */ mcc_errcodes_t correct2(const coordval_pair_t *target){ - static PIDpair_t pidX = {0}, pidY = {0}; - if(!pidX.PIDC){ - pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidX.PIDC) return MCC_E_FATAL; - pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidX.PIDV) return MCC_E_FATAL; + static PIDController_t *pidX = NULL, *pidY = NULL; + if(!pidX){ + pidX = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidX) return MCC_E_FATAL; } - if(!pidY.PIDC){ - pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidY.PIDC) return MCC_E_FATAL; - pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidY.PIDV) return MCC_E_FATAL; + if(!pidY){ + pidY = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidY) return MCC_E_FATAL; } mountdata_t m; coordpair_t tagspeed; // absolute value of speed @@ -179,7 +190,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ axis.state = m.Xstate; axis.position = m.encXposition; axis.speed = m.encXspeed; - tagspeed.X = getspeed(&target->X, &pidX, &axis); + tagspeed.X = getspeed(&target->X, pidX, &axis); if(isnan(tagspeed.X)){ // max speed if(target->X.val < axis.position.val) Xsign = -1.; tagspeed.X = Xlimits.max.speed; @@ -191,7 +202,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ axis.state = m.Ystate; axis.position = m.encYposition; axis.speed = m.encYspeed; - tagspeed.Y = getspeed(&target->Y, &pidY, &axis); + tagspeed.Y = getspeed(&target->Y, pidY, &axis); if(isnan(tagspeed.Y)){ // max speed if(target->Y.val < axis.position.val) Ysign = -1.; tagspeed.Y = Ylimits.max.speed; @@ -205,6 +216,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ setStat(xstate, ystate); } coordpair_t endpoint; +#if 0 // allow at least PIDMaxDt moving with target speed double dv = fabs(tagspeed.X - m.encXspeed.val); double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed @@ -216,6 +228,16 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ + Conf.PIDMaxDt * tagspeed.Y + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.; endpoint.Y = m.encYposition.val + Ysign * adder; +#endif + // allow 10s moving but not more than 10deg and not less than 1deg + double adder = fabs(tagspeed.X) * 10.; + if(adder > 0.17453) adder = 0.17453; + else if(adder < 0.017453) adder = 0.017453; + endpoint.X = m.encXposition.val + Xsign * adder; + adder = fabs(tagspeed.Y) * 10.; + if(adder > 0.17453) adder = 0.17453; + else if(adder < 0.017453) adder = 0.017453; + endpoint.Y = m.encYposition.val + Ysign * adder; DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.); return Mount.moveWspeed(&endpoint, &tagspeed); } diff --git a/Auxiliary_utils/LibSidServo/PID.h b/Auxiliary_utils/LibSidServo/PID.h index 5ecfad0..79539f7 100644 --- a/Auxiliary_utils/LibSidServo/PID.h +++ b/Auxiliary_utils/LibSidServo/PID.h @@ -22,19 +22,10 @@ #include "sidservo.h" -typedef struct { - PIDpar_t gain; // PID gains - double prev_error; // Previous error - double integral; // Integral term - double *pidIarray; // array for Integral - struct timespec prevT; // time of previous correction - size_t pidIarrSize; // it's size - size_t curIidx; // and index of current element -} PIDController_t; - +/* PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz); void pid_clear(PIDController_t *pid); void pid_delete(PIDController_t **pid); double pid_calculate(PIDController_t *pid, double error, double dt); - +*/ mcc_errcodes_t correct2(const coordval_pair_t *target); diff --git a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c index 67ad655..71981d2 100644 --- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c +++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c @@ -109,17 +109,27 @@ static void runtraectory(traectory_fn tfn){ } if(!Mount.currentT(&tcur)) continue; if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind - DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val)); tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec; double t = Mount.timeFromStart(); - if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break; - if(!traectory_point(&traectXY, t)) break; + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax){ + if(fabs(telXY.X.val) > G.Xmax) DBG("X over maximal limit!"); + if(fabs(telXY.Y.val) > G.Ymax) DBG("Y over maximal limit!"); + if(t - tstart > G.tmax) DBG("Time over..."); + break; + } + if(!traectory_point(&traectXY, t)){ + DBG("Error in 'traectory_point', time from start=%g", t); + break; + } + DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f); traectory: %g'/%g' (%.6f/%.6f)", + RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), + RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y), RAD2DEG(traectXY.X), RAD2DEG(traectXY.Y)); target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.t = target.Y.t = tcur; if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0); else{ //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); - DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + DBG("%g, target-telescope: dX=%.4f'', dY=%.4f''", t-tstart, 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", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); @@ -146,10 +156,12 @@ int main(int argc, char **argv){ ERRX("Can't open error log %s", G.errlog); else fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n"); + setbuf(errlog, NULL); } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); + setbuf(fcoords, NULL); }else fcoords = stdout; Config = readServoConf(G.conffile); if(!Config || G.dumpconf){ @@ -164,21 +176,22 @@ int main(int argc, char **argv){ return 1; } coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; - if(!init_traectory(tfn, &c)){ - ERRX("Can't init traectory"); - return 1; - } mcc_errcodes_t e = Mount.init(Config); if(e != MCC_E_OK){ WARNX("Can't init devices"); return 1; } + // run this function only after mount inited! + if(!init_traectory(tfn, &c)){ + ERRX("Can't init traectory"); + return 1; + } signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - chk0(G.Ncycles); +// chk0(G.Ncycles); logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); ; diff --git a/Auxiliary_utils/LibSidServo/examples/traectories.c b/Auxiliary_utils/LibSidServo/examples/traectories.c index e51f15a..3f26a5a 100644 --- a/Auxiliary_utils/LibSidServo/examples/traectories.c +++ b/Auxiliary_utils/LibSidServo/examples/traectories.c @@ -42,6 +42,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){ cur_traectory = f; XYstart = *XY0; tstart = Mount.timeFromStart(); + DBG("inited @ %gs from start", tstart); mountdata_t mdata; int ntries = 0; for(; ntries < 10; ++ntries){ @@ -58,11 +59,21 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){ * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180) */ int traectory_point(coordpair_t *nextpt, double t){ - if(t < 0. || !cur_traectory) return FALSE; + if(t < 0. || !cur_traectory){ + if(t < 0.) DBG("time in past!"); + else DBG("no current traectory selected!"); + return FALSE; + } coordpair_t pt; - if(!cur_traectory(&pt, t)) return FALSE; + if(!cur_traectory(&pt, t)){ + DBG("error in cur_traectory"); + return FALSE; + } if(nextpt) *nextpt = pt; - if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; + if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI){ + DBG("some points is over limits, pt.x=%g, pt.y=%g degrees", RAD2DEG(pt.X), RAD2DEG(pt.Y)); + return FALSE; + } return TRUE; } @@ -88,6 +99,7 @@ int telpos(coordval_pair_t *curpos){ // X=X0+1'/s, Y=Y0+15''/s int Linear(coordpair_t *nextpt, double t){ coordpair_t pt; + DBG("t=%g, tfromstart=%g, dt=%g", t, tstart, t-tstart); pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart); pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); if(nextpt) *nextpt = pt; @@ -98,7 +110,7 @@ int Linear(coordpair_t *nextpt, double t){ int SinCos(coordpair_t *nextpt, double t){ coordpair_t pt; pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI); - pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI); + pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/10.*2*M_PI); if(nextpt) *nextpt = pt; return TRUE; } @@ -111,7 +123,7 @@ typedef struct{ static tr_names names[] = { {Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"}, - {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"}, + {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+1'*cos(t/10*2pi)"}, {NULL, NULL, NULL} }; diff --git a/Auxiliary_utils/LibSidServo/kalman.c b/Auxiliary_utils/LibSidServo/kalman.c new file mode 100644 index 0000000..0c569bd --- /dev/null +++ b/Auxiliary_utils/LibSidServo/kalman.c @@ -0,0 +1,169 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "kalman.h" + +void kalman3_init(Kalman3 *kf, double dt, double enc_var){ + kf->dt = dt; + + kf->x[0] = 0; + kf->x[1] = 0; + kf->x[2] = 0; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + kf->P[i][j] = 0; + + kf->P[0][0] = 1; + kf->P[1][1] = 1; + kf->P[2][2] = 1; + + // process noise + kf->Q[0][0] = 1e-6; + kf->Q[1][1] = 1e-4; + kf->Q[2][2] = 1e-3; + + kf->R = enc_var; // encoder noise variance +} + +void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j){ + double dt = kf->dt; + + double dt2 = dt*dt; + double dt3 = dt2*dt; + double dt4 = dt3*dt; + double dt5 = dt4*dt; + + double q = sigma_j * sigma_j; + + kf->Q[0][0] = q * dt5 / 20.0; + kf->Q[0][1] = q * dt4 / 8.0; + kf->Q[0][2] = q * dt3 / 6.0; + + kf->Q[1][0] = q * dt4 / 8.0; + kf->Q[1][1] = q * dt3 / 3.0; + kf->Q[1][2] = q * dt2 / 2.0; + + kf->Q[2][0] = q * dt3 / 6.0; + kf->Q[2][1] = q * dt2 / 2.0; + kf->Q[2][2] = q * dt; +} + +void kalman3_predict(Kalman3 *kf){ + double dt = kf->dt; + double dt2 = 0.5 * dt * dt; + + double theta = kf->x[0]; + double omega = kf->x[1]; + double alpha = kf->x[2]; + + // state prediction + kf->x[0] = theta + omega*dt + alpha*dt2; + kf->x[1] = omega + alpha*dt; + kf->x[2] = alpha; + + // transition matrix + double F[3][3] = + { + {1, dt, dt2}, + {0, 1, dt}, + {0, 0, 1} + }; + + // P = FPF^T + Q + + double FP[3][3]; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++){ + FP[i][j] = + F[i][0]*kf->P[0][j] + + F[i][1]*kf->P[1][j] + + F[i][2]*kf->P[2][j]; + } + + double Pnew[3][3]; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++){ + Pnew[i][j] = + FP[i][0]*F[j][0] + + FP[i][1]*F[j][1] + + FP[i][2]*F[j][2] + + kf->Q[i][j]; + } + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + kf->P[i][j] = Pnew[i][j]; +} + +void kalman3_update(Kalman3 *kf, double z){ + // innovation + double y = z - kf->x[0]; + + // S = HPH^T + R + double S = kf->P[0][0] + kf->R; + + // Kalman gain + double K[3]; + + K[0] = kf->P[0][0] / S; + K[1] = kf->P[1][0] / S; + K[2] = kf->P[2][0] / S; + + // state update + kf->x[0] += K[0] * y; + kf->x[1] += K[1] * y; + kf->x[2] += K[2] * y; + + // covariance update + double P00 = kf->P[0][0]; + double P01 = kf->P[0][1]; + double P02 = kf->P[0][2]; + + double P10 = kf->P[1][0]; + double P11 = kf->P[1][1]; + double P12 = kf->P[1][2]; + + double P20 = kf->P[2][0]; + double P21 = kf->P[2][1]; + double P22 = kf->P[2][2]; + + kf->P[0][0] = P00 - K[0]*P00; + kf->P[0][1] = P01 - K[0]*P01; + kf->P[0][2] = P02 - K[0]*P02; + + kf->P[1][0] = P10 - K[1]*P00; + kf->P[1][1] = P11 - K[1]*P01; + kf->P[1][2] = P12 - K[1]*P02; + + kf->P[2][0] = P20 - K[2]*P00; + kf->P[2][1] = P21 - K[2]*P01; + kf->P[2][2] = P22 - K[2]*P02; +} + + +// estimation of the R +double encoder_noise(int counts){ + double d = 2.0*M_PI / counts; + return d*d / 12.0; +} + diff --git a/Auxiliary_utils/LibSidServo/kalman.h b/Auxiliary_utils/LibSidServo/kalman.h new file mode 100644 index 0000000..f1aa7d8 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/kalman.h @@ -0,0 +1,34 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +typedef struct{ + double x[3]; // [theta, omega, alpha] + double P[3][3]; // covariance + double Q[3][3]; // process noise + double R; // measurement noise + double dt; +} Kalman3; + + +double encoder_noise(int counts); +void kalman3_update(Kalman3 *kf, double z); +void kalman3_predict(Kalman3 *kf); +void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j); +void kalman3_init(Kalman3 *kf, double dt, double enc_var); diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index 58c6b84..ba48f10 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/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 @@ -79,7 +79,7 @@ true true Builtin.DefaultTidyAndClazy - 8 + 4 true @@ -96,12 +96,12 @@ true Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /tmp/robo5/mountcontrol.git/LibSidServo + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions @@ -133,7 +133,7 @@ false - Default + По умолчанию GenericProjectManager.GenericBuildConfiguration 0 0 @@ -168,7 +168,6 @@ true true - %{RunConfig:Executable:Path} 1 @@ -204,7 +203,6 @@ true true - %{RunConfig:Executable:Path} 1 diff --git a/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files index aa9288a..4e1eee2 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.files +++ b/Auxiliary_utils/LibSidServo/libsidservo.files @@ -13,12 +13,14 @@ examples/dumpswing.c examples/goto.c examples/scmd_traectory.c examples/simpleconv.h +kalman.c main.c sidservo.h serial.c examples/CMakeLists.txt examples/traectories.c examples/traectories.h +kalman.h main.h movingmodel.c movingmodel.h diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index a6f04b0..827e420 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -161,6 +161,8 @@ 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]; + /*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt, + l->xsum, l->tsum, l->t2sum, l->xtsum);*/ double t2 = t * t, xt = x * t; l->x[idx] = x; l->t2[idx] = t2; l->t[idx] = t; l->xt[idx] = xt; @@ -172,9 +174,9 @@ double LS_calc_slope(less_square_t *l, double x, double t){ 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; + //DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator); // point: (sum_x - slope * sum_t) / n; return (numerator / denominator); } @@ -200,6 +202,7 @@ static mcc_errcodes_t init(conf_t *c){ if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; return MCC_E_OK; } + DBG("Try to open mount device"); if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; @@ -207,33 +210,53 @@ static mcc_errcodes_t init(conf_t *c){ DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed); ret = MCC_E_MOUNTDEV; } - if(Conf.SepEncoder){ - if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ - DBG("Define encoder device path"); - ret = MCC_E_BADFORMAT; - }else if(!openEncoder()){ - DBG("Can't open encoder device"); - ret = MCC_E_ENCODERDEV; - } - } // TODO: read hardware configuration on init if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ DBG("Wrong speed interval"); ret = MCC_E_BADFORMAT; } - if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; + DBG("Exit ACM, exit manual mode"); + SSrawcmd(CMD_EXITACM, NULL); + SStextcmd(CMD_AUTOX, NULL); + SStextcmd(CMD_AUTOY, NULL); // read HW config to update constants hardware_configuration_t HW; - if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; + DBG("Read hardware configuration"); + ret = MCC_E_FAILED; + for(int i = 0; i < MAX_ERR_CTR; ++i){ + DBG("TRY %d..", i); + ret = get_hwconf(&HW); + if(ret == MCC_E_OK) break; + } + if(MCC_E_OK != ret) return ret; // make a pause for actual encoder's values + DBG("Check encoders"); + if(Conf.SepEncoder){ + if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ + DBG("Define encoder device path"); + ret = MCC_E_BADFORMAT; + }else{ + ret = MCC_E_ENCODERDEV; + for(int i = 0; i < MAX_ERR_CTR; ++i){ + if(openEncoder()){ + ret = MCC_E_OK; + break; + } + } + } + } + if(MCC_E_OK != ret) return ret; double t0 = timefromstart(); - while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000); + DBG("Wait for first encoders' measurement"); + while(timefromstart() - t0 < Conf.EncoderReqInterval * 15.) usleep(1000); + DBG("Update motor position"); 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); + while(timefromstart() - t0 < Conf.MountReqInterval * 5.) usleep(1000); + DBG("ALL READY!"); return e; } @@ -288,10 +311,6 @@ 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); - if(r != MCC_E_OK) return r; - setslewingstate(); - return MCC_E_OK;*/ return shortcmd(&cmd); } @@ -328,10 +347,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed cmd.Ymot = target->Y; cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y; - mcc_errcodes_t r = shortcmd(&cmd); - if(r != MCC_E_OK) return r; - setslewingstate(); - return MCC_E_OK; + return shortcmd(&cmd); } /** diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index d4fa65e..d22916e 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -31,6 +31,7 @@ #include #include +#include "kalman.h" #include "main.h" #include "movingmodel.h" #include "serial.h" @@ -49,7 +50,13 @@ 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 = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; +// this values will be modified later +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder reading timeout + mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading + mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings + +static volatile int GlobExit = 0; + // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -71,7 +78,6 @@ void getXspeed(){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; } - //DBG("Xspeed=%g", mountdata.encXspeed.val); } void getYspeed(){ static less_square_t *ls = NULL; @@ -138,77 +144,6 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ //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); } -#if 0 -/** - * @brief getencval - get uint64_t data from encoder - * @param fd - encoder fd - * @param val - value read - * @param t - measurement time - * @return amount of data read or 0 if problem - */ -static int getencval(int fd, double *val, struct timespec *t){ - 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){ - //DBG("select()==0 - timeout, %.6g", timefromstart()); - break; - } - if(retval < 0){ - 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){ - DBG("read() < 0"); - return 0; // disconnected ?? - } - got += l; Lmax -= l; - buf[got] = 0; - } else continue; - 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){ - DBG("No EOL"); - return 0; - } - *estr = 0; - char *bgn = strrchr(buf, '\n'); - if(bgn) ++bgn; - else bgn = buf; - char *eptr; - long data = strtol(bgn, &eptr, 10); - if(eptr != estr){ - DBG("NAN"); - return 0; // wrong number - } - if(val) *val = (double) data; - if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} - return got; -} -#endif - // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected static int getencbyte(){ if(encfd[0] < 0) return -1; @@ -232,43 +167,60 @@ static int getencbyte(){ }while(1); return (int)byte; } -// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected -static int getmntbyte(){ - if(mntfd < 0) return -1; - uint8_t byte; + +/** + * @brief readmntdata - read data + * @param buffer - input buffer + * @param maxlen - maximal buffer length + * @return amount of bytes read or -1 in case of error + */ +static int readmntdata(uint8_t *buffer, int maxlen){ + if(mntfd < 0){ + DBG("mntfd non opened"); + return -1; + } + if(!buffer || maxlen < 1) return 0; + //DBG("ask for %d bytes", maxlen); + int got = 0; fd_set rfds; - /* ssize_t l = read(mntfd, &byte, 1); - //DBG("MNT read=%zd byte=0x%X", l, byte); - if(l == 0) return -1; - if(l != 1) return -2; // disconnected ?? - return (int) byte;*/ + struct timeval tv = mnt1Rtmout; do{ FD_ZERO(&rfds); FD_SET(mntfd, &rfds); - struct timeval tv = mntRtmout; + //DBG("select"); int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); + //DBG("returned %d", retval); if(retval < 0){ if(errno == EINTR) continue; DBG("Error in select()"); return -1; } - //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds)); if(FD_ISSET(mntfd, &rfds)){ - ssize_t l = read(mntfd, &byte, 1); - //DBG("MNT read=%zd byte=0x%X", l, byte); - if(l != 1){ + ssize_t l = read(mntfd, buffer, maxlen); + if(l == 0){ + DBG("read ZERO"); + continue; + } + if(l < 0){ DBG("Mount disconnected?"); return -2; // disconnected ?? } + buffer += l; + maxlen -= l; + got += l; + }else{ + DBG("no new data after %d bytes (%s)", got, buffer - got); break; - } else return -1; - }while(1); - return (int)byte; + } + tv = mntRtmout; + }while(maxlen); + return got; } + // clear data from input buffer static void clrmntbuf(){ if(mntfd < 0) return; - uint8_t byte; + uint8_t bytes[256]; fd_set rfds; do{ FD_ZERO(&rfds); @@ -281,9 +233,10 @@ static void clrmntbuf(){ break; } if(FD_ISSET(mntfd, &rfds)){ - ssize_t l = read(mntfd, &byte, 1); - if(l != 1) break; - } else break; + ssize_t l = read(mntfd, &bytes, 256); + if(l < 1) break; + DBG("clr got %zd bytes: %s", l, bytes); + }else break; }while(1); } @@ -293,7 +246,7 @@ static void *encoderthread1(void _U_ *u){ uint8_t databuf[ENC_DATALEN]; int wridx = 0, errctr = 0; struct timespec tcur; - while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ + while(encfd[0] > -1 && errctr < MAX_ERR_CTR && !GlobExit){ int b = getencbyte(); if(b == -2) ++errctr; if(b < 0) continue; @@ -328,9 +281,12 @@ typedef struct{ // write to buffer next data portion; return FALSE in case of error static int readstrings(buf_t *buf, int fd){ - FNAME(); if(!buf){DBG("Empty buffer"); return FALSE;} int L = XYBUFSZ - buf->len; + if(L < 0){ + DBG("buf not initialized!"); + buf->len = 0; + } if(L == 0){ DBG("buffer overfull: %d!", buf->len); char *lastn = strrchr(buf->buf, '\n'); @@ -344,6 +300,7 @@ static int readstrings(buf_t *buf, int fd){ }else buf->len = 0; L = XYBUFSZ - buf->len; } + //DBG("read %d bytes from %d", L, fd); int got = read(fd, &buf->buf[buf->len], L); if(got < 0){ DBG("read()"); @@ -351,13 +308,16 @@ static int readstrings(buf_t *buf, int fd){ }else if(got == 0){ DBG("NO data"); return TRUE; } buf->len += got; buf->buf[buf->len] = 0; - DBG("buf[%d]: %s", buf->len, buf->buf); + //DBG("buf[%d]: %s", buf->len, buf->buf); return TRUE; } // return TRUE if got, FALSE if no data found static int getdata(buf_t *buf, long *out){ - if(!buf || buf->len < 1) return FALSE; + if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){ + return FALSE; + } +// DBG("got data"); // read record between last '\n' and previous (or start of string) char *last = &buf->buf[buf->len - 1]; //DBG("buf: _%s_", buf->buf); @@ -378,7 +338,7 @@ static int getdata(buf_t *buf, long *out){ // try to write '\n' asking new data portion; return FALSE if failed static int asknext(int fd){ - FNAME(); + //FNAME(); if(fd < 0) return FALSE; int i = 0; for(; i < 5; ++i){ @@ -399,12 +359,24 @@ static void *encoderthread2(void _U_ *u){ pfds[0].fd = encfd[0]; pfds[0].events = POLLIN; pfds[1].fd = encfd[1]; pfds[1].events = POLLIN; double t0[2], tstart; - buf_t strbuf[2]; + buf_t strbuf[2] = {0}; long msrlast[2]; // last encoder data double mtlast[2]; // last measurement time asknext(encfd[0]); asknext(encfd[1]); t0[0] = t0[1] = tstart = timefromstart(); int errctr = 0; + + // init Kalman for both axes + Kalman3 kf[2]; + double dt = Conf.EncoderReqInterval; // 1ms encoders step + double sigma_jx = 1e-6, sigma_jy = 1e-6; // "jerk" sigma + double xnoice = encoder_noise(X_ENC_STEPSPERREV); + double ynoice = encoder_noise(Y_ENC_STEPSPERREV); + kalman3_init(&kf[0], dt, xnoice); + kalman3_init(&kf[1], dt, ynoice); + kalman3_set_jerk_noise(&kf[0], sigma_jx); + kalman3_set_jerk_noise(&kf[1], sigma_jy); + do{ // main cycle if(poll(pfds, 2, 0) < 0){ DBG("poll()"); @@ -414,6 +386,7 @@ static void *encoderthread2(void _U_ *u){ for(int i = 0; i < 2; ++i){ if(pfds[i].revents && POLLIN){ if(!readstrings(&strbuf[i], encfd[i])){ + DBG("ERR"); ++errctr; break; } @@ -421,16 +394,37 @@ static void *encoderthread2(void _U_ *u){ double curt = timefromstart(); if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt; if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records + //DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]); if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ + //DBG("time OK"); pthread_mutex_lock(&datamutex); + double pos = (double)msrlast[i]; if(i == 0){ - mountdata.encXposition.val = Xenc2rad((double)msrlast[i]); + pos = Xenc2rad(pos); + // Kalman filtering + kalman3_predict(&kf[i]); + kalman3_update(&kf[i], pos); + //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + mountdata.encXposition.val = kf[i].x[0]; curtime(&mountdata.encXposition.t); + /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g", + msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec, + X_ENC_ZERO, X_ENC_STEPSPERREV);*/ getXspeed(); + //mountdata.encXspeed.val = kf[i].x[1]; + //mountdata.encXspeed.t = mountdata.encXposition.t; }else{ - mountdata.encYposition.val = Yenc2rad((double)msrlast[i]); + pos = Yenc2rad(pos); + kalman3_predict(&kf[i]); + kalman3_update(&kf[i], pos); + //DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + // pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + mountdata.encYposition.val = kf[i].x[0]; curtime(&mountdata.encYposition.t); getYspeed(); + //mountdata.encYspeed.val = kf[i].x[1]; + //mountdata.encYspeed.t = mountdata.encYposition.t; } pthread_mutex_unlock(&datamutex); } @@ -443,7 +437,7 @@ static void *encoderthread2(void _U_ *u){ } } if(got == 2) errctr = 0; - }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR); + }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR && !GlobExit); DBG("\n\nEXIT: ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ @@ -490,7 +484,7 @@ static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t // main mount thread static void *mountthread(void _U_ *u){ int errctr = 0; - uint8_t buf[2*sizeof(SSstat)]; + uint8_t buf[sizeof(SSstat)]; SSstat *status = (SSstat*) buf; bzero(&mountdata, sizeof(mountdata)); double t0 = timefromstart(), tstart = t0, tcur = t0; @@ -499,7 +493,7 @@ static void *mountthread(void _U_ *u){ if(Conf.RunModel){ double Xprev = NAN, Yprev = NAN; // previous coordinates int xcnt = 0, ycnt = 0; - while(1){ + while(!GlobExit){ coordpair_t c; movestate_t xst, yst; // now change data @@ -508,20 +502,20 @@ static void *mountthread(void _U_ *u){ if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue; pthread_mutex_lock(&datamutex); mountdata.encXposition.t = mountdata.encYposition.t = tnow; - mountdata.encXposition.val = c.X; - mountdata.encYposition.val = c.Y; + mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error + mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6; //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); if(tcur - oldmt > Conf.MountReqInterval){ oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3); mountdata.motYposition.t = mountdata.motXposition.t = tnow; if(xst == ST_MOVE) mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.; - else - mountdata.motXposition.val = c.X; + //else + // mountdata.motXposition.val = c.X; if(yst == ST_MOVE) mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.; - else - mountdata.motYposition.val = c.Y; + //else + // mountdata.motYposition.val = c.Y; oldmt = tcur; }else mountdata.millis = oldmillis; chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate); @@ -537,7 +531,7 @@ static void *mountthread(void _U_ *u){ // cmd to send data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); if(!cmd_getstat) goto failed; - while(mntfd > -1 && errctr < MAX_ERR_CTR){ + while(mntfd > -1 && errctr < MAX_ERR_CTR && !GlobExit){ // read data to status struct timespec tcur; if(!curtime(&tcur)) continue; @@ -594,8 +588,8 @@ static int ttyopen(const char *path, speed_t speed){ tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl tty.c_ispeed = speed; tty.c_ospeed = speed; - //tty.c_cc[VMIN] = 0; // non-canonical mode - //tty.c_cc[VTIME] = 5; + tty.c_cc[VMIN] = 0; // non-canonical mode + tty.c_cc[VTIME] = 5; if(ioctl(fd, TCSETS2, &tty)){ DBG("Can't set TTY settings"); close(fd); @@ -610,15 +604,18 @@ static int ttyopen(const char *path, speed_t speed){ // return FALSE if failed int openEncoder(){ + // TODO: open real devices in "model" mode too! if(Conf.RunModel) return TRUE; if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent + /* + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes +*/ if(Conf.SepEncoder == 1){ // only one device DBG("One device"); if(encfd[0] > -1) close(encfd[0]); encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed); if(encfd[0] < 0) return FALSE; - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ close(encfd[0]); encfd[0] = -1; @@ -632,8 +629,6 @@ int openEncoder(){ encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed); if(encfd[i] < 0) return FALSE; } - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ for(int i = 0; i < 2; ++i){ close(encfd[i]); @@ -648,6 +643,7 @@ int openEncoder(){ // return FALSE if failed int openMount(){ + // TODO: open real devices in "model" mode too! if(Conf.RunModel) goto create_thread; if(mntfd > -1) close(mntfd); DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); @@ -655,16 +651,13 @@ int openMount(){ if(mntfd < 0) return FALSE; DBG("mntfd=%d", mntfd); // clear buffer - while(getmntbyte() > -1); - /*int g = write(mntfd, "XXS\r", 4); - DBG("Written %d", g); - uint8_t buf[100]; - do{ - ssize_t l = read(mntfd, buf, 100); - DBG("got %zd", l); - }while(1);*/ + clrmntbuf(); + /* + mnt1Rtmout.tv_sec = 0; + mnt1Rtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed mntRtmout.tv_sec = 0; - mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed + mntRtmout.tv_usec = mnt1Rtmout.tv_usec / 50; +*/ create_thread: if(pthread_create(&mntthread, NULL, mountthread, NULL)){ DBG("Can't create mount thread"); @@ -680,15 +673,17 @@ create_thread: // close all opened serial devices and quit threads void closeSerial(){ - // TODO: close devices in "model" mode too! - if(Conf.RunModel) return; + GlobExit = 1; + DBG("Give 100ms to proper close"); + usleep(100000); + DBG("Force closed all devices"); if(mntfd > -1){ DBG("Cancel mount thread"); pthread_cancel(mntthread); DBG("join mount thread"); pthread_join(mntthread, NULL); DBG("close mount fd"); - close(mntfd); + if(mntfd > -1) close(mntfd); mntfd = -1; } if(encfd[0] > -1){ @@ -697,13 +692,14 @@ void closeSerial(){ DBG("join encoder thread"); pthread_join(encthread, NULL); DBG("close encoder's fd"); - close(encfd[0]); + if(encfd[0] > -1) close(encfd[0]); encfd[0] = -1; if(Conf.SepEncoder == 2 && encfd[1] > -1){ close(encfd[1]); encfd[1] = -1; } } + GlobExit = 0; } // get fresh encoder information @@ -731,26 +727,29 @@ static int wr(const data_t *out, data_t *in, int needeol){ DBG("Wrong arguments or no mount fd"); return FALSE; } + //DBG("clrbuf"); clrmntbuf(); if(out){ + //DBG("write %zd bytes (%s)", out->len, out->buf); if(out->len != (size_t)write(mntfd, out->buf, out->len)){ DBG("written bytes not equal to need"); return FALSE; } + //DBG("eol, mntfd=%d", mntfd); if(needeol){ int g = write(mntfd, "\r", 1); // add EOL (void) g; } - usleep(50000); // add little pause so that the idiot has time to swallow + //usleep(50000); // add little pause so that the idiot has time to swallow } - if(!in) return TRUE; - in->len = 0; - for(size_t i = 0; i < in->maxlen; ++i){ - int b = getmntbyte(); - if(b < 0) break; // nothing to read -> go out - in->buf[in->len++] = (uint8_t) b; + if(!in || in->maxlen < 1) return TRUE; + int got = readmntdata(in->buf, in->maxlen); + if(got < 0){ + DBG("Error reading mount data!"); + in->len = 0; + return FALSE; } - while(getmntbyte() > -1); + in->len = got; return TRUE; } @@ -761,22 +760,24 @@ static int wr(const data_t *out, data_t *in, int needeol){ * @return FALSE if failed */ int MountWriteRead(const data_t *out, data_t *in){ - if(Conf.RunModel) return -1; + if(Conf.RunModel) return FALSE; + //double t0 = timefromstart(); pthread_mutex_lock(&mntmutex); int ret = wr(out, in, 1); pthread_mutex_unlock(&mntmutex); + //DBG("Got %gus", (timefromstart()-t0)*1e6); return ret; } // send binary data - without EOL int MountWriteReadRaw(const data_t *out, data_t *in){ - if(Conf.RunModel) return -1; + if(Conf.RunModel) return FALSE; pthread_mutex_lock(&mntmutex); int ret = wr(out, in, 0); pthread_mutex_unlock(&mntmutex); return ret; } -#ifdef EBUG +#if 0 static void logscmd(SSscmd *c){ printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed); printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits); @@ -799,31 +800,37 @@ static int bincmd(uint8_t *cmd, int len){ if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD); 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}; if(len == sizeof(SSscmd)){ ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); - DBG("Short command"); -#ifdef EBUG + //DBG("Short command"); +#if 0 logscmd((SSscmd*)cmd); #endif if(!wr(dscmd, NULL, 1)) goto rtn; }else if(len == sizeof(SSlcmd)){ ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); - DBG("Long command"); -#ifdef EBUG + // DBG("Long command"); +#if 0 loglcmd((SSlcmd*)cmd); #endif if(!wr(dlcmd, NULL, 1)) goto rtn; }else{ goto rtn; } - data_t d; + SSstat ans; + data_t d, in; d.buf = cmd; d.len = d.maxlen = len; - ret = wr(&d, NULL, 0); + in.buf = (uint8_t*)&ans; in.maxlen = sizeof(SSstat); + ret = wr(&d, &in, 0); DBG("%s", ret ? "SUCCESS" : "FAIL"); + if(ret){ + SSscmd *sc = (SSscmd*)cmd; + mountdata.Xtarget = sc->Xmot; + mountdata.Ytarget = sc->Ymot; + DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d; Xtag/Ytag: %d/%d", + ans.Xmot, ans.Ymot, ans.XLast, ans.YLast, mountdata.Xtarget, mountdata.Ytarget); + } rtn: pthread_mutex_unlock(&mntmutex); return ret; diff --git a/Auxiliary_utils/LibSidServo/servo_real.conf b/Auxiliary_utils/LibSidServo/servo_real.conf new file mode 100644 index 0000000..a09fb0e --- /dev/null +++ b/Auxiliary_utils/LibSidServo/servo_real.conf @@ -0,0 +1,23 @@ +MountDevPath=/dev/ttyUSB0 +MountDevSpeed=19200 +EncoderDevSpeed=1000000 +MountReqInterval=0.1 +EncoderReqInterval=0.001 +SepEncoder=2 +EncoderXDevPath=/dev/encoder_X0 +EncoderYDevPath=/dev/encoder_Y0 +EncoderSpeedInterval=0.05 +RunModel=0 +# telescope is in "pointing state" when coordinate error less than MaxFinePointingErr and goes to "slewing state" +# when this error greater than MaxPointingErr +MaxPointingErr = 0.3490658504 # "pointing zone" - 20 degr +MaxFinePointingErr = 0.1745329252 # "guiding zone" - 10 degr +MaxGuidingErr = 4.8481368e-6 # "on target zone" - 1 arcsec +XPIDVP=0.9 +XPIDVI=0.0005 +XPIDVD=0.0 +YPIDVP=0.5 +YPIDVI=0.005 +YPIDVD=0. +XEncZero=36627112 +YEncZero=36067741 diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index ed4feab..bd50257 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -136,10 +136,11 @@ typedef struct{ } extradata_t; typedef enum{ - AXIS_STOPPED, - AXIS_SLEWING, - AXIS_POINTING, - AXIS_GUIDING, + AXIS_STOPPED, // stop + AXIS_GONNASTOP, // stop command run + AXIS_SLEWING, // go to target with maximal speed + AXIS_POINTING, // axis is in pointing zone, use PID + AXIS_GUIDING, // near target AXIS_ERROR, } axis_status_t; @@ -157,6 +158,9 @@ typedef struct{ uint32_t millis; double temperature; double voltage; + // target X/Y position by last `short` or `long` command + int32_t Xtarget; // in SidServo's counts + int32_t Ytarget; // -//- } mountdata_t; typedef struct{ diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index 2c88e52..e7c81cc 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -26,8 +26,12 @@ #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.; +int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; // will be filled later from config +// defaults until read from controller +double X_MOT_STEPSPERREV = 13312000., + Y_MOT_STEPSPERREV = 17578668., + X_ENC_STEPSPERREV = 67108864., + Y_ENC_STEPSPERREV = 67108864.; uint16_t SScalcChecksum(uint8_t *buf, int len){ uint16_t checksum = 0; @@ -41,27 +45,36 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ } // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! -static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){ +static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){ if(*prev == INT32_MAX){ - *stat = AXIS_STOPPED; + stat = AXIS_STOPPED; DBG("START"); - }else if(*stat != AXIS_STOPPED){ - if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){ - *stat = AXIS_STOPPED; - DBG("AXIS stopped"); - } + }else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target + if(*prev == cur){ + DBG("Test for stop, nstopped=%d", *nstopped); + if(++(*nstopped) > MOTOR_STOPPED_CNT){ + stat = AXIS_STOPPED; + DBG("AXIS stopped"); + } + }else *nstopped = 0; }else if(*prev != cur){ DBG("AXIS moving"); *nstopped = 0; } *prev = cur; + return stat; } // check for stopped/pointing states static void ChkStopped(const SSstat *s, mountdata_t *m){ static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state - chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); - chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate); + axis_status_t Xstat, Ystat; + Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate); + Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate); + if(Xstat != m->Xstate || Ystat != m->Ystate){ + DBG("Status changed"); + setStat(Xstat, Ystat); + } } /** @@ -78,6 +91,7 @@ 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){ + DBG("ENCODER from SSII"); m->encXposition.val = Xenc2rad(s->Xenc); DBG("encx: %g", m->encXposition.val); m->encYposition.val = Yenc2rad(s->Yenc); @@ -165,14 +179,17 @@ int SSsetterI(const char *cmd, int32_t ival){ } int SSstop(int emerg){ + FNAME(); int i = 0; const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX; const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY; + setStat(AXIS_GONNASTOP, AXIS_GONNASTOP); for(; i < 10; ++i){ if(!SStextcmd(cmdx, NULL)) continue; if(SStextcmd(cmdy, NULL)) break; } if(i == 10) return FALSE; + DBG("Stopped"); return TRUE; } diff --git a/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index 39d65a0..0748369 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -209,12 +209,14 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE // convert angle in radians to +-pi static inline __attribute__((always_inline)) double ang2half(double ang){ + ang = fmod(ang, 2.*M_PI); 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 __attribute__((always_inline)) double ang2full(double ang){ + ang = fmod(ang, 2.*M_PI); if(ang < 0.) ang += 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI; return ang;