eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add new encoders' controller

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-04-08 22:47:46 +03:00
parent 16dd3239de
commit 8ca8183cb8
8 changed files with 183 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
LibSidServo/serial.c
View File

@@ -34,7 +34,7 @@
#include "serial.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
static int encfd[2] = {-1, -1}, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};
@@ -122,24 +122,72 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
//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);
}
/**
* @brief getencval - get uint64_t data from encoder
* @param fd - encoder fd
* @param val - value read
* @param tv - measurement time
* @return amount of data read or 0 if problem
*/
static int getencval(int fd, double *val, struct timeval *tv){
if(fd < 0) return FALSE;
char buf[128];
int got = 0, Lmax = 127;
double t0 = dtime();
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) continue;
if(retval < 0){
if(errno == EINTR) continue;
return 0;
}
if(FD_ISSET(fd, &rfds)){
ssize_t l = read(fd, &buf[got], Lmax);
if(l < 1) return 0; // disconnected ??
got += l; Lmax -= l;
buf[got] = 0;
} else continue;
if(strchr(buf, '\n')) break;
}while(Lmax && dtime() - t0 > Conf.EncoderReqInterval);
if(got == 0) return 0; // WTF?
char *estr = strrchr(buf, '\n');
if(!estr) 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(tv) gettimeofday(tv, NULL);
return got;
}
// 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) return -1;
if(encfd[0] < 0) return -1;
uint8_t byte = 0;
fd_set rfds;
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(encfd, &rfds);
FD_SET(encfd[0], &rfds);
tv = encRtmout;
int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
if(errno == EINTR) continue;
return -1;
}
if(FD_ISSET(encfd, &rfds)){
ssize_t l = read(encfd, &byte, 1);
if(FD_ISSET(encfd[0], &rfds)){
ssize_t l = read(encfd[0], &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
@@ -179,11 +227,12 @@ static int getmntbyte(){
}
// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread(void _U_ *u){
static void *encoderthread1(void _U_ *u){
if(Conf.SepEncoder != 1) return NULL;
uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
int b = getencbyte();
if(b == -2) ++errctr;
if(b < 0) continue;
@@ -202,9 +251,42 @@ static void *encoderthread(void _U_ *u){
wridx = 0;
}
}
if(encfd > -1){
close(encfd);
encfd = -1;
if(encfd[0] > -1){
close(encfd[0]);
encfd[0] = -1;
}
return NULL;
}
static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started");
int errctr = 0;
double t0 = dtime();
const char *req = "next\n";
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
if(5 != write(encfd[0], req, 5)) ++errctr;
if(5 != write(encfd[1], req, 5)) ++errctr;
double v;
struct timeval tv;
if(getencval(encfd[0], &v, &tv)){
mountdata.encposition.X = X_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
if(getencval(encfd[1], &v, &tv)){
mountdata.encposition.Y = Y_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
errctr = 0;
} else ++errctr;
} else ++errctr;
while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
//DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
t0 = dtime();
}
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){
close(encfd[i]);
encfd[i] = -1;
}
}
return NULL;
}
@@ -293,7 +375,7 @@ static int ttyopen(const char *path, speed_t speed){
//tty.c_cc[VMIN] = 0; // non-canonical mode
//tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
DBG("Check speed");
DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
@@ -301,27 +383,47 @@ static int ttyopen(const char *path, speed_t speed){
}
// return FALSE if failed
int openEncoder(const char *path, int speed){
int openEncoder(){
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
if(encfd > -1) close(encfd);
encfd = ttyopen(path, (speed_t) speed);
if(encfd < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 200000000 / speed; // 20 bytes
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
return FALSE;
}
if(Conf.SepEncoder == 1){ // only one device
DBG("One device");
if(encfd[0] > -1) close(encfd[0]);
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
if(encfd[0] < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
close(encfd[0]);
encfd[0] = -1;
return FALSE;
}
}else if(Conf.SepEncoder == 2){
DBG("Two devices!");
const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath};
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1) close(encfd[i]);
encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
if(encfd[i] < 0) return FALSE;
}
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 1000; // 1ms
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
for(int i = 0; i < 2; ++i){
close(encfd[i]);
encfd[i] = -1;
}
return FALSE;
}
}else return FALSE;
DBG("Encoder opened, thread started");
return TRUE;
}
// return FALSE if failed
int openMount(const char *path, int speed){
int openMount(){
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", path, speed);
mntfd = ttyopen(path, (speed_t) speed);
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
if(mntfd < 0) return FALSE;
DBG("mntfd=%d", mntfd);
// clear buffer
@@ -334,7 +436,7 @@ int openMount(const char *path, int speed){
DBG("got %zd", l);
}while(1);*/
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create thread");
close(mntfd);
@@ -354,12 +456,16 @@ void closeSerial(){
close(mntfd);
mntfd = -1;
}
if(encfd > -1){
if(encfd[0] > -1){
DBG("Kill encoder thread");
pthread_cancel(encthread);
DBG("close fd");
close(encfd);
encfd = -1;
close(encfd[0]);
encfd[0] = -1;
if(Conf.SepEncoder == 2){
close(encfd[1]);
encfd[1] = -1;
}
}
}