timur/ASIBFM700
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

LibSidServo: fix velocity calculation errors in real mode

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-03-11 14:06:58 +03:00
parent b44e695e1c
commit a35b62d21d
4 changed files with 151 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
LibSidServo/main.c
View File

@@ -47,10 +47,10 @@ static movemodel_t *Xmodel, *Ymodel;
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
limits_t
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 = 0.174533, .accel = 0.219911}},
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 = 0.139626, .accel = 0.165806}}
;
static mcc_errcodes_t shortcmd(short_command_t *cmd);
@@ -93,7 +93,7 @@ static int initstarttime(){
// return difference (in seconds) between time1 and time0
double timediff(const struct timespec *time1, const struct timespec *time0){
if(!time1 || !time0) return -1.;
return (double)(time1->tv_sec - time0->tv_sec) + (double)(time1->tv_nsec - time0->tv_nsec) / 1e9;
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){
@@ -161,6 +161,8 @@ double LS_calc_slope(less_square_t *l, double x, double t){
if(!l) return 0.;
size_t idx = l->idx;
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
/*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
l->xsum, l->tsum, l->t2sum, l->xtsum);*/
double t2 = t * t, xt = x * t;
l->x[idx] = x; l->t2[idx] = t2;
l->t[idx] = t; l->xt[idx] = xt;
@@ -172,9 +174,9 @@ double LS_calc_slope(less_square_t *l, double x, double t){
l->xtsum += xt - oldxt;
double n = (double)l->arraysz;
double denominator = n * l->t2sum - l->tsum * l->tsum;
//DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator);
if(fabs(denominator) < 1e-7) return 0.;
double numerator = n * l->xtsum - l->xsum * l->tsum;
//DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
// point: (sum_x - slope * sum_t) / n;
return (numerator / denominator);
}
@@ -319,8 +321,8 @@ static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
*/
static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){
if(!target || !speed) 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(!chkX(target->X) || !chkY(target->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;
short_command_t cmd = {0};

189
LibSidServo/serial.c
View File

@@ -20,6 +20,7 @@
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <pthread.h>
#include <signal.h>
#include <stdint.h>
@@ -64,15 +65,12 @@ void getXspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double dt = timediff0(&mountdata.encXposition.t);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
if(fabs(speed) < 1.5 * Xlimits.max.speed){
mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t;
}
pthread_mutex_unlock(&datamutex);
//DBG("Xspeed=%g", mountdata.encXspeed.val);
}
void getYspeed(){
static less_square_t *ls = NULL;
@@ -80,14 +78,12 @@ void getYspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double dt = timediff0(&mountdata.encYposition.t);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
if(fabs(speed) < 1.5 * Ylimits.max.speed){
mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t;
}
pthread_mutex_unlock(&datamutex);
}
/**
@@ -136,13 +132,12 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = *t;
mountdata.encYposition.t = *t;
pthread_mutex_unlock(&datamutex);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) 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);
}
#if 0
/**
* @brief getencval - get uint64_t data from encoder
* @param fd - encoder fd
@@ -211,6 +206,8 @@ static int getencval(int fd, double *val, struct timespec *t){
if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
return got;
}
#endif
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getencbyte(){
if(encfd[0] < 0) return -1;
@@ -322,65 +319,137 @@ static void *encoderthread1(void _U_ *u){
return NULL;
}
#define XYBUFSZ (128)
typedef struct{
char buf[XYBUFSZ+1];
int len;
} buf_t;
// write to buffer next data portion; return FALSE in case of error
static int readstrings(buf_t *buf, int fd){
if(!buf){DBG("Empty buffer"); return FALSE;}
int L = XYBUFSZ - buf->len;
if(L < 0){
DBG("buf not initialized!");
buf->len = 0;
}
if(L == 0){
DBG("buffer overfull: %d!", buf->len);
char *lastn = strrchr(buf->buf, '\n');
if(lastn){
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
++lastn;
buf->len = XYBUFSZ - (lastn - buf->buf);
DBG("Memmove %d", buf->len);
memmove(lastn, buf->buf, buf->len);
buf->buf[buf->len] = 0;
}else buf->len = 0;
L = XYBUFSZ - buf->len;
}
//DBG("read %d bytes from %d", L, fd);
int got = read(fd, &buf->buf[buf->len], L);
if(got < 0){
DBG("read()");
return FALSE;
}else if(got == 0){ DBG("NO data"); return TRUE; }
buf->len += got;
buf->buf[buf->len] = 0;
//DBG("buf[%d]: %s", buf->len, buf->buf);
return TRUE;
}
// return TRUE if got, FALSE if no data found
static int getdata(buf_t *buf, long *out){
if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE;
// read record between last '\n' and previous (or start of string)
char *last = &buf->buf[buf->len - 1];
//DBG("buf: _%s_", buf->buf);
if(*last != '\n') return FALSE;
*last = 0;
//DBG("buf: _%s_", buf->buf);
char *prev = strrchr(buf->buf, '\n');
if(!prev) prev = buf->buf;
else{
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
++prev; // after last '\n'
}
if(out) *out = atol(prev);
// clear buffer
buf->len = 0;
return TRUE;
}
// try to write '\n' asking new data portion; return FALSE if failed
static int asknext(int fd){
//FNAME();
if(fd < 0) return FALSE;
int i = 0;
for(; i < 5; ++i){
int l = write(fd, "\n", 1);
//DBG("l=%d", l);
if(1 == l) return TRUE;
usleep(100);
}
DBG("5 tries... failed!");
return FALSE;
}
// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started");
struct pollfd pfds[2];
pfds[0].fd = encfd[0]; pfds[0].events = POLLIN;
pfds[1].fd = encfd[1]; pfds[1].events = POLLIN;
double t0[2], tstart;
buf_t strbuf[2] = {0};
long msrlast[2]; // last encoder data
double mtlast[2]; // last measurement time
asknext(encfd[0]); asknext(encfd[1]);
t0[0] = t0[1] = tstart = timefromstart();
int errctr = 0;
double t0 = timefromstart();
const char *req = "\n";
int need2ask = 0; // need or not to ask encoder for new data
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
struct timespec t;
if(need2ask){
//DBG("ASK for new data, tfs=%.6g", timefromstart());
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
//DBG("OK");
need2ask = 0;
usleep(100);
do{ // main cycle
if(poll(pfds, 2, 0) < 0){
DBG("poll()");
break;
}
if(!curtime(&t)){
DBG("Where is time?");
continue;
int got = 0;
for(int i = 0; i < 2; ++i){
if(pfds[i].revents && POLLIN){
if(!readstrings(&strbuf[i], encfd[i])){
++errctr;
break;
}
double v;
if(getencval(encfd[0], &v, &t)){
}
double curt = timefromstart();
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
pthread_mutex_lock(&datamutex);
mountdata.encXposition.val = Xenc2rad(v);
mountdata.encXposition.t = t;
pthread_mutex_unlock(&datamutex);
//DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart());
if(i == 0){
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
curtime(&mountdata.encXposition.t);
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
X_ENC_ZERO, X_ENC_STEPSPERREV);*/
getXspeed();
if(getencval(encfd[1], &v, &t)){
pthread_mutex_lock(&datamutex);
mountdata.encYposition.val = Yenc2rad(v);
mountdata.encYposition.t = t;
pthread_mutex_unlock(&datamutex);
//DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV);
getYspeed();
errctr = 0;
need2ask = 0;
//DBG("tgot=%.6g", timefromstart());
while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); }
t0 = timefromstart();
}else{
DBG("NO Y DATA!!!");
/*if(need2ask) ++errctr;
else need2ask = 1;
continue;*/
mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
curtime(&mountdata.encYposition.t);
getYspeed();
}
pthread_mutex_unlock(&datamutex);
}
if(!asknext(encfd[i])){
++errctr;
need2ask = 1;
break;
}
}else{ // no data - ask for new
//DBG("Need new, tfs=%.6g", timefromstart());
/*if(need2ask) ++errctr;
else need2ask = 1;
continue;*/
++errctr;
need2ask = 1;
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
++got;
}
}
if(got == 2) errctr = 0;
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
DBG("\n\nEXIT: ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){
@@ -445,26 +514,26 @@ static void *mountthread(void _U_ *u){
if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
pthread_mutex_lock(&datamutex);
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
mountdata.encXposition.val = c.X;
mountdata.encYposition.val = c.Y;
mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tcur - oldmt > Conf.MountReqInterval){
oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
if(xst == ST_MOVE)
mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motXposition.val = c.X;
//else
// mountdata.motXposition.val = c.X;
if(yst == ST_MOVE)
mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
else
mountdata.motYposition.val = c.Y;
//else
// mountdata.motYposition.val = c.Y;
oldmt = tcur;
}else mountdata.millis = oldmillis;
chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
pthread_mutex_unlock(&datamutex);
getXspeed(); getYspeed();
pthread_mutex_unlock(&datamutex);
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = timefromstart();
}

8
LibSidServo/ssii.c
View File

@@ -26,8 +26,12 @@
#include "serial.h"
#include "ssii.h"
int X_ENC_ZERO, Y_ENC_ZERO;
double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.;
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
// defaults until read from controller
double X_MOT_STEPSPERREV = 13312000.,
Y_MOT_STEPSPERREV = 17578668.,
X_ENC_STEPSPERREV = 67108864.,
Y_ENC_STEPSPERREV = 67108864.;
uint16_t SScalcChecksum(uint8_t *buf, int len){
uint16_t checksum = 0;

2
LibSidServo/ssii.h
View File

@@ -209,12 +209,14 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE
// convert angle in radians to +-pi
static inline __attribute__((always_inline)) double ang2half(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI;
return ang;
}
// convert to only positive: 0..2pi
static inline __attribute__((always_inline)) double ang2full(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang;