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

kalman added

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 12:10:20 +03:00
parent 8698dfa016
commit 9e7c732afd
7 changed files with 270 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
serial.c
View File

@@ -31,6 +31,7 @@
#include <sys/stat.h>
#include <unistd.h>
#include "kalman.h"
#include "main.h"
#include "movingmodel.h"
#include "serial.h"
@@ -141,77 +142,6 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
//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
* @param val - value read
* @param t - measurement time
* @return amount of data read or 0 if problem
*/
static int getencval(int fd, double *val, struct timespec *t){
if(fd < 0){
DBG("Encoder fd < 0!");
return FALSE;
}
char buf[128];
int got = 0, Lmax = 127;
double t0 = timefromstart();
//DBG("start: %.6g", t0);
do{
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
struct timeval tv = encRtmout;
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
if(!retval){
//DBG("select()==0 - timeout, %.6g", timefromstart());
break;
}
if(retval < 0){
if(errno == EINTR){
DBG("EINTR");
continue;
}
DBG("select() < 0");
return 0;
}
if(FD_ISSET(fd, &rfds)){
ssize_t l = read(fd, &buf[got], Lmax);
if(l < 1){
DBG("read() < 0");
return 0; // disconnected ??
}
got += l; Lmax -= l;
buf[got] = 0;
} else continue;
if(buf[got-1] == '\n') break; // got EOL as last symbol
}while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
if(got == 0){
//DBG("No data from encoder, tfs=%.6g", timefromstart());
return 0;
}
char *estr = strrchr(buf, '\n');
if(!estr){
DBG("No EOL");
return 0;
}
*estr = 0;
char *bgn = strrchr(buf, '\n');
if(bgn) ++bgn;
else bgn = buf;
char *eptr;
long data = strtol(bgn, &eptr, 10);
if(eptr != estr){
DBG("NAN");
return 0; // wrong number
}
if(val) *val = (double) data;
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;
@@ -385,7 +315,7 @@ static int getdata(buf_t *buf, long *out){
if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){
return FALSE;
}
DBG("got data");
// DBG("got data");
// read record between last '\n' and previous (or start of string)
char *last = &buf->buf[buf->len - 1];
//DBG("buf: _%s_", buf->buf);
@@ -433,6 +363,18 @@ static void *encoderthread2(void _U_ *u){
asknext(encfd[0]); asknext(encfd[1]);
t0[0] = t0[1] = tstart = timefromstart();
int errctr = 0;
// init Kalman for both axes
Kalman3 kf[2];
double dt = Conf.EncoderReqInterval; // 1ms encoders step
double sigma_jx = 1e-6, sigma_jy = 1e-6; // "jerk" sigma
double xnoice = encoder_noise(X_ENC_STEPSPERREV);
double ynoice = encoder_noise(Y_ENC_STEPSPERREV);
kalman3_init(&kf[0], dt, xnoice);
kalman3_init(&kf[1], dt, ynoice);
kalman3_set_jerk_noise(&kf[0], sigma_jx);
kalman3_set_jerk_noise(&kf[1], sigma_jy);
do{ // main cycle
if(poll(pfds, 2, 0) < 0){
DBG("poll()");
@@ -450,25 +392,37 @@ static void *encoderthread2(void _U_ *u){
double curt = timefromstart();
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
//DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
DBG("time OK");
//DBG("time OK");
pthread_mutex_lock(&datamutex);
double pos = (double)msrlast[i];
//DBG("pos[%d]=%g", i, pos);
;;
//DBG("Got med: %g", pos);
if(i == 0){
mountdata.encXposition.val = Xenc2rad(pos);
pos = Xenc2rad(pos);
// Kalman filtering
kalman3_predict(&kf[i]);
kalman3_update(&kf[i], pos);
DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
mountdata.encXposition.val = kf[i].x[0];
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();
//getXspeed();
mountdata.encXspeed.val = kf[i].x[1];
mountdata.encXspeed.t = mountdata.encXposition.t;
}else{
mountdata.encYposition.val = Yenc2rad(pos);
pos = Yenc2rad(pos);
kalman3_predict(&kf[i]);
kalman3_update(&kf[i], pos);
DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
mountdata.encYposition.val = kf[i].x[0];
curtime(&mountdata.encYposition.t);
getYspeed();
//getYspeed();
mountdata.encYspeed.val = kf[i].x[1];
mountdata.encYspeed.t = mountdata.encYposition.t;
}
pthread_mutex_unlock(&datamutex);
}
@@ -860,11 +814,17 @@ static int bincmd(uint8_t *cmd, int len){
}else{
goto rtn;
}
data_t d;
SSstat ans;
data_t d, in;
d.buf = cmd;
d.len = d.maxlen = len;
ret = wr(&d, NULL, 0);
in.buf = (uint8_t*)&ans; in.maxlen = sizeof(SSstat);
ret = wr(&d, &in, 0);
DBG("%s", ret ? "SUCCESS" : "FAIL");
if(ret){
DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d",
ans.Xmot, ans.Ymot, ans.XLast, ans.YLast);
}
rtn:
pthread_mutex_unlock(&mntmutex);
return ret;