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
Download Patch File Download Diff File Expand all files Collapse all files

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)};