eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed model for STOPPED state

Browse Source
This commit is contained in:
Edward Emelianov 2025-11-14 14:07:07 +03:00
parent 078e3f38f2
commit 1ea5fb623d
2 changed files with 56 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
LibSidServo/examples/dump.c
View File

@ -137,18 +137,23 @@ void waitmoving(int N){
mountdata_t mdata;
int ctr = -1;
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){
usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
else ++ctr;
/*
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
//DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
ctr = 0;
}else ++ctr;
*/
}
}

78
LibSidServo/serial.c
View File

@ -390,6 +390,22 @@ void data_free(data_t **x){
*x = NULL;
}
static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
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");
}
}else if(*prev != cur){
DBG("AXIS moving");
*nstopped = 0;
}
*prev = cur;
}
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
@ -399,34 +415,40 @@ static void *mountthread(void _U_ *u){
double t0 = nanotime(), tstart = t0;
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){
coordval_pair_t c;
movestate_t xst, yst;
// now change data
getModData(&c, &xst, &yst);
pthread_mutex_lock(&datamutex);
double tnow = c.X.t;
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
mountdata.encXposition.val = c.X.val;
mountdata.encYposition.val = c.Y.val;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
if(xst == ST_MOVE)
mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motXposition.val = c.X.val;
if(yst == ST_MOVE)
mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motYposition.val = c.Y.val;
oldmt = tnow;
}else mountdata.millis = oldmillis;
pthread_mutex_unlock(&datamutex);
getXspeed(); getYspeed();
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = nanotime();
if(Conf.RunModel){
double Xprev = NAN, Yprev = NAN; // previous coordinates
int xcnt = 0, ycnt = 0;
while(1){
coordval_pair_t c;
movestate_t xst, yst;
// now change data
getModData(&c, &xst, &yst);
pthread_mutex_lock(&datamutex);
double tnow = c.X.t;
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
mountdata.encXposition.val = c.X.val;
mountdata.encYposition.val = c.Y.val;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
if(xst == ST_MOVE)
mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motXposition.val = c.X.val;
if(yst == ST_MOVE)
mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motYposition.val = c.Y.val;
oldmt = tnow;
}else mountdata.millis = oldmillis;
chkModStopped(&Xprev, c.X.val, &xcnt, &mountdata.Xstate);
chkModStopped(&Yprev, c.Y.val, &ycnt, &mountdata.Ystate);
pthread_mutex_unlock(&datamutex);
getXspeed(); getYspeed();
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = nanotime();
}
}
// data to get
data_t d = {.buf = buf, .maxlen = sizeof(buf)};