eddy/small_tel
1
0
Fork 0
mirror of https://github.com/eddyem/small_tel.git synced 2026-01-31 20:35:09 +03:00
Code Issues Packages Projects Releases Wiki Activity

..

Browse Source
This commit is contained in:
Edward Emelianov 2025-12-08 21:04:06 +03:00
parent 2e9241f079
commit 74d9ebb75f
20 changed files with 386 additions and 314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
Auxiliary_utils/LibSidServo/PID.c
View File

@ -84,49 +84,51 @@ typedef struct{
* @return calculated new speed or -1 for max speed * @return calculated new speed or -1 for max speed
*/ */
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){ double dt = timediff(&tagpos->t, &axis->position.t);
DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT); 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 return axis->speed.val; // data is too old or wrong
} }
double error = tagpos->val - axis->position.val, fe = fabs(error); double error = tagpos->val - axis->position.val, fe = fabs(error);
DBG("error: %g", error);
PIDController_t *pid = NULL; PIDController_t *pid = NULL;
switch(axis->state){ switch(axis->state){
case AXIS_SLEWING: case AXIS_SLEWING:
if(fe < MCC_MAX_POINTING_ERR){ if(fe < Conf.MaxPointingErr){
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
DBG("--> Pointing"); DBG("--> Pointing");
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else{ }else{
DBG("Slewing..."); DBG("Slewing...");
return -1.; // max speed for given axis return NAN; // max speed for given axis
} }
break; break;
case AXIS_POINTING: case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){ if(fe < Conf.MaxFinePointingErr){
axis->state = AXIS_GUIDING; axis->state = AXIS_GUIDING;
DBG("--> Guiding"); DBG("--> Guiding");
pid = pidpair->PIDV; pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){ }else if(fe > Conf.MaxPointingErr){
DBG("--> Slewing"); DBG("--> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return -1.; return NAN;
} else pid = pidpair->PIDC; } else pid = pidpair->PIDC;
break; break;
case AXIS_GUIDING: case AXIS_GUIDING:
pid = pidpair->PIDV; pid = pidpair->PIDV;
if(fe > MCC_MAX_GUIDING_ERR){ if(fe > Conf.MaxFinePointingErr){
DBG("--> Pointing"); DBG("--> Pointing");
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){ }else if(fe < Conf.MaxGuidingErr){
DBG("At target"); DBG("At target");
// TODO: we can point somehow that we are at target or introduce new axis state // TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g", fe); }else DBG("Current error: %g", fe);
break; break;
case AXIS_STOPPED: // start pointing to target; will change speed next time case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!!"); DBG("AXIS STOPPED!!!! --> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return -1.; return getspeed(tagpos, pidpair, axis);
case AXIS_ERROR: case AXIS_ERROR:
DBG("Can't move from erroneous state"); DBG("Can't move from erroneous state");
return 0.; return 0.;
@ -135,16 +137,16 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
DBG("WTF? Where is a PID?"); DBG("WTF? Where is a PID?");
return axis->speed.val; return axis->speed.val;
} }
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){ double dtpid = timediff(&tagpos->t, &pid->prevT);
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
DBG("time diff too big: clear PID"); DBG("time diff too big: clear PID");
pid_clear(pid); pid_clear(pid);
} }
double dt = tagpos->t - pid->prevT; if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t; pid->prevT = tagpos->t;
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state); DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
double tagspeed = pid_calculate(pid, error, dt); double tagspeed = pid_calculate(pid, error, dtpid);
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
return tagspeed; // coordinate-based return tagspeed; // coordinate-based
} }
@ -154,22 +156,23 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
* @param endpoint - stop point (some far enough point to stop in case of hang) * @param endpoint - stop point (some far enough point to stop in case of hang)
* @return error code * @return error code
*/ */
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){ mcc_errcodes_t correct2(const coordval_pair_t *target){
static PIDpair_t pidX = {0}, pidY = {0}; static PIDpair_t pidX = {0}, pidY = {0};
if(!pidX.PIDC){ if(!pidX.PIDC){
pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDC) return MCC_E_FATAL; if(!pidX.PIDC) return MCC_E_FATAL;
pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDV) return MCC_E_FATAL; if(!pidX.PIDV) return MCC_E_FATAL;
} }
if(!pidY.PIDC){ if(!pidY.PIDC){
pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDC) return MCC_E_FATAL; if(!pidY.PIDC) return MCC_E_FATAL;
pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDV) return MCC_E_FATAL; if(!pidY.PIDV) return MCC_E_FATAL;
} }
mountdata_t m; mountdata_t m;
coordpair_t tagspeed; coordpair_t tagspeed; // absolute value of speed
double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED; if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
axisdata_t axis; axisdata_t axis;
DBG("state: %d/%d", m.Xstate, m.Ystate); DBG("state: %d/%d", m.Xstate, m.Ystate);
@ -177,20 +180,42 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
axis.position = m.encXposition; axis.position = m.encXposition;
axis.speed = m.encXspeed; axis.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axis); tagspeed.X = getspeed(&target->X, &pidX, &axis);
if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X; if(isnan(tagspeed.X)){ // max speed
if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED; if(target->X.val < axis.position.val) Xsign = -1.;
tagspeed.X = Xlimits.max.speed;
}else{
if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
}
axis_status_t xstate = axis.state; axis_status_t xstate = axis.state;
axis.state = m.Ystate; axis.state = m.Ystate;
axis.position = m.encYposition; axis.position = m.encYposition;
axis.speed = m.encYspeed; axis.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axis); tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y; if(isnan(tagspeed.Y)){ // max speed
if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED; if(target->Y.val < axis.position.val) Ysign = -1.;
tagspeed.Y = Ylimits.max.speed;
}else{
if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
}
axis_status_t ystate = axis.state; axis_status_t ystate = axis.state;
if(m.Xstate != xstate || m.Ystate != ystate){ if(m.Xstate != xstate || m.Ystate != ystate){
DBG("State changed"); DBG("State changed");
setStat(xstate, ystate); setStat(xstate, ystate);
} }
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y); coordpair_t endpoint;
return Mount.moveWspeed(endpoint, &tagspeed); // 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
+ Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
+ tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
endpoint.X = m.encXposition.val + Xsign * adder;
dv = fabs(tagspeed.Y - m.encYspeed.val);
adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
+ Conf.PIDMaxDt * tagspeed.Y
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
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);
} }

4
Auxiliary_utils/LibSidServo/PID.h
View File

@ -27,7 +27,7 @@ typedef struct {
double prev_error; // Previous error double prev_error; // Previous error
double integral; // Integral term double integral; // Integral term
double *pidIarray; // array for Integral double *pidIarray; // array for Integral
double prevT; // time of previous correction struct timespec prevT; // time of previous correction
size_t pidIarrSize; // it's size size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element size_t curIidx; // and index of current element
} PIDController_t; } PIDController_t;
@ -37,4 +37,4 @@ void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid); void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt); double pid_calculate(PIDController_t *pid, double error, double dt);
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint); mcc_errcodes_t correct2(const coordval_pair_t *target);

49
Auxiliary_utils/LibSidServo/examples/conf.c
View File

@ -24,25 +24,32 @@
static conf_t Config = { static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0", .MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200, .MountDevSpeed = 19200,
.EncoderXDevPath = "/dev/encoderX0", .EncoderXDevPath = "/dev/encoder_X0",
.EncoderYDevPath = "/dev/encoderY0", .EncoderYDevPath = "/dev/encoder_Y0",
.EncoderDevSpeed = 153000, .EncoderDevSpeed = 153000,
.MountReqInterval = 0.1, .MountReqInterval = 0.1,
.EncoderReqInterval = 0.05, .EncoderReqInterval = 0.001,
.SepEncoder = 2, .SepEncoder = 2,
.EncoderSpeedInterval = 0.1, .EncoderSpeedInterval = 0.05,
.XPIDC.P = 0.8, .EncodersDisagreement = 1e-5, // 2''
.PIDMaxDt = 1.,
.PIDRefreshDt = 0.1,
.PIDCycleDt = 5.,
.XPIDC.P = 0.5,
.XPIDC.I = 0.1, .XPIDC.I = 0.1,
.XPIDC.D = 0.3, .XPIDC.D = 0.2,
.XPIDV.P = 1., .XPIDV.P = 0.09,
.XPIDV.I = 0.01, .XPIDV.I = 0.0,
.XPIDV.D = 0.2, .XPIDV.D = 0.05,
.YPIDC.P = 0.8, .YPIDC.P = 0.5,
.YPIDC.I = 0.1, .YPIDC.I = 0.1,
.YPIDC.D = 0.3, .YPIDC.D = 0.2,
.YPIDV.P = 0.5, .YPIDV.P = 0.09,
.YPIDV.I = 0.2, .YPIDV.I = 0.0,
.YPIDV.D = 0.5, .YPIDV.D = 0.05,
.MaxPointingErr = 0.13962634,
.MaxFinePointingErr = 0.026179939,
.MaxGuidingErr = 4.8481368e-7,
}; };
static sl_option_t opts[] = { static sl_option_t opts[] = {
@ -50,13 +57,17 @@ static sl_option_t opts[] = {
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"}, {"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
{"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"}, {"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"},
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"}, {"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", NEED_ARG, 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)"}, {"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)"}, {"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
{"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
{"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)"},
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"}, {"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"}, {"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"}, {"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"}, {"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"}, {"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
@ -69,6 +80,10 @@ static sl_option_t opts[] = {
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"}, {"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"}, {"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"}, {"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
{"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
end_option end_option
}; };
@ -93,5 +108,5 @@ void dumpConf(){
} }
void confHelp(){ void confHelp(){
sl_showhelp(-1, opts); sl_conf_showhelp(-1, opts);
} }

38
Auxiliary_utils/LibSidServo/examples/dump.c
View File

@ -24,7 +24,7 @@
#include "simpleconv.h" #include "simpleconv.h"
// starting dump time (to conform different logs) // starting dump time (to conform different logs)
static double dumpT0 = -1.; static struct timespec dumpT0 = {0};
#if 0 #if 0
// amount of elements used for encoders' data filtering // amount of elements used for encoders' data filtering
@ -63,7 +63,9 @@ static double filter(double val, int idx){
#endif #endif
// return starting time of dump // return starting time of dump
double dumpt0(){ return dumpT0; } void dumpt0(struct timespec *t){
if(t) *t = dumpT0;
}
/** /**
@ -75,12 +77,12 @@ void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords) return; if(!fcoords) return;
//DBG("LOG %s", m ? "data" : "header"); //DBG("LOG %s", m ? "data" : "header");
if(!m){ // write header if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return; return;
}else if(dumpT0 < 0.) dumpT0 = m->encXposition.t; }else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
// write data // write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n", fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
m->encXposition.t - dumpT0, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val), Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val), RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val), RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
m->millis); m->millis);
@ -106,16 +108,17 @@ void dumpmoving(FILE *fcoords, double t, int N){
LOGWARN("Can't get mount data"); LOGWARN("Can't get mount data");
} }
uint32_t mdmillis = mdata.millis; uint32_t mdmillis = mdata.millis;
double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.; struct timespec encXt = mdata.encXposition.t;
int ctr = -1; int ctr = -1;
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val; double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
double t0 = Mount.currentT(); double t0 = Mount.timeFromStart();
while(Mount.currentT() - t0 < t && ctr < N){ while(Mount.timeFromStart() - t0 < t && ctr < N){
usleep(1000); usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.; //double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
if(tmsr == enct) continue; struct timespec msrt = mdata.encXposition.t;
enct = tmsr; if(msrt.tv_nsec == encXt.tv_nsec) continue;
encXt = msrt;
if(fcoords) logmnt(fcoords, &mdata); if(fcoords) logmnt(fcoords, &mdata);
if(mdata.millis == mdmillis) continue; if(mdata.millis == mdmillis) continue;
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val); //DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
@ -126,7 +129,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
ctr = 0; ctr = 0;
}else ++ctr; }else ++ctr;
} }
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0); DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
} }
/** /**
@ -137,18 +140,15 @@ void waitmoving(int N){
mountdata_t mdata; mountdata_t mdata;
int ctr = -1; int ctr = -1;
uint32_t millis = 0; uint32_t millis = 0;
double xlast = 0., ylast = 0.; //double xlast = 0., ylast = 0.;
DBG("Wait moving for %d stopped times", N);
while(ctr < N){ while(ctr < N){
usleep(10000); usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue; if(mdata.millis == millis) continue;
millis = mdata.millis; millis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
xlast = mdata.motXposition.val; else ++ctr;
ylast = mdata.motYposition.val;
//DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
ctr = 0;
}else ++ctr;
} }
} }

2
Auxiliary_utils/LibSidServo/examples/dump.h
View File

@ -27,4 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N); void waitmoving(int N);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc); int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles); void chk0(int ncycles);
double dumpt0(); void dumpt0(struct timespec *t);

1
Auxiliary_utils/LibSidServo/examples/dumpmoving.c
View File

@ -73,6 +73,7 @@ int main(int argc, char **argv){
conf_t *Config = readServoConf(G.conffile); conf_t *Config = readServoConf(G.conffile);
if(!Config){ if(!Config){
dumpConf(); dumpConf();
confHelp();
return 1; return 1;
} }
if(G.coordsoutput){ if(G.coordsoutput){

6
Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c
View File

@ -139,8 +139,10 @@ static mcc_errcodes_t return2zero(){
short_command_t cmd = {0}; short_command_t cmd = {0};
DBG("Try to move to zero"); DBG("Try to move to zero");
cmd.Xmot = 0.; cmd.Ymot = 0.; cmd.Xmot = 0.; cmd.Ymot = 0.;
cmd.Xspeed = MCC_MAX_X_SPEED; coordpair_t maxspd;
cmd.Yspeed = MCC_MAX_Y_SPEED; if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
cmd.Xspeed = maxspd.X;
cmd.Yspeed = maxspd.Y;
/*cmd.xychange = 1; /*cmd.xychange = 1;
cmd.XBits = 100; cmd.XBits = 100;
cmd.YBits = 20;*/ cmd.YBits = 20;*/

12
Auxiliary_utils/LibSidServo/examples/dumpswing.c
View File

@ -83,7 +83,7 @@ void waithalf(double t){
uint32_t millis = 0; uint32_t millis = 0;
double xlast = 0., ylast = 0.; double xlast = 0., ylast = 0.;
while(ctr < 5){ while(ctr < 5){
if(Mount.currentT() >= t) return; if(Mount.timeFromStart() >= t) return;
usleep(1000); usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue; if(mdata.millis == millis) continue;
@ -158,24 +158,24 @@ int main(int argc, char **argv){
}else{ }else{
tagX = 0.; tagY = DEG2RAD(G.amplitude); tagX = 0.; tagY = DEG2RAD(G.amplitude);
} }
double t = Mount.currentT(), t0 = t; double t = Mount.timeFromStart(), t0 = t;
coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY}; coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
double divide = 2.; double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){ for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tag); Mount.moveTo(&tag);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
t += G.period / divide; t += G.period / divide;
divide = 1.; divide = 1.;
waithalf(t); waithalf(t);
DBG("Moved to +, t=%g", t-t0); DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
Mount.moveTo(&rtag); Mount.moveTo(&rtag);
t += G.period; t += G.period;
waithalf(t); waithalf(t);
DBG("Moved to -, t=%g", t-t0); DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
} }
green("Move to zero @ %g\n", Mount.currentT()); green("Move to zero @ %g\n", Mount.timeFromStart());
tag = (coordpair_t){0}; tag = (coordpair_t){0};
// be sure to move @ 0,0 // be sure to move @ 0,0
if(MCC_E_OK != Mount.moveTo(&tag)){ if(MCC_E_OK != Mount.moveTo(&tag)){

3
Auxiliary_utils/LibSidServo/examples/goto.c
View File

@ -91,8 +91,7 @@ int main(int _U_ argc, char _U_ **argv){
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coordval_pair_t M, E; coordval_pair_t M, E;
if(!getPos(&M, &E)) ERRX("Can't get current position"); if(!getPos(&M, &E)) ERRX("Can't get current position");
printf("Current time: %.10f\n", Mount.currentT()); printf("Current time: %.10f\n", Mount.timeFromStart());
DBG("xt: %g, x: %g", M.X.t, M.X.val);
if(G.coordsoutput){ if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n"); if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
G.wait = 1; G.wait = 1;

38
Auxiliary_utils/LibSidServo/examples/scmd_traectory.c
View File

@ -44,6 +44,7 @@ typedef struct{
char *conffile; char *conffile;
} parameters; } parameters;
static conf_t *Config = NULL;
static FILE *fcoords = NULL, *errlog = NULL; static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr; static pthread_t dthr;
static parameters G = { static parameters G = {
@ -96,38 +97,35 @@ static void runtraectory(traectory_fn tfn){
if(!tfn) return; if(!tfn) return;
coordval_pair_t telXY; coordval_pair_t telXY;
coordval_pair_t target; coordval_pair_t target;
coordpair_t traectXY, endpoint; coordpair_t traectXY;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax; double tlast = 0., tstart = Mount.timeFromStart();
double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT(); long tlastXnsec = 0, tlastYnsec = 0;
double tlastX = 0., tlastY = 0.; struct timespec tcur, t0 = {0};
dumpt0(&t0);
while(1){ while(1){
if(!telpos(&telXY)){ if(!telpos(&telXY)){
WARNX("No next telescope position"); WARNX("No next telescope position");
return; return;
} }
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind if(!Mount.currentT(&tcur)) continue;
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); if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
tlastX = telXY.X.t; tlastY = telXY.Y.t; DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
double t = Mount.currentT(); 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(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
if(!traectory_point(&traectXY, t)) break; if(!traectory_point(&traectXY, t)) break;
target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = t; target.X.t = target.Y.t = tcur;
// check whether we should change direction if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
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;
if(t0 < 0.) t0 = dumpt0();
else{ else{
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); //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("%g: 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)); //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) 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)); 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));
} }
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!"); if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50); while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
tlast = t; tlast = t;
} }
WARNX("No next traectory point or emulation ends"); WARNX("No next traectory point or emulation ends");
@ -153,7 +151,7 @@ int main(int argc, char **argv){
if(!(fcoords = fopen(G.coordsoutput, "w"))) if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput); ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout; }else fcoords = stdout;
conf_t *Config = readServoConf(G.conffile); Config = readServoConf(G.conffile);
if(!Config || G.dumpconf){ if(!Config || G.dumpconf){
dumpConf(); dumpConf();
return 1; return 1;

2
Auxiliary_utils/LibSidServo/examples/traectories.c
View File

@ -41,7 +41,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
if(!f || !XY0) return FALSE; if(!f || !XY0) return FALSE;
cur_traectory = f; cur_traectory = f;
XYstart = *XY0; XYstart = *XY0;
tstart = Mount.currentT(); tstart = Mount.timeFromStart();
mountdata_t mdata; mountdata_t mdata;
int ntries = 0; int ntries = 0;
for(; ntries < 10; ++ntries){ for(; ntries < 10; ++ntries){

9
Auxiliary_utils/LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.2, 2025-11-01T14:58:43. --> <!-- Written by QtCreator 18.0.0, 2025-12-08T13:32:37. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
@ -86,6 +86,7 @@
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/> <valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value> <value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap> </valuemap>
<value type="int" key="RcSync">0</value>
</valuemap> </valuemap>
</data> </data>
<data> <data>
@ -164,6 +165,7 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value> <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value> <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap> </valuemap>
@ -198,6 +200,7 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value> <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value> <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap> </valuemap>
@ -208,10 +211,6 @@
<variable>ProjectExplorer.Project.TargetCount</variable> <variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value> <value type="qlonglong">1</value>
</data> </data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">22</value>
</data>
<data> <data>
<variable>Version</variable> <variable>Version</variable>
<value type="int">22</value> <value type="int">22</value>

147
Auxiliary_utils/LibSidServo/main.c
View File

@ -32,39 +32,80 @@
#include "ssii.h" #include "ssii.h"
#include "PID.h" #include "PID.h"
// adder for monotonic time by realtime: inited any call of init()
static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC
t0 = {0}, // curtime() for initstarttime() call
starttime = {0}; // starting time by monotonic (for timefromstart())
conf_t Conf = {0}; conf_t Conf = {0};
// parameters for model // parameters for model
static movemodel_t *Xmodel, *Ymodel; static movemodel_t *Xmodel, *Ymodel;
// limits for model and/or real mount (in latter case data should be read from mount on init)
// radians, rad/sec, rad/sec^2 // radians, rad/sec, rad/sec^2
static limits_t // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
limits_t
Xlimits = { Xlimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .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}}, .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
Ylimits = { Ylimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .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}} .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
; ;
static mcc_errcodes_t shortcmd(short_command_t *cmd); static mcc_errcodes_t shortcmd(short_command_t *cmd);
/** /**
* @brief nanotime - monotonic time from first run * @brief curtime - monotonic time from first run
* @return time in seconds * @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
* @return TRUE if all OK
* FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
*/ */
double nanotime(){ int curtime(struct timespec *t){
static double t0 = -1.; struct timespec now;
if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
now.tv_sec += timeadder.tv_sec;
now.tv_nsec += timeadder.tv_nsec;
if(now.tv_nsec > 999999999L){
++now.tv_sec;
now.tv_nsec -= 1000000000L;
}
if(t) *t = now;
return TRUE;
}
// init starttime; @return TRUE if all OK
static int initstarttime(){
struct timespec start;
if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
if(timeadder.tv_nsec < 0){
--timeadder.tv_sec;
timeadder.tv_nsec += 1000000000L;
}
curtime(&t0);
return TRUE;
}
// return difference (in seconds) between time1 and time0
double timediff(const struct timespec *time1, const struct timespec *time0){
if(!time1 || !time0) return -1.;
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
}
// difference between given time and last initstarttime() call
double timediff0(const struct timespec *time1){
return timediff(time1, &t0);
}
// time from last initstarttime() call
double timefromstart(){
struct timespec now; struct timespec now;
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.; if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
if(t0 < 0.){ return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
struct timespec start;
if(clock_gettime(CLOCK_REALTIME, &start)) return -1.;
t0 = (double)start.tv_sec + (double)start.tv_nsec * 1e-9
- (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
}
return (double)now.tv_sec + (double)now.tv_nsec * 1e-9 + t0;
} }
/** /**
* @brief quit - close all opened and return to default state * @brief quit - close all opened and return to default state
* TODO: close serial devices even in "model" mode
*/ */
static void quit(){ static void quit(){
if(Conf.RunModel) return; if(Conf.RunModel) return;
@ -74,18 +115,17 @@ static void quit(){
DBG("Exit"); DBG("Exit");
} }
void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst){ void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
if(!c || !Xmodel || !Ymodel) return; if(!c || !Xmodel || !Ymodel) return;
double tnow = nanotime(); double tnow = timefromstart();
moveparam_t Xp, Yp; moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp); movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
//DBG("Xstate = %d", Xst); //DBG("Xstate = %d", Xst);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow); if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp); movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow); if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
c->X.t = c->Y.t = tnow; c->X = Xp.coord;
c->X.val = Xp.coord; c->Y = Yp.coord;
c->Y.val = Yp.coord;
if(xst) *xst = Xst; if(xst) *xst = Xst;
if(yst) *yst = Yst; if(yst) *yst = Yst;
} }
@ -136,7 +176,6 @@ double LS_calc_slope(less_square_t *l, double x, double t){
return (numerator / denominator); return (numerator / denominator);
} }
/** /**
* @brief init - open serial devices and do other job * @brief init - open serial devices and do other job
* @param c - initial configuration * @param c - initial configuration
@ -145,15 +184,20 @@ double LS_calc_slope(less_square_t *l, double x, double t){
static mcc_errcodes_t init(conf_t *c){ static mcc_errcodes_t init(conf_t *c){
FNAME(); FNAME();
if(!c) return MCC_E_BADFORMAT; if(!c) return MCC_E_BADFORMAT;
if(!initstarttime()) return MCC_E_FAILED;
Conf = *c; Conf = *c;
mcc_errcodes_t ret = MCC_E_OK; mcc_errcodes_t ret = MCC_E_OK;
Xmodel = model_init(&Xlimits); Xmodel = model_init(&Xlimits);
Ymodel = model_init(&Ylimits); Ymodel = model_init(&Ylimits);
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
DBG("Bad value of MountReqInterval");
ret = MCC_E_BADFORMAT;
}
if(Conf.RunModel){ if(Conf.RunModel){
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
return MCC_E_OK; return MCC_E_OK;
} }
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
DBG("Define mount device path and speed"); DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT; ret = MCC_E_BADFORMAT;
}else if(!openMount()){ }else if(!openMount()){
@ -169,16 +213,11 @@ static mcc_errcodes_t init(conf_t *c){
ret = MCC_E_ENCODERDEV; ret = MCC_E_ENCODERDEV;
} }
} }
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ // TODO: read hardware configuration on init
DBG("Bad value of MountReqInterval");
ret = MCC_E_BADFORMAT;
}
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
DBG("Wrong speed interval"); DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT; ret = MCC_E_BADFORMAT;
} }
//uint8_t buf[1024];
//data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret; if(ret != MCC_E_OK) return ret;
return updateMotorPos(); return updateMotorPos();
@ -187,19 +226,19 @@ static mcc_errcodes_t init(conf_t *c){
// check coordinates (rad) and speeds (rad/s); return FALSE if failed // check coordinates (rad) and speeds (rad/s); return FALSE if failed
// TODO fix to real limits!!! // TODO fix to real limits!!!
static int chkX(double X){ static int chkX(double X){
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE; if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
return TRUE; return TRUE;
} }
static int chkY(double Y){ static int chkY(double Y){
if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE; if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
return TRUE; return TRUE;
} }
static int chkXs(double s){ static int chkXs(double s){
if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE; if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
return TRUE; return TRUE;
} }
static int chkYs(double s){ static int chkYs(double s){
if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE; if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
return TRUE; return TRUE;
} }
@ -246,8 +285,8 @@ static mcc_errcodes_t move2(const coordpair_t *target){
DBG("x,y: %g, %g", target->X, target->Y); DBG("x,y: %g, %g", target->X, target->Y);
cmd.Xmot = target->X; cmd.Xmot = target->X;
cmd.Ymot = target->Y; cmd.Ymot = target->Y;
cmd.Xspeed = MCC_MAX_X_SPEED; cmd.Xspeed = Xlimits.max.speed;
cmd.Yspeed = MCC_MAX_Y_SPEED; cmd.Yspeed = Ylimits.max.speed;
mcc_errcodes_t r = shortcmd(&cmd); mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r; if(r != MCC_E_OK) return r;
setslewingstate(); setslewingstate();
@ -280,6 +319,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
if(!target || !speed) return MCC_E_BADFORMAT; if(!target || !speed) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) 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(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
// updateMotorPos() here can make a problem; TODO: remove?
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0}; short_command_t cmd = {0};
cmd.Xmot = target->X; cmd.Xmot = target->X;
@ -299,7 +339,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
static mcc_errcodes_t emstop(){ static mcc_errcodes_t emstop(){
FNAME(); FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
Xmodel->emergency_stop(Xmodel, curt); Xmodel->emergency_stop(Xmodel, curt);
Ymodel->emergency_stop(Ymodel, curt); Ymodel->emergency_stop(Ymodel, curt);
return MCC_E_OK; return MCC_E_OK;
@ -311,7 +351,7 @@ static mcc_errcodes_t emstop(){
static mcc_errcodes_t stop(){ static mcc_errcodes_t stop(){
FNAME(); FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
Xmodel->stop(Xmodel, curt); Xmodel->stop(Xmodel, curt);
Ymodel->stop(Ymodel,curt); Ymodel->stop(Ymodel,curt);
return MCC_E_OK; return MCC_E_OK;
@ -328,7 +368,7 @@ static mcc_errcodes_t stop(){
static mcc_errcodes_t shortcmd(short_command_t *cmd){ static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT; if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
moveparam_t param = {0}; moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed; param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
@ -360,7 +400,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
static mcc_errcodes_t longcmd(long_command_t *cmd){ static mcc_errcodes_t longcmd(long_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT; if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
moveparam_t param = {0}; moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed; param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
@ -498,19 +538,38 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
config.Xsetpr = __bswap_32(hwConfig->Xsetpr); config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
config.Ysetpr = __bswap_32(hwConfig->Ysetpr); config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
config.Xmetpr = __bswap_32(hwConfig->Xmetpr); config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4);
config.Ymetpr = __bswap_32(hwConfig->Ymetpr); config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4);
// TODO - next // TODO - next
(void) config; (void) config;
return MCC_E_OK; return MCC_E_OK;
} }
// getters of max/min speed and acceleration
mcc_errcodes_t maxspeed(coordpair_t *v){
if(!v) return MCC_E_BADFORMAT;
v->X = Xlimits.max.speed;
v->Y = Ylimits.max.speed;
return MCC_E_OK;
}
mcc_errcodes_t minspeed(coordpair_t *v){
if(!v) return MCC_E_BADFORMAT;
v->X = Xlimits.min.speed;
v->Y = Ylimits.min.speed;
return MCC_E_OK;
}
mcc_errcodes_t acceleration(coordpair_t *a){
if(!a) return MCC_E_BADFORMAT;
a->X = Xlimits.max.accel;
a->Y = Ylimits.max.accel;
return MCC_E_OK;
}
// init mount class // init mount class
mount_t Mount = { mount_t Mount = {
.init = init, .init = init,
.quit = quit, .quit = quit,
.getMountData = getMD, .getMountData = getMD,
// .slewTo = slew2,
.moveTo = move2, .moveTo = move2,
.moveWspeed = move2s, .moveWspeed = move2s,
.setSpeed = setspeed, .setSpeed = setspeed,
@ -520,7 +579,13 @@ mount_t Mount = {
.longCmd = longcmd, .longCmd = longcmd,
.getHWconfig = get_hwconf, .getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf, .saveHWconfig = write_hwconf,
.currentT = nanotime, .currentT = curtime,
.timeFromStart = timefromstart,
.timeDiff = timediff,
.timeDiff0 = timediff0,
.correctTo = correct2, .correctTo = correct2,
.getMaxSpeed = maxspeed,
.getMinSpeed = minspeed,
.getAcceleration = acceleration,
}; };

12
Auxiliary_utils/LibSidServo/main.h
View File

@ -28,8 +28,12 @@
#include "sidservo.h" #include "sidservo.h"
extern conf_t Conf; extern conf_t Conf;
double nanotime(); extern limits_t Xlimits, Ylimits;
void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst); int curtime(struct timespec *t);
double timediff(const struct timespec *time1, const struct timespec *time0);
double timediff0(const struct timespec *time1);
double timefromstart();
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst);
typedef struct{ typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
@ -43,10 +47,6 @@ double LS_calc_slope(less_square_t *l, double x, double t);
// unused arguments of functions // unused arguments of functions
#define _U_ __attribute__((__unused__)) #define _U_ __attribute__((__unused__))
// break absent in `case`
#define FALLTHRU __attribute__ ((fallthrough))
// and synonym for FALLTHRU
#define NOBREAKHERE __attribute__ ((fallthrough))
// weak functions // weak functions
#define WEAK __attribute__ ((weak)) #define WEAK __attribute__ ((weak))

2
Auxiliary_utils/LibSidServo/movingmodel.c
View File

@ -60,7 +60,7 @@ movemodel_t *model_init(limits_t *l){
int model_move2(movemodel_t *model, moveparam_t *target, double t){ int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE; if(!target || !model) return FALSE;
DBG("MOVE to %g at speed %g", target->coord, target->speed); DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
// only positive velocity // only positive velocity
if(target->speed < 0.) target->speed = -target->speed; if(target->speed < 0.) target->speed = -target->speed;
if(fabs(target->speed) < model->Min.speed){ if(fabs(target->speed) < model->Min.speed){

2
Auxiliary_utils/LibSidServo/movingmodel.h
View File

@ -44,7 +44,7 @@ typedef struct{
typedef struct{ typedef struct{
moveparam_t min; moveparam_t min;
moveparam_t max; moveparam_t max;
double acceleration; //double acceleration;
} limits_t; } limits_t;
typedef enum{ typedef enum{

202
Auxiliary_utils/LibSidServo/serial.c
View File

@ -59,49 +59,35 @@ typedef struct __attribute__((packed)){
// calculate current X/Y speeds // calculate current X/Y speeds
void getXspeed(){ 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; static less_square_t *ls = NULL;
if(!ls){ if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0); double dt = timediff0(&mountdata.encXposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
if(fabs(speed) < 1.5 * Xlimits.max.speed){
mountdata.encXspeed.val = speed; mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t; mountdata.encXspeed.t = mountdata.encXposition.t;
} }
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("Xspeed=%g", mountdata.encXspeed.val); //DBG("Xspeed=%g", mountdata.encXspeed.val);
#if 0
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
lastXenc.val = mountdata.encXposition.val;
lastXenc.t = t;
#endif
} }
void getYspeed(){ 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; static less_square_t *ls = NULL;
if(!ls){ if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0); double dt = timediff0(&mountdata.encYposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
if(fabs(speed) < 1.5 * Ylimits.max.speed){
mountdata.encYspeed.val = speed; mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t; mountdata.encYspeed.t = mountdata.encYposition.t;
} }
pthread_mutex_unlock(&datamutex); 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.;
lastYenc.val = mountdata.encYposition.val;
lastYenc.t = t;
#endif
} }
/** /**
@ -109,7 +95,8 @@ void getYspeed(){
* @param databuf - input buffer with 13 bytes of data * @param databuf - input buffer with 13 bytes of data
* @param t - time when databuf[0] got * @param t - time when databuf[0] got
*/ */
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
if(!t) return;
enc_t *edata = (enc_t*) databuf; enc_t *edata = (enc_t*) databuf;
/* /*
#ifdef EBUG #ifdef EBUG
@ -147,12 +134,12 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
mountdata.encXposition.val = X_ENC2RAD(edata->encX); mountdata.encXposition.val = X_ENC2RAD(edata->encX);
mountdata.encYposition.val = Y_ENC2RAD(edata->encY); mountdata.encYposition.val = Y_ENC2RAD(edata->encY);
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = t; mountdata.encXposition.t = *t;
mountdata.encYposition.t = t; mountdata.encYposition.t = *t;
pthread_mutex_unlock(&datamutex);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getXspeed(); 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); //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);
} }
@ -163,11 +150,11 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
* @param t - measurement time * @param t - measurement time
* @return amount of data read or 0 if problem * @return amount of data read or 0 if problem
*/ */
static int getencval(int fd, double *val, double *t){ static int getencval(int fd, double *val, struct timespec *t){
if(fd < 0) return FALSE; if(fd < 0) return FALSE;
char buf[128]; char buf[128];
int got = 0, Lmax = 127; int got = 0, Lmax = 127;
double t0 = nanotime(); double t0 = timefromstart();
do{ do{
fd_set rfds; fd_set rfds;
FD_ZERO(&rfds); FD_ZERO(&rfds);
@ -186,7 +173,7 @@ static int getencval(int fd, double *val, double *t){
buf[got] = 0; buf[got] = 0;
} else continue; } else continue;
if(strchr(buf, '\n')) break; if(strchr(buf, '\n')) break;
}while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval); }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval);
if(got == 0) return 0; // WTF? if(got == 0) return 0; // WTF?
char *estr = strrchr(buf, '\n'); char *estr = strrchr(buf, '\n');
if(!estr) return 0; if(!estr) return 0;
@ -201,7 +188,7 @@ static int getencval(int fd, double *val, double *t){
return 0; // wrong number return 0; // wrong number
} }
if(val) *val = (double) data; if(val) *val = (double) data;
if(t) *t = t0; if(t){ if(!curtime(t)) return 0; }
return got; return got;
} }
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@ -265,8 +252,6 @@ static void clrmntbuf(){
if(mntfd < 0) return; if(mntfd < 0) return;
uint8_t byte; uint8_t byte;
fd_set rfds; fd_set rfds;
//double t0 = nanotime();
//int n = 0;
do{ do{
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(mntfd, &rfds); FD_SET(mntfd, &rfds);
@ -280,10 +265,8 @@ static void clrmntbuf(){
if(FD_ISSET(mntfd, &rfds)){ if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1); ssize_t l = read(mntfd, &byte, 1);
if(l != 1) break; if(l != 1) break;
//++n;
} else break; } else break;
}while(1); }while(1);
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
} }
// main encoder thread (for separate encoder): read next data and make parsing // main encoder thread (for separate encoder): read next data and make parsing
@ -291,7 +274,7 @@ static void *encoderthread1(void _U_ *u){
if(Conf.SepEncoder != 1) return NULL; if(Conf.SepEncoder != 1) return NULL;
uint8_t databuf[ENC_DATALEN]; uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0; int wridx = 0, errctr = 0;
double t = 0.; struct timespec tcur;
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
int b = getencbyte(); int b = getencbyte();
if(b == -2) ++errctr; if(b == -2) ++errctr;
@ -302,13 +285,14 @@ static void *encoderthread1(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){ if((uint8_t)b == ENC_MAGICK){
// DBG("Got magic -> start filling packet"); // DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b; databuf[wridx++] = (uint8_t) b;
t = nanotime();
} }
continue; continue;
}else databuf[wridx++] = (uint8_t) b; }else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){ if(wridx == ENC_DATALEN){
parse_encbuf(databuf, t); if(curtime(&tcur)){
wridx = 0; parse_encbuf(databuf, &tcur);
wridx = 0;
}
} }
} }
if(encfd[0] > -1){ if(encfd[0] > -1){
@ -323,30 +307,28 @@ static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL; if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started"); DBG("Thread started");
int errctr = 0; int errctr = 0;
double t0 = nanotime(); double t0 = timefromstart();
const char *req = "\n"; const char *req = "\n";
int need2ask = 0; // need or not to ask encoder for new data int need2ask = 0; // need or not to ask encoder for new data
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
struct timespec t;
if(!curtime(&t)) continue;
if(need2ask){ if(need2ask){
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
} }
double v, t; double v;
if(getencval(encfd[0], &v, &t)){ if(getencval(encfd[0], &v, &t)){
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
mountdata.encXposition.val = X_ENC2RAD(v); mountdata.encXposition.val = X_ENC2RAD(v);
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
mountdata.encXposition.t = t; mountdata.encXposition.t = t;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
getXspeed(); getXspeed();
if(getencval(encfd[1], &v, &t)){ if(getencval(encfd[1], &v, &t)){
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
mountdata.encYposition.val = Y_ENC2RAD(v); mountdata.encYposition.val = Y_ENC2RAD(v);
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
mountdata.encYposition.t = t; mountdata.encYposition.t = t;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getYspeed(); getYspeed();
errctr = 0; errctr = 0;
need2ask = 0; need2ask = 0;
@ -360,9 +342,8 @@ static void *encoderthread2(void _U_ *u){
else need2ask = 1; else need2ask = 1;
continue; continue;
} }
while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); } while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); }
//DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval); t0 = timefromstart();
t0 = nanotime();
} }
DBG("ERRCTR=%d", errctr); DBG("ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
@ -390,43 +371,67 @@ void data_free(data_t **x){
*x = NULL; *x = NULL;
} }
static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
if(!prev || !nstopped || !stat) return;
if(isnan(*prev)){
*stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != AXIS_STOPPED){
if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = AXIS_STOPPED;
DBG("AXIS stopped; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped);
}
}else if(*prev != cur){
DBG("AXIS moving");
*nstopped = 0;
}
*prev = cur;
}
// main mount thread // main mount thread
static void *mountthread(void _U_ *u){ static void *mountthread(void _U_ *u){
int errctr = 0; int errctr = 0;
uint8_t buf[2*sizeof(SSstat)]; uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf; SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata)); bzero(&mountdata, sizeof(mountdata));
double t0 = nanotime(), tstart = t0; double t0 = timefromstart(), tstart = t0, tcur = t0;
static double oldmt = -100.; // old `millis measurement` time double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0; static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){ if(Conf.RunModel){
coordval_pair_t c; double Xprev = NAN, Yprev = NAN; // previous coordinates
movestate_t xst, yst; int xcnt = 0, ycnt = 0;
// now change data while(1){
getModData(&c, &xst, &yst); coordpair_t c;
pthread_mutex_lock(&datamutex); movestate_t xst, yst;
double tnow = c.X.t; // now change data
mountdata.encXposition.t = mountdata.encYposition.t = tnow; getModData(&c, &xst, &yst);
mountdata.encXposition.val = c.X.val; struct timespec tnow;
mountdata.encYposition.val = c.Y.val; if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); pthread_mutex_lock(&datamutex);
if(tnow - oldmt > Conf.MountReqInterval){ mountdata.encXposition.t = mountdata.encYposition.t = tnow;
oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3); mountdata.encXposition.val = c.X;
mountdata.motYposition.t = mountdata.motXposition.t = tnow; mountdata.encYposition.val = c.Y;
if(xst == ST_MOVE) //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.; if(tcur - oldmt > Conf.MountReqInterval){
else oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
mountdata.motXposition.val = c.X.val; mountdata.motYposition.t = mountdata.motXposition.t = tnow;
if(yst == ST_MOVE) if(xst == ST_MOVE)
mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.; mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
else else
mountdata.motYposition.val = c.Y.val; mountdata.motXposition.val = c.X;
oldmt = tnow; if(yst == ST_MOVE)
}else mountdata.millis = oldmillis; mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
pthread_mutex_unlock(&datamutex); else
getXspeed(); getYspeed(); mountdata.motYposition.val = c.Y;
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); oldmt = tcur;
t0 = nanotime(); }else mountdata.millis = oldmillis;
chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
pthread_mutex_unlock(&datamutex);
getXspeed(); getYspeed();
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = timefromstart();
}
} }
// data to get // data to get
data_t d = {.buf = buf, .maxlen = sizeof(buf)}; data_t d = {.buf = buf, .maxlen = sizeof(buf)};
@ -435,31 +440,8 @@ static void *mountthread(void _U_ *u){
if(!cmd_getstat) goto failed; if(!cmd_getstat) goto failed;
while(mntfd > -1 && errctr < MAX_ERR_CTR){ while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status // read data to status
double t0 = nanotime(); struct timespec tcur;
#if 0 if(!curtime(&tcur)) continue;
// 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 // 80 milliseconds to get answer on GETSTAT
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG #ifdef EBUG
@ -476,14 +458,13 @@ static void *mountthread(void _U_ *u){
errctr = 0; errctr = 0;
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
// now change data // now change data
SSconvstat(status, &mountdata, t0); SSconvstat(status, &mountdata, &tcur);
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("GOT FULL stat by %g", nanotime() - t0);
// allow writing & getters // allow writing & getters
do{ do{
usleep(500); usleep(500);
}while(nanotime() - t0 < Conf.MountReqInterval); }while(timefromstart() - t0 < Conf.MountReqInterval);
t0 = nanotime(); t0 = timefromstart();
} }
data_free(&cmd_getstat); data_free(&cmd_getstat);
failed: failed:
@ -589,6 +570,7 @@ create_thread:
// close all opened serial devices and quit threads // close all opened serial devices and quit threads
void closeSerial(){ void closeSerial(){
// TODO: close devices in "model" mode too!
if(Conf.RunModel) return; if(Conf.RunModel) return;
if(mntfd > -1){ if(mntfd > -1){
DBG("Cancel mount thread"); DBG("Cancel mount thread");
@ -638,30 +620,23 @@ static int wr(const data_t *out, data_t *in, int needeol){
return FALSE; return FALSE;
} }
clrmntbuf(); clrmntbuf();
//double t0 = nanotime();
if(out){ if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need"); DBG("written bytes not equal to need");
return FALSE; return FALSE;
} }
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
if(needeol){ if(needeol){
int g = write(mntfd, "\r", 1); // add EOL int g = write(mntfd, "\r", 1); // add EOL
(void) g; (void) g;
} }
} }
//DBG("sent by %g", nanotime() - t0);
//uint8_t buf[256];
//data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) return TRUE; 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; in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){ for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte(); int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b; in->buf[in->len++] = (uint8_t) b;
} }
//DBG("got %zd bytes by %g", in->len, nanotime() - t0);
while(getmntbyte() > -1); while(getmntbyte() > -1);
return TRUE; return TRUE;
} }
@ -768,13 +743,14 @@ int cmdC(SSconfig *conf, int rw){
d.len = 0; d.maxlen = sizeof(SSconfig); d.len = 0; d.maxlen = sizeof(SSconfig);
ret = wr(rcmd, &d, 1); ret = wr(rcmd, &d, 1);
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen); DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
if(d.len != d.maxlen) return FALSE; if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
// simplest checksum // simplest checksum
uint16_t sum = 0; uint16_t sum = 0;
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i]; for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
if(sum != conf->checksum){ if(sum != conf->checksum){
DBG("got sum: %u, need: %u", conf->checksum, sum); DBG("got sum: %u, need: %u", conf->checksum, sum);
return FALSE; ret = FALSE;
goto rtn;
} }
} }
rtn: rtn:

50
Auxiliary_utils/LibSidServo/sidservo.h
View File

@ -32,38 +32,13 @@ extern "C"
#include <stdint.h> #include <stdint.h>
#include <sys/time.h> #include <sys/time.h>
// acceptable position error - 0.1'' // minimal serial speed of mount device
#define MCC_POSITION_ERROR (5e-7) #define MOUNT_BAUDRATE_MIN (1200)
// acceptable disagreement between motor and axis encoders - 2''
#define MCC_ENCODERS_ERROR (1e-7)
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
#define MCC_MAX_X_SPEED (0.174533)
#define MCC_MAX_Y_SPEED (0.139626)
// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
#define MCC_X_ACCELERATION (0.219911)
#define MCC_Y_ACCELERATION (0.165806)
// max speed interval, seconds // max speed interval, seconds
#define MCC_CONF_MAX_SPEEDINT (2.) #define MCC_CONF_MAX_SPEEDINT (2.)
// minimal speed interval in parts of EncoderReqInterval // minimal speed interval in parts of EncoderReqInterval
#define MCC_CONF_MIN_SPEEDC (3.) #define MCC_CONF_MIN_SPEEDC (3.)
// PID I cycle time (analog of "RC" for PID on opamps)
#define MCC_PID_CYCLE_TIME (5.)
// maximal PID refresh time interval (if larger all old data will be cleared)
#define MCC_PID_MAX_DT (1.)
// 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": 8 degrees
//#define MCC_MAX_POINTING_ERR (0.20943951)
//#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''
#define MCC_MAX_ATTARGET_ERR (4.8481368e-7)
// error codes // error codes
typedef enum{ typedef enum{
@ -87,14 +62,21 @@ typedef struct{
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
char* EncoderXDevPath; // paths to new controller devices char* EncoderXDevPath; // paths to new controller devices
char* EncoderYDevPath; char* EncoderYDevPath;
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
double MountReqInterval; // interval between subsequent mount requests (seconds) double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds) double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
double EncoderSpeedInterval; // interval between speed calculations double EncoderSpeedInterval; // interval between speed calculations
int RunModel; // == 1 if you want to use model instead of real mount int RunModel; // == 1 if you want to use model instead of real mount
double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared)
double PIDRefreshDt; // normal PID refresh interval
double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps)
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven) PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
PIDpar_t XPIDV; PIDpar_t XPIDV;
PIDpar_t YPIDC; PIDpar_t YPIDC;
PIDpar_t YPIDV; PIDpar_t YPIDV;
double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
} conf_t; } conf_t;
// coordinates/speeds in degrees or d/s: X, Y // coordinates/speeds in degrees or d/s: X, Y
@ -105,7 +87,7 @@ typedef struct{
// coordinate/speed and time of last measurement // coordinate/speed and time of last measurement
typedef struct{ typedef struct{
double val; double val;
double t; struct timespec t;
} coordval_t; } coordval_t;
typedef struct{ typedef struct{
@ -247,7 +229,7 @@ typedef struct{
void (*quit)(); // deinit void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); // mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint); mcc_errcodes_t (*correctTo)(const coordval_pair_t *target);
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
@ -257,7 +239,13 @@ typedef struct{
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
double (*currentT)(); // current time int (*currentT)(struct timespec *t); // current time
double (*timeFromStart)(); // amount of seconds from last init
double (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times
double (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time
mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//-
mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
} mount_t; } mount_t;
extern mount_t Mount; extern mount_t Mount;

34
Auxiliary_utils/LibSidServo/ssii.c
View File

@ -67,17 +67,17 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
* @param m (o) - output * @param m (o) - output
* @param t - measurement time * @param t - measurement time
*/ */
void SSconvstat(const SSstat *s, mountdata_t *m, double t){ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
if(!s || !m) return; if(!s || !m || !t) return;
m->motXposition.val = X_MOT2RAD(s->Xmot); m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot); m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m); ChkStopped(s, m);
m->motXposition.t = m->motYposition.t = t; m->motXposition.t = m->motYposition.t = *t;
// fill encoder data from here, as there's no separate enc thread // fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){ if(!Conf.SepEncoder){
m->encXposition.val = X_ENC2RAD(s->Xenc); m->encXposition.val = X_ENC2RAD(s->Xenc);
m->encYposition.val = Y_ENC2RAD(s->Yenc); m->encYposition.val = Y_ENC2RAD(s->Yenc);
m->encXposition.t = m->encYposition.t = t; m->encXposition.t = m->encYposition.t = *t;
getXspeed(); getYspeed(); getXspeed(); getYspeed();
} }
m->keypad = s->keypad; m->keypad = s->keypad;
@ -176,33 +176,37 @@ int SSstop(int emerg){
mcc_errcodes_t updateMotorPos(){ mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0}; mountdata_t md = {0};
if(Conf.RunModel) return MCC_E_OK; if(Conf.RunModel) return MCC_E_OK;
double t0 = nanotime(), t = 0.; double t0 = timefromstart(), t = 0.;
struct timespec curt;
DBG("start @ %g", t0); DBG("start @ %g", t0);
do{ do{
t = nanotime(); t = timefromstart();
if(!curtime(&curt)){
usleep(10000);
continue;
}
if(MCC_E_OK == getMD(&md)){ if(MCC_E_OK == getMD(&md)){
if(md.encXposition.t == 0 || md.encYposition.t == 0){ if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
DBG("Just started, t-t0 = %g!", t - t0); DBG("Just started, t-t0 = %g!", t - t0);
sleep(1); usleep(10000);
DBG("t-t0 = %g", nanotime() - t0);
//usleep(10000);
continue; continue;
} }
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t); if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
mcc_errcodes_t OK = MCC_E_OK; mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!"); DBG("Xpos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0); }else DBG("Xpos sync OK, Dt=%g", t - t0);
} }
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!"); DBG("Ypos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Ypos sync OK, Dt=%g", nanotime() - t0); }else DBG("Ypos sync OK, Dt=%g", t - t0);
} }
if(MCC_E_OK == OK){ if(MCC_E_OK == OK){
DBG("Encoders synced"); DBG("Encoders synced");

4
Auxiliary_utils/LibSidServo/ssii.h
View File

@ -173,7 +173,7 @@
#define SITECH_LOOP_FREQUENCY (1953.) #define SITECH_LOOP_FREQUENCY (1953.)
// amount of consequent same coordinates to detect stop // amount of consequent same coordinates to detect stop
#define MOTOR_STOPPED_CNT (4) #define MOTOR_STOPPED_CNT (19)
// TODO: take it from settings? // TODO: take it from settings?
// steps per revolution (SSI - x4 - for SSI) // steps per revolution (SSI - x4 - for SSI)
@ -331,7 +331,7 @@ typedef struct{
} __attribute__((packed)) SSconfig; } __attribute__((packed)) SSconfig;
uint16_t SScalcChecksum(uint8_t *buf, int len); uint16_t SScalcChecksum(uint8_t *buf, int len);
void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t); void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
int SStextcmd(const char *cmd, data_t *answer); int SStextcmd(const char *cmd, data_t *answer);
int SSrawcmd(const char *cmd, data_t *answer); int SSrawcmd(const char *cmd, data_t *answer);
int SSgetint(const char *cmd, int64_t *ans); int SSgetint(const char *cmd, int64_t *ans);