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

PID works

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 17:21:33 +03:00
parent 9e7c732afd
commit 071e7fc189
6 changed files with 85 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
serial.c
View File

@@ -55,6 +55,8 @@ static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, // encoder read
mnt1Rtmout = {.tv_sec = 0, .tv_usec = 200000}, // first reading
mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // next readings
static volatile int GlobExit = 0;
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
@@ -178,16 +180,16 @@ static int readmntdata(uint8_t *buffer, int maxlen){
return -1;
}
if(!buffer || maxlen < 1) return 0;
DBG("ask for %d bytes", maxlen);
//DBG("ask for %d bytes", maxlen);
int got = 0;
fd_set rfds;
struct timeval tv = mnt1Rtmout;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
DBG("select");
//DBG("select");
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
DBG("returned %d", retval);
//DBG("returned %d", retval);
if(retval < 0){
if(errno == EINTR) continue;
DBG("Error in select()");
@@ -244,7 +246,7 @@ static void *encoderthread1(void _U_ *u){
uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0;
struct timespec tcur;
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
while(encfd[0] > -1 && errctr < MAX_ERR_CTR && !GlobExit){
int b = getencbyte();
if(b == -2) ++errctr;
if(b < 0) continue;
@@ -402,27 +404,27 @@ static void *encoderthread2(void _U_ *u){
// 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]);
//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();
mountdata.encXspeed.val = kf[i].x[1];
mountdata.encXspeed.t = mountdata.encXposition.t;
getXspeed();
//mountdata.encXspeed.val = kf[i].x[1];
//mountdata.encXspeed.t = mountdata.encXposition.t;
}else{
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]);
//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();
mountdata.encYspeed.val = kf[i].x[1];
mountdata.encYspeed.t = mountdata.encYposition.t;
getYspeed();
//mountdata.encYspeed.val = kf[i].x[1];
//mountdata.encYspeed.t = mountdata.encYposition.t;
}
pthread_mutex_unlock(&datamutex);
}
@@ -435,7 +437,7 @@ static void *encoderthread2(void _U_ *u){
}
}
if(got == 2) errctr = 0;
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR && !GlobExit);
DBG("\n\nEXIT: ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){
@@ -491,7 +493,7 @@ static void *mountthread(void _U_ *u){
if(Conf.RunModel){
double Xprev = NAN, Yprev = NAN; // previous coordinates
int xcnt = 0, ycnt = 0;
while(1){
while(!GlobExit){
coordpair_t c;
movestate_t xst, yst;
// now change data
@@ -529,7 +531,7 @@ static void *mountthread(void _U_ *u){
// cmd to send
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
if(!cmd_getstat) goto failed;
while(mntfd > -1 && errctr < MAX_ERR_CTR){
while(mntfd > -1 && errctr < MAX_ERR_CTR && !GlobExit){
// read data to status
struct timespec tcur;
if(!curtime(&tcur)) continue;
@@ -671,14 +673,17 @@ create_thread:
// close all opened serial devices and quit threads
void closeSerial(){
DBG("CLOSE all devices");
GlobExit = 1;
DBG("Give 100ms to proper close");
usleep(100000);
DBG("Force closed all devices");
if(mntfd > -1){
DBG("Cancel mount thread");
pthread_cancel(mntthread);
DBG("join mount thread");
pthread_join(mntthread, NULL);
DBG("close mount fd");
close(mntfd);
if(mntfd > -1) close(mntfd);
mntfd = -1;
}
if(encfd[0] > -1){
@@ -687,13 +692,14 @@ void closeSerial(){
DBG("join encoder thread");
pthread_join(encthread, NULL);
DBG("close encoder's fd");
close(encfd[0]);
if(encfd[0] > -1) close(encfd[0]);
encfd[0] = -1;
if(Conf.SepEncoder == 2 && encfd[1] > -1){
close(encfd[1]);
encfd[1] = -1;
}
}
GlobExit = 0;
}
// get fresh encoder information
@@ -721,10 +727,10 @@ static int wr(const data_t *out, data_t *in, int needeol){
DBG("Wrong arguments or no mount fd");
return FALSE;
}
DBG("clrbuf");
//DBG("clrbuf");
clrmntbuf();
if(out){
DBG("write %zd bytes (%s)", out->len, out->buf);
//DBG("write %zd bytes (%s)", out->len, out->buf);
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need");
return FALSE;
@@ -755,11 +761,11 @@ static int wr(const data_t *out, data_t *in, int needeol){
*/
int MountWriteRead(const data_t *out, data_t *in){
if(Conf.RunModel) return FALSE;
double t0 = timefromstart();
//double t0 = timefromstart();
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex);
DBG("Got %gus", (timefromstart()-t0)*1e6);
//DBG("Got %gus", (timefromstart()-t0)*1e6);
return ret;
}
// send binary data - without EOL
@@ -771,7 +777,7 @@ int MountWriteReadRaw(const data_t *out, data_t *in){
return ret;
}
#ifdef EBUG
#if 0
static void logscmd(SSscmd *c){
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits);
@@ -794,20 +800,17 @@ static int bincmd(uint8_t *cmd, int len){
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
// dummy buffer to clear trash in input
//char ans[300];
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
#ifdef EBUG
//DBG("Short command");
#if 0
logscmd((SSscmd*)cmd);
#endif
if(!wr(dscmd, NULL, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
#ifdef EBUG
// DBG("Long command");
#if 0
loglcmd((SSlcmd*)cmd);
#endif
if(!wr(dlcmd, NULL, 1)) goto rtn;
@@ -822,8 +825,11 @@ static int bincmd(uint8_t *cmd, int len){
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);
SSscmd *sc = (SSscmd*)cmd;
mountdata.Xtarget = sc->Xmot;
mountdata.Ytarget = sc->Ymot;
DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d; Xtag/Ytag: %d/%d",
ans.Xmot, ans.Ymot, ans.XLast, ans.YLast, mountdata.Xtarget, mountdata.Ytarget);
}
rtn:
pthread_mutex_unlock(&mntmutex);