From 04c98ed557241d374f94df06bfcf1b99c6a8e378 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Wed, 19 Feb 2025 23:09:17 +0300 Subject: [PATCH] add LibSidServo - pre-pre-...-pre-alpha --- Auxiliary_utils/LibSidServo/CMakeLists.txt | 87 ++++ Auxiliary_utils/LibSidServo/dbg.h | 64 +++ .../LibSidServo/examples/CMakeLists.txt | 12 + Auxiliary_utils/LibSidServo/examples/dump.c | 144 ++++++ Auxiliary_utils/LibSidServo/examples/dump.h | 29 ++ .../LibSidServo/examples/dumpmoving.c | 105 ++++ .../LibSidServo/examples/dumpmoving_scmd.c | 174 +++++++ .../LibSidServo/examples/dumpswing.c | 172 +++++++ Auxiliary_utils/LibSidServo/examples/goto.c | 117 +++++ .../LibSidServo/examples/scmd_traectory.c | 159 ++++++ .../LibSidServo/examples/simpleconv.h | 29 ++ .../LibSidServo/examples/traectories.c | 143 ++++++ .../LibSidServo/examples/traectories.h | 32 ++ .../LibSidServo/libsidservo.cflags | 1 + .../LibSidServo/libsidservo.config | 6 + .../LibSidServo/libsidservo.creator | 1 + .../LibSidServo/libsidservo.creator.user | 184 +++++++ .../libsidservo.creator.user.7bd84e3 | 165 +++++++ .../libsidservo.creator.user.cf63021 | 184 +++++++ .../LibSidServo/libsidservo.cxxflags | 1 + Auxiliary_utils/LibSidServo/libsidservo.files | 19 + .../LibSidServo/libsidservo.includes | 2 + Auxiliary_utils/LibSidServo/main.c | 161 ++++++ Auxiliary_utils/LibSidServo/mainconf.conf | 7 + Auxiliary_utils/LibSidServo/serial.c | 461 ++++++++++++++++++ Auxiliary_utils/LibSidServo/serial.h | 40 ++ Auxiliary_utils/LibSidServo/sidservo.h | 108 ++++ Auxiliary_utils/LibSidServo/sidservo.pc.in | 10 + Auxiliary_utils/LibSidServo/ssii.c | 146 ++++++ Auxiliary_utils/LibSidServo/ssii.h | 169 +++++++ 30 files changed, 2932 insertions(+) create mode 100644 Auxiliary_utils/LibSidServo/CMakeLists.txt create mode 100644 Auxiliary_utils/LibSidServo/dbg.h create mode 100644 Auxiliary_utils/LibSidServo/examples/CMakeLists.txt create mode 100644 Auxiliary_utils/LibSidServo/examples/dump.c create mode 100644 Auxiliary_utils/LibSidServo/examples/dump.h create mode 100644 Auxiliary_utils/LibSidServo/examples/dumpmoving.c create mode 100644 Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c create mode 100644 Auxiliary_utils/LibSidServo/examples/dumpswing.c create mode 100644 Auxiliary_utils/LibSidServo/examples/goto.c create mode 100644 Auxiliary_utils/LibSidServo/examples/scmd_traectory.c create mode 100644 Auxiliary_utils/LibSidServo/examples/simpleconv.h create mode 100644 Auxiliary_utils/LibSidServo/examples/traectories.c create mode 100644 Auxiliary_utils/LibSidServo/examples/traectories.h create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.cflags create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.config create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.creator create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.creator.user create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.creator.user.7bd84e3 create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.creator.user.cf63021 create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.cxxflags create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.files create mode 100644 Auxiliary_utils/LibSidServo/libsidservo.includes create mode 100644 Auxiliary_utils/LibSidServo/main.c create mode 100644 Auxiliary_utils/LibSidServo/mainconf.conf create mode 100644 Auxiliary_utils/LibSidServo/serial.c create mode 100644 Auxiliary_utils/LibSidServo/serial.h create mode 100644 Auxiliary_utils/LibSidServo/sidservo.h create mode 100644 Auxiliary_utils/LibSidServo/sidservo.pc.in create mode 100644 Auxiliary_utils/LibSidServo/ssii.c create mode 100644 Auxiliary_utils/LibSidServo/ssii.h diff --git a/Auxiliary_utils/LibSidServo/CMakeLists.txt b/Auxiliary_utils/LibSidServo/CMakeLists.txt new file mode 100644 index 0000000..bdf2c99 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.30) +set(PROJ sidservo) +set(MINOR_VERSION "1") +set(MID_VERSION "0") +set(MAJOR_VERSION "0") +set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}") + +project(${PROJ} VERSION ${VERSION} LANGUAGES C) + +# default flags +set(CMAKE_C_FLAGS "${CFLAGS} -O2") +set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS}") +set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS} -Wextra -Wall -Werror -W") +set(CMAKE_COLOR_MAKEFILE ON) + +option(DEBUG "Compile in debug mode" OFF) +option(EXAMPLES "Compile also some examples" ON) + +# cmake -DDEBUG=on -> debugging +if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + if(DEBUG) + set(CMAKE_BUILD_TYPE "Debug") + else() + set(CMAKE_BUILD_TYPE "Release") + endif() +endif() + +if(CMAKE_BUILD_TYPE STREQUAL "Debug") + add_definitions(-DEBUG) + set(CMAKE_VERBOSE_MAKEFILE true) + if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) + message("install to ${CMAKE_CURRENT_SOURCE_DIR}/install ") + set(CMAKE_INSTALL_PREFIX ${CMAKE_CURRENT_SOURCE_DIR}/install) + endif() + set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS_DEBUG}) +else() + set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS_RELEASE}) +endif() + +message("Build type: ${CMAKE_BUILD_TYPE}, cflags: ${CMAKE_C_FLAGS}") + +aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES) + +###### pkgconfig ###### +# pkg-config modules (for pkg-check-modules) +#set(MODULES cfitsio fftw3) +# find packages: +#find_package(PkgConfig REQUIRED) +#pkg_check_modules(${PROJ} REQUIRED ${MODULES}) + +###### additional flags ###### +#list(APPEND ${PROJ}_LIBRARIES "-lfftw3_threads") + +# library +add_library(${PROJ} SHARED ${SOURCES}) +# library header files +set(LIBHEADER "sidservo.h") +# -I +include_directories(${${PROJ}_INCLUDE_DIRS}) +# -L +link_directories(${${PROJ}_LIBRARY_DIRS}) +# -D +add_definitions( + -DPACKAGE_VERSION=\"${VERSION}\" -DMINOR_VERSION=\"${MINOR_VERSION}\" + -DMID_VERSION=\"${MID_VERSION}\" -DMAJOR_VERSION=\"${MAJOR_VESION}\" +) + +# -l +target_link_libraries(${PROJ} ${${PROJ}_LIBRARIES}) + +set(PCFILE "${CMAKE_BINARY_DIR}/${PROJ}.pc") +configure_file("${PROJ}.pc.in" ${PCFILE} @ONLY) + +set_target_properties(${PROJ} PROPERTIES VERSION ${VERSION}) +set_target_properties(${PROJ} PROPERTIES PUBLIC_HEADER ${LIBHEADER}) + +# Installation of the program +include(GNUInstallDirs) +#install(TARGETS ${PROJ} DESTINATION "bin") +install(TARGETS ${PROJ} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(FILES ${PCFILE} DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig) + +# EXAMPLES +if(EXAMPLES) + add_subdirectory(examples) +endif() diff --git a/Auxiliary_utils/LibSidServo/dbg.h b/Auxiliary_utils/LibSidServo/dbg.h new file mode 100644 index 0000000..5cacdb1 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/dbg.h @@ -0,0 +1,64 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "sidservo.h" + +extern conf_t Conf; + +// unused arguments of functions +#define _U_ __attribute__((__unused__)) +// break absent in `case` +#define FALLTHRU __attribute__ ((fallthrough)) +// and synonym for FALLTHRU +#define NOBREAKHERE __attribute__ ((fallthrough)) +// weak functions +#define WEAK __attribute__ ((weak)) + +#ifndef DBL_EPSILON +#define DBL_EPSILON (2.2204460492503131e-16) +#endif + +#ifndef FALSE +#define FALSE (0) +#endif + +#ifndef TRUE +#define TRUE (1) +#endif + + +#ifdef EBUG +#include + #define COLOR_RED "\033[1;31;40m" + #define COLOR_GREEN "\033[1;32;40m" + #define COLOR_OLD "\033[0;0;0m" + #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \ + fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0) + #define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \ + fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + fprintf(stderr, "\n");} while(0) + +#else // EBUG + #define FNAME() do{}while(0) + #define DBG(...) do{}while(0) +#endif // EBUG diff --git a/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt new file mode 100644 index 0000000..54128f0 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt @@ -0,0 +1,12 @@ +project(examples) + +# common includes & library +include_directories(../) +link_libraries(sidservo usefull_macros -lm) + +# exe list +add_executable(goto goto.c dump.c) +add_executable(dump dumpmoving.c dump.c) +add_executable(dump_s dumpmoving_scmd.c dump.c) +add_executable(dumpswing dumpswing.c dump.c) +add_executable(traectory_s scmd_traectory.c dump.c traectories.c) diff --git a/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c new file mode 100644 index 0000000..4b57543 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/dump.c @@ -0,0 +1,144 @@ +/* + * 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 . + */ + +// logging of mount position + +#include + +#include "dump.h" +#include "simpleconv.h" + +/** + * @brief logmnt - log mount data into file + * @param fcoords - file to dump + * @param m - mount data + */ +void logmnt(FILE *fcoords, mountdata_t *m){ + if(!fcoords) return; + //DBG("LOG %s", m ? "data" : "header"); + static double t0 = -1.; + if(!m){ // write header + fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n"); + return; + } + if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6; + double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0; + // write data + fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n", + t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y), + RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y), + m->millis, m->temperature, m->voltage); + fflush(fcoords); +} + +/** + * @brief dumpmoving - dump conf while moving + * @param fcoords - dump file + * @param t - max waiting time + * @param N - number of cycles to wait while motors aren't moving + */ +void dumpmoving(FILE *fcoords, double t, int N){ + if(!fcoords) return; + mountdata_t mdata; + DBG("Start dump"); + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10){ + WARNX("Can't get mount data"); + LOGWARN("Can't get mount data"); + } + uint32_t millis = mdata.millis; + int ctr = -1; + double xlast = mdata.motposition.X, ylast = mdata.motposition.Y; + double t0 = sl_dtime(); + //DBG("millis = %u", millis); + while(sl_dtime() - t0 < t && ctr < N){ + usleep(10000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + //DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y); + millis = mdata.millis; + if(fcoords) logmnt(fcoords, &mdata); + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else ++ctr; + } +} + +/** + * @brief waitmoving - wait until moving by both axes stops at least for N cycles + * @param N - amount of stopped cycles + */ +void waitmoving(int N){ + mountdata_t mdata; + int ctr = -1; + uint32_t millis = 0; + double xlast = 0., ylast = 0.; + while(ctr < N){ + usleep(10000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else ++ctr; + } +} + +/** + * @brief getMotPos - get current + * @param mot (o) - motor position (or NULL) + * @param Y (o) - encoder position (or NULL) + * @return FALSE if failed + */ +int getPos(coords_t *mot, coords_t *enc){ + mountdata_t mdata; + int errcnt = 0; + do{ + if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; + else{ + errcnt = 0; + if(mdata.millis) break; + } + }while(errcnt < 10); + if(errcnt >= 10){ + WARNX("Can't read mount status"); + return FALSE; + } + if(mot) *mot = mdata.motposition; + if(enc) *enc = mdata.encposition; + return TRUE; +} + +// check current position and go to 0 if non-zero +void chk0(int ncycles){ + coords_t M; + if(!getPos(&M, NULL)) signals(2); + if(M.X || M.Y){ + WARNX("Mount position isn't @ zero; moving"); + Mount.moveTo(0., 0.); + waitmoving(ncycles); + green("Now mount @ zero\n"); + } +} diff --git a/Auxiliary_utils/LibSidServo/examples/dump.h b/Auxiliary_utils/LibSidServo/examples/dump.h new file mode 100644 index 0000000..414473c --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/dump.h @@ -0,0 +1,29 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "sidservo.h" + +void logmnt(FILE *fcoords, mountdata_t *m); +void dumpmoving(FILE *fcoords, double t, int N); +void waitmoving(int N); +int getPos(coords_t *mot, coords_t *enc); +void chk0(int ncycles); diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving.c new file mode 100644 index 0000000..e08b80e --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving.c @@ -0,0 +1,105 @@ +/* + * 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 . + */ + +// dump telescope moving using simplest goto command + +#include +#include +#include +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int verbose; + int Ncycles; + char *logfile; + char *coordsoutput; +} parameters; + +static parameters G = { + .Ncycles = 40, +}; +static FILE *fcoords = NULL; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"}, + {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + LOGERR("Exit with status %d", sig); + Mount.quit(); + exit(sig); +} + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .SepEncoder = 0 +}; + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR; + if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1; + if(G.logfile) OPENLOG(G.logfile, lvl, 1); + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + logmnt(fcoords, NULL); + time_t curtime = time(NULL); + LOGMSG("Started @ %s", ctime(&curtime)); + LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed); + LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed); + mcc_errcodes_t e = Mount.init(&Config); + if(e != MCC_E_OK){ + WARNX("Can't init devices"); + return 1; + } + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.))) + ERRX("Can't move to 45, 45"); + dumpmoving(fcoords, 30., G.Ncycles); + Mount.moveTo(0., 0.); + dumpmoving(fcoords, 30., G.Ncycles); + signals(0); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c new file mode 100644 index 0000000..2adedd3 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c @@ -0,0 +1,174 @@ +/* + * 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 . + */ + +// dump telescope moving using short binary commands + +#include +#include +#include +#include +#include +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int Ncycles; + double reqint; + char *coordsoutput; + char *axis; +} parameters; + +static parameters G = { + .Ncycles = 40, + .reqint = 0.1, + .axis = "X", +}; +static FILE *fcoords = NULL; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"}, + {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .SepEncoder = 0 +}; + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// return TRUE if motor position is reached +- 0.1 degrees +#define XYcount (DEG2RAD(0.1)) +static int Wait(double tag){ + mountdata_t mdata; + red("Wait for %g degrees\n", RAD2DEG(tag)); + int errcnt = 0; + double sign = 0.; + uint32_t millis = 0; + double curpos = 0.; + do{ + if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; + else{ + errcnt = 0; + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(*G.axis == 'X') curpos = mdata.motposition.X; + else curpos = mdata.motposition.Y; + if(sign == 0.) sign = (curpos > tag) ? 1. : -1.; + //printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos), + // RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.); + } + }while(sign*(curpos - tag) > XYcount && errcnt < 10); + if(errcnt >= 10){ + WARNX("Too much errors"); + return FALSE; + } + green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag)); + return TRUE; +} + +// move X to 40 degr with given speed until given coord +static void move(double target, double limit, double speed){ +#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0) + green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed); + short_command_t cmd = {0}; + if(*G.axis == 'X'){ + cmd.Xmot = DEG2RAD(target); + cmd.Xspeed = DEG2RAD(speed); + }else{ + cmd.Ymot = DEG2RAD(target); + cmd.Yspeed = DEG2RAD(speed); + } + SCMD(); + if(!Wait(DEG2RAD(limit))) signals(9); +#undef SCMD +} + + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){ + WARNX("\"Axis\" should be X or Y"); + return 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + Config.MountReqInterval = G.reqint; + mcc_errcodes_t e = Mount.init(&Config); + if(e != MCC_E_OK){ + WARNX("Can't init devices"); + return 1; + } + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + // move to X=40 degr with different speeds + pthread_t dthr; + chk0(G.Ncycles); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + // goto 1 degr with 1'/s + move(10., 1., 1./60.); + // goto 2 degr with 2'/s + move(10., 2., 2./60.); + // goto 3 degr with 5'/s + move(10., 3., 5./60.); + // goto 4 degr with 10'/s + move(10., 4., 10./60.); + // and go back with 5deg/s + move(0., 0., 5.); + // be sure to move @ 0,0 + Mount.moveTo(0., 0.); + // wait moving ends + pthread_join(dthr, NULL); +#undef SCMD + signals(0); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/dumpswing.c b/Auxiliary_utils/LibSidServo/examples/dumpswing.c new file mode 100644 index 0000000..b2bbba0 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/dumpswing.c @@ -0,0 +1,172 @@ +/* + * 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 +#include +#include +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min) + +typedef struct{ + int help; + int Ncycles; + int Nswings; + double period; + double amplitude; + char *coordsoutput; + char *axis; +} parameters; + +static parameters G = { + .Ncycles = 20, + .axis = "X", + .Nswings = 10, + .period = 1., + .amplitude = 5., +}; +static FILE *fcoords = NULL; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 20)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"}, + {"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"}, + {"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"}, + {"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.05, + .SepEncoder = 0 +}; + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// wait until mount is stopped within 5 cycles or until time reached t +void waithalf(double t){ + mountdata_t mdata; + int ctr = -1; + uint32_t millis = 0; + double xlast = 0., ylast = 0.; + while(ctr < 5){ + if(sl_dtime() >= t) return; + usleep(1000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else{ + DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); + ++ctr; + } + } +} + + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){ + WARNX("\"Axis\" should be X or Y"); + return 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + if(G.Ncycles < 7) ERRX("Ncycles should be >7"); + if(G.amplitude < 0.01 || G.amplitude > 45.) + ERRX("Amplitude should be from 0.01 to 45 degrees"); + if(G.period < 0.1 || G.period > 900.) + ERRX("Period should be from 0.1 to 900s"); + if(G.Nswings < 1) ERRX("Nswings should be more than 0"); + mcc_errcodes_t e = Mount.init(&Config); + if(e != MCC_E_OK){ + WARNX("Can't init devices"); + return 1; + } + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + pthread_t dthr; + chk0(G.Ncycles); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + G.period /= 2.; // pause between commands + double tagX, tagY; + if(*G.axis == 'X'){ + tagX = DEG2RAD(G.amplitude); tagY = 0.; + }else{ + tagX = 0.; tagY = DEG2RAD(G.amplitude); + } + double t = sl_dtime(), t0 = t; + double divide = 2.; + for(int i = 0; i < G.Nswings; ++i){ + Mount.moveTo(tagX, tagY); + DBG("CMD: %g", sl_dtime()-t0); + t += G.period / divide; + divide = 1.; + waithalf(t); + DBG("Moved to +, t=%g", t-t0); + DBG("CMD: %g", sl_dtime()-t0); + Mount.moveTo(-tagX, -tagY); + t += G.period; + waithalf(t); + DBG("Moved to -, t=%g", t-t0); + DBG("CMD: %g", sl_dtime()-t0); + } + // be sure to move @ 0,0 + Mount.moveTo(0., 0.); + // wait moving ends + pthread_join(dthr, NULL); +#undef SCMD + signals(0); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c new file mode 100644 index 0000000..18ebab0 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/goto.c @@ -0,0 +1,117 @@ +/* + * 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 . + */ + +// move telescope to given MOTOR position in degrees + +#include +#include +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int Ncycles; + int wait; + char *coordsoutput; + double X; + double Y; +} parameters; + +static parameters G = { + .Ncycles = 40, + .X = NAN, + .Y = NAN, +}; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles of waiting in stopped state (default: 40)"}, + {"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"}, + {"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"}, + {"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"}, + {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"}, + end_option +}; + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .SepEncoder = 0 +}; + +static FILE* fcoords = NULL; +static pthread_t dthr; + +void signals(int sig){ + pthread_cancel(dthr); + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + if(fcoords) fclose(fcoords); + exit(sig); +} + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +int main(int _U_ argc, char _U_ **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount"); + coords_t M; + if(!getPos(&M, NULL)) ERRX("Can't get current position"); + if(G.coordsoutput){ + if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); + G.wait = 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + } + printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + if(isnan(G.X) && isnan(G.Y)) goto out; + if(isnan(G.X)) G.X = RAD2DEG(M.X); + if(isnan(G.Y)) G.Y = RAD2DEG(M.Y); + printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y); + Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y)); + if(G.wait){ + sleep(1); + waitmoving(G.Ncycles); + if(!getPos(&M, NULL)) WARNX("Can't get current position"); + else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + } +out: + if(G.coordsoutput) pthread_join(dthr, NULL); + if(G.wait) Mount.quit(); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c new file mode 100644 index 0000000..19ae664 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c @@ -0,0 +1,159 @@ +/* + * 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 +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" +#include "traectories.h" + +// calculate some traectory and try to run over it + +typedef struct{ + int help; + int Ncycles; // n cycles to wait stop + double reqint; // requests interval (seconds) + double Xmax; // maximal X to stop + double Ymax; // maximal Y to stop + double tmax; // maximal time of emulation + double X0; // starting point of traectory (-30..30 degr) + double Y0; // -//- + char *coordsoutput; // dump file + char *tfn; // traectory function name +} parameters; + +static FILE *fcoords = NULL; +static pthread_t dthr; +static parameters G = { + .Ncycles = 40, + .reqint = 0.1, + .tfn = "sincos", + .Xmax = 45., + .Ymax = 45., + .tmax = 300., // 5 minutes + .X0 = 10., + .Y0 = 10., +}; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"}, + {"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"}, + {"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal X coordinate for traectory (default: 45 degrees)"}, + {"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X coordinate for traectory (default: 45 degrees)"}, + {"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"}, + {"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"}, + {"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"}, + end_option +}; + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.05, + .SepEncoder = 0 +}; + +void signals(int sig){ + pthread_cancel(dthr); + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + if(fcoords) fclose(fcoords); + exit(sig); +} + +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// calculate +static void runtraectory(traectory_fn tfn){ + if(!tfn) return; + coords_t telXY, traectXY; + double t0 = sl_dtime(); + uint32_t susec_last = 0; + while(1){ + if(!telpos(&telXY)){ + WARNX("No next telescope position"); + return; + } + if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind + susec_last = telXY.msrtime.tv_usec; + double t = sl_dtime(); + if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break; + if(!traectory_point(&traectXY, t)) break; + DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y)); + } + WARNX("No next traectory point"); +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees"); + if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees"); + // convert to radians + G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax); + if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.) + ERRX("X0 and Y0 should be -30..30 degrees"); + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + Config.MountReqInterval = G.reqint; + traectory_fn tfn = traectory_by_name(G.tfn); + if(!tfn){ + WARNX("Bad traectory name %s, should be one of", G.tfn); + print_tr_names(); + return 1; + } + coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; + if(!init_traectory(tfn, &c)){ + ERRX("Can't init traectory"); + return 1; + } + mcc_errcodes_t e = Mount.init(&Config); + if(e != MCC_E_OK){ + WARNX("Can't init devices"); + return 1; + } + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + chk0(G.Ncycles); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + ; + runtraectory(tfn); + signals(0); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/simpleconv.h b/Auxiliary_utils/LibSidServo/examples/simpleconv.h new file mode 100644 index 0000000..8200def --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/simpleconv.h @@ -0,0 +1,29 @@ +/* + * 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 . + */ + +// simple conversion macros + +#include + +#define DEG2RAD(d) (d/180.*M_PI) +#define ASEC2RAD(d) (d/180.*M_PI/3600.) +#define AMIN2RAD(d) (d/180.*M_PI/60.) +#define RAD2DEG(r) (r/M_PI*180.) +#define RAD2ASEC(r) (r/M_PI*180.*3600.) +#define RAD2AMIN(r) (r/M_PI*180.*60.) + diff --git a/Auxiliary_utils/LibSidServo/examples/traectories.c b/Auxiliary_utils/LibSidServo/examples/traectories.c new file mode 100644 index 0000000..decd645 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/traectories.c @@ -0,0 +1,143 @@ +/* + * 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 . + */ + +// some simplest traectories +// all traectories runs increasing X and Y from starting point + +#include +#include +#include + +#include "simpleconv.h" +#include "traectories.h" + +static traectory_fn cur_traectory = NULL; +// starting point of traectory +static coords_t XYstart = {0}; +static double tstart = 0.; +// convert Xe/Ye to approximate motor coordinates: +// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have +// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0 +static coords_t XYcor = {0}; + +/** + * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders + * @param f - function calculating next point + * @param XY0 - starting point + * @return FALSE if failed + */ +int init_traectory(traectory_fn f, coords_t *XY0){ + if(!f || !XY0) return FALSE; + cur_traectory = f; + XYstart = *XY0; + tstart = sl_dtime(); + mountdata_t mdata; + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10) return FALSE; + XYcor.X = mdata.motposition.X - mdata.encposition.X; + XYcor.Y = mdata.motposition.X + mdata.encposition.Y; + DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y)); + return TRUE; +} + +/** + * @brief traectory_point - get traectory point for given time + * @param nextpt (o) - next point coordinates + * @param t - UNIX-time of event + * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180) + */ +int traectory_point(coords_t *nextpt, double t){ + if(t < 0. || !cur_traectory) return FALSE; + coords_t pt; + if(!cur_traectory(&pt, t)) return FALSE; + pt.msrtime.tv_sec = floor(t); + pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec; + if(nextpt) *nextpt = pt; + if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; + return TRUE; +} + +// current telescope position according to starting motor coordinates +// @return FALSE if failed to get current coordinates +int telpos(coords_t *curpos){ + mountdata_t mdata; + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10) return FALSE; + coords_t pt; + pt.X = XYcor.X + mdata.encposition.X; + pt.Y = XYcor.Y + mdata.encposition.Y; + pt.msrtime = mdata.encposition.msrtime; + if(curpos) *curpos = pt; + return TRUE; +} + +// X=X0+1'/s, Y=Y0+15''/s +int Linear(coords_t *nextpt, double t){ + coords_t pt; + pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart); + pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); + if(nextpt) *nextpt = pt; + return TRUE; +} + +// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi) +int SinCos(coords_t *nextpt, double t){ + coords_t pt; + pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI); + pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI); + if(nextpt) *nextpt = pt; + return TRUE; +} + +typedef struct{ + traectory_fn f; + const char *name; + const char *help; +} tr_names; + +static tr_names names[] = { + {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"}, + {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"}, + {NULL, NULL, NULL} +}; + +traectory_fn traectory_by_name(const char *name){ + traectory_fn f = NULL; + for(int i = 0; ; ++i){ + if(!names[i].f) break; + if(strcmp(names[i].name, name) == 0){ + f = names[i].f; + break; + } + } + return f; +} + +// print all acceptable traectories names with help +void print_tr_names(){ + for(int i = 0; ; ++i){ + if(!names[i].f) break; + printf("%s: %s\n", names[i].name, names[i].help); + } +} diff --git a/Auxiliary_utils/LibSidServo/examples/traectories.h b/Auxiliary_utils/LibSidServo/examples/traectories.h new file mode 100644 index 0000000..cfd9b2c --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/traectories.h @@ -0,0 +1,32 @@ +/* + * 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 . + */ + +#pragma once + +#include "sidservo.h" + +// traectory +typedef int (*traectory_fn)(coords_t *, double); + +int init_traectory(traectory_fn f, coords_t *XY0); +traectory_fn traectory_by_name(const char *name); +void print_tr_names(); +int traectory_point(coords_t *nextpt, double t); +int telpos(coords_t *curpos); +int Linear(coords_t *nextpt, double t); +int SinCos(coords_t *nextpt, double t); diff --git a/Auxiliary_utils/LibSidServo/libsidservo.cflags b/Auxiliary_utils/LibSidServo/libsidservo.cflags new file mode 100644 index 0000000..85d51b3 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.cflags @@ -0,0 +1 @@ +-std=c17 diff --git a/Auxiliary_utils/LibSidServo/libsidservo.config b/Auxiliary_utils/LibSidServo/libsidservo.config new file mode 100644 index 0000000..33039ab --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.config @@ -0,0 +1,6 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define EBUG +#define _POSIX_C_SOURCE +#define PACKAGE_VERSION "0.0.1" + diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator b/Auxiliary_utils/LibSidServo/libsidservo.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator @@ -0,0 +1 @@ +[General] diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user new file mode 100644 index 0000000..524f200 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/C-sources/erfa + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user.7bd84e3 b/Auxiliary_utils/LibSidServo/libsidservo.creator.user.7bd84e3 new file mode 100644 index 0000000..e594907 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user.7bd84e3 @@ -0,0 +1,165 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + false + true + false + + + + ProjectExplorer.Project.PluginSettings + + + true + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/C-sources/erfa + + + + all + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Сборка + + ProjectExplorer.BuildSteps.Build + + + + + clean + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Очистка + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + По умолчанию + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Развёртывание + + ProjectExplorer.BuildSteps.Deploy + + 1 + Конфигурация развёртывания + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + 2 + + + Особая программа + + ProjectExplorer.CustomExecutableRunConfiguration + + 3768 + false + true + false + false + true + + + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user.cf63021 b/Auxiliary_utils/LibSidServo/libsidservo.creator.user.cf63021 new file mode 100644 index 0000000..002b8fd --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user.cf63021 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/Auxiliary_utils/LibSidServo/libsidservo.cxxflags b/Auxiliary_utils/LibSidServo/libsidservo.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files new file mode 100644 index 0000000..4adc54f --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.files @@ -0,0 +1,19 @@ +CMakeLists.txt +dbg.h +examples/dump.c +examples/dump.h +examples/dumpmoving.c +examples/dumpmoving_scmd.c +examples/dumpswing.c +examples/goto.c +examples/scmd_traectory.c +examples/simpleconv.h +main.c +sidservo.h +serial.c +examples/CMakeLists.txt +examples/traectories.c +examples/traectories.h +serial.h +ssii.c +ssii.h diff --git a/Auxiliary_utils/LibSidServo/libsidservo.includes b/Auxiliary_utils/LibSidServo/libsidservo.includes new file mode 100644 index 0000000..d494155 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/libsidservo.includes @@ -0,0 +1,2 @@ +. +.. diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c new file mode 100644 index 0000000..e7b4a20 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/main.c @@ -0,0 +1,161 @@ +/* + * 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 +#include + +#include "dbg.h" +#include "serial.h" +#include "ssii.h" + +conf_t Conf = {0}; + +/** + * @brief quit - close all opened and return to default state + */ +static void quit(){ + DBG("Close serial devices"); + for(int i = 0; i < 10; ++i) if(SSemergStop()) break; + closeSerial(); + 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){ + FNAME(); + if(!c) return MCC_E_BADFORMAT; + Conf = *c; + mcc_errcodes_t ret = MCC_E_OK; + if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ + DBG("Define mount device path and speed"); + ret = MCC_E_BADFORMAT; + }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.EncoderDevPath || Conf.EncoderDevSpeed < 1200){ + DBG("Define encoder device path and speed"); + ret = MCC_E_BADFORMAT; + }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 < 0.001){ + DBG("Bad value of MountReqInterval"); + ret = MCC_E_BADFORMAT; + } + if(ret != MCC_E_OK) quit(); + return ret; +} + +/** + * @brief move2 - simple move to given point and stop + * @param X - new X coordinate (radians: -pi..pi) + * @param Y - new Y coordinate (radians: -pi..pi) + * @return error code + */ +static mcc_errcodes_t move2(double X, double Y){ + if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){ + DBG("Wrong coords: X=%g, Y=%g", X, Y); + return MCC_E_BADFORMAT; + } + if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED; + return MCC_E_OK; +} + +/** + * @brief emstop - emergency stop + * @return errcode + */ +static mcc_errcodes_t emstop(){ + if(!SSemergStop()) return MCC_E_FAILED; + return MCC_E_OK; +} + +/** + * @brief shortcmd - send and receive short binary command + * @param cmd (io) - command + * @return errcode + */ +static mcc_errcodes_t shortcmd(short_command_t *cmd){ + if(!cmd) return MCC_E_BADFORMAT; + SSscmd s = {0}; + DBG("xmot=%g, ymot=%g", cmd->Xmot, cmd->Ymot); + s.Xmot = X_RAD2MOT(cmd->Xmot); + s.Ymot = Y_RAD2MOT(cmd->Ymot); + s.Xspeed = X_RS2MOTSPD(cmd->Xspeed); + s.Yspeed = Y_RS2MOTSPD(cmd->Yspeed); + s.xychange = cmd->xychange; + s.XBits = cmd->XBits; + s.YBits = cmd->YBits; + DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed); + if(!cmdS(&s)) return MCC_E_FAILED; + cmd->Xmot = X_MOT2RAD(s.Xmot); + cmd->Ymot = Y_MOT2RAD(s.Ymot); + cmd->Xspeed = X_MOTSPD2RS(s.Xspeed); + cmd->Yspeed = Y_MOTSPD2RS(s.Yspeed); + cmd->xychange = s.xychange; + cmd->XBits = s.XBits; + cmd->YBits = s.YBits; + return MCC_E_OK; +} + +/** + * @brief shortcmd - send and receive long binary command + * @param cmd (io) - command + * @return errcode + */ +static mcc_errcodes_t longcmd(long_command_t *cmd){ + if(!cmd) return MCC_E_BADFORMAT; + SSlcmd l = {0}; + l.Xmot = X_RAD2MOT(cmd->Xmot); + l.Ymot = Y_RAD2MOT(cmd->Ymot); + l.Xspeed = X_RS2MOTSPD(cmd->Xspeed); + l.Yspeed = Y_RS2MOTSPD(cmd->Yspeed); + l.Xadder = X_RS2MOTSPD(cmd->Xadder); + l.Yadder = Y_RS2MOTSPD(cmd->Yadder); + l.Xatime = S2ADDER(cmd->Xatime); + l.Yatime = S2ADDER(cmd->Yatime); + if(!cmdL(&l)) return MCC_E_FAILED; + cmd->Xmot = X_MOT2RAD(l.Xmot); + cmd->Ymot = Y_MOT2RAD(l.Ymot); + cmd->Xspeed = X_MOTSPD2RS(l.Xspeed); + cmd->Yspeed = Y_MOTSPD2RS(l.Yspeed); + cmd->Xadder = X_MOTSPD2RS(l.Xadder); + cmd->Yadder = Y_MOTSPD2RS(l.Yadder); + cmd->Xatime = ADDER2S(l.Xatime); + cmd->Yatime = ADDER2S(l.Yatime); + return MCC_E_OK; +} + +// init mount class +mount_t Mount = { + .init = init, + .quit = quit, + .getMountData = getMD, + .moveTo = move2, + .emergStop = emstop, + .shortCmd = shortcmd, + .longCmd = longcmd, +}; diff --git a/Auxiliary_utils/LibSidServo/mainconf.conf b/Auxiliary_utils/LibSidServo/mainconf.conf new file mode 100644 index 0000000..dd4d0a0 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/mainconf.conf @@ -0,0 +1,7 @@ +# configuration file for SSII driven equatorial mount +verbose = 5 +mountpath = /dev/ttyS1 +mountspeed = 19200 +encoderpath = /dev/ttyUSB0 +encoderspeed = 153000 + diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c new file mode 100644 index 0000000..7fc46d1 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -0,0 +1,461 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dbg.h" +#include "serial.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; + +/** + * @brief dtime - UNIX time with microsecond + * @return value + */ +double dtime(){ + double t; + struct timeval tv; + gettimeofday(&tv, NULL); + t = tv.tv_sec + ((double)tv.tv_usec)/1e6; + return t; +} + +#if 0 +// 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.encposition.X = X_ENC2RAD(edata->encX); + mountdata.encposition.Y = Y_ENC2RAD(edata->encY); + mountdata.encposition.msrtime = *tv; + pthread_mutex_unlock(&datamutex); + DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.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; + /* ssize_t l = read(mntfd, &byte, 1); + //DBG("MNT read=%zd byte=0x%X", l, byte); + if(l == 0) return -1; + if(l != 1) return -2; // disconnected ?? + return (int) byte;*/ + do{ + FD_ZERO(&rfds); + FD_SET(mntfd, &rfds); + tv = mntRtmout; + int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); + if(retval < 0){ + if(errno == EINTR) continue; + DBG("Error in select()"); + return -1; + } + //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds)); + if(FD_ISSET(mntfd, &rfds)){ + ssize_t l = read(mntfd, &byte, 1); + //DBG("MNT read=%zd byte=0x%X", l, byte); + 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; +} + +data_t *cmd2dat(const char *cmd){ + if(!cmd) return NULL; + data_t *d = calloc(1, sizeof(data_t)); + if(!d) return NULL; + d->buf = (uint8_t*)strdup(cmd); + d->len = strlen(cmd); + d->maxlen = d->len + 1; + return d; +} +void data_free(data_t **x){ + if(!x || !*x) return; + free((*x)->buf); + free(*x); + *x = NULL; +} + +// main mount thread +static void *mountthread(void _U_ *u){ + int errctr = 0; + uint8_t buf[2*sizeof(SSstat)]; + SSstat *status = (SSstat*) buf; + // data to get + data_t d = {.buf = buf, .maxlen = sizeof(buf)}; + // cmd to send + data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); + if(!cmd_getstat) goto failed; + double t0 = dtime(); +/* +#ifdef EBUG + double t00 = t0; +#endif +*/ + while(mntfd > -1 && errctr < MAX_ERR_CTR){ + // read data to status + struct timeval tgot; + if(0 != gettimeofday(&tgot, NULL)) continue; + if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ + DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); + ++errctr; continue; + } + if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){ + DBG("BAD checksum of SSstat, need %d", status->checksum); + ++errctr; continue; + } + errctr = 0; + pthread_mutex_lock(&datamutex); + // now change data + SSconvstat(status, &mountdata, &tgot); + pthread_mutex_unlock(&datamutex); + // allow writing & getters + //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00); + if(dtime() - t0 >= Conf.MountReqInterval) usleep(50); + while(dtime() - t0 < Conf.MountReqInterval); + t0 = dtime(); + } + data_free(&cmd_getstat); +failed: + 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 + if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");} + 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 = 200000000 / speed; // 20 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); + DBG("Open mount %s @ %d", path, speed); + mntfd = ttyopen(path, (speed_t) speed); + if(mntfd < 0) return FALSE; + DBG("mntfd=%d", mntfd); + // clear buffer + while(getmntbyte() > -1); + /*int g = write(mntfd, "XXS\r", 4); + DBG("Written %d", g); + uint8_t buf[100]; + do{ + ssize_t l = read(mntfd, buf, 100); + DBG("got %zd", l); + }while(1);*/ + mntRtmout.tv_sec = 0; + mntRtmout.tv_usec = 500000000 / speed; // 50 bytes + if(pthread_create(&mntthread, NULL, mountthread, NULL)){ + DBG("Can't create thread"); + 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("Kill mount thread"); + pthread_cancel(mntthread); + DBG("close fd"); + close(mntfd); + mntfd = -1; + } + if(encfd > -1){ + DBG("Kill encoder thread"); + pthread_cancel(encthread); + DBG("close fd"); + 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; +} + +// write-read without locking mutex (to be used inside other functions) +static int wr(const data_t *out, data_t *in, int needeol){ + if((!out && !in) || mntfd < 0) return FALSE; + if(out){ + if(out->len != (size_t)write(mntfd, out->buf, out->len)){ + DBG("written bytes not equal to need"); + return FALSE; + } + //DBG("Send to mount %zd bytes: %s", out->len, out->buf); + if(needeol){ + int g = write(mntfd, "\r", 1); // add EOL + (void) g; + } + } + 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; + } + } + return TRUE; +} + +/** + * @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){ + pthread_mutex_lock(&mntmutex); + int ret = wr(out, in, 1); + pthread_mutex_unlock(&mntmutex); + return ret; +} + +#ifdef EBUG +static void logscmd(SSscmd *c){ + printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed); + printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits); + if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSscmd)-2)) printf("Checksum failed\n"); + else printf("Checksum OK\n"); +} +static void loglcmd(SSlcmd *c){ + printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed); + printf("Xadder=%d, Yadder=%d, Xatime=%d, Yatime=%d\n", c->Xadder, c->Yadder, c->Xatime, c->Yatime); + if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSlcmd)-2)) printf("Checksum failed\n"); + else printf("Checksum OK\n"); +} +#endif + +// send short/long binary command; return FALSE if failed +static int bincmd(uint8_t *cmd, int len){ + static data_t *dscmd = NULL, *dlcmd = NULL; + if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD); + if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD); + int ret = FALSE; + pthread_mutex_lock(&mntmutex); + // dummy buffer to clear trash in input + char ans[300]; + data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; + if(len == sizeof(SSscmd)){ + ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); + DBG("Short command"); + logscmd((SSscmd*)cmd); + if(!wr(dscmd, &a, 1)) goto rtn; + }else if(len == sizeof(SSlcmd)){ + ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); + DBG("Long command"); + loglcmd((SSlcmd*)cmd); + if(!wr(dlcmd, &a, 1)) goto rtn; + }else{ + goto rtn; + } + DBG("Write %d bytes and wait for ans", len); + data_t d; + d.buf = cmd; + d.len = d.maxlen = len; + ret = wr(&d, &d, 0); +#ifdef EBUG + if(len == sizeof(SSscmd)) logscmd((SSscmd*)cmd); + else loglcmd((SSlcmd*)cmd); +#endif + DBG("%s", ret ? "SUCCESS" : "FAIL"); +rtn: + pthread_mutex_unlock(&mntmutex); + return ret; +} + +// return TRUE if OK +int cmdS(SSscmd *cmd){ + return bincmd((uint8_t *)cmd, sizeof(SSscmd)); +} +int cmdL(SSlcmd *cmd){ + return bincmd((uint8_t *)cmd, sizeof(SSlcmd)); +} diff --git a/Auxiliary_utils/LibSidServo/serial.h b/Auxiliary_utils/LibSidServo/serial.h new file mode 100644 index 0000000..d92542b --- /dev/null +++ b/Auxiliary_utils/LibSidServo/serial.h @@ -0,0 +1,40 @@ +/* + * 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 . + */ + +#pragma once + +#include "sidservo.h" +#include "ssii.h" + +// magick starting sequence +#define ENC_MAGICK (204) +// encoder data sequence length +#define ENC_DATALEN (13) +// max error counter (when read() returns -1) +#define MAX_ERR_CTR (100) + +double dtime(); +data_t *cmd2dat(const char *cmd); +void data_free(data_t **x); +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); +int cmdS(SSscmd *cmd); +int cmdL(SSlcmd *cmd); diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h new file mode 100644 index 0000000..79ea016 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -0,0 +1,108 @@ +/* + * 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 . + */ + +#pragma once + +#include +#include +#include + +// error codes +typedef enum{ + MCC_E_OK = 0, // all OK + MCC_E_FATAL, // some fatal error + MCC_E_BADFORMAT, // wrong arguments of function + MCC_E_ENCODERDEV, // encoder device error or can't open + MCC_E_MOUNTDEV, // mount device error or can't open + MCC_E_FAILED, // failed to run command - protocol error +} mcc_errcodes_t; + +typedef struct{ + 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 + double MountReqInterval; // maximal interval between subsequent mount requests (seconds) + ; +} conf_t; + +// coordinates in degrees: X, Y and time when they were reached +typedef struct{ + double X; double Y; struct timeval msrtime; +} coords_t; + +// data to read/write +typedef struct{ + 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; + uint8_t keypad; + extradata_t extradata; + uint32_t millis; + double temperature; + double voltage; +} mountdata_t; + +typedef struct{ + double Xmot; // 0 X motor position (rad) + double Xspeed; // 4 X speed (rad/s) + double Ymot; // 8 + double Yspeed; // 12 + uint8_t xychange; // 16 change Xbits/Ybits value + uint8_t XBits; // 17 + uint8_t YBits; // 18 +} short_command_t; // short command + +typedef struct{ + double Xmot; // 0 X motor position (rad) + double Xspeed; // 4 X speed (rad/s) + double Ymot; // 8 + double Yspeed; // 12 + double Xadder; // 16 - X adder (rad/s) + double Yadder; // 20 + double Xatime; // 24 X adder time, sec + double Yatime; // 28 +} long_command_t; // long command + +// mount class +typedef struct{ + mcc_errcodes_t (*init)(conf_t *c); // init device + void (*quit)(); // deinit + mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data + mcc_errcodes_t (*moveTo)(double X, double Y); // move to given position ans stop + mcc_errcodes_t (*emergStop)(); // emergency stop + mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command + mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command +} mount_t; + +extern mount_t Mount; diff --git a/Auxiliary_utils/LibSidServo/sidservo.pc.in b/Auxiliary_utils/LibSidServo/sidservo.pc.in new file mode 100644 index 0000000..431e00d --- /dev/null +++ b/Auxiliary_utils/LibSidServo/sidservo.pc.in @@ -0,0 +1,10 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${prefix}/include + +Name: @PROJ@ +Description: library for managing SiderealServo II based equatorial mount +Version: @VERSION@ +Libs: -L${libdir} -l@PROJ@ +Cflags: -I${includedir} diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c new file mode 100644 index 0000000..9a008b3 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -0,0 +1,146 @@ +/* + * 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 +#include +#include + +#include "dbg.h" +#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++){ + //DBG("data[%d]=0x%X", i, *buf); + checksum += *buf++; + } + checksum ^= 0xFF00; // invert high byte + //DBG("Checksum of %d bytes: 0x%04x", len, checksum); + return checksum; +} + +/** + * @brief SSconvstat - convert stat from SSII format to human + * @param status (i) - just read data + * @param mountdata (o) - output + */ +void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){ + if(!s || !m || !tdat) return; +/* +#ifdef EBUG + static double t0 = -1.; + if(t0 < 0.) t0 = dtime(); +#endif + DBG("Convert, t=%g", dtime()-t0); +*/ + m->motposition.X = X_MOT2RAD(s->Xmot); + m->motposition.Y = Y_MOT2RAD(s->Ymot); + m->motposition.msrtime = *tdat; + // fill encoder data from here, as there's no separate enc thread + if(!Conf.SepEncoder){ + m->encposition.X = X_ENC2RAD(s->Xenc); + m->encposition.Y = Y_ENC2RAD(s->Yenc); + m->encposition.msrtime = *tdat; + } + m->lastmotposition.X = X_MOT2RAD(s->XLast); + m->lastmotposition.Y = Y_MOT2RAD(s->YLast); + m->lastmotposition.msrtime = *tdat; + m->keypad = s->keypad; + m->extradata.ExtraBits = s->ExtraBits; + m->extradata.ain0 = s->ain0; + m->extradata.ain1 = s->ain1; + m->extradata.XBits = s->XBits; + m->extradata.YBits = s->YBits; + m->millis = s->millis; + m->voltage = (double)s->voltage / 10.; + m->temperature = ((double)s->tF - 32.) * 5. / 9.; +} + +/** + * @brief SStextcmd - send simple text command to mount and return answer + * @param cmd (i) - command to send + * @param answer (o) - answer (or NULL) + * @return + */ +int SStextcmd(const char *cmd, data_t *answer){ + if(!cmd){ + DBG("try to send empty command"); + return FALSE; + } + data_t d; + d.buf = (uint8_t*) cmd; + d.len = d.maxlen = strlen(cmd); + DBG("send %zd bytes: %s", d.len, d.buf); + return MountWriteRead(&d, answer); +} + +/** + * @brief SSgetint - send text command and return integer answer + * @param cmd (i) - command to send + * @param ans (o) - intval (INT64_MAX if error) + * @return FALSE if failed + */ +int SSgetint(const char *cmd, int64_t *ans){ + if(!cmd || !ans) return FALSE; + uint8_t buf[64]; + data_t d = {.buf = buf, .len = 0, .maxlen = 64}; + if(!SStextcmd(cmd, &d)) return FALSE; + int64_t retval = INT64_MAX; + if(d.len > 1){ + char *ptr = (char*) buf; + size_t i = 0; + for(; i < d.len; ++i){ + if(isdigit(*ptr)) break; + ++ptr; + } + if(i < d.len) retval = atol(ptr); + } + DBG("read int: %" PRIi64, retval); + *ans = retval; + return TRUE; +} + +// commands to move X and Y to given motor position in radians; @return FALSE if failed +// BE CAREFUL: after each poweron X and Y are 0 +// BE CAREFUL: angle isn't checking here +int SSXmoveto(double pos){ + char buf[64]; + int64_t target = X_RAD2MOT(pos); + DBG("move to angle %grad = %ld", pos, target); + snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target); + return SStextcmd(buf, NULL); +} +int SSYmoveto(double pos){ + char buf[64]; + int64_t target = Y_RAD2MOT(pos); + DBG("move to angle %grad = %ld", pos, target); + snprintf(buf, 63, "%s%" PRIi64, CMD_MOTY, target); + return SStextcmd(buf, NULL); +} + +int SSemergStop(){ + int i = 0; + for(; i < 10; ++i){ + if(!SStextcmd(CMD_EMSTOPX, NULL)) continue; + if(SStextcmd(CMD_EMSTOPY, NULL)) break; + } + if(i == 10) return FALSE; + return TRUE; +} + diff --git a/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h new file mode 100644 index 0000000..9250dd9 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -0,0 +1,169 @@ +/* + * 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 +#include + +#include "sidservo.h" + +#if 0 +// 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") +#endif + +// get binary data of all statistics +#define CMD_GETSTAT ("XXS") +// send short command +#define CMD_SHORTCMD ("XXR") +// send long command +#define CMD_LONGCMD ("YXR") +// get/set X/Y in motsteps +#define CMD_MOTX ("X") +#define CMD_MOTY ("Y") +// -//- in encoders' ticks +#define CMD_ENCX ("XZ") +#define CMD_ENCY ("YZ") +// normal stop X/Y +#define CMD_STOPX ("XN") +#define CMD_STOPY ("YN") +// emergency stop +#define CMD_EMSTOPX ("XG") +#define CMD_EMSTOPY ("YG") +// getters of motor's encoders per rev +#define CMD_GETXMEPR ("XXU") +#define CMD_GETYMEPR ("XXV") +// -//- axis encoders +#define CMD_GETXAEPR ("XXT") +#define CMD_GETYAEPR ("XXZ") +// exit ASCII checksum mode +#define CMD_EXITACM ("YXY0\r\xb8") + +// steps per revolution +//#define X_MOT_STEPSPERREV (3325440.) +#define X_MOT_STEPSPERREV (3325952.) +//#define Y_MOT_STEPSPERREV (4394496.) +#define Y_MOT_STEPSPERREV (4394960.) + +// motor position to radians and back +#define X_MOT2RAD(n) (2.*M_PI * (double)n / X_MOT_STEPSPERREV) +#define Y_MOT2RAD(n) (2.*M_PI * (double)n / Y_MOT_STEPSPERREV) +#define X_RAD2MOT(r) ((int32_t)(r / 2./M_PI * X_MOT_STEPSPERREV)) +#define Y_RAD2MOT(r) ((int32_t)(r / 2./M_PI * Y_MOT_STEPSPERREV)) +// motor speed in rad/s and back +#define X_MOTSPD2RS(n) (X_MOT2RAD(n)/65536.*1953.) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r)*65536./1953.)) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.)) +// adder time to seconds vice versa +#define ADDER2S(a) (a*1953.) +#define S2ADDER(s) (s/1953.) + +// encoder per revolution +#define X_ENC_STEPSPERREV (67108864.) +#define Y_ENC_STEPSPERREV (67108864.) +// encoder position to radians and back +#define X_ENC2RAD(n) (2.*M_PI * (double)n / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) (2.*M_PI * (double)n / Y_ENC_STEPSPERREV) +#define X_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * X_ENC_STEPSPERREV)) +#define Y_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * Y_ENC_STEPSPERREV)) + +// encoder's tolerance (ticks) +#define YencTOL (25.) +#define XencTOL (25.) + + +// all need data in one +typedef struct{ // 41 bytes + uint8_t ctrlAddr; // 0 a8 + controller address + int32_t Xmot; // 1 Dec/HA motor position + int32_t Ymot; // 5 + int32_t Xenc; // 9 Dec/HA encoder position + int32_t Yenc; // 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 XLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change + uint32_t YLast; // 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 Xmot; // 0 X motor position + int32_t Xspeed; // 4 X speed + int32_t Ymot; // 8 + int32_t Yspeed; // 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 Xmot; // 0 X motor position + int32_t Xspeed; // 4 X speed + int32_t Ymot; // 8 + int32_t Yspeed; // 12 + int32_t Xadder; // 16 - X adder + int32_t Yadder; // 20 + int32_t Xatime; // 24 X adder time (1953 == 1s) + int32_t Yatime; // 28 + uint16_t checksum; // 32 +} __attribute__((packed)) SSlcmd; // long command + +uint16_t SScalcChecksum(uint8_t *buf, int len); +void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat); +int SStextcmd(const char *cmd, data_t *answer); +int SSgetint(const char *cmd, int64_t *ans); +int SSXmoveto(double pos); +int SSYmoveto(double pos); +int SSemergStop(); +int SSshortCmd(SSscmd *cmd);