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; 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.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
else ++ctr;
/*
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val; xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val; ylast = mdata.motYposition.val;
//DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast)); //DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast));
ctr = 0; ctr = 0;
}else ++ctr; }else ++ctr;
*/
} }
} }

78
LibSidServo/serial.c
View File

@ -390,6 +390,22 @@ void data_free(data_t **x){
*x = NULL; *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 // main mount thread
static void *mountthread(void _U_ *u){ static void *mountthread(void _U_ *u){
int errctr = 0; int errctr = 0;
@ -399,34 +415,40 @@ static void *mountthread(void _U_ *u){
double t0 = nanotime(), tstart = t0; double t0 = nanotime(), tstart = t0;
static double oldmt = -100.; // old `millis measurement` time static 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); coordval_pair_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; pthread_mutex_lock(&datamutex);
mountdata.encYposition.val = c.Y.val; double tnow = c.X.t;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); mountdata.encXposition.t = mountdata.encYposition.t = tnow;
if(tnow - oldmt > Conf.MountReqInterval){ mountdata.encXposition.val = c.X.val;
oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3); mountdata.encYposition.val = c.Y.val;
mountdata.motYposition.t = mountdata.motXposition.t = tnow; //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(xst == ST_MOVE) if(tnow - oldmt > Conf.MountReqInterval){
mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.; oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3);
else mountdata.motYposition.t = mountdata.motXposition.t = tnow;
mountdata.motXposition.val = c.X.val; if(xst == ST_MOVE)
if(yst == ST_MOVE) mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.; else
else mountdata.motXposition.val = c.X.val;
mountdata.motYposition.val = c.Y.val; if(yst == ST_MOVE)
oldmt = tnow; mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
}else mountdata.millis = oldmillis; else
pthread_mutex_unlock(&datamutex); mountdata.motYposition.val = c.Y.val;
getXspeed(); getYspeed(); oldmt = tnow;
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); }else mountdata.millis = oldmillis;
t0 = nanotime(); 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 to get
data_t d = {.buf = buf, .maxlen = sizeof(buf)}; data_t d = {.buf = buf, .maxlen = sizeof(buf)};