From aefdf3912c41ba303d1c486546c2ba176dd96ce9 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Tue, 4 Feb 2025 21:33:02 +0300 Subject: [PATCH] nxt --- LibSidServo/examples/dumpmoving.c | 13 ++-- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 1 + LibSidServo/main.c | 16 +++-- LibSidServo/serial.c | 99 ++++++++++++++++++++++++---- LibSidServo/serial.h | 1 + LibSidServo/sidservo.h | 40 +++++++++-- LibSidServo/ssii.c | 70 ++++++++++++++++++++ LibSidServo/ssii.h | 6 ++ 9 files changed, 215 insertions(+), 33 deletions(-) create mode 100644 LibSidServo/ssii.c diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index e34fef4..9ad241c 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -51,10 +51,11 @@ void signals(int sig){ } static conf_t Config = { - .MountPath = "/dev/ttyS1", - .MountSpeed = 19200, - .EncoderPath = "/dev/ttyUSB0", - .EncoderSpeed = 153000 + .MountDevPath = "/dev/ttyS1", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB0", + //.EncoderDevSpeed = 153000, + .SepEncoder = 0 }; int main(int argc, char **argv){ @@ -67,8 +68,8 @@ int main(int argc, char **argv){ time_t curtime = time(NULL); LOGMSG("Started @ %s", ctime(&curtime)); DBG("Devices ready"); - LOGMSG("Mount device %s @ %d", Config.MountPath, Config.MountSpeed); - LOGMSG("Encoder device %s @ %d", Config.EncoderPath, Config.EncoderSpeed); + LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed); + LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index da2590f..481cf10 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 320f853..e6551f6 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -6,4 +6,5 @@ sidservo.h serial.c examples/CMakeLists.txt serial.h +ssii.c ssii.h diff --git a/LibSidServo/main.c b/LibSidServo/main.c index d0716e6..414b22b 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -42,22 +42,26 @@ static mcc_errcodes_t init(conf_t *c){ if(!c) return MCC_E_BADFORMAT; Conf = *c; mcc_errcodes_t ret = MCC_E_OK; - if(!Conf.MountPath || Conf.MountSpeed < 1200){ + if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; - }else if(!openMount(Conf.MountPath, Conf.MountSpeed)){ - DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed); + }else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){ + DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed); ret = MCC_E_MOUNTDEV; } if(Conf.SepEncoder){ - if(!Conf.EncoderPath || Conf.EncoderSpeed < 1200){ + if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){ DBG("Define encoder device path and speed"); ret = MCC_E_BADFORMAT; - }else if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){ - DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed); + }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){ + DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed); ret = MCC_E_ENCODERDEV; } } + if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){ + DBG("Bad value of MountReqInterval"); + retr = MCC_E_FATAL; + } if(ret != MCC_E_OK) quit(); return ret; } diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index b913b54..21b307a 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -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; +} diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index d007ae1..9dd2aa6 100644 --- a/LibSidServo/serial.h +++ b/LibSidServo/serial.h @@ -34,3 +34,4 @@ int openEncoder(const char *path, int speed); int openMount(const char *path, int speed); void closeSerial(); mcc_errcodes_t getMD(mountdata_t *d); +int MountWriteRead(const data_t *out, data_t *in); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index da43127..da3eef7 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -18,6 +18,8 @@ #pragma once +#include +#include #include // error codes @@ -30,11 +32,13 @@ typedef enum{ } mcc_errcodes_t; typedef struct{ - char* MountPath; // path to mount device - int MountSpeed; // serial speed - char* EncoderPath; // path to encoder device - int EncoderSpeed; // serial speed - int SepEncoder; // ==1 if encoder works as separate serial device + char* MountDevPath; // path to mount device + int MountDevSpeed; // serial speed + char* EncoderDevPath; // path to encoder device + int EncoderDevSpeed; // serial speed + int SepEncoder; // ==1 if encoder works as separate serial device + suseconds_t MountReqInterval;// interval between subsequent mount requests (microseconds) + ; } conf_t; // coordinates in degrees: X, Y and time when they were reached @@ -42,9 +46,31 @@ typedef struct{ double X; double Y; struct timeval msrtime; } coords_t; +// data to read/write typedef struct{ - coords_t position; - coords_t speed; + uint8_t *buf; // data buffer + size_t len; // its length + size_t maxlen; // maximal buffer size +} data_t; + +typedef struct{ + uint8_t XBits; + uint8_t YBits; + uint8_t ExtraBits; + uint16_t ain0; + uint16_t ain1; +} extradata_t; + +typedef struct{ + coords_t motposition; + coords_t encposition; + coords_t lastmotposition; + coords_t motspeed; + uint8_t keypad; + extradata_t extradata; + uint32_t millis; + double temperature; + double voltage; } mountdata_t; // mount class diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c new file mode 100644 index 0000000..ec52002 --- /dev/null +++ b/LibSidServo/ssii.c @@ -0,0 +1,70 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include "serial.h" +#include "ssii.h" + +uint16_t SScalcChecksum(uint8_t *buf, int len){ + uint16_t checksum = 0; + for(int i = 0; i < len; i++) + checksum += *buf++; + checksum ^= 0xFF00; // invert high byte + //DBG("Checksum of %d bytes: 0x%04x", len, checksum); + return checksum; +} + +// send short/long binary command +static int bincmd(uint8_t *cmd, int len){ + data_t d; + if(len == sizeof(SSscmd)){ + ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); + DBG("Short command"); + d.buf = CMD_SHORTCMD; + d.len = sizeof(CMD_SHORTCMD) - 1; + if(!MountWriteRead(&d, NULL)) return -1; + }else if(len == sizeof(SSlcmd)){ + ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); + DBG("Long command"); + d.buf = CMD_LONGCMD; + d.len = sizeof(CMD_LONGCMD) - 1; + if(!MountWriteRead(&d, NULL)) return -1; + }else{ + return -1; + } + DBG("Write %d bytes and wait for ans", len); + d.buf = cmd; + d.len = d.maxlen = len; + return MountWriteRead(&d, &d); +} +// return TRUE if OK +int SScmdS(SSscmd *cmd){ + return bincmd(cmd, sizeof(SSscmd)); +} +int SScmdL(SSlcmd *cmd){ + return bincmd(cmd, sizeof(SSlcmd)); +} + + +/** + * @brief SSconvstat - convert stat from SSII format to human + * @param status (i) - just read data + * @param mountdata (o) - output + */ +void SSconvstat(const SSstat *status, mountdata_t *mountdata){ + ; +} diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 3c84945..df4a979 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -20,6 +20,8 @@ #include +#include "sidservo.h" + // ASCII commands #define U8P(x) ((uint8_t*)x) // get binary data of all statistics @@ -105,3 +107,7 @@ typedef struct{ uint16_t checksum; // 32 } __attribute__((packed)) SSlcmd; // long command +int SScmdS(SSscmd *cmd); +int SScmdL(SSlcmd *cmd); +uint16_t SScalcChecksum(uint8_t *buf, int len); +void SSconvstat(const SSstat *status, mountdata_t *mountdata);