remove dependence of libusefull_macros
This commit is contained in:
@@ -16,35 +16,62 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <asm-generic/termbits.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <pthread.h>
|
||||
#include <stdatomic.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <usefull_macros.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "dbg.h"
|
||||
#include "serial.h"
|
||||
#include "sidservo.h"
|
||||
|
||||
// serial devices
|
||||
static sl_tty_t *EncDev = NULL, *MntDev = NULL;
|
||||
// serial devices FD
|
||||
static int encfd = -1, mntfd = -1;
|
||||
// time of last EncData started
|
||||
static double tgot = 0.;
|
||||
static _Atomic double tgot = 0.;
|
||||
// last Enc values
|
||||
static uint32_t encX = 0, encY = 0;
|
||||
static _Atomic int32_t encX = 0, encY = 0;
|
||||
|
||||
// mutex for RW operations with mount device
|
||||
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
// encoders thread
|
||||
static pthread_t encthread;
|
||||
|
||||
// encoders raw data
|
||||
typedef struct __attribute__((packed)){
|
||||
uint8_t magick;
|
||||
uint32_t encX;
|
||||
uint32_t encY;
|
||||
int32_t encX;
|
||||
int32_t encY;
|
||||
uint8_t CRC[4];
|
||||
} enc_t;
|
||||
|
||||
/**
|
||||
* @brief dtime - UNIX time with microsecond
|
||||
* @return value
|
||||
*/
|
||||
static double dtime(){
|
||||
double t;
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
||||
return t;
|
||||
}
|
||||
|
||||
|
||||
static void parce_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
|
||||
/**
|
||||
* @brief parse_encbuf - check encoder buffer and fill fresh data
|
||||
* @param databuf - input buffer with 13 bytes of data
|
||||
* @param nexttime - time when databuf[0] got
|
||||
*/
|
||||
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
|
||||
enc_t *edata = (enc_t*) databuf;
|
||||
if(edata->magick != ENC_MAGICK){
|
||||
DBG("No magick");
|
||||
@@ -74,67 +101,132 @@ static void parce_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
|
||||
encX = edata->encX;
|
||||
encY = edata->encY;
|
||||
tgot = nexttime;
|
||||
DBG("time = %g, X=%d, Y=%d", tgot, encX, encY);
|
||||
}
|
||||
|
||||
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
|
||||
static int getbyte(){
|
||||
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};
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(encfd, &rfds);
|
||||
tv = tvdeflt;
|
||||
int retval = select(encfd + 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(l != 1) return -2; // disconnected ??
|
||||
break;
|
||||
}
|
||||
}while(1);
|
||||
return (int)byte;
|
||||
}
|
||||
|
||||
// main encoder thread: read next data and make parsing
|
||||
static void *encoderthread(void _U_ *u){
|
||||
uint8_t databuf[ENC_DATALEN];
|
||||
int wridx = 0;
|
||||
double nexttime = 0.;
|
||||
void add2buf(){
|
||||
size_t len = ENC_DATALEN - wridx;
|
||||
if(EncDev->buflen < len) len = EncDev->buflen;
|
||||
memcpy(databuf+wridx, EncDev->buf, len);
|
||||
wridx += len;
|
||||
}
|
||||
while(EncDev){
|
||||
if(sl_tty_read(EncDev)){
|
||||
DBG("Got %zd bytes from Encoder", EncDev->buflen);
|
||||
if(EncDev->buflen <= ENC_DATALEN){
|
||||
if(wridx == 0){
|
||||
if((uint8_t)EncDev->buf[0] == ENC_MAGICK){
|
||||
add2buf();
|
||||
nexttime = sl_dtime();
|
||||
}
|
||||
}else add2buf();
|
||||
if(wridx == ENC_DATALEN){
|
||||
parce_encbuf(databuf, nexttime);
|
||||
wridx = 0;
|
||||
}
|
||||
int wridx = 0, errctr = 0;
|
||||
double starttime = 0.;
|
||||
while(encfd > -1 && errctr < MAX_ERR_CTR){
|
||||
int b = getbyte();
|
||||
if(b == -2) ++errctr;
|
||||
if(b < 0) continue;
|
||||
errctr = 0;
|
||||
DBG("Got byte from Encoder: 0x%02X", b);
|
||||
if(wridx == 0){
|
||||
if((uint8_t)b == ENC_MAGICK){
|
||||
DBG("Got magic -> start filling packet");
|
||||
databuf[wridx++] = (uint8_t) b;
|
||||
starttime = dtime();
|
||||
}
|
||||
continue;
|
||||
}else databuf[wridx++] = (uint8_t) b;
|
||||
if(wridx == ENC_DATALEN){
|
||||
parse_encbuf(databuf, starttime);
|
||||
wridx = 0;
|
||||
}
|
||||
}
|
||||
if(encfd > -1){
|
||||
close(encfd);
|
||||
encfd = -1;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// open device and return its FD or -1
|
||||
static int ttyopen(const char *path, int speed){
|
||||
int fd = -1;
|
||||
struct termios2 tty;
|
||||
DBG("Try to open %s @ %d", path, speed);
|
||||
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
|
||||
if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
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;
|
||||
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);
|
||||
return fd;
|
||||
}
|
||||
|
||||
// return FALSE if failed
|
||||
int openEncoder(const char *path, int speed){
|
||||
EncDev = sl_tty_new((char*)path, speed, 256);
|
||||
if(EncDev) EncDev = sl_tty_open(EncDev, 1);
|
||||
if(!EncDev) return FALSE;
|
||||
sl_tty_tmout(5.); // 5us timeout
|
||||
if(encfd > -1) close(encfd);
|
||||
encfd = ttyopen(path, speed);
|
||||
if(encfd < 0) return FALSE;
|
||||
if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE;
|
||||
DBG("Encoder opened");
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return FALSE if failed
|
||||
int openMount(const char *path, int speed){
|
||||
MntDev = sl_tty_new((char*)path, speed, 256);
|
||||
if(MntDev) MntDev = sl_tty_open(MntDev, 1);
|
||||
if(!MntDev) return FALSE;
|
||||
if(mntfd > -1) close(mntfd);
|
||||
mntfd = ttyopen(path, speed);
|
||||
if(mntfd < 0) return FALSE;
|
||||
DBG("Mount opened");
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// close all opened serial devices and quit threads
|
||||
void closeSerial(){
|
||||
if(MntDev){
|
||||
if(mntfd > -1){
|
||||
DBG("Close mount");
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
sl_tty_close(&MntDev);
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
}
|
||||
if(EncDev){
|
||||
if(encfd > -1){
|
||||
DBG("Close encoder");
|
||||
pthread_cancel(encthread);
|
||||
pthread_join(encthread, NULL);
|
||||
sl_tty_close(&EncDev);
|
||||
close(encfd);
|
||||
encfd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// get fresh encoder information
|
||||
mcc_errcodes_t getEnc(coords_t *c){
|
||||
if(!c) return MCC_E_BADFORMAT;
|
||||
if(encfd < 0) return MCC_E_ENCODERDEV;
|
||||
c->msrtime = tgot;
|
||||
c->X = (double)encX / ENC_TURN_XTICKS * 360.;
|
||||
c->Y = (double)encY / ENC_TURN_YTICKS * 360.;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user