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

some more

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-01-30 22:14:10 +03:00
parent 5441a87fff
commit 31c885ba02
8 changed files with 224 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
LibSidServo/serial.c
View File

@@ -19,32 +19,31 @@
#include <asm-generic/termbits.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <pthread.h>
#include <stdatomic.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.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 "ssii.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
// time of last EncData started
static _Atomic double tgot = 0.;
// last Enc values
static _Atomic int32_t encX = 0, encY = 0;
// main mount data
static mountdata_t mountdata = {0};
// mutex for RW operations with mount device
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread
static pthread_t encthread;
// 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;
// encoders raw data
typedef struct __attribute__((packed)){
@@ -54,6 +53,10 @@ 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
@@ -66,12 +69,21 @@ static double dtime(){
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 and fill fresh data
* @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], double nexttime){
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");
@@ -98,10 +110,12 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
return;
}
encX = edata->encX;
encY = edata->encY;
tgot = nexttime;
DBG("time = %g, X=%d, Y=%d", tgot, encX, encY);
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
@@ -130,11 +144,11 @@ static int getbyte(){
return (int)byte;
}
// main encoder thread: read next data and make parsing
// 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;
double starttime = 0.;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
int b = getbyte();
if(b == -2) ++errctr;
@@ -145,12 +159,12 @@ static void *encoderthread(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){
DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b;
starttime = dtime();
gettimeofday(&tv, NULL);
}
continue;
}else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){
parse_encbuf(databuf, starttime);
parse_encbuf(databuf, &tv);
wridx = 0;
}
}
@@ -161,6 +175,26 @@ static void *encoderthread(void _U_ *u){
return NULL;
}
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
while(mntfd > -1 && errctr < MAX_ERR_CTR){
;
pthread_mutex_lock(&mntmutex);
;
if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
;
}
;
pthread_mutex_unlock(&mntmutex);
}
if(mntfd > -1){
close(mntfd);
mntfd = -1;
}
return NULL;
}
// open device and return its FD or -1
static int ttyopen(const char *path, int speed){
int fd = -1;
@@ -186,11 +220,16 @@ static int ttyopen(const char *path, int speed){
// 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);
if(encfd < 0) return FALSE;
if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE;
DBG("Encoder opened");
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
return FALSE;
}
DBG("Encoder opened, thread started");
return TRUE;
}
@@ -199,7 +238,12 @@ int openMount(const char *path, int speed){
if(mntfd > -1) close(mntfd);
mntfd = ttyopen(path, speed);
if(mntfd < 0) return FALSE;
DBG("Mount opened");
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
close(mntfd);
mntfd = -1;
return FALSE;
}
DBG("Mount opened, thread started");
return TRUE;
}
@@ -222,11 +266,10 @@ void closeSerial(){
}
// 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.;
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;
}