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

nxt

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-02-04 21:33:02 +03:00
parent a2e2896f29
commit aefdf3912c
9 changed files with 215 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
LibSidServo/serial.c
View File

@@ -44,7 +44,8 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
datamutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount
static struct timeval encRtmout = {0}, mntRtmout = {0};
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
@@ -119,16 +120,15 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getbyte(){
static int getencbyte(){
if(encfd < 0) return -1;
uint8_t byte;
fd_set rfds;
// default timeot = 100us, 1.5 bytes
struct timeval tv, tvdeflt = {.tv_sec = 0, .tv_usec = 100};
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(encfd, &rfds);
tv = tvdeflt;
tv = encRtmout;
int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
@@ -139,7 +139,31 @@ static int getbyte(){
ssize_t l = read(encfd, &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
}while(1);
return (int)byte;
}
// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
static int getmntbyte(){
if(mntfd < 0) return -1;
uint8_t byte;
fd_set rfds;
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
tv = mntRtmout;
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
if(errno == EINTR) continue;
return -1;
}
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
}while(1);
return (int)byte;
}
@@ -150,7 +174,7 @@ static void *encoderthread(void _U_ *u){
int wridx = 0, errctr = 0;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
int b = getbyte();
int b = getencbyte();
if(b == -2) ++errctr;
if(b < 0) continue;
errctr = 0;
@@ -178,15 +202,32 @@ static void *encoderthread(void _U_ *u){
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
SSstat status;
// data to get
data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)};
// cmd to send
const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1};
while(mntfd > -1 && errctr < MAX_ERR_CTR){
;
pthread_mutex_lock(&mntmutex);
;
// read data to status
if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){
DBG("Can't read SSstat");
++errctr; continue;
}
if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){
DBG("BAD checksum of SSstat");
++errctr; continue;
}
errctr = 0;
pthread_mutex_lock(&datamutex);
// now change data
SSconvstat(&status, &mountdata);
if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
;
}
;
pthread_mutex_unlock(&mntmutex);
pthread_mutex_unlock(&datamutex);
// allow writing & getters
usleep(Conf.MountReqInterval);
}
if(mntfd > -1){
close(mntfd);
@@ -196,7 +237,7 @@ static void *mountthread(void _U_ *u){
}
// open device and return its FD or -1
static int ttyopen(const char *path, int speed){
static int ttyopen(const char *path, speed_t speed){
int fd = -1;
struct termios2 tty;
DBG("Try to open %s @ %d", path, speed);
@@ -222,8 +263,10 @@ static int ttyopen(const char *path, int speed){
int openEncoder(const char *path, int speed){
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
if(encfd > -1) close(encfd);
encfd = ttyopen(path, speed);
encfd = ttyopen(path, (speed_t) speed);
if(encfd < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
@@ -236,8 +279,10 @@ int openEncoder(const char *path, int speed){
// return FALSE if failed
int openMount(const char *path, int speed){
if(mntfd > -1) close(mntfd);
mntfd = ttyopen(path, speed);
mntfd = ttyopen(path, (speed_t) speed);
if(mntfd < 0) return FALSE;
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 20000000 / speed; // 2 bytes
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
close(mntfd);
mntfd = -1;
@@ -273,3 +318,31 @@ mcc_errcodes_t getMD(mountdata_t *d){
pthread_mutex_unlock(&datamutex);
return MCC_E_OK;
}
/**
* @brief MountWriteRead - write and read @ once (or only read/write)
* @param out (o) - data to write or NULL if not need
* @param in (i) - data to read or NULL if not need
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
if(!out && !in) return FALSE;
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
if(out){
if(out->len != write(mntfd, out->buf, out->len)) goto ext;
write(mntfd, "\r", 1); // add EOL
}
if(in){
in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b;
}
}
ret = TRUE;
ext:
pthread_mutex_unlock(&mntmutex);
return ret;
}