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-08 19:48:44 +03:00
parent 55d8d359b4
commit d7df8e5bf1
8 changed files with 387 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
LibSidServo/serial.c
View File

@@ -54,15 +54,11 @@ typedef struct __attribute__((packed)){
uint8_t CRC[4];
} enc_t;
#define Xpos2rad(pos) (2. * (double)pos / ENC_TURN_XTICKS * M_PI)
#define Ypos2rad(pos) (2. * (double)pos / ENC_TURN_YTICKS * M_PI)
#if 0
/**
* @brief dtime - UNIX time with microsecond
* @return value
*/
static double dtime(){
double dtime(){
double t;
struct timeval tv;
gettimeofday(&tv, NULL);
@@ -70,6 +66,7 @@ static double dtime(){
return t;
}
#if 0
// init start time
static void gttime(){
struct timeval tv;
@@ -112,11 +109,11 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
return;
}
pthread_mutex_lock(&datamutex);
mountdata.position.X = Xpos2rad(edata->encX);
mountdata.position.Y = Ypos2rad(edata->encY);
mountdata.position.msrtime = *tv;
mountdata.encposition.X = X_ENC2RAD(edata->encX);
mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
mountdata.encposition.msrtime = *tv;
pthread_mutex_unlock(&datamutex);
DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.position.X*180./M_PI, mountdata.position.Y*180./M_PI);
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);
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@@ -149,18 +146,25 @@ static int getmntbyte(){
uint8_t byte;
fd_set rfds;
struct timeval tv;
/* ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l == 0) return -1;
if(l != 1) return -2; // disconnected ??
return (int) byte;*/
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;
DBG("Error in select()");
return -1;
}
//DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
@@ -199,35 +203,58 @@ static void *encoderthread(void _U_ *u){
return NULL;
}
data_t *cmd2dat(const char *cmd){
if(!cmd) return NULL;
data_t *d = calloc(1, sizeof(data_t));
if(!d) return NULL;
d->buf = (uint8_t*)strdup(cmd);
d->len = strlen(cmd);
d->maxlen = d->len + 1;
return d;
}
void data_free(data_t **x){
if(!x || !*x) return;
free((*x)->buf);
free(*x);
*x = NULL;
}
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
SSstat status;
uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
// data to get
data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)};
data_t d = {.buf = buf, .maxlen = sizeof(buf)};
// cmd to send
const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1};
const data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
double t0 = dtime();
/*
#ifdef EBUG
double t00 = t0;
#endif
*/
while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status
if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){
DBG("Can't read SSstat");
struct timeval tgot;
if(0 != gettimeofday(&tgot, NULL)) continue;
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
++errctr; continue;
}
if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){
DBG("BAD checksum of SSstat");
if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){
DBG("BAD checksum of SSstat, need %d", status->checksum);
++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
;
}
;
SSconvstat(status, &mountdata, &tgot);
pthread_mutex_unlock(&datamutex);
// allow writing & getters
usleep(Conf.MountReqInterval);
//DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
while(dtime() - t0 < Conf.MountReqInterval);
t0 = dtime();
}
if(mntfd > -1){
close(mntfd);
@@ -249,13 +276,13 @@ static int ttyopen(const char *path, speed_t speed){
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
tty.c_ispeed = speed;
tty.c_ospeed = speed;
tty.c_cc[VMIN] = 0; // non-canonical mode
tty.c_cc[VTIME] = 5;
//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");
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive
ioctl(fd, TIOCEXCL);
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
return fd;
}
@@ -266,7 +293,7 @@ int openEncoder(const char *path, int speed){
encfd = ttyopen(path, (speed_t) speed);
if(encfd < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes
encRtmout.tv_usec = 200000000 / speed; // 20 bytes
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
@@ -279,11 +306,23 @@ int openEncoder(const char *path, int speed){
// return FALSE if failed
int openMount(const char *path, int speed){
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", path, speed);
mntfd = ttyopen(path, (speed_t) speed);
if(mntfd < 0) return FALSE;
DBG("mntfd=%d", mntfd);
// clear buffer
while(getmntbyte() > -1);
/*int g = write(mntfd, "XXS\r", 4);
DBG("Written %d", g);
uint8_t buf[100];
do{
ssize_t l = read(mntfd, buf, 100);
DBG("got %zd", l);
}while(1);*/
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 20000000 / speed; // 2 bytes
mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create thread");
close(mntfd);
mntfd = -1;
return FALSE;
@@ -295,16 +334,18 @@ int openMount(const char *path, int speed){
// close all opened serial devices and quit threads
void closeSerial(){
if(mntfd > -1){
DBG("Close mount");
pthread_mutex_lock(&mntmutex);
DBG("Kill mount thread");
if(0 == pthread_cancel(mntthread))
pthread_join(mntthread, NULL);
DBG("close fd");
close(mntfd);
mntfd = -1;
pthread_mutex_unlock(&mntmutex);
}
if(encfd > -1){
DBG("Close encoder");
pthread_cancel(encthread);
pthread_join(encthread, NULL);
DBG("Kill encoder thread");
if(0 == pthread_cancel(encthread))
pthread_join(encthread, NULL);
DBG("close fd");
close(encfd);
encfd = -1;
}
@@ -326,12 +367,14 @@ mcc_errcodes_t getMD(mountdata_t *d){
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
if(!out && !in) return FALSE;
if((!out && !in) || mntfd < 0) 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(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext;
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
}
if(in){
in->len = 0;