From 31c885ba026c7429586b0f9b454d7bd312034071 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Thu, 30 Jan 2025 22:14:10 +0300 Subject: [PATCH] some more --- LibSidServo/examples/dumpmoving.c | 3 +- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 1 + LibSidServo/main.c | 58 ++++++++------- LibSidServo/serial.c | 101 +++++++++++++++++-------- LibSidServo/serial.h | 2 +- LibSidServo/sidservo.h | 12 ++- LibSidServo/ssii.h | 107 +++++++++++++++++++++++++++ 8 files changed, 224 insertions(+), 62 deletions(-) create mode 100644 LibSidServo/ssii.h diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index 72e4ec8..e34fef4 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -74,7 +74,8 @@ int main(int argc, char **argv){ signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - ;;; + double t0 = sl_dtime(); + while(sl_dtime() - t0 < 10.); Mount.quit(); return 0; } diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index b4b5c69..da2590f 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 54d6894..320f853 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -6,3 +6,4 @@ sidservo.h serial.c examples/CMakeLists.txt serial.h +ssii.h diff --git a/LibSidServo/main.c b/LibSidServo/main.c index aba180e..d0716e6 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -24,33 +24,6 @@ conf_t Conf = {0}; -/** - * @brief init - open serial devices and do other job - * @param c - initial configuration - * @return error code - */ -static mcc_errcodes_t init(conf_t *c){ - if(!c) return MCC_E_BADFORMAT; - Conf = *c; - if(!Conf.EncoderPath || Conf.EncoderSpeed < 1200){ - DBG("Define encoder device path and speed"); - return MCC_E_BADFORMAT; - } - if(!Conf.MountPath || Conf.MountSpeed < 1200){ - DBG("Define mount device path and speed"); - return MCC_E_BADFORMAT; - } - if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){ - DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed); - return MCC_E_ENCODERDEV; - } - if(!openMount(Conf.MountPath, Conf.MountSpeed)){ - DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed); - return MCC_E_MOUNTDEV; - } - return MCC_E_OK; -} - /** * @brief quit - close all opened and return to default state */ @@ -60,9 +33,38 @@ static void quit(){ DBG("Exit"); } +/** + * @brief init - open serial devices and do other job + * @param c - initial configuration + * @return error code + */ +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){ + 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); + ret = MCC_E_MOUNTDEV; + } + if(Conf.SepEncoder){ + if(!Conf.EncoderPath || Conf.EncoderSpeed < 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); + ret = MCC_E_ENCODERDEV; + } + } + if(ret != MCC_E_OK) quit(); + return ret; +} + // init mount class mount_t Mount = { .init = init, .quit = quit, - .getEnc = getEnc + .getMountData = getMD }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 3faab78..b913b54 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -19,32 +19,31 @@ #include #include #include +#include #include -#include #include #include #include #include #include #include -#include #include #include #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; } diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index 9fc7bd5..d007ae1 100644 --- a/LibSidServo/serial.h +++ b/LibSidServo/serial.h @@ -33,4 +33,4 @@ int openEncoder(const char *path, int speed); int openMount(const char *path, int speed); void closeSerial(); -mcc_errcodes_t getEnc(coords_t *c); +mcc_errcodes_t getMD(mountdata_t *d); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 703a92f..da43127 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -18,6 +18,8 @@ #pragma once +#include + // error codes typedef enum{ MCC_E_OK = 0, // all OK @@ -32,18 +34,24 @@ typedef struct{ int MountSpeed; // serial speed char* EncoderPath; // path to encoder device int EncoderSpeed; // serial speed + int SepEncoder; // ==1 if encoder works as separate serial device } conf_t; // coordinates in degrees: X, Y and time when they were reached typedef struct{ - double X; double Y; double msrtime; + double X; double Y; struct timeval msrtime; } coords_t; +typedef struct{ + coords_t position; + coords_t speed; +} mountdata_t; + // mount class typedef struct{ mcc_errcodes_t (*init)(conf_t *c); void (*quit)(); - mcc_errcodes_t (*getEnc)(coords_t *c); + mcc_errcodes_t (*getMountData)(mountdata_t *d); } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h new file mode 100644 index 0000000..3c84945 --- /dev/null +++ b/LibSidServo/ssii.h @@ -0,0 +1,107 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 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 . + */ + +#pragma once + +#include + +// ASCII commands +#define U8P(x) ((uint8_t*)x) +// get binary data of all statistics +#define CMD_GETSTAT U8P("XXS") +// send short command +#define CMD_SHORTCMD U8P("XXR") +// send long command +#define CMD_LONGCMD U8P("YXR") +// get/set X/Y in motsteps +#define CMD_MOTX U8P("X") +#define CMD_MOTY U8P("Y") +// -//- in encoders' ticks +#define CMD_ENCX U8P("XZ") +#define CMD_ENCY U8P("YZ") +// normal stop X/Y +#define CMD_STOPX U8P("XN") +#define CMD_STOPY U8P("YN") +// emergency stop +#define CMD_EMSTOPX U8P("XG") +#define CMD_EMSTOPY U8P("YG") +// getters of motor's encoders per rev +#define CMD_GETXMEPR U8P("XXU") +#define CMD_GETYMEPR U8P("XXV") +// -//- axis encoders +#define CMD_GETXAEPR U8P("XXT") +#define CMD_GETYAEPR U8P("XXZ") +// exit ASCII checksum mode +#define CMD_EXITACM U8P("YXY0\r\xb8") + +// timeout (seconds) of reading answer (from last symbol read) +#define READTIMEOUT (0.05) + +// steps per revolution; TODO: move to Conf +#define HA_MOT_STEPSPERREV (4394294) +#define DEC_MOT_STEPSPERREV (3325291) + +// encoder's tolerance (ticks); TODO: move to Conf +#define HAENCTOL (25) +#define DECENCTOL (25) + + +// all need data in one +typedef struct{ // 41 bytes + uint8_t ctrlAddr; // 0 a8 + controller address + int32_t DECmot; // 1 Dec/HA motor position + int32_t HAmot; // 5 + int32_t DECenc; // 9 Dec/HA encoder position + int32_t HAenc; // 13 + uint8_t keypad; // 17 keypad status + uint8_t XBits; // 18 + uint8_t YBits; // 19 + uint8_t ExtraBits; // 20 + uint16_t ain0; // 21 analog inputs + uint16_t ain1; // 23 + uint32_t millis; // 25 milliseconds clock + int8_t tF; // 29 temperature (degF) + uint8_t voltage; // 30 input voltage *10 (RA worm phase?) + uint32_t DecLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change + uint32_t HALast; // 35 Az/RA motor location at last Az/RA scope encoder location change + uint16_t checksum; // 39 checksum, H inverted +}__attribute__((packed)) SSstat; + +typedef struct{ + int32_t DECmot; // 0 DEC motor position + int32_t DECspeed; // 4 DEC speed + int32_t HAmot; // 8 + int32_t HAspeed; // 12 + uint8_t xychange; // 16 change Xbits/Ybits value + uint8_t XBits; // 17 + uint8_t YBits; // 18 + uint16_t checksum; // 19 +} __attribute__((packed)) SSscmd; // short command + +typedef struct{ + int32_t DECmot; // 0 DEC motor position + int32_t DECspeed; // 4 DEC speed + int32_t HAmot; // 8 + int32_t HAspeed; // 12 + int32_t DECadder; // 16 - DEC adder + int32_t HAadder; // 20 + int32_t DECatime; // 24 DEC adder time + int32_t HAatime; // 28 + uint16_t checksum; // 32 +} __attribute__((packed)) SSlcmd; // long command +