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;
*/
}
}

24
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,7 +415,10 @@ 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){
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
@ -423,11 +442,14 @@ static void *mountthread(void _U_ *u){
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)};
// cmd to send