349 lines
10 KiB
C
349 lines
10 KiB
C
/*
|
|
* This file is part of the libsidservo project.
|
|
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <asm-generic/termbits.h>
|
|
#include <errno.h>
|
|
#include <fcntl.h>
|
|
#include <math.h>
|
|
#include <pthread.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/stat.h>
|
|
#include <time.h>
|
|
#include <unistd.h>
|
|
|
|
#include "dbg.h"
|
|
#include "serial.h"
|
|
#include "ssii.h"
|
|
|
|
// serial devices FD
|
|
static int encfd = -1, mntfd = -1;
|
|
// main mount data
|
|
static mountdata_t mountdata = {0};
|
|
|
|
// mutexes for RW operations with mount device and data
|
|
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;
|
|
int32_t encX;
|
|
int32_t encY;
|
|
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 t;
|
|
struct timeval tv;
|
|
gettimeofday(&tv, NULL);
|
|
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
|
return t;
|
|
}
|
|
|
|
// init start time
|
|
static void gttime(){
|
|
struct timeval tv;
|
|
gettimeofday(&tv, NULL);
|
|
tv_sec_got = tv.tv_sec;
|
|
tv_usec_got = tv.tv_usec;
|
|
}
|
|
#endif
|
|
|
|
/**
|
|
* @brief parse_encbuf - check encoder buffer (for separate encoder) 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], struct timeval *tv){
|
|
enc_t *edata = (enc_t*) databuf;
|
|
if(edata->magick != ENC_MAGICK){
|
|
DBG("No magick");
|
|
return;
|
|
}
|
|
if(edata->CRC[3]){
|
|
DBG("No 0 @ end: 0x%02x", edata->CRC[3]);
|
|
return;
|
|
}
|
|
uint32_t POS_SUM = 0;
|
|
for(int i = 1; i < 9; ++i) POS_SUM += databuf[i];
|
|
uint8_t x = POS_SUM >> 8;
|
|
if(edata->CRC[0] != x){
|
|
DBG("CRC[0] = 0x%02x, need 0x%02x", edata->CRC[0], x);
|
|
return;
|
|
}
|
|
uint8_t y = ((0xFFFF - POS_SUM) & 0xFF) - x;
|
|
if(edata->CRC[1] != y){
|
|
DBG("CRC[1] = 0x%02x, need 0x%02x", edata->CRC[1], y);
|
|
return;
|
|
}
|
|
y = (0xFFFF - POS_SUM) >> 8;
|
|
if(edata->CRC[2] != y){
|
|
DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
|
|
return;
|
|
}
|
|
pthread_mutex_lock(&datamutex);
|
|
mountdata.position.X = Xpos2rad(edata->encX);
|
|
mountdata.position.Y = Ypos2rad(edata->encY);
|
|
mountdata.position.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);
|
|
}
|
|
|
|
// 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;
|
|
uint8_t byte;
|
|
fd_set rfds;
|
|
struct timeval tv;
|
|
do{
|
|
FD_ZERO(&rfds);
|
|
FD_SET(encfd, &rfds);
|
|
tv = encRtmout;
|
|
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;
|
|
} 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;
|
|
}
|
|
|
|
// main encoder thread (for separate encoder): read next data and make parsing
|
|
static void *encoderthread(void _U_ *u){
|
|
uint8_t databuf[ENC_DATALEN];
|
|
int wridx = 0, errctr = 0;
|
|
struct timeval tv;
|
|
while(encfd > -1 && errctr < MAX_ERR_CTR){
|
|
int b = getencbyte();
|
|
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;
|
|
gettimeofday(&tv, NULL);
|
|
}
|
|
continue;
|
|
}else databuf[wridx++] = (uint8_t) b;
|
|
if(wridx == ENC_DATALEN){
|
|
parse_encbuf(databuf, &tv);
|
|
wridx = 0;
|
|
}
|
|
}
|
|
if(encfd > -1){
|
|
close(encfd);
|
|
encfd = -1;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
// 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){
|
|
// 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(&datamutex);
|
|
// allow writing & getters
|
|
usleep(Conf.MountReqInterval);
|
|
}
|
|
if(mntfd > -1){
|
|
close(mntfd);
|
|
mntfd = -1;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
// open device and return its FD or -1
|
|
static int ttyopen(const char *path, speed_t 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){
|
|
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 = 15000000 / speed; // 1.5 bytes
|
|
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
|
|
close(encfd);
|
|
encfd = -1;
|
|
return FALSE;
|
|
}
|
|
DBG("Encoder opened, thread started");
|
|
return TRUE;
|
|
}
|
|
|
|
// return FALSE if failed
|
|
int openMount(const char *path, int speed){
|
|
if(mntfd > -1) close(mntfd);
|
|
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;
|
|
return FALSE;
|
|
}
|
|
DBG("Mount opened, thread started");
|
|
return TRUE;
|
|
}
|
|
|
|
// close all opened serial devices and quit threads
|
|
void closeSerial(){
|
|
if(mntfd > -1){
|
|
DBG("Close mount");
|
|
pthread_mutex_lock(&mntmutex);
|
|
close(mntfd);
|
|
mntfd = -1;
|
|
pthread_mutex_unlock(&mntmutex);
|
|
}
|
|
if(encfd > -1){
|
|
DBG("Close encoder");
|
|
pthread_cancel(encthread);
|
|
pthread_join(encthread, NULL);
|
|
close(encfd);
|
|
encfd = -1;
|
|
}
|
|
}
|
|
|
|
// get fresh encoder information
|
|
mcc_errcodes_t getMD(mountdata_t *d){
|
|
if(!d) return MCC_E_BADFORMAT;
|
|
pthread_mutex_lock(&datamutex);
|
|
*d = mountdata;
|
|
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;
|
|
}
|