From a4d4e15f18b5e66ff1300bf569700d2265d60327 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Fri, 20 Feb 2026 08:51:09 +0300 Subject: [PATCH] ... --- .gitignore | 83 ++ .gitmodules | 3 - CMakeLists.txt | 159 ++++ LibSidServo/CMakeLists.txt | 87 ++ LibSidServo/PID.c | 221 +++++ LibSidServo/PID.h | 40 + LibSidServo/PID_test.deprecated/Dramp.c | 141 +++ LibSidServo/PID_test.deprecated/Dramp.h | 23 + LibSidServo/PID_test.deprecated/Sramp.c | 27 + LibSidServo/PID_test.deprecated/Sramp.h | 23 + LibSidServo/PID_test.deprecated/Tramp.c | 223 +++++ LibSidServo/PID_test.deprecated/Tramp.h | 23 + LibSidServo/PID_test.deprecated/main.c | 243 +++++ LibSidServo/PID_test.deprecated/moving.c | 105 +++ LibSidServo/PID_test.deprecated/moving.h | 70 ++ .../PID_test.deprecated/moving_model.cflags | 1 + .../PID_test.deprecated/moving_model.config | 4 + .../PID_test.deprecated/moving_model.creator | 1 + .../moving_model.creator.user | 221 +++++ .../moving_model.creator.user.7bd84e3 | 184 ++++ .../PID_test.deprecated/moving_model.cxxflags | 1 + .../PID_test.deprecated/moving_model.files | 10 + .../PID_test.deprecated/moving_model.includes | 0 .../PID_test.deprecated/moving_private.h | 26 + LibSidServo/PID_test.deprecated/plot | 4 + LibSidServo/PID_test.deprecated/plot.cont | 8 + LibSidServo/PID_test.deprecated/plot_jpg | 6 + LibSidServo/PID_test.deprecated/plot_pdf | 5 + LibSidServo/PID_test.deprecated/plotacc | 4 + LibSidServo/PID_test.deprecated/ploterr | 4 + LibSidServo/PID_test.deprecated/ploterr.cont | 6 + LibSidServo/PID_test.deprecated/ploterr_jpg | 5 + LibSidServo/PID_test/PID.c | 64 ++ LibSidServo/PID_test/PID.h | 39 + LibSidServo/PID_test/Tramp.c | 223 +++++ LibSidServo/PID_test/Tramp.h | 23 + LibSidServo/PID_test/main.c | 222 +++++ LibSidServo/PID_test/moving.c | 89 ++ LibSidServo/PID_test/moving.h | 62 ++ LibSidServo/PID_test/moving_model.cflags | 1 + LibSidServo/PID_test/moving_model.config | 4 + LibSidServo/PID_test/moving_model.creator | 1 + .../PID_test/moving_model.creator.user | 221 +++++ .../moving_model.creator.user.7bd84e3 | 184 ++++ LibSidServo/PID_test/moving_model.cxxflags | 1 + LibSidServo/PID_test/moving_model.files | 12 + LibSidServo/PID_test/moving_model.includes | 0 LibSidServo/PID_test/moving_private.h | 26 + LibSidServo/PID_test/plot | 4 + LibSidServo/PID_test/plot.cont | 8 + LibSidServo/PID_test/plot_jpg | 6 + LibSidServo/PID_test/plot_pdf | 5 + LibSidServo/PID_test/plotacc | 4 + LibSidServo/PID_test/ploterr | 4 + LibSidServo/PID_test/ploterr.cont | 6 + LibSidServo/PID_test/ploterr_jpg | 5 + LibSidServo/TODO | 3 + LibSidServo/examples/CMakeLists.txt | 14 + LibSidServo/examples/Readme.md | 28 + LibSidServo/examples/SSIIconf.c | 163 ++++ LibSidServo/examples/conf.c | 128 +++ LibSidServo/examples/conf.h | 28 + LibSidServo/examples/dump.c | 197 ++++ LibSidServo/examples/dump.h | 30 + LibSidServo/examples/dumpmoving.c | 105 +++ LibSidServo/examples/dumpmoving_dragNtrack.c | 228 +++++ LibSidServo/examples/dumpmoving_scmd.c | 177 ++++ LibSidServo/examples/dumpswing.c | 190 ++++ LibSidServo/examples/goto.c | 145 +++ LibSidServo/examples/scmd_traectory.c | 188 ++++ LibSidServo/examples/servo.conf | 25 + LibSidServo/examples/simpleconv.h | 29 + LibSidServo/examples/traectories.c | 136 +++ LibSidServo/examples/traectories.h | 32 + LibSidServo/libsidservo.cflags | 1 + LibSidServo/libsidservo.config | 7 + LibSidServo/libsidservo.creator | 1 + LibSidServo/libsidservo.creator.user | 220 +++++ LibSidServo/libsidservo.creator.user.7bd84e3 | 165 ++++ LibSidServo/libsidservo.creator.user.cf63021 | 184 ++++ LibSidServo/libsidservo.cxxflags | 1 + LibSidServo/libsidservo.files | 30 + LibSidServo/libsidservo.includes | 2 + LibSidServo/main.c | 619 +++++++++++++ LibSidServo/main.h | 81 ++ LibSidServo/movingfilter.c- | 46 + LibSidServo/movingmodel.c | 73 ++ LibSidServo/movingmodel.h | 76 ++ LibSidServo/polltest/main.c | 197 ++++ LibSidServo/ramp.c | 259 ++++++ LibSidServo/ramp.h | 23 + LibSidServo/serial.c | 815 ++++++++++++++++ LibSidServo/serial.h | 44 + LibSidServo/sidservo.h | 261 ++++++ LibSidServo/sidservo.pc.in | 10 + LibSidServo/ssii.c | 225 +++++ LibSidServo/ssii.h | 348 +++++++ asibfm700_common.h | 26 + asibfm700_configfile.h | 870 ++++++++++++++++++ asibfm700_mount.cpp | 337 +++++++ asibfm700_mount.h | 184 ++++ asibfm700_netserver.cpp | 73 ++ asibfm700_netserver.h | 136 +++ asibfm700_netserver_endpoint.h | 507 ++++++++++ asibfm700_netserver_main.cpp | 179 ++++ asibfm700_servocontroller.cpp | 266 ++++++ asibfm700_servocontroller.h | 160 ++++ main.cpp | 16 + mcc | 1 - 109 files changed, 11460 insertions(+), 4 deletions(-) create mode 100644 .gitignore delete mode 100644 .gitmodules create mode 100644 CMakeLists.txt create mode 100644 LibSidServo/CMakeLists.txt create mode 100644 LibSidServo/PID.c create mode 100644 LibSidServo/PID.h create mode 100644 LibSidServo/PID_test.deprecated/Dramp.c create mode 100644 LibSidServo/PID_test.deprecated/Dramp.h create mode 100644 LibSidServo/PID_test.deprecated/Sramp.c create mode 100644 LibSidServo/PID_test.deprecated/Sramp.h create mode 100644 LibSidServo/PID_test.deprecated/Tramp.c create mode 100644 LibSidServo/PID_test.deprecated/Tramp.h create mode 100644 LibSidServo/PID_test.deprecated/main.c create mode 100644 LibSidServo/PID_test.deprecated/moving.c create mode 100644 LibSidServo/PID_test.deprecated/moving.h create mode 100644 LibSidServo/PID_test.deprecated/moving_model.cflags create mode 100644 LibSidServo/PID_test.deprecated/moving_model.config create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 create mode 100644 LibSidServo/PID_test.deprecated/moving_model.cxxflags create mode 100644 LibSidServo/PID_test.deprecated/moving_model.files create mode 100644 LibSidServo/PID_test.deprecated/moving_model.includes create mode 100644 LibSidServo/PID_test.deprecated/moving_private.h create mode 100755 LibSidServo/PID_test.deprecated/plot create mode 100755 LibSidServo/PID_test.deprecated/plot.cont create mode 100755 LibSidServo/PID_test.deprecated/plot_jpg create mode 100755 LibSidServo/PID_test.deprecated/plot_pdf create mode 100755 LibSidServo/PID_test.deprecated/plotacc create mode 100755 LibSidServo/PID_test.deprecated/ploterr create mode 100755 LibSidServo/PID_test.deprecated/ploterr.cont create mode 100755 LibSidServo/PID_test.deprecated/ploterr_jpg create mode 100644 LibSidServo/PID_test/PID.c create mode 100644 LibSidServo/PID_test/PID.h create mode 100644 LibSidServo/PID_test/Tramp.c create mode 100644 LibSidServo/PID_test/Tramp.h create mode 100644 LibSidServo/PID_test/main.c create mode 100644 LibSidServo/PID_test/moving.c create mode 100644 LibSidServo/PID_test/moving.h create mode 100644 LibSidServo/PID_test/moving_model.cflags create mode 100644 LibSidServo/PID_test/moving_model.config create mode 100644 LibSidServo/PID_test/moving_model.creator create mode 100644 LibSidServo/PID_test/moving_model.creator.user create mode 100644 LibSidServo/PID_test/moving_model.creator.user.7bd84e3 create mode 100644 LibSidServo/PID_test/moving_model.cxxflags create mode 100644 LibSidServo/PID_test/moving_model.files create mode 100644 LibSidServo/PID_test/moving_model.includes create mode 100644 LibSidServo/PID_test/moving_private.h create mode 100755 LibSidServo/PID_test/plot create mode 100755 LibSidServo/PID_test/plot.cont create mode 100755 LibSidServo/PID_test/plot_jpg create mode 100755 LibSidServo/PID_test/plot_pdf create mode 100755 LibSidServo/PID_test/plotacc create mode 100755 LibSidServo/PID_test/ploterr create mode 100755 LibSidServo/PID_test/ploterr.cont create mode 100755 LibSidServo/PID_test/ploterr_jpg create mode 100644 LibSidServo/TODO create mode 100644 LibSidServo/examples/CMakeLists.txt create mode 100644 LibSidServo/examples/Readme.md create mode 100644 LibSidServo/examples/SSIIconf.c create mode 100644 LibSidServo/examples/conf.c create mode 100644 LibSidServo/examples/conf.h create mode 100644 LibSidServo/examples/dump.c create mode 100644 LibSidServo/examples/dump.h create mode 100644 LibSidServo/examples/dumpmoving.c create mode 100644 LibSidServo/examples/dumpmoving_dragNtrack.c create mode 100644 LibSidServo/examples/dumpmoving_scmd.c create mode 100644 LibSidServo/examples/dumpswing.c create mode 100644 LibSidServo/examples/goto.c create mode 100644 LibSidServo/examples/scmd_traectory.c create mode 100644 LibSidServo/examples/servo.conf create mode 100644 LibSidServo/examples/simpleconv.h create mode 100644 LibSidServo/examples/traectories.c create mode 100644 LibSidServo/examples/traectories.h create mode 100644 LibSidServo/libsidservo.cflags create mode 100644 LibSidServo/libsidservo.config create mode 100644 LibSidServo/libsidservo.creator create mode 100644 LibSidServo/libsidservo.creator.user create mode 100644 LibSidServo/libsidservo.creator.user.7bd84e3 create mode 100644 LibSidServo/libsidservo.creator.user.cf63021 create mode 100644 LibSidServo/libsidservo.cxxflags create mode 100644 LibSidServo/libsidservo.files create mode 100644 LibSidServo/libsidservo.includes create mode 100644 LibSidServo/main.c create mode 100644 LibSidServo/main.h create mode 100644 LibSidServo/movingfilter.c- create mode 100644 LibSidServo/movingmodel.c create mode 100644 LibSidServo/movingmodel.h create mode 100644 LibSidServo/polltest/main.c create mode 100644 LibSidServo/ramp.c create mode 100644 LibSidServo/ramp.h create mode 100644 LibSidServo/serial.c create mode 100644 LibSidServo/serial.h create mode 100644 LibSidServo/sidservo.h create mode 100644 LibSidServo/sidservo.pc.in create mode 100644 LibSidServo/ssii.c create mode 100644 LibSidServo/ssii.h create mode 100644 asibfm700_common.h create mode 100644 asibfm700_configfile.h create mode 100644 asibfm700_mount.cpp create mode 100644 asibfm700_mount.h create mode 100644 asibfm700_netserver.cpp create mode 100644 asibfm700_netserver.h create mode 100644 asibfm700_netserver_endpoint.h create mode 100644 asibfm700_netserver_main.cpp create mode 100644 asibfm700_servocontroller.cpp create mode 100644 asibfm700_servocontroller.h create mode 100644 main.cpp delete mode 160000 mcc diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ab7e9da --- /dev/null +++ b/.gitignore @@ -0,0 +1,83 @@ +# This file is used to ignore files which are generated +# ---------------------------------------------------------------------------- + +*~ +*.autosave +*.a +*.core +*.moc +*.o +*.obj +*.orig +*.rej +*.so +*.so.* +*_pch.h.cpp +*_resource.rc +*.qm +.#* +*.*# +core +!core/ +tags +.DS_Store +.directory +*.debug +Makefile* +*.prl +*.app +moc_*.cpp +ui_*.h +qrc_*.cpp +Thumbs.db +*.res +*.rc +/.qmake.cache +/.qmake.stash + +# qtcreator generated files +*.pro.user* +*.qbs.user* +CMakeLists.txt.user* + +# xemacs temporary files +*.flc + +# Vim temporary files +.*.swp + +# Visual Studio generated files +*.ib_pdb_index +*.idb +*.ilk +*.pdb +*.sln +*.suo +*.vcproj +*vcproj.*.*.user +*.ncb +*.sdf +*.opensdf +*.vcxproj +*vcxproj.* + +# MinGW generated files +*.Debug +*.Release + +# Python byte code +*.pyc + +# Binaries +# -------- +*.dll +*.exe + +# Directories with generated files +.moc/ +.obj/ +.pch/ +.rcc/ +.uic/ +/build*/ +.qtcreator/ diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 2c02ed5..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "mcc"] - path = mcc - url = https://git.sao.ru/timur/mcc.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9bdb7fb --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,159 @@ +cmake_minimum_required(VERSION 3.16) + +project(ASIBFM700 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# for ccls +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# pass to mcc-library +option(USE_BSPLINE_PCM "Use of FITPACK bivariate splines for PCM" ON) + +find_package(cxxopts REQUIRED) + +set(EXAMPLES OFF CACHE BOOL "" FORCE) +# set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_BUILD_TYPE "Debug") +add_subdirectory(LibSidServo) + +# include(FetchContent) + +# FetchContent_Declare( +# mcc +# GIT_REPOSITORY https://git.sao.ru/timur/mcc.git +# GIT_PROGRESS 1 +# OVERRIDE_FIND_PACKAGE +# ) +# FetchContent_MakeAvailable(mcc) +# set(CMAKE_MODULE_PATH +# ${CMAKE_MODULE_PATH} +# "${CMAKE_BINARY_DIR}/_deps/mcc-build" +# ) +# find_package(mcc REQUIRED) + +# execute_process( +# WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/ +# COMMAND ninja -C ${CMAKE_BINARY_DIR}/erfa_lib +# ) + +# include(ExternalProject) + +# ExternalProject_Add( +# mcclib +# # PREFIX ${CMAKE_BINARY_DIR} +# GIT_REPOSITORY https://git.sao.ru/timur/mcc.git +# GIT_PROGRESS 1 +# GIT_SHALLOW 1 +# BINARY_DIR ${CMAKE_BINARY_DIR}/mcc_build +# # INSTALL_DIR ${CMAKE_BINARY_DIR}/mcc_build +# CMAKE_ARGS +# -DBUILD_TESTS=OFF -USE_BSPLINE_PCM=${USE_BSPLINE_PCM} +# -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR}/mcc_build +# # BUILD_COMMAND make install +# # BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/mcc_build/mccTargets.cmake +# # INSTALL_COMMAND ${CMAKE_COMMAND} -E echo "Starting $ build" +# ) + +# message(STATUS "MCC: ${mcc_INCLUDE_DIR}") + +# make_directory(${CMAKE_BINARY_DIR}/mcc_build/include) + +# set(CMAKE_MODULE_PATH +# ${CMAKE_MODULE_PATH} +# "${CMAKE_BINARY_DIR}/mcc_build" +# ) + +# find_package(mcc CONFIG REQUIRED PATHS ${CMAKE_BINARY_DIR}/mcc_build) + +# add_library(mcc INTERFACE IMPORTED GLOBAL) +# add_dependencies(mcc mcclib) + +# set_target_properties( +# mcc +# PROPERTIES +# INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_BINARY_DIR}/mcc_build/include" +# ) +# target_link_libraries(mcc INTERFACE mcclib) + +# get_target_property(ii mcc INTERFACE_INCLUDE_DIRECTORIES) +# message(STATUS "$INC: ${ii}") + +include(FetchContent) + +FetchContent_Declare( + mcclib + # GIT_REPOSITORY https://git.sao.ru/timur/mcc.git + GIT_REPOSITORY ssh://git@95.140.147.151:/home/git/mcc.git + GIT_SHALLOW 1 + GIT_PROGRESS 1 + SOURCE_DIR + ${CMAKE_BINARY_DIR}/mcc + # OVERRIDE_FIND_PACKAGE +) +set(BUILD_TESTS OFF) + +FetchContent_MakeAvailable(mcclib) + +# add_subdirectory(${CMAKE_BINARY_DIR}/mcc) + +if(USE_BSPLINE_PCM) + FetchContent_GetProperties(mcclib BINARY_DIR mcclib_bindir) + # to fix libfitpack_project.a target path issue + # file(CREATE_LINK ${mcclib_bindir}/fitpack fitpack SYMBOLIC) +endif() + +set(ASIBFM700_LIB_SRC + asibfm700_common.h + asibfm700_servocontroller.h + asibfm700_servocontroller.cpp +) + +set(ASIBFM700_LIB asibfm700mount) +add_library( + ${ASIBFM700_LIB} + STATIC + ${ASIBFM700_LIB_SRC} + asibfm700_mount.h + asibfm700_mount.cpp + asibfm700_configfile.h + asibfm700_netserver.cpp + asibfm700_netserver.h +) + +# add_dependencies(${ASIBFM700_LIB} mcc) + +# target_include_directories(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_INCLUDE_DIR}) +# target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_LIBFILE}) +# target_link_libraries(${ASIBFM700_LIB} PUBLIC mcclib sidservo) +target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc sidservo atomic) +if(USE_BSPLINE_PCM) + target_compile_definitions(${ASIBFM700_LIB} PRIVATE USE_BSPLINE_PCM) +endif() + +set(ASIBFM700_NETSERVER_APP asibfm700_netserver) +add_executable(${ASIBFM700_NETSERVER_APP} asibfm700_netserver_main.cpp) +target_link_libraries( + ${ASIBFM700_NETSERVER_APP} + PUBLIC cxxopts::cxxopts ${ASIBFM700_LIB} +) + +# include(CMakePrintHelpers) +# cmake_print_properties( +# TARGETS ${ASIBFM700_LIB} +# PROPERTIES +# INCLUDE_DIRECTORIES +# INTERFACE_INCLUDE_DIRECTORIES +# LINK_INTERFACE_LIBRARIES +# ) + +# get_target_property(TARGET_INCLUDES ${ASIBFM700_LIB} INCLUDE_DIRECTORIES) +# message(STATUS "${TARGET_INCLUDES}") + +include(GNUInstallDirs) +install( + TARGETS ${ASIBFM700_NETSERVER_APP} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) diff --git a/LibSidServo/CMakeLists.txt b/LibSidServo/CMakeLists.txt new file mode 100644 index 0000000..bdf2c99 --- /dev/null +++ b/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/LibSidServo/PID.c b/LibSidServo/PID.c new file mode 100644 index 0000000..3f642cd --- /dev/null +++ b/LibSidServo/PID.c @@ -0,0 +1,221 @@ +/* + * 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 "main.h" +#include "PID.h" +#include "serial.h" + +PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){ + if(!gain || Iarrsz < 3) return NULL; + PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); + pid->gain = *gain; + DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D); + pid->pidIarrSize = Iarrsz; + pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); + return pid; +} + +// don't clear lastT! +void pid_clear(PIDController_t *pid){ + if(!pid) return; + DBG("CLEAR PID PARAMETERS"); + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +void pid_delete(PIDController_t **pid){ + if(!pid || !*pid) return; + if((*pid)->pidIarray) free((*pid)->pidIarray); + free(*pid); + *pid = NULL; +} + +double pid_calculate(PIDController_t *pid, double error, double dt){ + // calculate flowing integral + double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; + //DBG("oldi/new: %g, %g", oldi, newi); + pid->pidIarray[pid->curIidx++] = newi; + if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0; + pid->integral += newi - oldi; + double derivative = (error - pid->prev_error) / dt; + pid->prev_error = error; + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; + DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); + return sum; +} + +typedef struct{ + PIDController_t *PIDC; + PIDController_t *PIDV; +} PIDpair_t; + +typedef struct{ + axis_status_t state; + coordval_t position; + coordval_t speed; +} axisdata_t; +/** + * @brief process - Process PID for given axis + * @param tagpos - given coordinate of target position + * @param endpoint - endpoint for this coordinate + * @param pid - pid itself + * @return calculated new speed or -1 for max speed + */ +static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ + double dt = timediff(&tagpos->t, &axis->position.t); + if(dt < 0 || dt > Conf.PIDMaxDt){ + DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt); + return axis->speed.val; // data is too old or wrong + } + double error = tagpos->val - axis->position.val, fe = fabs(error); + DBG("error: %g", error); + PIDController_t *pid = NULL; + switch(axis->state){ + case AXIS_SLEWING: + if(fe < Conf.MaxPointingErr){ + axis->state = AXIS_POINTING; + DBG("--> Pointing"); + pid = pidpair->PIDC; + }else{ + DBG("Slewing..."); + return NAN; // max speed for given axis + } + break; + case AXIS_POINTING: + if(fe < Conf.MaxFinePointingErr){ + axis->state = AXIS_GUIDING; + DBG("--> Guiding"); + pid = pidpair->PIDV; + }else if(fe > Conf.MaxPointingErr){ + DBG("--> Slewing"); + axis->state = AXIS_SLEWING; + return NAN; + } else pid = pidpair->PIDC; + break; + case AXIS_GUIDING: + pid = pidpair->PIDV; + if(fe > Conf.MaxFinePointingErr){ + DBG("--> Pointing"); + axis->state = AXIS_POINTING; + pid = pidpair->PIDC; + }else if(fe < Conf.MaxGuidingErr){ + DBG("At target"); + // TODO: we can point somehow that we are at target or introduce new axis state + }else DBG("Current error: %g", fe); + break; + case AXIS_STOPPED: // start pointing to target; will change speed next time + DBG("AXIS STOPPED!!!! --> Slewing"); + axis->state = AXIS_SLEWING; + return getspeed(tagpos, pidpair, axis); + case AXIS_ERROR: + DBG("Can't move from erroneous state"); + return 0.; + } + if(!pid){ + DBG("WTF? Where is a PID?"); + return axis->speed.val; + } + double dtpid = timediff(&tagpos->t, &pid->prevT); + if(dtpid < 0 || dtpid > Conf.PIDMaxDt){ + DBG("time diff too big: clear PID"); + pid_clear(pid); + } + if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt; + pid->prevT = tagpos->t; + DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state); + double tagspeed = pid_calculate(pid, error, dtpid); + if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based + return tagspeed; // coordinate-based +} + +/** + * @brief correct2 - recalculate PID and move telescope to new point with new speed + * @param target - target position (for error calculations) + * @param endpoint - stop point (some far enough point to stop in case of hang) + * @return error code + */ +mcc_errcodes_t correct2(const coordval_pair_t *target){ + static PIDpair_t pidX = {0}, pidY = {0}; + if(!pidX.PIDC){ + pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidX.PIDC) return MCC_E_FATAL; + pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidX.PIDV) return MCC_E_FATAL; + } + if(!pidY.PIDC){ + pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidY.PIDC) return MCC_E_FATAL; + pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidY.PIDV) return MCC_E_FATAL; + } + mountdata_t m; + coordpair_t tagspeed; // absolute value of speed + double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation) + if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED; + axisdata_t axis; + DBG("state: %d/%d", m.Xstate, m.Ystate); + axis.state = m.Xstate; + axis.position = m.encXposition; + axis.speed = m.encXspeed; + tagspeed.X = getspeed(&target->X, &pidX, &axis); + if(isnan(tagspeed.X)){ // max speed + if(target->X.val < axis.position.val) Xsign = -1.; + tagspeed.X = Xlimits.max.speed; + }else{ + if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; } + if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed; + } + axis_status_t xstate = axis.state; + axis.state = m.Ystate; + axis.position = m.encYposition; + axis.speed = m.encYspeed; + tagspeed.Y = getspeed(&target->Y, &pidY, &axis); + if(isnan(tagspeed.Y)){ // max speed + if(target->Y.val < axis.position.val) Ysign = -1.; + tagspeed.Y = Ylimits.max.speed; + }else{ + if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; } + if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed; + } + axis_status_t ystate = axis.state; + if(m.Xstate != xstate || m.Ystate != ystate){ + DBG("State changed"); + setStat(xstate, ystate); + } + coordpair_t endpoint; + // allow at least PIDMaxDt moving with target speed + double dv = fabs(tagspeed.X - m.encXspeed.val); + double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed + + Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving + + tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping + endpoint.X = m.encXposition.val + Xsign * adder; + dv = fabs(tagspeed.Y - m.encYspeed.val); + adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.) + + Conf.PIDMaxDt * tagspeed.Y + + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.; + endpoint.Y = m.encYposition.val + Ysign * adder; + DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.); + return Mount.moveWspeed(&endpoint, &tagspeed); +} diff --git a/LibSidServo/PID.h b/LibSidServo/PID.h new file mode 100644 index 0000000..5ecfad0 --- /dev/null +++ b/LibSidServo/PID.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 + +#include "sidservo.h" + +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + struct timespec prevT; // time of previous correction + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz); +void pid_clear(PIDController_t *pid); +void pid_delete(PIDController_t **pid); +double pid_calculate(PIDController_t *pid, double error, double dt); + +mcc_errcodes_t correct2(const coordval_pair_t *target); diff --git a/LibSidServo/PID_test.deprecated/Dramp.c b/LibSidServo/PID_test.deprecated/Dramp.c new file mode 100644 index 0000000..09a4f0e --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Dramp.c @@ -0,0 +1,141 @@ +/* + * This file is part of the moving_model 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 "Dramp.h" + +static movestate_t state = ST_STOP; +static moveparam_t target, Min, Max; +static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate +static moveparam_t curparams = {0}; // current coordinate/speed/acceleration +static double T1 = -1.; // time to switch into minimal speed for best coord tolerance + +typedef enum{ + STAGE_NORMALSPEED, + STAGE_MINSPEED, + STAGE_STOPPED +} movingsage_t; + +static movingsage_t movingstage = STAGE_STOPPED; + +static int initlims(limits_t *lim){ + if(!lim) return FALSE; + Min = lim->min; + Max = lim->max; + return TRUE; +} + +static int calc(moveparam_t *x, double t){ + DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed); + if(!x || t < 0.) return FALSE; + if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE; + double adist = fabs(x->coord - curparams.coord); + DBG("want dist: %g", adist); + if(adist < coord_tolerance) return TRUE; // we are at place + if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters + target = *x; + if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position + if(target.speed < Min.speed) target.speed = Min.speed; + DBG("Approximate tag speed: %g", target.speed); + T0 = t; + // calculate time to switch into minimal speed + T1 = -1.; // no min speed phase + if(target.speed > Min.speed){ + double dxpertick = target.speed * time_tick; + DBG("dX per one tick: %g", dxpertick); + double ticks_need = floor(adist / dxpertick); + DBG("ticks need: %g", ticks_need); + if(ticks_need < 1.) return FALSE; // cannot reach + if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){ + DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need); + double dxpersmtick = Min.speed * time_tick; + DBG("dX per smallest tick: %g", dxpersmtick); + while(--ticks_need > 1.){ + double part = adist - ticks_need * dxpertick; + double smticks = floor(part / dxpersmtick); + double least = part - smticks * dxpersmtick; + if(least < coord_tolerance) break; + } + DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick); + T1 = t + ticks_need * time_tick; + } + } + state = ST_MOVE; + Xlast0 = curparams.coord; + if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED; + else movingstage = STAGE_MINSPEED; + if(x->coord < curparams.coord) target.speed *= -1.; // real speed + curparams.speed = target.speed; + return TRUE; +} + +static void stop(double _U_ t){ + T0 = -1.; + curparams.accel = 0.; + curparams.speed = 0.; + state = ST_STOP; + movingstage = STAGE_STOPPED; +} + +static movestate_t proc(moveparam_t *next, double t){ + if(T0 < 0.) return ST_STOP; + curparams.coord = Xlast0 + (t - T0) * curparams.speed; + //DBG("coord: %g (dTmon: %g, speed: %g)", curparams.coord, t-T0, curparams.speed); + int ooops = FALSE; // oops - we are over target! + if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;} + else{ if(curparams.coord > target.coord) ooops = TRUE; } + if(ooops){ + DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord); + stop(t); + if(next) *next = curparams; + return state; + } + if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1 + if(t >= T1){ + DBG("T1=%g, t=%g -->", T1, t); + curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed; + movingstage = STAGE_MINSPEED; + Xlast0 = curparams.coord; + T0 = T1; + DBG("Go further with minimal speed"); + } + } + if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place + DBG("OK, we are in place"); + stop(t); + } + if(next) *next = curparams; + return state; +} + +static movestate_t getst(moveparam_t *cur){ + if(cur) *cur = curparams; + return state; +} + +movemodel_t dumb = { + .init_limits = initlims, + .calculate = calc, + .proc_move = proc, + .stop = stop, + .emergency_stop = stop, + .get_state = getst, +}; diff --git a/LibSidServo/PID_test.deprecated/Dramp.h b/LibSidServo/PID_test.deprecated/Dramp.h new file mode 100644 index 0000000..e765c77 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Dramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t dumb; diff --git a/LibSidServo/PID_test.deprecated/Sramp.c b/LibSidServo/PID_test.deprecated/Sramp.c new file mode 100644 index 0000000..47f03ed --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Sramp.c @@ -0,0 +1,27 @@ +/* + * This file is part of the moving_model 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 . + */ + +// quasi non-jerk s-ramp + +#include +#include + +#include "Sramp.h" + + +movemodel_t s_shaped = { 0 }; diff --git a/LibSidServo/PID_test.deprecated/Sramp.h b/LibSidServo/PID_test.deprecated/Sramp.h new file mode 100644 index 0000000..3decd8b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Sramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t s_shaped; diff --git a/LibSidServo/PID_test.deprecated/Tramp.c b/LibSidServo/PID_test.deprecated/Tramp.c new file mode 100644 index 0000000..f164463 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Tramp.c @@ -0,0 +1,223 @@ +/* + * This file is part of the moving_model 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 . + */ + +// simplest trapezioidal ramp + +#include +#include +#include +#include + +#include "Tramp.h" + +#undef DBG +#define DBG(...) + +static movestate_t state = ST_STOP; +static moveparam_t Min, Max; // `Min` acceleration not used! + +typedef enum{ + STAGE_ACCEL, // start from zero speed and accelerate to Max speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +static movingstage_t movingstage = STAGE_STOPPED; +static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts +static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage +static moveparam_t curparams = {0}; // current coordinate/speed/acceleration + +static int initlims(limits_t *lim){ + if(!lim) return FALSE; + Min = lim->min; + Max = lim->max; + return TRUE; +} + +static void emstop(double _U_ t){ + curparams.accel = 0.; + curparams.speed = 0.; + bzero(Times, sizeof(Times)); + bzero(Params, sizeof(Params)); + state = ST_STOP; + movingstage = STAGE_STOPPED; +} + +static void stop(double t){ + if(state == ST_STOP || movingstage == STAGE_STOPPED) return; + movingstage = STAGE_DECEL; + state = ST_MOVE; + Times[STAGE_DECEL] = t; + Params[STAGE_DECEL].speed = curparams.speed; + if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel; + else Params[STAGE_DECEL].accel = Max.accel; + Params[STAGE_DECEL].coord = curparams.coord; + // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2 + Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel; + // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2 + double dt = Times[STAGE_STOPPED] - t; + Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt + + Params[STAGE_DECEL].accel * dt * dt / 2.; +} + +/** + * @brief calc - moving calculation + * @param x - using max speed (>0!!!) and coordinate + * @param t - current time value + * @return FALSE if can't move with given parameters + */ +static int calc(moveparam_t *x, double t){ + if(!x) return FALSE; + if(x->coord < Min.coord || x->coord > Max.coord) return FALSE; + if(x->speed < Min.speed || x->speed > Max.speed) return FALSE; + double Dx = fabs(x->coord - curparams.coord); // full distance + double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds + // we have two variants: with or without stage with constant speed + double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed + double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs) + DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23); + double setspeed = x->speed; // new max speed (we can change it if need) + double dt01, dx01; // we'll fill them depending on starting conditions + Times[0] = t; + Params[0].speed = curparams.speed; + Params[0].coord = curparams.coord; + + double curspeed = fabs(curparams.speed); + double dt0s = curspeed / Max.accel; // time of stopping phase + double dx0s = curspeed * dt0s / 2.; // distance + DBG("dt0s=%g, dx0s=%g", dt0s, dx0s); + if(dx0s > Dx){ + WARNX("distance too short"); + return FALSE; + } + if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target + DBG("Distance good to just stop"); + stop(t); + return TRUE; + } + if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign + // after stop we will have full profile + double dxs3 = Dx - dx0s; + double newspeed = sqrt(Max.accel * dxs3); + if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed + DBG("dxs3=%g, setspeed=%g", dxs3, setspeed); + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + Params[0].accel = sign * Max.accel; + if(state == ST_STOP) dx01 = setspeed * dt01 / 2.; + else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed); + DBG("dx01=%g, dt01=%g", dx01, dt01); + }else{ // increase or decrease speed without stopping phase + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + double a = sign * Max.accel; + if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;} + else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;} + //double a = (curspeed > setspeed) ? -Max.accel : Max.accel; + dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; + DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); + if(dx01 + dx23 > Dx){ // calculate max speed + setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.); + if(setspeed < curspeed){ + setspeed = curparams.speed; + dt01 = 0.; dx01 = 0.; + Params[0].accel = 0.; + }else{ + Params[0].accel = a; + dt01 = fabs(setspeed - curspeed) / Max.accel; + dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.; + } + }else Params[0].accel = a; + } + if(setspeed < Min.speed){ + WARNX("New speed should be too small"); + return FALSE; + } + moveparam_t *p = &Params[STAGE_MAXSPEED]; + p->accel = 0.; p->speed = sign * setspeed; + p->coord = curparams.coord + dx01 * sign; + Times[STAGE_MAXSPEED] = Times[0] + dt01; + dt23 = setspeed / Max.accel; + dx23 = setspeed * dt23 / 2.; + // calculate dx12 and dt12 + double dx12 = Dx - dx01 - dx23; + if(dx12 < -coord_tolerance){ + WARNX("Oops, WTF dx12=%g?", dx12); + return FALSE; + } + double dt12 = dx12 / setspeed; + p = &Params[STAGE_DECEL]; + p->accel = -sign * Max.accel; + p->speed = sign * setspeed; + p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12; + Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12; + p = &Params[STAGE_STOPPED]; + p->accel = 0.; p->speed = 0.; p->coord = x->coord; + Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23; + for(int i = 0; i < 4; ++i) + DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i, + Times[i], Params[i].coord, Params[i].speed, Params[i].accel); + state = ST_MOVE; + movingstage = STAGE_ACCEL; + return TRUE; +} + +static movestate_t proc(moveparam_t *next, double t){ + if(state == ST_STOP) goto ret; + for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){ + if(Times[s] <= t){ // check time for current stage + movingstage = s; + break; + } + } + if(movingstage == STAGE_STOPPED){ + curparams.coord = Params[STAGE_STOPPED].coord; + emstop(t); + goto ret; + } + // calculate current parameters + double dt = t - Times[movingstage]; + double a = Params[movingstage].accel; + double v0 = Params[movingstage].speed; + double x0 = Params[movingstage].coord; + curparams.accel = a; + curparams.speed = v0 + a * dt; + curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; +ret: + if(next) *next = curparams; + return state; +} + +static movestate_t getst(moveparam_t *cur){ + if(cur) *cur = curparams; + return state; +} + +static double gettstop(){ + return Times[STAGE_STOPPED]; +} + +movemodel_t trapez = { + .init_limits = initlims, + .stop = stop, + .emergency_stop = emstop, + .get_state = getst, + .calculate = calc, + .proc_move = proc, + .stoppedtime = gettstop, +}; diff --git a/LibSidServo/PID_test.deprecated/Tramp.h b/LibSidServo/PID_test.deprecated/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Tramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t trapez; diff --git a/LibSidServo/PID_test.deprecated/main.c b/LibSidServo/PID_test.deprecated/main.c new file mode 100644 index 0000000..5a67b97 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/main.c @@ -0,0 +1,243 @@ +/* + * This file is part of the moving_model 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 "moving.h" + +// errors for states: slewing/pointing/guiding +#define MAX_POINTING_ERR (50.) +#define MAX_GUIDING_ERR (5.) +// timeout to "forget" old data from I sum array; seconds +#define PID_I_PERIOD (3.) + +static movemodel_t *model = NULL; +static FILE *coordslog = NULL; + +typedef enum{ + Slewing, + Pointing, + Guiding +} state_t; + +static state_t state = Slewing; + +typedef struct{ + int help; + char *ramptype; + char *xlog; + double dTmon; + double dTcorr; + double Tend; + double minerr; + double P, I, D; +} pars; + +static pars G = { + .ramptype = "t", + .dTmon = 0.01, + .dTcorr = 0.05, + .Tend = 100., + .minerr = 0.1, + .P = 0.8, +}; + +static limits_t limits = { + .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1}, + .max = {.coord = 1e6, .speed = 1e3, .accel = 500.}, + .jerk = 10. +}; + +typedef struct { + double kp, ki, kd; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController; + +static PIDController pid; + +static sl_option_t opts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"}, + {"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"}, + {"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"}, + {"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"}, + {"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"}, + {"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"}, + {"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"}, + {"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"}, + {"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"}, + // TODO: add parameters for limits setting + end_option +}; + +// calculate coordinate target for given time (starting from zero) +static double target_coord(double t){ + if(t > 20. && t < 30.) return target_coord(20.); + double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5); + return pos; +} + +/* P-only == oscillations +static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ + double error = targcoord - p->coord; + if(fabs(error) < G.minerr) return p->speed; + return p->speed + error / dt / 500.; +} +*/ + +static void pid_init(PIDController *pid, double kp, double ki, double kd) { + pid->kp = fabs(kp); + pid->ki = fabs(ki); + pid->kd = fabs(kd); + pid->prev_error = 0.; + pid->integral = 0.; + pid->curIidx = 0; + pid->pidIarrSize = PID_I_PERIOD / G.dTcorr; + if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements"); + pid->pidIarray = MALLOC(double, pid->pidIarrSize); +} + +static void pid_clear(PIDController *pid){ + if(!pid) return; + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ + double error = targcoord - p->coord, fe = fabs(error); + switch(state){ + case Slewing: + if(fe < MAX_POINTING_ERR){ + pid_clear(&pid); + state = Pointing; + green("--> Pointing\n"); + }else{ + red("Slewing...\n"); + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } + break; + case Pointing: + if(fe < MAX_GUIDING_ERR){ + pid_clear(&pid); + state = Guiding; + green("--> Guiding\n"); + }else if(fe > MAX_POINTING_ERR){ + red("--> Slewing\n"); + state = Slewing; + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } + break; + case Guiding: + if(fe > MAX_GUIDING_ERR){ + red("--> Pointing\n"); + state = Pointing; + }else if(fe < G.minerr){ + green("At target\n"); + //pid_clear(&pid); + //return p->speed; + } + break; + } + + red("Calculate PID\n"); + double oldi = pid.pidIarray[pid.curIidx], newi = error * dt; + pid.pidIarray[pid.curIidx++] = oldi; + if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0; + pid.integral += newi - oldi; + double derivative = (error - pid.prev_error) / dt; + pid.prev_error = error; + DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative); + double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative); + if(state == Pointing) add /= 3.; + else if(state == Guiding) add /= 7.; + DBG("ADD = %g; new speed = %g", add, p->speed + add); + if(state == Guiding) return p->speed + add / dt / 10.; + return add / dt; +} +// ./moving -l coords -P.5 -I.05 -D1.5 +// ./moving -l coords -P1.3 -D1.6 + +static void start_model(double Tend){ + double T = 0., Tcorr = 0.;//, Tlast = 0.; + moveparam_t target; + while(T <= Tend){ + moveparam_t p; + movestate_t st = model->get_state(&p); + if(st == ST_MOVE) st = model->proc_move(&p, T); + double nextcoord = target_coord(T); + double error = nextcoord - p.coord; + if(T - Tcorr >= G.dTcorr){ // check correction + double speed = getNewSpeed(&p, nextcoord, T - Tcorr); + target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5; + target.speed = fabs(speed); + double res_speed = limits.max.speed / 2.; + if(target.speed > limits.max.speed){ + target.speed = limits.max.speed; + res_speed = limits.max.speed / 4.; + }else if(target.speed < limits.min.speed){ + target.speed = limits.min.speed; + res_speed = limits.min.speed * 4.; + } + if(!move_to(&target, T)){ + target.speed = res_speed; + if(!move_to(&target, T)) + WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed); + } + DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error); + Tcorr = T; + } + // make log + fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n", + T, nextcoord, p.coord, p.speed, p.accel, error); + T += G.dTmon; + } +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, opts); + if(G.help) sl_showhelp(-1, opts); + if(G.xlog){ + coordslog = fopen(G.xlog, "w"); + if(!coordslog) ERR("Can't open %s", G.xlog); + } else coordslog = stdout; + if(G.dTmon <= 0.) ERRX("tmon should be > 0."); + if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1."); + if(G.Tend <= 0.) ERRX("tend should be > 0."); + pid_init(&pid, G.P, G.I, G.D); + fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error"); + ramptype_t ramp = RAMP_AMOUNT; + if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB; + else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM; + else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S; + else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type"); + model = init_moving(ramp, &limits); + if(!model) ERRX("Can't init moving model: check parameters"); + start_model(G.Tend); + fclose(coordslog); + return 0; +} diff --git a/LibSidServo/PID_test.deprecated/moving.c b/LibSidServo/PID_test.deprecated/moving.c new file mode 100644 index 0000000..d7264c0 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving.c @@ -0,0 +1,105 @@ +/* + * This file is part of the moving_model 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 "moving.h" +#include "moving_private.h" +#include "Dramp.h" +#include "Sramp.h" +#include "Tramp.h" + +//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +static movemodel_t *model = NULL; +double coord_tolerance = COORD_TOLERANCE_DEFAULT; +double time_tick = TIME_TICK_DEFAULT; + +// difference of time from first call, using nanoseconds +double nanot(){ + static struct timespec *start = NULL; + struct timespec now; + if(!start){ + start = MALLOC(struct timespec, 1); + if(!start) return -1.; + if(clock_gettime(CLOCK_REALTIME, start)) return -1.; + } + if(clock_gettime(CLOCK_REALTIME, &now)) return -1.; + //DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec); + double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; + double sd = (double)now.tv_sec - (double)start->tv_sec; + return sd + nd; +} + +static void chkminmax(double *min, double *max){ + if(*min <= *max) return; + double t = *min; + *min = *max; + *max = t; +} + +movemodel_t *init_moving(ramptype_t type, limits_t *l){ + if(!l) return FALSE; + switch(type){ + case RAMP_DUMB: + model = &dumb; + break; + case RAMP_TRAPEZIUM: + model = &trapez; + break; + case RAMP_S: + model = &s_shaped; + break; + default: + return FALSE; + } + if(!model->init_limits) return NULL; + moveparam_t *max = &l->max, *min = &l->min; + if(min->speed < 0.) min->speed = -min->speed; + if(max->speed < 0.) max->speed = -max->speed; + if(min->accel < 0.) min->accel = -min->accel; + if(max->accel < 0.) max->accel = -max->accel; + chkminmax(&min->coord, &max->coord); + chkminmax(&min->speed, &max->speed); + chkminmax(&min->accel, &max->accel); + if(!model->init_limits(l)) return NULL; + return model; +} + +int move_to(moveparam_t *target, double t){ + if(!target || !model) return FALSE; + DBG("MOVE to %g at speed %g", target->coord, target->speed); + // only positive velocity + if(target->speed < 0.) target->speed = -target->speed; + // don't mind about acceleration - user cannot set it now + return model->calculate(target, t); +} + +int init_coordtol(double tolerance){ + if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE; + coord_tolerance = tolerance; + return TRUE; +} +int init_timetick(double tick){ + if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE; + time_tick = tick; + return TRUE; +} diff --git a/LibSidServo/PID_test.deprecated/moving.h b/LibSidServo/PID_test.deprecated/moving.h new file mode 100644 index 0000000..6854308 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving.h @@ -0,0 +1,70 @@ +/* + * This file is part of the moving_model 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 + +// tolerance, time ticks +#define COORD_TOLERANCE_DEFAULT (0.01) +#define COORD_TOLERANCE_MIN (0.0001) +#define COORD_TOLERANCE_MAX (10.) +#define TIME_TICK_DEFAULT (0.0001) +#define TIME_TICK_MIN (1e-9) +#define TIME_TICK_MAX (10.) + +typedef enum{ + RAMP_DUMB, // no ramp: infinite acceleration/deceleration + RAMP_TRAPEZIUM, // trapezium ramp + RAMP_S, // s-shaped ramp + RAMP_AMOUNT +} ramptype_t; + +typedef enum{ + ST_STOP, // stopped + ST_MOVE, // moving + ST_AMOUNT +} movestate_t; + +typedef struct{ // all values could be both as positive and negative + double coord; + double speed; + double accel; +} moveparam_t; + +typedef struct{ + moveparam_t min; + moveparam_t max; + double jerk; +} limits_t; + +typedef struct{ + int (*init_limits)(limits_t *lim); // init values of limits, jerk + int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(moveparam_t *cur); // get current moving state + void (*stop)(double t); // stop by ramp + void (*emergency_stop)(double t); // stop with highest acceleration + double (*stoppedtime)(); // time when moving will ends +} movemodel_t; + +extern double coord_tolerance; + +double nanot(); +movemodel_t *init_moving(ramptype_t type, limits_t *l); +int init_coordtol(double tolerance); +int init_timetick(double tick); +int move_to(moveparam_t *target, double t); diff --git a/LibSidServo/PID_test.deprecated/moving_model.cflags b/LibSidServo/PID_test.deprecated/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.config b/LibSidServo/PID_test.deprecated/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.config @@ -0,0 +1,4 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/LibSidServo/PID_test.deprecated/moving_model.creator b/LibSidServo/PID_test.deprecated/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/LibSidServo/PID_test.deprecated/moving_model.creator.user b/LibSidServo/PID_test.deprecated/moving_model.creator.user new file mode 100644 index 0000000..eaf30ec --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.creator.user @@ -0,0 +1,221 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 1 + 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 + Builtin.DefaultTidyAndClazy + 1 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/C-files/mountcontrol.git/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + 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/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 b/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + 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 + 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/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + 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/LibSidServo/PID_test.deprecated/moving_model.cxxflags b/LibSidServo/PID_test.deprecated/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.files b/LibSidServo/PID_test.deprecated/moving_model.files new file mode 100644 index 0000000..e522e0e --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.files @@ -0,0 +1,10 @@ +Dramp.c +Dramp.h +Sramp.c +Sramp.h +Tramp.c +Tramp.h +main.c +moving.c +moving.h +moving_private.h diff --git a/LibSidServo/PID_test.deprecated/moving_model.includes b/LibSidServo/PID_test.deprecated/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/LibSidServo/PID_test.deprecated/moving_private.h b/LibSidServo/PID_test.deprecated/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_private.h @@ -0,0 +1,26 @@ +/* + * This file is part of the moving_model 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 "moving.h" + +extern double coord_tolerance; +extern double time_tick; + + diff --git a/LibSidServo/PID_test.deprecated/plot b/LibSidServo/PID_test.deprecated/plot new file mode 100755 index 0000000..476e1f3 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/plot.cont b/LibSidServo/PID_test.deprecated/plot.cont new file mode 100755 index 0000000..16dc0f5 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot.cont @@ -0,0 +1,8 @@ +#!/usr/bin/gnuplot + +#set term pdf +#set output "output.pdf" +while(1){ + plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader + pause 1 +} \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/plot_jpg b/LibSidServo/PID_test.deprecated/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot_jpg @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +set terminal jpeg size 1000,500 +set output "all.jpg" + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader diff --git a/LibSidServo/PID_test.deprecated/plot_pdf b/LibSidServo/PID_test.deprecated/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot_pdf @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term pdf +set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader diff --git a/LibSidServo/PID_test.deprecated/plotacc b/LibSidServo/PID_test.deprecated/plotacc new file mode 100755 index 0000000..f15faaa --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plotacc @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:5 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr b/LibSidServo/PID_test.deprecated/ploterr new file mode 100755 index 0000000..d2b990d --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:6 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr.cont b/LibSidServo/PID_test.deprecated/ploterr.cont new file mode 100755 index 0000000..95bcd4b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr.cont @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +while(1){ + plot 'coords' using 1:6 with lines title columnheader + pause 1 +} diff --git a/LibSidServo/PID_test.deprecated/ploterr_jpg b/LibSidServo/PID_test.deprecated/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr_jpg @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term jpeg size 1000,500 +set output "error.jpg" +plot 'coords' using 1:6 with lines title columnheader diff --git a/LibSidServo/PID_test/PID.c b/LibSidServo/PID_test/PID.c new file mode 100644 index 0000000..7774258 --- /dev/null +++ b/LibSidServo/PID_test/PID.c @@ -0,0 +1,64 @@ +/* + * This file is part of the moving_model 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 "PID.h" + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){ + if(!gain || Iarrsz < 3) return NULL; + PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); + pid->gain = *gain; + pid->pidIarrSize = Iarrsz; + pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); + return pid; +} + +void pid_clear(PIDController_t *pid){ + if(!pid) return; + DBG("CLEAR PID PARAMETERS"); + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +void pid_delete(PIDController_t **pid){ + if(!pid || !*pid) return; + if((*pid)->pidIarray) free((*pid)->pidIarray); + free(*pid); + *pid = NULL; +} + +double pid_calculate(PIDController_t *pid, double error, double dt){ + // calculate flowing integral + double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; + DBG("oldi/new: %g, %g", oldi, newi); + pid->pidIarray[pid->curIidx++] = newi; + if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0; + pid->integral += newi - oldi; + double derivative = (error - pid->prev_error) / dt; + pid->prev_error = error; + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; + DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); + return sum; +} diff --git a/LibSidServo/PID_test/PID.h b/LibSidServo/PID_test/PID.h new file mode 100644 index 0000000..e60ddd6 --- /dev/null +++ b/LibSidServo/PID_test/PID.h @@ -0,0 +1,39 @@ +/* + * This file is part of the moving_model 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 + +typedef struct{ + double P, I, D; +} PIDpar_t; + +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz); +void pid_clear(PIDController_t *pid); +void pid_delete(PIDController_t **pid); +double pid_calculate(PIDController_t *pid, double error, double dt); diff --git a/LibSidServo/PID_test/Tramp.c b/LibSidServo/PID_test/Tramp.c new file mode 100644 index 0000000..f164463 --- /dev/null +++ b/LibSidServo/PID_test/Tramp.c @@ -0,0 +1,223 @@ +/* + * This file is part of the moving_model 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 . + */ + +// simplest trapezioidal ramp + +#include +#include +#include +#include + +#include "Tramp.h" + +#undef DBG +#define DBG(...) + +static movestate_t state = ST_STOP; +static moveparam_t Min, Max; // `Min` acceleration not used! + +typedef enum{ + STAGE_ACCEL, // start from zero speed and accelerate to Max speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +static movingstage_t movingstage = STAGE_STOPPED; +static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts +static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage +static moveparam_t curparams = {0}; // current coordinate/speed/acceleration + +static int initlims(limits_t *lim){ + if(!lim) return FALSE; + Min = lim->min; + Max = lim->max; + return TRUE; +} + +static void emstop(double _U_ t){ + curparams.accel = 0.; + curparams.speed = 0.; + bzero(Times, sizeof(Times)); + bzero(Params, sizeof(Params)); + state = ST_STOP; + movingstage = STAGE_STOPPED; +} + +static void stop(double t){ + if(state == ST_STOP || movingstage == STAGE_STOPPED) return; + movingstage = STAGE_DECEL; + state = ST_MOVE; + Times[STAGE_DECEL] = t; + Params[STAGE_DECEL].speed = curparams.speed; + if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel; + else Params[STAGE_DECEL].accel = Max.accel; + Params[STAGE_DECEL].coord = curparams.coord; + // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2 + Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel; + // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2 + double dt = Times[STAGE_STOPPED] - t; + Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt + + Params[STAGE_DECEL].accel * dt * dt / 2.; +} + +/** + * @brief calc - moving calculation + * @param x - using max speed (>0!!!) and coordinate + * @param t - current time value + * @return FALSE if can't move with given parameters + */ +static int calc(moveparam_t *x, double t){ + if(!x) return FALSE; + if(x->coord < Min.coord || x->coord > Max.coord) return FALSE; + if(x->speed < Min.speed || x->speed > Max.speed) return FALSE; + double Dx = fabs(x->coord - curparams.coord); // full distance + double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds + // we have two variants: with or without stage with constant speed + double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed + double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs) + DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23); + double setspeed = x->speed; // new max speed (we can change it if need) + double dt01, dx01; // we'll fill them depending on starting conditions + Times[0] = t; + Params[0].speed = curparams.speed; + Params[0].coord = curparams.coord; + + double curspeed = fabs(curparams.speed); + double dt0s = curspeed / Max.accel; // time of stopping phase + double dx0s = curspeed * dt0s / 2.; // distance + DBG("dt0s=%g, dx0s=%g", dt0s, dx0s); + if(dx0s > Dx){ + WARNX("distance too short"); + return FALSE; + } + if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target + DBG("Distance good to just stop"); + stop(t); + return TRUE; + } + if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign + // after stop we will have full profile + double dxs3 = Dx - dx0s; + double newspeed = sqrt(Max.accel * dxs3); + if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed + DBG("dxs3=%g, setspeed=%g", dxs3, setspeed); + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + Params[0].accel = sign * Max.accel; + if(state == ST_STOP) dx01 = setspeed * dt01 / 2.; + else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed); + DBG("dx01=%g, dt01=%g", dx01, dt01); + }else{ // increase or decrease speed without stopping phase + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + double a = sign * Max.accel; + if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;} + else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;} + //double a = (curspeed > setspeed) ? -Max.accel : Max.accel; + dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; + DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); + if(dx01 + dx23 > Dx){ // calculate max speed + setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.); + if(setspeed < curspeed){ + setspeed = curparams.speed; + dt01 = 0.; dx01 = 0.; + Params[0].accel = 0.; + }else{ + Params[0].accel = a; + dt01 = fabs(setspeed - curspeed) / Max.accel; + dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.; + } + }else Params[0].accel = a; + } + if(setspeed < Min.speed){ + WARNX("New speed should be too small"); + return FALSE; + } + moveparam_t *p = &Params[STAGE_MAXSPEED]; + p->accel = 0.; p->speed = sign * setspeed; + p->coord = curparams.coord + dx01 * sign; + Times[STAGE_MAXSPEED] = Times[0] + dt01; + dt23 = setspeed / Max.accel; + dx23 = setspeed * dt23 / 2.; + // calculate dx12 and dt12 + double dx12 = Dx - dx01 - dx23; + if(dx12 < -coord_tolerance){ + WARNX("Oops, WTF dx12=%g?", dx12); + return FALSE; + } + double dt12 = dx12 / setspeed; + p = &Params[STAGE_DECEL]; + p->accel = -sign * Max.accel; + p->speed = sign * setspeed; + p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12; + Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12; + p = &Params[STAGE_STOPPED]; + p->accel = 0.; p->speed = 0.; p->coord = x->coord; + Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23; + for(int i = 0; i < 4; ++i) + DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i, + Times[i], Params[i].coord, Params[i].speed, Params[i].accel); + state = ST_MOVE; + movingstage = STAGE_ACCEL; + return TRUE; +} + +static movestate_t proc(moveparam_t *next, double t){ + if(state == ST_STOP) goto ret; + for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){ + if(Times[s] <= t){ // check time for current stage + movingstage = s; + break; + } + } + if(movingstage == STAGE_STOPPED){ + curparams.coord = Params[STAGE_STOPPED].coord; + emstop(t); + goto ret; + } + // calculate current parameters + double dt = t - Times[movingstage]; + double a = Params[movingstage].accel; + double v0 = Params[movingstage].speed; + double x0 = Params[movingstage].coord; + curparams.accel = a; + curparams.speed = v0 + a * dt; + curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; +ret: + if(next) *next = curparams; + return state; +} + +static movestate_t getst(moveparam_t *cur){ + if(cur) *cur = curparams; + return state; +} + +static double gettstop(){ + return Times[STAGE_STOPPED]; +} + +movemodel_t trapez = { + .init_limits = initlims, + .stop = stop, + .emergency_stop = emstop, + .get_state = getst, + .calculate = calc, + .proc_move = proc, + .stoppedtime = gettstop, +}; diff --git a/LibSidServo/PID_test/Tramp.h b/LibSidServo/PID_test/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/LibSidServo/PID_test/Tramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t trapez; diff --git a/LibSidServo/PID_test/main.c b/LibSidServo/PID_test/main.c new file mode 100644 index 0000000..d1b3c0f --- /dev/null +++ b/LibSidServo/PID_test/main.c @@ -0,0 +1,222 @@ +/* + * This file is part of the moving_model 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 "moving.h" +#include "PID.h" + +// errors for states: slewing/pointing/guiding +// 10-degrees zone - Coordinate-driven PID +#define MAX_POINTING_ERR (36000.) +// 1-arcminute zone - Velocity-dtiven PID +#define MAX_GUIDING_ERR (60.) +// timeout to "forget" old data from I sum array; seconds +#define PID_I_PERIOD (3.) + +// PID for coordinate-driven and velocity-driven parts +static PIDController_t *pidC = NULL, *pidV = NULL; +static movemodel_t *model = NULL; +static FILE *coordslog = NULL; + +typedef enum{ + Slewing, + Pointing, + Guiding +} state_t; + +static state_t state = Slewing; + +typedef struct{ + int help; + char *ramptype; + char *xlog; + double dTmon; + double dTcorr; + double Tend; + double minerr; + double startcoord; + double error; + PIDpar_t gainC, gainV; +} pars; + +static pars G = { + .dTmon = 0.01, + .dTcorr = 0.05, + .Tend = 100., + .minerr = 0.1, + .gainC.P = 0.1, + .gainV.P = 0.1, + .startcoord = 100., +}; + +static limits_t limits = { + .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1}, + .max = {.coord = 6648000, .speed = 36000., .accel = 36000.} +}; + + +static sl_option_t opts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"}, + {"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"}, + {"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"}, + {"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"}, + {"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"}, + {"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"}, + {"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"}, + {"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"}, + {"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"}, + {"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"}, + {"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"}, + {"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"}, + {"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"}, + // TODO: add parameters for limits setting + end_option +}; + +// calculate coordinate target for given time (starting from zero) +static double target_coord(double t){ + if(t > 20. && t < 30.) return 0.; + //double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5); + double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5); + return pos; +} + +static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ + double error = targcoord - p->coord, fe = fabs(error); + PIDController_t *pid = NULL; + switch(state){ + case Slewing: + if(fe < MAX_POINTING_ERR){ + pid_clear(pidC); + state = Pointing; + green("--> Pointing\n"); + pid = pidC; + }else{ + red("Slewing...\n"); + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } + break; + case Pointing: + if(fe < MAX_GUIDING_ERR){ + pid_clear(pidV); + state = Guiding; + green("--> Guiding\n"); + pid = pidV; + }else if(fe > MAX_POINTING_ERR){ + red("--> Slewing\n"); + state = Slewing; + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } else pid = pidC; + break; + case Guiding: + pid= pidV; + if(fe > MAX_GUIDING_ERR){ + red("--> Pointing\n"); + state = Pointing; + pid_clear(pidC); + pid = pidC; + }else if(fe < G.minerr){ + green("At target\n"); + }else printf("Current error: %g\n", fe); + break; + } + if(!pid){ + WARNX("where is PID?"); return p->speed; + } + double tagspeed = pid_calculate(pid, error, dt); + if(state == Guiding) return p->speed + tagspeed; + return tagspeed; +} +// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02 +// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1 + +static void start_model(double Tend){ + double T = 0., Tcorr = 0.; + moveparam_t target; + uint64_t N = 0; + double errmax = 0., errsum = 0., errsum2 = 0.; + while(T <= Tend){ + moveparam_t p; + movestate_t st = model->get_state(&p); + if(st == ST_MOVE) st = model->proc_move(&p, T); + double nextcoord = target_coord(T); + double error = nextcoord - p.coord; + if(state == Guiding){ + double ae = fabs(error); + if(ae > errmax) errmax = ae; + errsum += error; errsum2 += error * error; + ++N; + } + if(T - Tcorr >= G.dTcorr){ // check correction + double speed = getNewSpeed(&p, nextcoord, T - Tcorr); + target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5; + target.speed = fabs(speed); + double res_speed = limits.max.speed / 2.; + if(target.speed > limits.max.speed){ + target.speed = limits.max.speed; + res_speed = limits.max.speed / 4.; + }else if(target.speed < limits.min.speed){ + target.speed = limits.min.speed; + res_speed = limits.min.speed * 4.; + } + if(!move_to(&target, T)){ + target.speed = res_speed; + if(!move_to(&target, T)) + WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed); + } + DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error); + Tcorr = T; + } + // make log + fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n", + T, nextcoord, p.coord, p.speed, p.accel, error); + T += G.dTmon; + } + printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n"); + double mean = errsum / (double)N; + printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean)); +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, opts); + if(G.help) sl_showhelp(-1, opts); + if(G.xlog){ + coordslog = fopen(G.xlog, "w"); + if(!coordslog) ERR("Can't open %s", G.xlog); + } else coordslog = stdout; + if(G.dTmon <= 0.) ERRX("tmon should be > 0."); + if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1."); + if(G.Tend <= 0.) ERRX("tend should be > 0."); + pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr); + pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr); + if(!pidC || !pidV) ERRX("Can't init PID regulators"); + model = init_moving(&limits); + if(!model) ERRX("Can't init moving model: check parameters"); + fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error"); + start_model(G.Tend); + pid_delete(&pidC); + pid_delete(&pidV); + fclose(coordslog); + return 0; +} diff --git a/LibSidServo/PID_test/moving.c b/LibSidServo/PID_test/moving.c new file mode 100644 index 0000000..4b77040 --- /dev/null +++ b/LibSidServo/PID_test/moving.c @@ -0,0 +1,89 @@ +/* + * This file is part of the moving_model 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 "moving.h" +#include "moving_private.h" +#include "Tramp.h" + +static movemodel_t *model = &trapez; +double coord_tolerance = COORD_TOLERANCE_DEFAULT; +double time_tick = TIME_TICK_DEFAULT; + +// difference of time from first call, using nanoseconds +double nanot(){ + static struct timespec *start = NULL; + struct timespec now; + if(!start){ + start = MALLOC(struct timespec, 1); + if(!start) return -1.; + if(clock_gettime(CLOCK_REALTIME, start)) return -1.; + } + if(clock_gettime(CLOCK_REALTIME, &now)) return -1.; + //DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec); + double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; + double sd = (double)now.tv_sec - (double)start->tv_sec; + return sd + nd; +} + +static void chkminmax(double *min, double *max){ + if(*min <= *max) return; + double t = *min; + *min = *max; + *max = t; +} + +movemodel_t *init_moving(limits_t *l){ + if(!l) return FALSE; + if(!model->init_limits) return NULL; + moveparam_t *max = &l->max, *min = &l->min; + if(min->speed < 0.) min->speed = -min->speed; + if(max->speed < 0.) max->speed = -max->speed; + if(min->accel < 0.) min->accel = -min->accel; + if(max->accel < 0.) max->accel = -max->accel; + chkminmax(&min->coord, &max->coord); + chkminmax(&min->speed, &max->speed); + chkminmax(&min->accel, &max->accel); + if(!model->init_limits(l)) return NULL; + return model; +} + +int move_to(moveparam_t *target, double t){ + if(!target || !model) return FALSE; + DBG("MOVE to %g at speed %g", target->coord, target->speed); + // only positive velocity + if(target->speed < 0.) target->speed = -target->speed; + // don't mind about acceleration - user cannot set it now + return model->calculate(target, t); +} + +int init_coordtol(double tolerance){ + if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE; + coord_tolerance = tolerance; + return TRUE; +} +int init_timetick(double tick){ + if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE; + time_tick = tick; + return TRUE; +} diff --git a/LibSidServo/PID_test/moving.h b/LibSidServo/PID_test/moving.h new file mode 100644 index 0000000..eb77824 --- /dev/null +++ b/LibSidServo/PID_test/moving.h @@ -0,0 +1,62 @@ +/* + * This file is part of the moving_model 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 + +// tolerance, time ticks +#define COORD_TOLERANCE_DEFAULT (0.01) +#define COORD_TOLERANCE_MIN (0.0001) +#define COORD_TOLERANCE_MAX (10.) +#define TIME_TICK_DEFAULT (0.0001) +#define TIME_TICK_MIN (1e-9) +#define TIME_TICK_MAX (10.) + +typedef enum{ + ST_STOP, // stopped + ST_MOVE, // moving + ST_AMOUNT +} movestate_t; + +typedef struct{ // all values could be both as positive and negative + double coord; + double speed; + double accel; +} moveparam_t; + +typedef struct{ + moveparam_t min; + moveparam_t max; +} limits_t; + +typedef struct{ + int (*init_limits)(limits_t *lim); // init values of limits, jerk + int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(moveparam_t *cur); // get current moving state + void (*stop)(double t); // stop by ramp + void (*emergency_stop)(double t); // stop with highest acceleration + double (*stoppedtime)(); // time when moving will ends +} movemodel_t; + +extern double coord_tolerance; + +double nanot(); +movemodel_t *init_moving(limits_t *l); +int init_coordtol(double tolerance); +int init_timetick(double tick); +int move_to(moveparam_t *target, double t); diff --git a/LibSidServo/PID_test/moving_model.cflags b/LibSidServo/PID_test/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/LibSidServo/PID_test/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/LibSidServo/PID_test/moving_model.config b/LibSidServo/PID_test/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/LibSidServo/PID_test/moving_model.config @@ -0,0 +1,4 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/LibSidServo/PID_test/moving_model.creator b/LibSidServo/PID_test/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/LibSidServo/PID_test/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/LibSidServo/PID_test/moving_model.creator.user b/LibSidServo/PID_test/moving_model.creator.user new file mode 100644 index 0000000..eaf30ec --- /dev/null +++ b/LibSidServo/PID_test/moving_model.creator.user @@ -0,0 +1,221 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 1 + 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 + Builtin.DefaultTidyAndClazy + 1 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/C-files/mountcontrol.git/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + 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/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 b/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + 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 + 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/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + 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/LibSidServo/PID_test/moving_model.cxxflags b/LibSidServo/PID_test/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/LibSidServo/PID_test/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/LibSidServo/PID_test/moving_model.files b/LibSidServo/PID_test/moving_model.files new file mode 100644 index 0000000..0086d29 --- /dev/null +++ b/LibSidServo/PID_test/moving_model.files @@ -0,0 +1,12 @@ +Dramp.c +Dramp.h +PID.c +PID.h +Sramp.c +Sramp.h +Tramp.c +Tramp.h +main.c +moving.c +moving.h +moving_private.h diff --git a/LibSidServo/PID_test/moving_model.includes b/LibSidServo/PID_test/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/LibSidServo/PID_test/moving_private.h b/LibSidServo/PID_test/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/LibSidServo/PID_test/moving_private.h @@ -0,0 +1,26 @@ +/* + * This file is part of the moving_model 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 "moving.h" + +extern double coord_tolerance; +extern double time_tick; + + diff --git a/LibSidServo/PID_test/plot b/LibSidServo/PID_test/plot new file mode 100755 index 0000000..476e1f3 --- /dev/null +++ b/LibSidServo/PID_test/plot @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test/plot.cont b/LibSidServo/PID_test/plot.cont new file mode 100755 index 0000000..16dc0f5 --- /dev/null +++ b/LibSidServo/PID_test/plot.cont @@ -0,0 +1,8 @@ +#!/usr/bin/gnuplot + +#set term pdf +#set output "output.pdf" +while(1){ + plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader + pause 1 +} \ No newline at end of file diff --git a/LibSidServo/PID_test/plot_jpg b/LibSidServo/PID_test/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/LibSidServo/PID_test/plot_jpg @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +set terminal jpeg size 1000,500 +set output "all.jpg" + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader diff --git a/LibSidServo/PID_test/plot_pdf b/LibSidServo/PID_test/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/LibSidServo/PID_test/plot_pdf @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term pdf +set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader diff --git a/LibSidServo/PID_test/plotacc b/LibSidServo/PID_test/plotacc new file mode 100755 index 0000000..f15faaa --- /dev/null +++ b/LibSidServo/PID_test/plotacc @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:5 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test/ploterr b/LibSidServo/PID_test/ploterr new file mode 100755 index 0000000..d2b990d --- /dev/null +++ b/LibSidServo/PID_test/ploterr @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:6 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test/ploterr.cont b/LibSidServo/PID_test/ploterr.cont new file mode 100755 index 0000000..95bcd4b --- /dev/null +++ b/LibSidServo/PID_test/ploterr.cont @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +while(1){ + plot 'coords' using 1:6 with lines title columnheader + pause 1 +} diff --git a/LibSidServo/PID_test/ploterr_jpg b/LibSidServo/PID_test/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/LibSidServo/PID_test/ploterr_jpg @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term jpeg size 1000,500 +set output "error.jpg" +plot 'coords' using 1:6 with lines title columnheader diff --git a/LibSidServo/TODO b/LibSidServo/TODO new file mode 100644 index 0000000..1d2dcb7 --- /dev/null +++ b/LibSidServo/TODO @@ -0,0 +1,3 @@ +fix encoders opening for several tries +encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request) +Read HW config even in model mode diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt new file mode 100644 index 0000000..5afbbd6 --- /dev/null +++ b/LibSidServo/examples/CMakeLists.txt @@ -0,0 +1,14 @@ +project(examples) + +# common includes & library +include_directories(../) +link_libraries(sidservo usefull_macros -lm) + +# exe list +add_executable(goto goto.c dump.c conf.c) +add_executable(dump dumpmoving.c dump.c conf.c) +add_executable(dump_s dumpmoving_scmd.c dump.c conf.c) +add_executable(dumpswing dumpswing.c dump.c conf.c) +add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c) +add_executable(SSIIconf SSIIconf.c conf.c) +add_executable(slewNtrack dumpmoving_dragNtrack.c dump.c conf.c) diff --git a/LibSidServo/examples/Readme.md b/LibSidServo/examples/Readme.md new file mode 100644 index 0000000..6a7e1dc --- /dev/null +++ b/LibSidServo/examples/Readme.md @@ -0,0 +1,28 @@ +Some examples of usage of libsidservo +===================================== + +## Auxiliary files + +*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes + +*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero. + +*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero. + +*simpleconv.h* + + +## Examples + +*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y". + +*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command. + +*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis. + +*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands). + +*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands. + +*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller + diff --git a/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c new file mode 100644 index 0000000..7980878 --- /dev/null +++ b/LibSidServo/examples/SSIIconf.c @@ -0,0 +1,163 @@ +/* + * 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 "conf.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int helpargs; + int writeconf; + char *conffile; + char *hwconffile; +} parameters; + +static hardware_configuration_t HW = {0}; + +static parameters G = { + .conffile = "servo.conf", +}; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"help-opts", NO_ARGS, NULL, 'H', arg_int, APTR(&G.helpargs), "configuration help"}, + {"serconf", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "serial configuration file name"}, + {"hwconf", NEED_ARG, NULL, 'i', arg_string, APTR(&G.hwconffile),"SSII configuration file name"}, + {"writeconf", NO_ARGS, NULL, 0, arg_int, APTR(&G.writeconf), "write configuration (BE CAREFUL!)"}, + end_option +}; + +static sl_option_t confopts[] = { + {"Xaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Xconf.accel), "X Default Acceleration, rad/s^2"}, + {"Yaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Yconf.accel), "Y Default Acceleration, rad/s^2"}, + end_option +}; + +static void dumpaxis(char axis, axis_config_t *c){ +#define STRUCTPAR(p) (c)->p +#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0) +#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0) + DUMPD(accel); + DUMPD(backlash); + DUMPD(errlimit); + DUMP(propgain); + DUMP(intgain); + DUMP(derivgain); + DUMP(outplimit); + DUMP(currlimit); + DUMP(intlimit); + DUMP(motor_stepsperrev); + DUMP(axis_stepsperrev); +#undef DUMP +#undef DUMPD +} + +static void dumpxbits(xbits_t *c){ +#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0) + DUMPBIT(motrev); + DUMPBIT(motpolarity); + DUMPBIT(encrev); + DUMPBIT(dragtrack); + DUMPBIT(trackplat); + DUMPBIT(handpaden); + DUMPBIT(newpad); + DUMPBIT(guidemode); +#undef DUMPBIT +} + +static void dumpybits(ybits_t *c){ +#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0) + DUMPBIT(motrev); + DUMPBIT(motpolarity); + DUMPBIT(encrev); + DUMPBIT(slewtrack); + DUMPBIT(digin_sens); + printf("Ydigin=%d\n", c->digin); +#undef DUMPBIT +} + +static void dumpHWconf(){ +#undef STRUCTPAR +#define STRUCTPAR(p) (HW).p +#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0) +#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0) +#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0) +#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0) + green("X axis configuration:\n"); + dumpaxis('X', &HW.Xconf); + green("X bits:\n"); + dumpxbits(&HW.xbits); + green("Y axis configuration:\n"); + dumpaxis('Y', &HW.Yconf); + green("Y bits:\n"); + dumpybits(&HW.ybits); + green("Other:\n"); + printf("address=%d\n", HW.address); + DUMP(eqrate); + DUMP(eqadj); + DUMP(trackgoal); + DUMPD(latitude); + DUMPU32(Xsetpr); + DUMPU32(Ysetpr); + DUMPU32(Xmetpr); + DUMPU32(Ymetpr); + DUMPD(Xslewrate); + DUMPD(Yslewrate); + DUMPD(Xpanrate); + DUMPD(Ypanrate); + DUMPD(Xguiderate); + DUMPD(Yguiderate); + DUMPU32(baudrate); + DUMPD(locsdeg); + DUMPD(locsspeed); + DUMPD(backlspd); +} + +int main(int argc, char** argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) + sl_showhelp(-1, cmdlnopts); + if(G.helpargs) + sl_showhelp(-1, confopts); + conf_t *sconf = readServoConf(G.conffile); + if(!sconf){ + dumpConf(); + return 1; + } + if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount"); + if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration"); + /* + char *c = sl_print_opts(confopts, TRUE); + green("Got configuration:\n"); + printf("%s\n", c); + FREE(c); + */ + dumpHWconf(); + /* + if(G.hwconffile && G.writeconf){ + ; + }*/ + Mount.quit(); + return 0; +} diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c new file mode 100644 index 0000000..8a77322 --- /dev/null +++ b/LibSidServo/examples/conf.c @@ -0,0 +1,128 @@ +/* + * 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 "conf.h" + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + .EncoderXDevPath = "/dev/encoder_X0", + .EncoderYDevPath = "/dev/encoder_Y0", + .EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .EncoderReqInterval = 0.001, + .SepEncoder = 2, + .EncoderSpeedInterval = 0.05, + .EncodersDisagreement = 1e-5, // 2'' + .PIDMaxDt = 1., + .PIDRefreshDt = 0.1, + .PIDCycleDt = 5., + .XPIDC.P = 0.5, + .XPIDC.I = 0.1, + .XPIDC.D = 0.2, + .XPIDV.P = 0.09, + .XPIDV.I = 0.0, + .XPIDV.D = 0.05, + .YPIDC.P = 0.5, + .YPIDC.I = 0.1, + .YPIDC.D = 0.2, + .YPIDV.P = 0.09, + .YPIDV.I = 0.0, + .YPIDV.D = 0.05, + .MaxPointingErr = 0.13962634, + .MaxFinePointingErr = 0.026179939, + .MaxGuidingErr = 4.8481368e-7, +}; + +static sl_option_t opts[] = { + {"MountDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.MountDevPath), "path to mount device"}, + {"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"}, + {"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"}, + {"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"}, + {"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"}, + {"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"}, + {"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"}, + {"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"}, + {"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"}, + {"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"}, + {"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"}, + {"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"}, + {"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"}, + {"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"}, + {"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"}, + {"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"}, + {"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"}, + {"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"}, + {"YPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.P), "P of Y PID (coordinate driven)"}, + {"YPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.I), "I of Y PID (coordinate driven)"}, + {"YPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.D), "D of Y PID (coordinate driven)"}, + {"XPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.P), "P of X PID (velocity driven)"}, + {"XPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.I), "I of X PID (velocity driven)"}, + {"XPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.D), "D of X PID (velocity driven)"}, + {"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"}, + {"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"}, + {"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"}, + {"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"}, + {"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"}, + {"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"}, + {"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"}, + {"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"}, + // {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""}, + end_option +}; + +conf_t *readServoConf(const char *filename){ + if(!filename) filename = DEFCONFFILE; + int n = sl_conf_readopts(filename, opts); + if(n < 0){ + WARNX("Can't read file %s", filename); + return NULL; + } + if(n == 0){ + WARNX("Got ZERO parameters from %s", filename); + return NULL; + } + return &Config; +} + +void dumpConf(){ + char *c = sl_print_opts(opts, TRUE); + printf("Current configuration:\n%s\n", c); + FREE(c); +} + +void confHelp(){ + sl_conf_showhelp(-1, opts); +} + +const char* errcodes[MCC_E_AMOUNT] = { + [MCC_E_OK] = "OK", + [MCC_E_FATAL] = "Fatal error", + [MCC_E_BADFORMAT] = "Wrong data format", + [MCC_E_ENCODERDEV] = "Encoder error", + [MCC_E_MOUNTDEV] = "Mount error", + [MCC_E_FAILED] = "Failed to run" +}; +// return string with error code +const char *EcodeStr(mcc_errcodes_t e){ + if(e >= MCC_E_AMOUNT) return "Wrong error code"; + return errcodes[e]; +} diff --git a/LibSidServo/examples/conf.h b/LibSidServo/examples/conf.h new file mode 100644 index 0000000..062528e --- /dev/null +++ b/LibSidServo/examples/conf.h @@ -0,0 +1,28 @@ +/* + * 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" + +#define DEFCONFFILE "servo.conf" + +void confHelp(); +conf_t *readServoConf(const char *filename); +void dumpConf(); +const char *EcodeStr(mcc_errcodes_t e); diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c new file mode 100644 index 0000000..de111ed --- /dev/null +++ b/LibSidServo/examples/dump.c @@ -0,0 +1,197 @@ +/* + * 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" + +// starting dump time (to conform different logs) +static struct timespec dumpT0 = {0}; + +#if 0 +// amount of elements used for encoders' data filtering +#define NFILT (10) + +static double filterK[NFILT]; +static double lastvals[2][NFILT] = {0}; +static int need2buildFilter = 1; + +static void buildFilter(){ + filterK[NFILT-1] = 1.; + double sum = 1.; + for(int i = NFILT-2; i > -1; --i){ + filterK[i] = (filterK[i+1] + 1.) * 1.1; + sum += filterK[i]; + } + for(int i = 0; i < NFILT; ++i) filterK[i] /= sum; +} + +static double filter(double val, int idx){ + if(need2buildFilter){ + buildFilter(); + need2buildFilter = 0; + } + static int ctr[2] = {0}; + for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1]; + lastvals[idx][0] = val; + double r = 0.; + if(ctr[idx] < NFILT){ + ++ctr[idx]; + return val; + } + for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i]; + return r; +} +#endif + +// return starting time of dump +void dumpt0(struct timespec *t){ + if(t) *t = dumpT0; +} + + +/** + * @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"); + if(!m){ // write header + fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); + return; + }else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t; + // write data + fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n", + Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val), + RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val), + RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val), + m->millis); + 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 mdmillis = mdata.millis; + struct timespec encXt = mdata.encXposition.t; + int ctr = -1; + double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val; + double t0 = Mount.timeFromStart(); + while(Mount.timeFromStart() - t0 < t && ctr < N){ + usleep(1000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + //double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.; + struct timespec msrt = mdata.encXposition.t; + if(msrt.tv_nsec == encXt.tv_nsec) continue; + encXt = msrt; + if(fcoords) logmnt(fcoords, &mdata); + if(mdata.millis == mdmillis) continue; + //DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val); + mdmillis = mdata.millis; + if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ + xlast = mdata.motXposition.val; + ylast = mdata.motYposition.val; + ctr = 0; + }else ++ctr; + } + DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0); +} + +/** + * @brief waitmoving - wait until moving by both axiss 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.; + DBG("Wait moving for %d stopped times", N); + 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.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0; + else ++ctr; + } +} + +/** + * @brief getPos - get current position + * @param mot (o) - motor position (or NULL) + * @param Y (o) - encoder position (or NULL) + * @return FALSE if failed + */ +int getPos(coordval_pair_t *mot, coordval_pair_t *enc){ + mountdata_t mdata = {0}; + 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->X = mdata.motXposition; + mot->Y = mdata.motYposition; + } + if(enc){ + enc->X = mdata.encXposition; + enc->Y = mdata.encYposition; + } + return TRUE; +} + +// check current position and go to 0 if non-zero +void chk0(int ncycles){ + coordval_pair_t M; + if(!getPos(&M, NULL)) signals(2); + if(M.X.val || M.Y.val){ + WARNX("Mount position isn't @ zero; moving"); + coordpair_t zero = {0., 0.}; + Mount.moveTo(&zero); + waitmoving(ncycles); + green("Now mount @ zero\n"); + } +} diff --git a/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h new file mode 100644 index 0000000..0731f2e --- /dev/null +++ b/LibSidServo/examples/dump.h @@ -0,0 +1,30 @@ +/* + * 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(coordval_pair_t *mot, coordval_pair_t *enc); +void chk0(int ncycles); +void dumpt0(struct timespec *t); diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c new file mode 100644 index 0000000..9b0c111 --- /dev/null +++ b/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 "conf.h" +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int verbose; + int Ncycles; + char *logfile; + char *coordsoutput; + char *conffile; +} 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"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + 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); +} + +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); + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + confHelp(); + return 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); + if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices"); + coordval_pair_t M; + if(!getPos(&M, NULL)) ERRX("Can't get current position"); + 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 + coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val}; + if(MCC_E_OK != Mount.moveTo(&tag)) + ERRX("Can't move to 45, 45"); + dumpmoving(fcoords, 30., G.Ncycles); + tag.X = M.X.val; tag.Y = M.Y.val; + Mount.moveTo(&tag); + dumpmoving(fcoords, 30., G.Ncycles); + signals(0); + return 0; +} diff --git a/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c new file mode 100644 index 0000000..07d5b71 --- /dev/null +++ b/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -0,0 +1,228 @@ +/* + * 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 target using short command and force it to track mode +// also do some corrections while moving + +#include +#include +#include +#include +#include +#include +#include + +#include "conf.h" +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +// Original XXI=6827 +// XXD=136546 +// XXB=4915666 + +typedef struct{ + int help; + int Ncycles; + double reqint; + char *coordsoutput; + char *conffile; +} parameters; + +static parameters G = { + .Ncycles = 40, + .reqint = -1., +}; + +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)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + end_option +}; + +static mcc_errcodes_t return2zero(); + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + return2zero(); + sleep(5); + Mount.quit(); + exit(sig); +} + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// return TRUE if motor position is reached +- 0.01 degrees +#define XYcount (DEG2RAD(0.3)) +// tag in degrees! +static int Wait(double tag, int isX){ + mountdata_t mdata; + red("Wait for %g degrees\n", tag); + tag = DEG2RAD(tag); + int errcnt = 0; + uint32_t millis = 0; + double curpos = 0.; + double t0 = sl_dtime(); + do{ + if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; + else{ + errcnt = 0; + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(isX) curpos = mdata.motXposition.val; + else curpos = mdata.motYposition.val; + } + double t = sl_dtime(); + if(t - t0 > 1.){ + t0 = t; + printf("\t\tCurrent MOT X/Y: %g / %g deg\n", RAD2DEG(mdata.motXposition.val), + RAD2DEG(mdata.motYposition.val)); + } + }while(fabs(curpos - tag) > XYcount && errcnt < 10); + if(errcnt >= 10){ + WARNX("Too much errors"); + return FALSE; + } + green("%s reached position %g degrees\n", (isX) ? "X" : "Y", RAD2DEG(tag)); + fflush(stdout); + return TRUE; +} + +// previous GOTO coords/speeds for `mkcorr` +static coordpair_t lastTag = {0}, lastSpeed = {0}; + +// slew to given position and start tracking +// pos/speed in deg and deg/s +static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){ + short_command_t cmd = {0}; + DBG("Try to move to (%g, %g) with speed (%g, %g)", + target->X, target->Y, speed->X, speed->Y); + cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y); + cmd.Xspeed = DEG2RAD(speed->X); + cmd.Yspeed = DEG2RAD(speed->Y); + lastTag = *target; + lastSpeed = *speed; + /*cmd.xychange = 1; + cmd.XBits = 108; + cmd.YBits = 28;*/ + return Mount.shortCmd(&cmd); +} + +static mcc_errcodes_t return2zero(){ + short_command_t cmd = {0}; + DBG("Try to move to zero"); + cmd.Xmot = 0.; cmd.Ymot = 0.; + coordpair_t maxspd; + if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED; + cmd.Xspeed = maxspd.X; + cmd.Yspeed = maxspd.Y; + /*cmd.xychange = 1; + cmd.XBits = 100; + cmd.YBits = 20;*/ + return Mount.shortCmd(&cmd); +} + +static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){ + long_command_t cmd = {0}; + cmd.Xspeed = DEG2RAD(lastSpeed.X); + cmd.Yspeed = DEG2RAD(lastSpeed.Y); + cmd.Xmot = DEG2RAD(lastTag.X); + cmd.Ymot = DEG2RAD(lastTag.Y); + cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y); + cmd.Xatime = time->X; cmd.Yatime = time->Y; + return Mount.longCmd(&cmd); +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + if(G.reqint > 0.) Config->MountReqInterval = G.reqint; + if(MCC_E_OK != Mount.init(Config)){ + WARNX("Can't init devices"); + return 1; + } + //if(!getPos(&M, NULL)) ERRX("Can't get current position"); + 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; + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + // move to 10/10 + coordpair_t coords, speeds, adders, tadd; + coords = (coordpair_t){.X = 10., .Y = 20.}; + speeds = (coordpair_t){.X = 1., .Y = 2.}; + adders = (coordpair_t){.X = 0.01, .Y = 0.01}; + tadd = (coordpair_t){.X = 1., .Y = 2.}; + green("Goto\n"); + if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go"); + DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y); + green("Waiting X==4\n"); + Wait(4., 1); + // now we are at point by Y but still moving by X; make small correction by X/Y into '+' + green("Mkcorr 1\n"); + if(MCC_E_OK != mkcorr(&adders, &tadd)) ERRX("Can't make corr"); + green("Waiting X==6\n"); + Wait(6., 1); + green("Goto more\n"); + coords = (coordpair_t){.X = 20., .Y = 30.}; + if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go"); + DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y); + green("Waiting Y==14\n"); + Wait(14., 0); + // now we are @ point, make the same small correction again + green("Mkcorr 2\n"); + if(MCC_E_OK != mkcorr(&coords, &speeds)) ERRX("Can't make corr"); + // wait for 5 seconds + green("Wait for 5 seconds\n"); + sleep(5); + // return to zero and wait + green("Return 2 zero and wait\n"); + if(MCC_E_OK != return2zero()) ERRX("Can't return"); + Wait(0., 0); + Wait(0., 1); + // wait moving ends + pthread_join(dthr, NULL); + signals(0); + return 0; +} diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c new file mode 100644 index 0000000..f16ee4b --- /dev/null +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -0,0 +1,177 @@ +/* + * 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 "conf.h" +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int Ncycles; + int relative; + double reqint; + char *coordsoutput; + char *conffile; + char *axis; +} parameters; + +static parameters G = { + .Ncycles = 40, + .reqint = -1., + .axis = "X", +}; + +static FILE *fcoords = NULL; + +static coordval_pair_t M; + +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, Y or B for both)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// return TRUE if motor position is reached +- 0.01 degrees +#define XYcount (DEG2RAD(0.01)) +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.motXposition.val; + else curpos = mdata.motYposition.val; + 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)); + fflush(stdout); + return TRUE; +} + +// move X/Y to 40 degr with given speed until given coord +static void move(double target, double limit, double speed){ + 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' || *G.axis == 'B'){ + cmd.Xmot = DEG2RAD(target) + M.X.val; + cmd.Xspeed = DEG2RAD(speed); + limit = DEG2RAD(limit) + M.X.val; + } + if(*G.axis == 'Y' || *G.axis == 'B'){ + cmd.Ymot = DEG2RAD(target) + M.Y.val; + cmd.Yspeed = DEG2RAD(speed); + if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val; + } + if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); + if(!Wait(limit)) signals(9); +} + + +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") && strcmp(G.axis, "B")){ + WARNX("\"Axis\" should be X, Y or B"); + return 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + if(G.reqint > 0.) Config->MountReqInterval = G.reqint; + if(MCC_E_OK != Mount.init(Config)){ + WARNX("Can't init devices"); + return 1; + } + if(!getPos(&M, NULL)) ERRX("Can't get current position"); + 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; + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + // goto 30' with 5'/s + move(10., 30./60., 5./60.); + // goto 1' with 10'/s + move(10., 1., 10./60.); + // goto 3degr with 15'/s + move(10., 3., 15./60.); + // and go back with 7deg/s + move(0., 0., 7.); + // be sure to move @ starting position + coordpair_t tag = {.X = M.X.val, .Y = M.Y.val}; + Mount.moveTo(&tag); + // wait moving ends + pthread_join(dthr, NULL); + signals(0); + return 0; +} diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c new file mode 100644 index 0000000..4d4b51f --- /dev/null +++ b/LibSidServo/examples/dumpswing.c @@ -0,0 +1,190 @@ +/* + * 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 "conf.h" +#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 *conffile; + 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 reached if period is too small): [-45:45]deg (default: 5)"}, + {"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +// 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(Mount.timeFromStart() >= 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.motXposition.val != xlast || mdata.motYposition.val != ylast){ + //DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); + xlast = mdata.motXposition.val; + ylast = mdata.motYposition.val; + ctr = 0; + }else{ + //DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); + ++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"))){ + WARNX("Can't open %s", G.coordsoutput); + return 1; + } + }else fcoords = stdout; + if(G.Ncycles < 2){ + WARNX("Ncycles should be >2"); + return 1; + } + double absamp = fabs(G.amplitude); + if(absamp < 0.01 || absamp > 45.){ + WARNX("Amplitude should be from 0.01 to 45 degrees"); + return 1; + } + if(G.period < 0.1 || G.period > 900.){ + WARNX("Period should be from 0.1 to 900s"); + return 1; + } + if(G.Nswings < 1){ + WARNX("Nswings should be more than 0"); + return 1; + } + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + 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 + 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 = Mount.timeFromStart(), t0 = t; + coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY}; + double divide = 2.; + for(int i = 0; i < G.Nswings; ++i){ + Mount.moveTo(&tag); + DBG("CMD: %g", Mount.timeFromStart()-t0); + t += G.period / divide; + divide = 1.; + waithalf(t); + DBG("Moved to +, t=%g", t-t0); + DBG("CMD: %g", Mount.timeFromStart()-t0); + Mount.moveTo(&rtag); + t += G.period; + waithalf(t); + DBG("Moved to -, t=%g", t-t0); + DBG("CMD: %g", Mount.timeFromStart()-t0); + } + green("Move to zero @ %g\n", Mount.timeFromStart()); + tag = (coordpair_t){0}; + // be sure to move @ 0,0 + if(MCC_E_OK != Mount.moveTo(&tag)){ + Mount.emergStop(); + Mount.moveTo(&tag); + } + // wait moving ends + pthread_join(dthr, NULL); +#undef SCMD + signals(0); + return 0; +} diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c new file mode 100644 index 0000000..b17deab --- /dev/null +++ b/LibSidServo/examples/goto.c @@ -0,0 +1,145 @@ +/* + * 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 "conf.h" +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int Ncycles; + int wait; + int relative; + char *coordsoutput; + char *conffile; + double X; + double Y; +} parameters; + +static parameters G = { + .Ncycles = 10, + .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"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + end_option +}; + +static FILE* fcoords = NULL; +static pthread_t dthr; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + DBG("Quit"); + Mount.quit(); + DBG("close"); + 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); + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); + coordval_pair_t M, E; + if(!getPos(&M, &E)) ERRX("Can't get current position"); + printf("Current time: %.10f\n", Mount.timeFromStart()); + if(G.coordsoutput){ + if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n"); + G.wait = 1; + 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"); + } + M.X.val = RAD2DEG(M.X.val); + M.Y.val = RAD2DEG(M.Y.val); + printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val, + RAD2DEG(E.X.val), RAD2DEG(E.Y.val)); + if(isnan(G.X) && isnan(G.Y)) goto out; + coordpair_t tag; + if(isnan(G.X)){ + if(G.relative) G.X = 0.; + else G.X = M.X.val; + } + if(isnan(G.Y)){ + if(G.relative) G.Y = 0.; + else G.Y = M.Y.val; + } + if(G.relative){ + G.X += M.X.val; + G.Y += M.Y.val; + } + printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y); + tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y); + mcc_errcodes_t e = Mount.moveTo(&tag); + if(MCC_E_OK != e){ + WARNX("Cant go to given coordinates: %s\n", EcodeStr(e)); + goto out; + } + 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.val), RAD2DEG(M.Y.val)); + } +out: + DBG("JOIN"); + if(G.coordsoutput) pthread_join(dthr, NULL); + DBG("QUIT"); + if(G.wait){ + usleep(250000); // pause to refresh coordinates + if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val), + RAD2DEG(E.X.val), RAD2DEG(E.Y.val)); + Mount.quit(); + } + return 0; +} diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c new file mode 100644 index 0000000..67ad655 --- /dev/null +++ b/LibSidServo/examples/scmd_traectory.c @@ -0,0 +1,188 @@ +/* + * 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 "conf.h" +#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 dumpconf; + 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 *errlog; // log with position errors + char *tfn; // traectory function name + char *conffile; +} parameters; + +static conf_t *Config = NULL; +static FILE *fcoords = NULL, *errlog = 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 abs X coordinate for traectory (default: 45 degrees)"}, + {"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y 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)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + {"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"}, + {"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.stop(); + sleep(1); + 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; + coordval_pair_t telXY; + coordval_pair_t target; + coordpair_t traectXY; + double tlast = 0., tstart = Mount.timeFromStart(); + long tlastXnsec = 0, tlastYnsec = 0; + struct timespec tcur, t0 = {0}; + dumpt0(&t0); + while(1){ + if(!telpos(&telXY)){ + WARNX("No next telescope position"); + return; + } + if(!Mount.currentT(&tcur)) continue; + if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind + DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val)); + tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec; + double t = Mount.timeFromStart(); + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break; + if(!traectory_point(&traectXY, t)) break; + target.X.val = traectXY.X; target.Y.val = traectXY.Y; + target.X.t = target.Y.t = tcur; + if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0); + else{ + //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); + DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); + if(errlog) + fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + } + if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!"); + while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500); + tlast = t; + } + WARNX("No next traectory point or emulation ends"); +} + +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.errlog){ + if(!(errlog = fopen(G.errlog, "w"))) + ERRX("Can't open error log %s", G.errlog); + else + fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n"); + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + Config = readServoConf(G.conffile); + if(!Config || G.dumpconf){ + dumpConf(); + return 1; + } + 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; + } + coordpair_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/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf new file mode 100644 index 0000000..9e7d3d4 --- /dev/null +++ b/LibSidServo/examples/servo.conf @@ -0,0 +1,25 @@ +# Current configuration +MountDevPath=/dev/ttyUSB0 +MountDevSpeed=19200 +EncoderDevPath=(null) +EncoderDevSpeed=1000000 +MountReqInterval=0.1 +EncoderReqInterval=0.001 +SepEncoder=2 +EncoderXDevPath=/dev/encoder_X0 +EncoderYDevPath=/dev/encoder_Y0 +EncoderSpeedInterval=0.05 +RunModel=1 +XPIDCP=0.8 +XPIDCI=0.0 +XPIDCD=0.0 +YPIDCP=0.5 +YPIDCI=0.0 +YPIDCD=0.0 +XPIDVP=0.2 +XPIDVI=0.1 +XPIDVD=0.0 +YPIDVP=0.2 +YPIDVI=0.1 +YPIDVD=0.0 + diff --git a/LibSidServo/examples/simpleconv.h b/LibSidServo/examples/simpleconv.h new file mode 100644 index 0000000..099ff86 --- /dev/null +++ b/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/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c new file mode 100644 index 0000000..e51f15a --- /dev/null +++ b/LibSidServo/examples/traectories.c @@ -0,0 +1,136 @@ +/* + * 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 coordpair_t XYstart = {0}; +static double tstart = 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, coordpair_t *XY0){ + if(!f || !XY0) return FALSE; + cur_traectory = f; + XYstart = *XY0; + tstart = Mount.timeFromStart(); + mountdata_t mdata; + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10) return FALSE; + 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(coordpair_t *nextpt, double t){ + if(t < 0. || !cur_traectory) return FALSE; + coordpair_t pt; + if(!cur_traectory(&pt, t)) return FALSE; + 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(coordval_pair_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; + coordval_pair_t pt; + //DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t); + pt.X.val = mdata.encXposition.val; + pt.Y.val = mdata.encYposition.val; + pt.X.t = mdata.encXposition.t; + pt.Y.t = mdata.encYposition.t; + if(curpos) *curpos = pt; + return TRUE; +} + +// X=X0+1'/s, Y=Y0+15''/s +int Linear(coordpair_t *nextpt, double t){ + coordpair_t pt; + pt.X = XYstart.X + ASEC2RAD(0.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(coordpair_t *nextpt, double t){ + coordpair_t pt; + pt.X = XYstart.X + ASEC2RAD(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+0.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/LibSidServo/examples/traectories.h b/LibSidServo/examples/traectories.h new file mode 100644 index 0000000..513eae6 --- /dev/null +++ b/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)(coordpair_t *, double); + +int init_traectory(traectory_fn f, coordpair_t *XY0); +traectory_fn traectory_by_name(const char *name); +void print_tr_names(); +int traectory_point(coordpair_t *nextpt, double t); +int telpos(coordval_pair_t *curpos); +int Linear(coordpair_t *nextpt, double t); +int SinCos(coordpair_t *nextpt, double t); diff --git a/LibSidServo/libsidservo.cflags b/LibSidServo/libsidservo.cflags new file mode 100644 index 0000000..85d51b3 --- /dev/null +++ b/LibSidServo/libsidservo.cflags @@ -0,0 +1 @@ +-std=c17 diff --git a/LibSidServo/libsidservo.config b/LibSidServo/libsidservo.config new file mode 100644 index 0000000..2a32b68 --- /dev/null +++ b/LibSidServo/libsidservo.config @@ -0,0 +1,7 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define EBUG +#define _POSIX_C_SOURCE 11111111 +#define PACKAGE_VERSION "0.0.1" +#define _XOPEN_SOURCE 666 +#define _DEFAULT_SOURCE diff --git a/LibSidServo/libsidservo.creator b/LibSidServo/libsidservo.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/LibSidServo/libsidservo.creator @@ -0,0 +1 @@ +[General] diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user new file mode 100644 index 0000000..58c6b84 --- /dev/null +++ b/LibSidServo/libsidservo.creator.user @@ -0,0 +1,220 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + 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 + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /tmp/robo5/mountcontrol.git/LibSidServo + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + 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 + %{RunConfig:Executable:Path} + + 1 + + 1 + + + 0 + Deploy + Deploy + 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 + %{RunConfig:Executable:Path} + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/LibSidServo/libsidservo.creator.user.7bd84e3 b/LibSidServo/libsidservo.creator.user.7bd84e3 new file mode 100644 index 0000000..e594907 --- /dev/null +++ b/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/LibSidServo/libsidservo.creator.user.cf63021 b/LibSidServo/libsidservo.creator.user.cf63021 new file mode 100644 index 0000000..002b8fd --- /dev/null +++ b/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/LibSidServo/libsidservo.cxxflags b/LibSidServo/libsidservo.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/LibSidServo/libsidservo.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files new file mode 100644 index 0000000..aa9288a --- /dev/null +++ b/LibSidServo/libsidservo.files @@ -0,0 +1,30 @@ +CMakeLists.txt +PID.c +PID.h +examples/SSIIconf.c +examples/conf.c +examples/conf.h +examples/dump.c +examples/dump.h +examples/dumpmoving.c +examples/dumpmoving_dragNtrack.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 +main.h +movingmodel.c +movingmodel.h +polltest/main.c +ramp.c +ramp.h +serial.h +ssii.c +ssii.h diff --git a/LibSidServo/libsidservo.includes b/LibSidServo/libsidservo.includes new file mode 100644 index 0000000..d494155 --- /dev/null +++ b/LibSidServo/libsidservo.includes @@ -0,0 +1,2 @@ +. +.. diff --git a/LibSidServo/main.c b/LibSidServo/main.c new file mode 100644 index 0000000..a6f04b0 --- /dev/null +++ b/LibSidServo/main.c @@ -0,0 +1,619 @@ +/* + * 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 . + */ + +/* + * main functions to fill struct `mount_t` + */ + +#include +#include +#include +#include +#include +#include + +#include "main.h" +#include "movingmodel.h" +#include "serial.h" +#include "ssii.h" +#include "PID.h" + +// adder for monotonic time by realtime: inited any call of init() +static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC + t0 = {0}, // curtime() for initstarttime() call + starttime = {0}; // starting time by monotonic (for timefromstart()) + +conf_t Conf = {0}; +// parameters for model +static movemodel_t *Xmodel, *Ymodel; +// limits for model and/or real mount (in latter case data should be read from mount on init) +// radians, rad/sec, rad/sec^2 +// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s +// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2 +limits_t + Xlimits = { + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}}, + Ylimits = { + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}} +; +static mcc_errcodes_t shortcmd(short_command_t *cmd); +static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig); + +/** + * @brief curtime - monotonic time from first run + * @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start + * @return TRUE if all OK + * FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME + */ +int curtime(struct timespec *t){ + struct timespec now; + if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE; + now.tv_sec += timeadder.tv_sec; + now.tv_nsec += timeadder.tv_nsec; + if(now.tv_nsec > 999999999L){ + ++now.tv_sec; + now.tv_nsec -= 1000000000L; + } + if(t) *t = now; + return TRUE; +} + +// init starttime; @return TRUE if all OK +static int initstarttime(){ + struct timespec start; + if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE; + if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE; + timeadder.tv_sec = start.tv_sec - starttime.tv_sec; + timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec; + if(timeadder.tv_nsec < 0){ + --timeadder.tv_sec; + timeadder.tv_nsec += 1000000000L; + } + curtime(&t0); + return TRUE; +} + +// return difference (in seconds) between time1 and time0 +double timediff(const struct timespec *time1, const struct timespec *time0){ + if(!time1 || !time0) return -1.; + return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9; +} +// difference between given time and last initstarttime() call +double timediff0(const struct timespec *time1){ + return timediff(time1, &t0); +} +// time from last initstarttime() call +double timefromstart(){ + struct timespec now; + if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.; + return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9; +} + +/** + * @brief quit - close all opened and return to default state + * TODO: close serial devices even in "model" mode + */ +static void quit(){ + if(Conf.RunModel) return; + for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break; + DBG("Close all serial devices"); + closeSerial(); + DBG("Exit"); +} + +void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){ + if(!c || !Xmodel || !Ymodel) return; + double tnow = timefromstart(); + moveparam_t Xp, Yp; + movestate_t Xst = Xmodel->get_state(Xmodel, &Xp); + //DBG("Xstate = %d", Xst); + if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow); + movestate_t Yst = Ymodel->get_state(Ymodel, &Yp); + if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow); + c->X = Xp.coord; + c->Y = Yp.coord; + if(xst) *xst = Xst; + if(yst) *yst = Yst; +} + +/** + * less square calculations of speed + */ +less_square_t *LS_init(size_t Ndata){ + if(Ndata < 5){ + DBG("Ndata=%zd - TOO SMALL", Ndata); + return NULL; + } + DBG("Init less squares: %zd", Ndata); + less_square_t *l = calloc(1, sizeof(less_square_t)); + l->x = calloc(Ndata, sizeof(double)); + l->t2 = calloc(Ndata, sizeof(double)); + l->t = calloc(Ndata, sizeof(double)); + l->xt = calloc(Ndata, sizeof(double)); + l->arraysz = Ndata; + return l; +} +void LS_delete(less_square_t **l){ + if(!l || !*l) return; + free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt); + free(*l); + *l = NULL; +} +// add next data portion and calculate current slope +double LS_calc_slope(less_square_t *l, double x, double t){ + if(!l) return 0.; + size_t idx = l->idx; + double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx]; + double t2 = t * t, xt = x * t; + l->x[idx] = x; l->t2[idx] = t2; + l->t[idx] = t; l->xt[idx] = xt; + ++idx; + l->idx = (idx >= l->arraysz) ? 0 : idx; + l->xsum += x - oldx; + l->t2sum += t2 - oldt2; + l->tsum += t - oldt; + l->xtsum += xt - oldxt; + double n = (double)l->arraysz; + double denominator = n * l->t2sum - l->tsum * l->tsum; + //DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator); + if(fabs(denominator) < 1e-7) return 0.; + double numerator = n * l->xtsum - l->xsum * l->tsum; + // point: (sum_x - slope * sum_t) / n; + return (numerator / denominator); +} + +/** + * @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; + if(!initstarttime()) return MCC_E_FAILED; + Conf = *c; + mcc_errcodes_t ret = MCC_E_OK; + Xmodel = model_init(&Xlimits); + Ymodel = model_init(&Ylimits); + if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ + DBG("Bad value of MountReqInterval"); + ret = MCC_E_BADFORMAT; + } + if(Conf.RunModel){ + if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; + return MCC_E_OK; + } + if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){ + DBG("Define mount device path and speed"); + ret = MCC_E_BADFORMAT; + }else if(!openMount()){ + DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed); + ret = MCC_E_MOUNTDEV; + } + if(Conf.SepEncoder){ + if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ + DBG("Define encoder device path"); + ret = MCC_E_BADFORMAT; + }else if(!openEncoder()){ + DBG("Can't open encoder device"); + ret = MCC_E_ENCODERDEV; + } + } + // TODO: read hardware configuration on init + if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ + DBG("Wrong speed interval"); + ret = MCC_E_BADFORMAT; + } + if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; + if(ret != MCC_E_OK) return ret; + // read HW config to update constants + hardware_configuration_t HW; + if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; + // make a pause for actual encoder's values + double t0 = timefromstart(); + while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000); + mcc_errcodes_t e = updateMotorPos(); + // and refresh data after updating + DBG("Wait for next mount reading"); + t0 = timefromstart(); + while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000); + return e; +} + +// check coordinates (rad) and speeds (rad/s); return FALSE if failed +// TODO fix to real limits!!! +static int chkX(double X){ + if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE; + return TRUE; +} +static int chkY(double Y){ + if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE; + return TRUE; +} +static int chkXs(double s){ + if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE; + return TRUE; +} +static int chkYs(double s){ + if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE; + return TRUE; +} + +// set SLEWING state if axis was stopped +static void setslewingstate(){ + //FNAME(); + mountdata_t d; + if(MCC_E_OK == getMD(&d)){ + axis_status_t newx = d.Xstate, newy = d.Ystate; + //DBG("old state: %d/%d", d.Xstate, d.Ystate); + if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING; + if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING; + if(newx != d.Xstate || newy != d.Ystate){ + DBG("Started moving -> slew"); + setStat(newx, newy); + } + }else DBG("CAN't GET MOUNT DATA!"); +} + +/** + * @brief move2 - simple move to given point and stop + * @param X - new X coordinate (radians: -pi..pi) or NULL + * @param Y - new Y coordinate (radians: -pi..pi) or NULL + * @return error code + */ +static mcc_errcodes_t move2(const coordpair_t *target){ + if(!target) return MCC_E_BADFORMAT; + if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; + short_command_t cmd = {0}; + DBG("x,y: %g, %g", target->X, target->Y); + cmd.Xmot = target->X; + cmd.Ymot = target->Y; + cmd.Xspeed = Xlimits.max.speed; + cmd.Yspeed = Ylimits.max.speed; + /*mcc_errcodes_t r = shortcmd(&cmd); + if(r != MCC_E_OK) return r; + setslewingstate(); + return MCC_E_OK;*/ + return shortcmd(&cmd); +} + +/** + * @brief setspeed - set maximal speed over axis by text command + * @param X (i) - max speed or NULL + * @param Y (i) - -//- + * @return errcode + */ +static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){ + if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT; + if(Conf.RunModel) return MCC_E_FAILED; + int32_t spd = X_RS2MOTSPD(tagspeed->X); + if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED; + spd = Y_RS2MOTSPD(tagspeed->Y); + if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED; + return MCC_E_OK; +} + +/** + * @brief move2s - move to target with given max speed + * @param target (i) - target or NULL + * @param speed (i) - speed or NULL + * @return + */ +static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ + if(!target || !speed) return MCC_E_BADFORMAT; + if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; + // updateMotorPos() here can make a problem; TODO: remove? + if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; + short_command_t cmd = {0}; + cmd.Xmot = target->X; + cmd.Ymot = target->Y; + cmd.Xspeed = speed->X; + cmd.Yspeed = speed->Y; + mcc_errcodes_t r = shortcmd(&cmd); + if(r != MCC_E_OK) return r; + setslewingstate(); + return MCC_E_OK; +} + +/** + * @brief emstop - emergency stop + * @return errcode + */ +static mcc_errcodes_t emstop(){ + FNAME(); + if(Conf.RunModel){ + double curt = timefromstart(); + Xmodel->emergency_stop(Xmodel, curt); + Ymodel->emergency_stop(Ymodel, curt); + return MCC_E_OK; + } + if(!SSstop(TRUE)) return MCC_E_FAILED; + return MCC_E_OK; +} +// normal stop +static mcc_errcodes_t stop(){ + FNAME(); + if(Conf.RunModel){ + double curt = timefromstart(); + Xmodel->stop(Xmodel, curt); + Ymodel->stop(Ymodel,curt); + return MCC_E_OK; + } + if(!SSstop(FALSE)) 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; + if(Conf.RunModel){ + double curt = timefromstart(); + moveparam_t param = {0}; + param.coord = cmd->Xmot; param.speed = cmd->Xspeed; + if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; + param.coord = cmd->Ymot; param.speed = cmd->Yspeed; + if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; + setslewingstate(); + return MCC_E_OK; + } + SSscmd s = {0}; + DBG("tag: xmot=%g rad, ymot=%g rad", 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; + setslewingstate(); + return MCC_E_OK; +} + +/** + * @brief longcmd - 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; + if(Conf.RunModel){ + double curt = timefromstart(); + moveparam_t param = {0}; + param.coord = cmd->Xmot; param.speed = cmd->Xspeed; + if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED; + param.coord = cmd->Ymot; param.speed = cmd->Yspeed; + if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED; + setslewingstate(); + return MCC_E_OK; + } + 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; + setslewingstate(); + return MCC_E_OK; +} + +static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ + if(!hwConfig) return MCC_E_BADFORMAT; + if(Conf.RunModel) return MCC_E_FAILED; + SSconfig config; + DBG("Read HW configuration"); + if(!cmdC(&config, FALSE)) return MCC_E_FAILED; + // Convert acceleration (ticks per loop^2 to rad/s^2) + hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel); + hwConfig->Yconf.accel = Y_MOTACC2RS(config.Yconf.accel); + // Convert backlash (ticks to radians) + hwConfig->Xconf.backlash = X_MOT2RAD(config.Xconf.backlash); + hwConfig->Yconf.backlash = Y_MOT2RAD(config.Yconf.backlash); + // Convert error limit (ticks to radians) + hwConfig->Xconf.errlimit = X_MOT2RAD(config.Xconf.errlimit); + hwConfig->Yconf.errlimit = Y_MOT2RAD(config.Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + hwConfig->Xconf.propgain = (double)config.Xconf.propgain; + hwConfig->Yconf.propgain = (double)config.Yconf.propgain; + hwConfig->Xconf.intgain = (double)config.Xconf.intgain; + hwConfig->Yconf.intgain = (double)config.Yconf.intgain; + hwConfig->Xconf.derivgain = (double)config.Xconf.derivgain; + hwConfig->Yconf.derivgain = (double)config.Yconf.derivgain; + // Output limit is a percentage (0-100) + hwConfig->Xconf.outplimit = (double)config.Xconf.outplimit / 255.0 * 100.0; + hwConfig->Yconf.outplimit = (double)config.Yconf.outplimit / 255.0 * 100.0; + // Current limit in amps + hwConfig->Xconf.currlimit = (double)config.Xconf.currlimit / 100.0; + hwConfig->Yconf.currlimit = (double)config.Yconf.currlimit / 100.0; + // Integral limit is unitless + hwConfig->Xconf.intlimit = (double)config.Xconf.intlimit; + hwConfig->Yconf.intlimit = (double)config.Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + hwConfig->xbits = config.xbits; + hwConfig->ybits = config.ybits; + // Copy address + hwConfig->address = config.address; + + // TODO: What to do with eqrate, eqadj and trackgoal? + config.latitude = __bswap_16(config.latitude); + // Convert latitude (degrees * 100 to radians) + hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0; + // Copy ticks per revolution + hwConfig->Xsetpr = __bswap_32(config.Xsetpr); + hwConfig->Ysetpr = __bswap_32(config.Ysetpr); + hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less + hwConfig->Ymetpr = __bswap_32(config.Ymetpr); + // Convert slew rates (ticks per loop to rad/s) + hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); + hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); + // Convert pan rates (ticks per loop to rad/s) + hwConfig->Xpanrate = X_MOTSPD2RS(config.Xpanrate); + hwConfig->Ypanrate = Y_MOTSPD2RS(config.Ypanrate); + // Convert guide rates (ticks per loop to rad/s) + hwConfig->Xguiderate = X_MOTSPD2RS(config.Xguiderate); + hwConfig->Yguiderate = Y_MOTSPD2RS(config.Yguiderate); + // copy baudrate + hwConfig->baudrate = (uint32_t) config.baudrate; + // Convert local search degrees (degrees * 100 to radians) + hwConfig->locsdeg = (double)config.locsdeg / 100.0 * M_PI / 180.0; + // Convert local search speed (arcsec per second to rad/s) + hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0); + // Convert backlash speed (ticks per loop to rad/s) + hwConfig->backlspd = X_MOTSPD2RS(config.backlspd); + // now read text commands + int64_t i64; + double Xticks, Yticks; + DBG("SERIAL"); + // motor's encoder ticks per rev + if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED; + Xticks = ((double) i64); // divide by 4 as these values stored ??? + if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED; + Yticks = ((double) i64); + X_ENC_ZERO = Conf.XEncZero; + Y_ENC_ZERO = Conf.YEncZero; + DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev); + X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks; + Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks; + DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV); + // axis encoder ticks per rev + if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED; + Xticks = (double) i64; + if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED; + Yticks = (double) i64; + DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev); + X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks; + Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks; + DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV); + return MCC_E_OK; +} + +static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ + SSconfig config; + if(Conf.RunModel) return MCC_E_FAILED; + // Convert acceleration (rad/s^2 to ticks per loop^2) + config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel); + config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel); + // Convert backlash (radians to ticks) + config.Xconf.backlash = X_RAD2MOT(hwConfig->Xconf.backlash); + config.Yconf.backlash = Y_RAD2MOT(hwConfig->Yconf.backlash); + // Convert error limit (radians to ticks) + config.Xconf.errlimit = X_RAD2MOT(hwConfig->Xconf.errlimit); + config.Yconf.errlimit = Y_RAD2MOT(hwConfig->Yconf.errlimit); + // Proportional, integral, and derivative gains are unitless, so no conversion needed + config.Xconf.propgain = (uint16_t)hwConfig->Xconf.propgain; + config.Yconf.propgain = (uint16_t)hwConfig->Yconf.propgain; + config.Xconf.intgain = (uint16_t)hwConfig->Xconf.intgain; + config.Yconf.intgain = (uint16_t)hwConfig->Yconf.intgain; + config.Xconf.derivgain = (uint16_t)hwConfig->Xconf.derivgain; + config.Yconf.derivgain = (uint16_t)hwConfig->Yconf.derivgain; + // Output limit is a percentage (0-100), so convert back to 0-255 + config.Xconf.outplimit = (uint8_t)(hwConfig->Xconf.outplimit / 100.0 * 255.0); + config.Yconf.outplimit = (uint8_t)(hwConfig->Yconf.outplimit / 100.0 * 255.0); + // Current limit is in amps (convert back to *100) + config.Xconf.currlimit = (uint16_t)(hwConfig->Xconf.currlimit * 100.0); + config.Yconf.currlimit = (uint16_t)(hwConfig->Yconf.currlimit * 100.0); + // Integral limit is unitless, so no conversion needed + config.Xconf.intlimit = (uint16_t)hwConfig->Xconf.intlimit; + config.Yconf.intlimit = (uint16_t)hwConfig->Yconf.intlimit; + // Copy XBits and YBits (no conversion needed) + config.xbits = hwConfig->xbits; + config.ybits = hwConfig->ybits; + // Convert latitude (radians to degrees * 100) + config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0)); + // Convert slew rates (rad/s to ticks per loop) + config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate); + config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate); + // Convert pan rates (rad/s to ticks per loop) + config.Xpanrate = X_RS2MOTSPD(hwConfig->Xpanrate); + config.Ypanrate = Y_RS2MOTSPD(hwConfig->Ypanrate); + // Convert guide rates (rad/s to ticks per loop) + config.Xguiderate = X_RS2MOTSPD(hwConfig->Xguiderate); + config.Yguiderate = Y_RS2MOTSPD(hwConfig->Yguiderate); + // Convert local search degrees (radians to degrees * 100) + config.locsdeg = (uint32_t)(hwConfig->locsdeg * 180.0 / M_PI * 100.0); + // Convert local search speed (rad/s to arcsec per second) + config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI); + // Convert backlash speed (rad/s to ticks per loop) + config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); + config.Xsetpr = __bswap_32(hwConfig->Xsetpr); + config.Ysetpr = __bswap_32(hwConfig->Ysetpr); + config.Xmetpr = __bswap_32(hwConfig->Xmetpr); + config.Ymetpr = __bswap_32(hwConfig->Ymetpr); + // todo - also write text params + // TODO - next + (void) config; + return MCC_E_OK; +} + +// getters of max/min speed and acceleration +mcc_errcodes_t maxspeed(coordpair_t *v){ + if(!v) return MCC_E_BADFORMAT; + v->X = Xlimits.max.speed; + v->Y = Ylimits.max.speed; + return MCC_E_OK; +} +mcc_errcodes_t minspeed(coordpair_t *v){ + if(!v) return MCC_E_BADFORMAT; + v->X = Xlimits.min.speed; + v->Y = Ylimits.min.speed; + return MCC_E_OK; +} +mcc_errcodes_t acceleration(coordpair_t *a){ + if(!a) return MCC_E_BADFORMAT; + a->X = Xlimits.max.accel; + a->Y = Ylimits.max.accel; + return MCC_E_OK; +} + +// init mount class +mount_t Mount = { + .init = init, + .quit = quit, + .getMountData = getMD, + .moveTo = move2, + .moveWspeed = move2s, + .setSpeed = setspeed, + .emergStop = emstop, + .stop = stop, + .shortCmd = shortcmd, + .longCmd = longcmd, + .getHWconfig = get_hwconf, + .saveHWconfig = write_hwconf, + .currentT = curtime, + .timeFromStart = timefromstart, + .timeDiff = timediff, + .timeDiff0 = timediff0, + .correctTo = correct2, + .getMaxSpeed = maxspeed, + .getMinSpeed = minspeed, + .getAcceleration = acceleration, +}; + diff --git a/LibSidServo/main.h b/LibSidServo/main.h new file mode 100644 index 0000000..769d305 --- /dev/null +++ b/LibSidServo/main.h @@ -0,0 +1,81 @@ +/* + * 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 . + */ + +/* + * Almost all here used for debug purposes + */ + +#pragma once + +#include + +#include "movingmodel.h" +#include "sidservo.h" + +extern conf_t Conf; +extern limits_t Xlimits, Ylimits; +int curtime(struct timespec *t); +double timediff(const struct timespec *time1, const struct timespec *time0); +double timediff0(const struct timespec *time1); +double timefromstart(); +void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst); +typedef struct{ + double *x, *t, *t2, *xt; // arrays of coord/time and multiply + double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply + size_t idx; // index of current data in array + size_t arraysz; // size of arrays +} less_square_t; + +less_square_t *LS_init(size_t Ndata); +void LS_delete(less_square_t **ls); +double LS_calc_slope(less_square_t *l, double x, double t); + +// unused arguments of functions +#define _U_ __attribute__((__unused__)) +// 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 "%s " COLOR_OLD, __func__); \ + fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + fprintf(stderr, "\n");} while(0) + +#else // EBUG + #define FNAME() + #define DBG(...) +#endif // EBUG diff --git a/LibSidServo/movingfilter.c- b/LibSidServo/movingfilter.c- new file mode 100644 index 0000000..16001b2 --- /dev/null +++ b/LibSidServo/movingfilter.c- @@ -0,0 +1,46 @@ +#include +#include +#include + +#define NFILT (5) + +static double filterK[NFILT]; + +static void buildFilter(){ + filterK[NFILT-1] = 1.; + double sum = 1.; + for(int i = NFILT-2; i > -1; --i){ + filterK[i] = (filterK[i+1] + 1.) * 1.1; + sum += filterK[i]; + } + for(int i = 0; i < NFILT; ++i){ + filterK[i] /= sum; + fprintf(stderr, "%d: %g\n", i, filterK[i]); + } +} + +static double filter(double val){ + static int ctr = 0; + static double lastvals[NFILT] = {0.}; + for(int i = NFILT-1; i > 0; --i) lastvals[i] = lastvals[i-1]; + lastvals[0] = val; + double r = 0.; + if(ctr < NFILT){ + ++ctr; + return val; + } + for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[i]; + return r; +} + +int main(int argc, char **argv){ + buildFilter(); + printf("Signal\tNoiced\tFiltered\n"); + for(int i = 0; i < 100; ++i){ + double di = (double)i; + double sig = di * di / 1e5 + sin(i * M_PI / 1500.); + double noiced = sig + 0.1 * (drand48() - 0.5); + printf("%.3f\t%.3f\t%.3f\n", sig, noiced, filter(noiced)); + } + return 0; +} \ No newline at end of file diff --git a/LibSidServo/movingmodel.c b/LibSidServo/movingmodel.c new file mode 100644 index 0000000..4326859 --- /dev/null +++ b/LibSidServo/movingmodel.c @@ -0,0 +1,73 @@ +/* + * This file is part of the moving_model 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 "main.h" +#include "movingmodel.h" +#include "ramp.h" + +extern movemodel_t trapez; + +static void chkminmax(double *min, double *max){ + if(*min <= *max) return; + double t = *min; + *min = *max; + *max = t; +} + +movemodel_t *model_init(limits_t *l){ + if(!l) return FALSE; + movemodel_t *m = calloc(1, sizeof(movemodel_t)); + // we can't use memcpy or assign as Times/Params would be common for all + *m = trapez; + m->Times = calloc(STAGE_AMOUNT, sizeof(double)); + m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t)); + moveparam_t *max = &l->max, *min = &l->min; + if(min->speed < 0.) min->speed = -min->speed; + if(max->speed < 0.) max->speed = -max->speed; + if(min->accel < 0.) min->accel = -min->accel; + if(max->accel < 0.) max->accel = -max->accel; + chkminmax(&min->coord, &max->coord); + chkminmax(&min->speed, &max->speed); + chkminmax(&min->accel, &max->accel); + m->Min = l->min; + m->Max = l->max; + m->movingstage = STAGE_STOPPED; + m->state = ST_STOP; + pthread_mutex_init(&m->mutex, NULL); + DBG("model inited"); + return m; +} + +int model_move2(movemodel_t *model, moveparam_t *target, double t){ + if(!target || !model) return FALSE; + DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.); + // only positive velocity + if(target->speed < 0.) target->speed = -target->speed; + if(fabs(target->speed) < model->Min.speed){ + DBG("STOP"); + model->stop(model, t); + return TRUE; + } + // don't mind about acceleration - user cannot set it now + return model->calculate(model, target, t); +} diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h new file mode 100644 index 0000000..dfd7ab9 --- /dev/null +++ b/LibSidServo/movingmodel.h @@ -0,0 +1,76 @@ +/* + * This file is part of the moving_model 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" + +// tolerance, time ticks +#define COORD_TOLERANCE_DEFAULT (1e-8) +#define COORD_TOLERANCE_MIN (1e-12) +#define COORD_TOLERANCE_MAX (10.) +#define TIME_TICK_DEFAULT (0.0001) +#define TIME_TICK_MIN (1e-9) +#define TIME_TICK_MAX (10.) + +typedef enum{ + ST_STOP, // stopped + ST_MOVE, // moving + ST_AMOUNT +} movestate_t; + +typedef struct{ + double coord; + double speed; + double accel; +} moveparam_t; + +typedef struct{ + moveparam_t min; + moveparam_t max; + //double acceleration; +} limits_t; + +typedef enum{ + STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +typedef struct movemodel{ + moveparam_t Min; + moveparam_t Max; + movingstage_t movingstage; + movestate_t state; + double *Times; + moveparam_t *Params; + moveparam_t curparams; // init values of limits, jerk + int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state + void (*stop)(struct movemodel *m, double t); // stop by ramp + void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration + double (*stoppedtime)(struct movemodel *m); // time when moving will ends + pthread_mutex_t mutex; +} movemodel_t; + +movemodel_t *model_init(limits_t *l); +int model_move2(movemodel_t *model, moveparam_t *target, double t); diff --git a/LibSidServo/polltest/main.c b/LibSidServo/polltest/main.c new file mode 100644 index 0000000..6ac84db --- /dev/null +++ b/LibSidServo/polltest/main.c @@ -0,0 +1,197 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 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 + +// suppose that we ONLY poll data +#define XYBUFSZ (128) + +struct{ + int help; + char *Xpath; + char *Ypath; + double dt; +} G = { + .Xpath = "/dev/encoder_X0", + .Ypath = "/dev/encoder_Y0", + .dt = 0.001, +}; + +sl_option_t options[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"Xpath", NEED_ARG, NULL, 'X', arg_string, APTR(&G.Xpath), "path to X encoder"}, + {"Ypath", NEED_ARG, NULL, 'Y', arg_string, APTR(&G.Ypath), "path to Y encoder"}, + {"dt", NEED_ARG, NULL, 'd', arg_double, APTR(&G.dt), "request interval (1e-4..10s)"}, +}; + +typedef struct{ + char buf[XYBUFSZ+1]; + int len; +} buf_t; + +static int Xfd = -1, Yfd = -1; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + DBG("close"); + if(Xfd > 0){ close(Xfd); Xfd = -1; } + if(Yfd > 0){ close(Yfd); Yfd = -1; } + exit(sig); +} + +static int op(const char *nm){ + int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK); + if(fd < 0) ERR("Can't open %s", nm); + struct termios2 tty; + if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings"); + 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 = 1000000; + tty.c_ospeed = 1000000; + if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings"); + // try to set exclusive + if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");} + return fd; +} + +// write to buffer next data portion; return FALSE in case of error +static int readstrings(buf_t *buf, int fd){ + FNAME(); + if(!buf){WARNX("Empty buffer"); return FALSE;} + int L = XYBUFSZ - buf->len; + if(L == 0){ + DBG("buffer overfull!", buf->len); + char *lastn = strrchr(buf->buf, '\n'); + if(lastn){ + fprintf(stderr, "BUFOVR: _%s_", buf->buf); + ++lastn; + buf->len = XYBUFSZ - (lastn - buf->buf); + DBG("Memmove %d", buf->len); + memmove(lastn, buf->buf, buf->len); + buf->buf[buf->len] = 0; + }else buf->len = 0; + L = XYBUFSZ - buf->len; + } + int got = read(fd, &buf->buf[buf->len], L); + if(got < 0){ + WARN("read()"); + return FALSE; + }else if(got == 0){ DBG("NO data"); return TRUE; } + buf->len += got; + buf->buf[buf->len] = 0; + DBG("buf[%d]: %s", buf->len, buf->buf); + return TRUE; +} +// return TRUE if got, FALSE if no data found +static int getdata(buf_t *buf, long *out){ + if(!buf || buf->len < 1) return FALSE; + // read record between last '\n' and previous (or start of string) + char *last = &buf->buf[buf->len - 1]; + //DBG("buf: _%s_", buf->buf); + if(*last != '\n') return FALSE; + *last = 0; + //DBG("buf: _%s_", buf->buf); + char *prev = strrchr(buf->buf, '\n'); + if(!prev) prev = buf->buf; + else{ + fprintf(stderr, "MORETHANONE: _%s_", buf->buf); + ++prev; // after last '\n' + } + if(out) *out = atol(prev); + // clear buffer + buf->len = 0; + return TRUE; +} +// try to write '\n' asking new data portion; return FALSE if failed +static int asknext(int fd){ + FNAME(); + if(fd < 0) return FALSE; + int i = 0; + for(; i < 5; ++i){ + int l = write(fd, "\n", 1); + //DBG("l=%d", l); + if(1 == l) return TRUE; + usleep(100); + } + DBG("5 tries... failed!"); + return FALSE; +} + +int main(int argc, char **argv){ + buf_t xbuf, ybuf; + long xlast, ylast; + double xtlast, ytlast; + sl_init(); + sl_parseargs(&argc, &argv, options); + if(G.help) sl_showhelp(-1, options); + if(G.dt < 1e-4) ERRX("dx too small"); + if(G.dt > 10.) ERRX("dx too big"); + Xfd = op(G.Xpath); + Yfd = op(G.Ypath); + struct pollfd pfds[2]; + pfds[0].fd = Xfd; pfds[0].events = POLLIN; + pfds[1].fd = Yfd; pfds[1].events = POLLIN; + double t0x, t0y, tstart; + asknext(Xfd); asknext(Yfd); + t0x = t0y = tstart = sl_dtime(); + DBG("Start"); + do{ // main cycle + if(poll(pfds, 2, 0) < 0){ + WARN("poll()"); + break; + } + if(pfds[0].revents && POLLIN){ + DBG("got X"); + if(!readstrings(&xbuf, Xfd)) break; + } + if(pfds[1].revents && POLLIN){ + DBG("got Y"); + if(!readstrings(&ybuf, Yfd)) break; + } + double curt = sl_dtime(); + if(getdata(&xbuf, &xlast)) xtlast = curt; + if(curt - t0x >= G.dt){ // get last records + if(curt - xtlast < 1.5*G.dt) + printf("%-14.4fX=%ld\n", xtlast-tstart, xlast); + if(!asknext(Xfd)) break; + t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt; + } + curt = sl_dtime(); + if(getdata(&ybuf, &ylast)) ytlast = curt; + if(curt - t0y >= G.dt){ // get last records + if(curt - ytlast < 1.5*G.dt) + printf("%-14.4fY=%ld\n", ytlast-tstart, ylast); + if(!asknext(Yfd)) break; + t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt; + } + }while(Xfd > 0 && Yfd > 0); + DBG("OOps: disconnected"); + signals(0); + return 0; +} diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c new file mode 100644 index 0000000..b9f4728 --- /dev/null +++ b/LibSidServo/ramp.c @@ -0,0 +1,259 @@ +/* + * This file is part of the moving_model 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 . + */ + +// simplest trapezioidal ramp + +#include +#include + +#include "main.h" +#include "ramp.h" + +#ifdef EBUG +#undef DBG +#define DBG(...) +#undef FNAME +#define FNAME() +#endif + +static double coord_tolerance = COORD_TOLERANCE_DEFAULT; + +static void emstop(movemodel_t *m, double _U_ t){ + FNAME(); + pthread_mutex_lock(&m->mutex); + m->curparams.accel = 0.; + m->curparams.speed = 0.; + bzero(m->Times, sizeof(double) * STAGE_AMOUNT); + bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT); + m->state = ST_STOP; + m->movingstage = STAGE_STOPPED; + pthread_mutex_unlock(&m->mutex); +} + +static void stop(movemodel_t *m, double t){ + FNAME(); + pthread_mutex_lock(&m->mutex); + if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret; + m->movingstage = STAGE_DECEL; + m->state = ST_MOVE; + m->Times[STAGE_DECEL] = t; + m->Params[STAGE_DECEL].speed = m->curparams.speed; + if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel; + else m->Params[STAGE_DECEL].accel = m->Max.accel; + m->Params[STAGE_DECEL].coord = m->curparams.coord; + // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2 + m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel; + // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2 + double dt = m->Times[STAGE_STOPPED] - t; + m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt + + m->Params[STAGE_DECEL].accel * dt * dt / 2.; +ret: + pthread_mutex_unlock(&m->mutex); +} + +// inner part of `calc`, could be called recoursively for hard case +static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){ + // signs + double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage + // times + double dt01 = 0., dt12 = 0., dt23 = 0.; + // absolute speed at stage 23 (or in that point); absolute max acceleration + double abs_vset = x->speed, abs_a = m->Max.accel; + // absolute target movement + double abs_Dx = fabs(x->coord - m->curparams.coord); + if(m->state == ST_STOP && abs_Dx < coord_tolerance){ + DBG("Movement too small -> stay at place"); + return; + } + // signs of Dx and current speed + double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.; + double v0 = m->curparams.speed; + double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0); + if(v0 == 0.) sign_v0 = 0.; + // preliminary calculations (vset and dependent values could be changed) + dt01 = fabs(abs_v0 - abs_vset) / abs_a; + double abs_dx23 = abs_vset * abs_vset / 2. / abs_a; + dt23 = abs_vset / abs_a; + double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a; + if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction + if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop + //DBG("Simplest case: stop"); + dt01 = dt12 = 0.; + sign_a23 = -sign_v0; + dt23 = abs_v0 / abs_a; + }else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case + //DBG("Move with smaller speed"); + sign_a01 = sign_a23 = -sign_v0; + sign_vset = sign_v0; + double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.; + double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23; + dt12 = abs_dx12 / abs_vset; + }else{// move with larget speed + //DBG("Move with larger speed"); + double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.; + if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other + abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.); + dt01 = fabs(abs_v0 - abs_vset) / abs_a; + abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.; + dt23 = abs_vset / abs_a; + abs_dx23 = abs_vset * abs_vset / 2. / abs_a; + DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset); + } + sign_a01 = sign_Dx; // sign_v0 could be ZERO!!! + sign_a23 = -sign_Dx; + sign_vset = sign_Dx; + double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23; + dt12 = abs_dx12 / abs_vset; + } + }else{ + // if we are here, we have the worst case: change speed direction + // DBG("Hardest case: change speed direction"); + // now we should calculate coordinate at which model stops and biuld new trapezium from that point + double x0 = m->curparams.coord, v0 = m->curparams.speed; + double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a; + m->state = ST_STOP; + m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.; + unlockedcalc(m, x, tstop); // calculate new ramp + // and change started conditions + m->curparams.coord = x0; m->curparams.speed = v0; + m->Times[STAGE_ACCEL] = t; + m->Params[STAGE_ACCEL].coord = x0; + m->Params[STAGE_ACCEL].speed = v0; + // DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0); + return; + } + m->state = ST_MOVE; + m->movingstage = STAGE_ACCEL; + // some knot parameters + double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a; + double v1, v2, x0, x1, x2; + v2 = v1 = sign_vset * abs_vset; + x0 = m->curparams.coord; + x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.; + x2 = x1 + v1 * dt12; + // fill knot parameters + moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed + p->accel = a01; + p->speed = m->curparams.speed; + p->coord = x0; + m->Times[STAGE_ACCEL] = t; + p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed + p->accel = 0.; + p->speed = v1; + p->coord = x1; + m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01; + p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed + p->accel = a23; + p->speed = v2; + p->coord = x2; + m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12; + p = &m->Params[STAGE_STOPPED]; // 3 - stop at target + p->accel = p->speed = 0.; + p->coord = x->coord; + m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23; +} + +/** + * @brief calc - moving calculation + * @param x - using max speed (>0!!!) and coordinate + * @param t - current time value + * @return FALSE if can't move with given parameters + */ +static int calc(movemodel_t *m, moveparam_t *x, double t) { + //DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed); + if (!x || !m) return FALSE; + pthread_mutex_lock(&m->mutex); + int ret = FALSE; + // Validate input parameters + if(x->coord < m->Min.coord || x->coord > m->Max.coord){ + DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord); + goto ret; + } + if(x->speed < m->Min.speed || x->speed > m->Max.speed){ + DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed); + goto ret; + } + ret = TRUE; // now there's no chanses to make error + unlockedcalc(m, x, t); + // Debug output + /*for(int i = 0; i < STAGE_AMOUNT; i++){ + DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f", + i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel); + }*/ +ret: + pthread_mutex_unlock(&m->mutex); + return ret; +} + +static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){ + pthread_mutex_lock(&m->mutex); + if(m->state == ST_STOP) goto ret; + for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){ + if(m->Times[s] <= t){ // check time for current stage + m->movingstage = s; + break; + } + } + if(m->movingstage == STAGE_STOPPED){ + m->curparams.coord = m->Params[STAGE_STOPPED].coord; + pthread_mutex_unlock(&m->mutex); + /* DBG("REACHED STOPping stage @ t=%g", t); + for(int s = STAGE_STOPPED; s >= 0; --s){ + DBG("T[%d]=%g, ", s, m->Times[s]); + }*/ + emstop(m, t); + goto ret; + } + // calculate current parameters + double dt = t - m->Times[m->movingstage]; + double a = m->Params[m->movingstage].accel; + double v0 = m->Params[m->movingstage].speed; + double x0 = m->Params[m->movingstage].coord; + m->curparams.accel = a; + m->curparams.speed = v0 + a * dt; + m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; +ret: + if(next) *next = m->curparams; + movestate_t st = m->state; + pthread_mutex_unlock(&m->mutex); + return st; +} + +static movestate_t getst(movemodel_t *m, moveparam_t *cur){ + pthread_mutex_lock(&m->mutex); + if(cur) *cur = m->curparams; + movestate_t st = m->state; + pthread_mutex_unlock(&m->mutex); + return st; +} + +static double gettstop(movemodel_t *m){ + pthread_mutex_lock(&m->mutex); + double r = m->Times[STAGE_STOPPED]; + pthread_mutex_unlock(&m->mutex); + return r; +} + +movemodel_t trapez = { + .stop = stop, + .emergency_stop = emstop, + .get_state = getst, + .calculate = calc, + .proc_move = proc, + .stoppedtime = gettstop, +}; diff --git a/LibSidServo/ramp.h b/LibSidServo/ramp.h new file mode 100644 index 0000000..486a367 --- /dev/null +++ b/LibSidServo/ramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "movingmodel.h" + +extern movemodel_t trapez; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c new file mode 100644 index 0000000..f4cc227 --- /dev/null +++ b/LibSidServo/serial.c @@ -0,0 +1,815 @@ +/* + * 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 "main.h" +#include "movingmodel.h" +#include "serial.h" +#include "ssii.h" + +// serial devices FD +static int encfd[2] = {-1, -1}, mntfd = -1; +// main mount data +static mountdata_t mountdata = {0}; +// last encoders time and last encoders data - for speed measurement +//static coordval_t lastXenc = {0}, lastYenc = {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 - for `select` +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; +// encoders raw data +typedef struct __attribute__((packed)){ + uint8_t magick; + int32_t encY; + int32_t encX; + uint8_t CRC[4]; +} enc_t; + +// calculate current X/Y speeds +void getXspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + pthread_mutex_lock(&datamutex); + double dt = timediff0(&mountdata.encXposition.t); + double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt); + if(fabs(speed) < 1.5 * Xlimits.max.speed){ + mountdata.encXspeed.val = speed; + mountdata.encXspeed.t = mountdata.encXposition.t; + } + pthread_mutex_unlock(&datamutex); + //DBG("Xspeed=%g", mountdata.encXspeed.val); +} +void getYspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + pthread_mutex_lock(&datamutex); + double dt = timediff0(&mountdata.encYposition.t); + double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt); + if(fabs(speed) < 1.5 * Ylimits.max.speed){ + mountdata.encYspeed.val = speed; + mountdata.encYspeed.t = mountdata.encYposition.t; + } + pthread_mutex_unlock(&datamutex); +} + +/** + * @brief parse_encbuf - check encoder buffer (for encoder data based on SSII proto) and fill fresh data + * @param databuf - input buffer with 13 bytes of data + * @param t - time when databuf[0] got + */ +static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ + if(!t) return; + enc_t *edata = (enc_t*) databuf; +/* +#ifdef EBUG + DBG("ENCBUF:"); + for(int i = 0; i < ENC_DATALEN; ++i) printf("%02X ", databuf[i]); + printf("\n"); +#endif +*/ + 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.encXposition.val = Xenc2rad(edata->encX); + mountdata.encYposition.val = Yenc2rad(edata->encY); + DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); + mountdata.encXposition.t = *t; + mountdata.encYposition.t = *t; + pthread_mutex_unlock(&datamutex); + //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); + //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); + getXspeed(); getYspeed(); + //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); +} + +/** + * @brief getencval - get uint64_t data from encoder + * @param fd - encoder fd + * @param val - value read + * @param t - measurement time + * @return amount of data read or 0 if problem + */ +static int getencval(int fd, double *val, struct timespec *t){ + if(fd < 0){ + DBG("Encoder fd < 0!"); + return FALSE; + } + char buf[128]; + int got = 0, Lmax = 127; + double t0 = timefromstart(); + //DBG("start: %.6g", t0); + do{ + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + struct timeval tv = encRtmout; + int retval = select(fd + 1, &rfds, NULL, NULL, &tv); + if(!retval){ + //DBG("select()==0 - timeout, %.6g", timefromstart()); + break; + } + if(retval < 0){ + if(errno == EINTR){ + DBG("EINTR"); + continue; + } + DBG("select() < 0"); + return 0; + } + if(FD_ISSET(fd, &rfds)){ + ssize_t l = read(fd, &buf[got], Lmax); + if(l < 1){ + DBG("read() < 0"); + return 0; // disconnected ?? + } + got += l; Lmax -= l; + buf[got] = 0; + } else continue; + if(buf[got-1] == '\n') break; // got EOL as last symbol + }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.); + if(got == 0){ + //DBG("No data from encoder, tfs=%.6g", timefromstart()); + return 0; + } + char *estr = strrchr(buf, '\n'); + if(!estr){ + DBG("No EOL"); + return 0; + } + *estr = 0; + char *bgn = strrchr(buf, '\n'); + if(bgn) ++bgn; + else bgn = buf; + char *eptr; + long data = strtol(bgn, &eptr, 10); + if(eptr != estr){ + DBG("NAN"); + return 0; // wrong number + } + if(val) *val = (double) data; + if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} + return got; +} +// 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] < 0) return -1; + uint8_t byte = 0; + fd_set rfds; + do{ + FD_ZERO(&rfds); + FD_SET(encfd[0], &rfds); + struct timeval tv = encRtmout; + int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv); + if(!retval) break; + if(retval < 0){ + if(errno == EINTR) continue; + return -1; + } + if(FD_ISSET(encfd[0], &rfds)){ + ssize_t l = read(encfd[0], &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; + /* 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); + struct timeval 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){ + DBG("Mount disconnected?"); + return -2; // disconnected ?? + } + break; + } else return -1; + }while(1); + return (int)byte; +} +// clear data from input buffer +static void clrmntbuf(){ + if(mntfd < 0) return; + uint8_t byte; + fd_set rfds; + do{ + FD_ZERO(&rfds); + FD_SET(mntfd, &rfds); + struct timeval tv = {.tv_sec=0, .tv_usec=10}; + int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); + if(retval < 0){ + if(errno == EINTR) continue; + DBG("Error in select()"); + break; + } + if(FD_ISSET(mntfd, &rfds)){ + ssize_t l = read(mntfd, &byte, 1); + if(l != 1) break; + } else break; + }while(1); +} + +// main encoder thread (for separate encoder): read next data and make parsing +static void *encoderthread1(void _U_ *u){ + if(Conf.SepEncoder != 1) return NULL; + uint8_t databuf[ENC_DATALEN]; + int wridx = 0, errctr = 0; + struct timespec tcur; + while(encfd[0] > -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; + } + continue; + }else databuf[wridx++] = (uint8_t) b; + if(wridx == ENC_DATALEN){ + if(curtime(&tcur)){ + parse_encbuf(databuf, &tcur); + wridx = 0; + } + } + } + if(encfd[0] > -1){ + close(encfd[0]); + encfd[0] = -1; + } + return NULL; +} + +// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0 +static void *encoderthread2(void _U_ *u){ + if(Conf.SepEncoder != 2) return NULL; + DBG("Thread started"); + int errctr = 0; + double t0 = timefromstart(); + const char *req = "\n"; + int need2ask = 0; // need or not to ask encoder for new data + while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ + struct timespec t; + if(need2ask){ + //DBG("ASK for new data, tfs=%.6g", timefromstart()); + if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } + else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } + //DBG("OK"); + need2ask = 0; + usleep(100); + } + if(!curtime(&t)){ + DBG("Where is time?"); + continue; + } + double v; + if(getencval(encfd[0], &v, &t)){ + pthread_mutex_lock(&datamutex); + mountdata.encXposition.val = Xenc2rad(v); + mountdata.encXposition.t = t; + pthread_mutex_unlock(&datamutex); + //DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart()); + getXspeed(); + if(getencval(encfd[1], &v, &t)){ + pthread_mutex_lock(&datamutex); + mountdata.encYposition.val = Yenc2rad(v); + mountdata.encYposition.t = t; + pthread_mutex_unlock(&datamutex); + //DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV); + getYspeed(); + errctr = 0; + need2ask = 0; + //DBG("tgot=%.6g", timefromstart()); + while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } + t0 = timefromstart(); + }else{ + DBG("NO Y DATA!!!"); + /*if(need2ask) ++errctr; + else need2ask = 1; + continue;*/ + ++errctr; + need2ask = 1; + } + }else{ // no data - ask for new + //DBG("Need new, tfs=%.6g", timefromstart()); + /*if(need2ask) ++errctr; + else need2ask = 1; + continue;*/ + ++errctr; + need2ask = 1; + } + } + DBG("\n\nEXIT: ERRCTR=%d", errctr); + for(int i = 0; i < 2; ++i){ + if(encfd[i] > -1){ + close(encfd[i]); + encfd[i] = -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; +} + +static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){ + if(!prev || !nstopped || !stat) return; + if(isnan(*prev)){ + *stat = AXIS_STOPPED; + DBG("START"); + }else if(*stat != AXIS_STOPPED){ + if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){ + *stat = AXIS_STOPPED; + DBG("AXIS stopped; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped); + } + }else if(*prev != cur){ + DBG("AXIS moving"); + *nstopped = 0; + } + *prev = cur; +} + +// main mount thread +static void *mountthread(void _U_ *u){ + int errctr = 0; + uint8_t buf[2*sizeof(SSstat)]; + SSstat *status = (SSstat*) buf; + bzero(&mountdata, sizeof(mountdata)); + double t0 = timefromstart(), tstart = t0, tcur = t0; + double oldmt = -100.; // old `millis measurement` time + static uint32_t oldmillis = 0; + if(Conf.RunModel){ + double Xprev = NAN, Yprev = NAN; // previous coordinates + int xcnt = 0, ycnt = 0; + while(1){ + coordpair_t c; + movestate_t xst, yst; + // now change data + getModData(&c, &xst, &yst); + struct timespec tnow; + if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue; + pthread_mutex_lock(&datamutex); + mountdata.encXposition.t = mountdata.encYposition.t = tnow; + mountdata.encXposition.val = c.X; + mountdata.encYposition.val = c.Y; + //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); + if(tcur - oldmt > Conf.MountReqInterval){ + oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3); + mountdata.motYposition.t = mountdata.motXposition.t = tnow; + if(xst == ST_MOVE) + mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motXposition.val = c.X; + if(yst == ST_MOVE) + mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motYposition.val = c.Y; + oldmt = tcur; + }else mountdata.millis = oldmillis; + chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate); + chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate); + pthread_mutex_unlock(&datamutex); + getXspeed(); getYspeed(); + while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50); + t0 = timefromstart(); + } + } + // 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; + while(mntfd > -1 && errctr < MAX_ERR_CTR){ + // read data to status + struct timespec tcur; + if(!curtime(&tcur)) continue; + // 80 milliseconds to get answer on GETSTAT + if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ +#ifdef EBUG + DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); + for(size_t i = 0; i < d.len; ++i) printf("%02X ", d.buf[i]); + printf("\n"); +#endif + ++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, &tcur); + pthread_mutex_unlock(&datamutex); + // allow writing & getters + do{ + usleep(500); + }while(timefromstart() - t0 < Conf.MountReqInterval); + t0 = timefromstart(); + } + 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){ + DBG("Can't open device %s: %s", path, strerror(errno)); + return -1; + } + if(ioctl(fd, TCGETS2, &tty)){ + DBG("Can't read TTY settings"); + 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)){ + DBG("Can't set TTY settings"); + close(fd); + return -1; + } + DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed); + 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(){ + if(Conf.RunModel) return TRUE; + if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent + if(Conf.SepEncoder == 1){ // only one device + DBG("One device"); + if(encfd[0] > -1) close(encfd[0]); + encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed); + if(encfd[0] < 0) return FALSE; + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes + if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ + close(encfd[0]); + encfd[0] = -1; + return FALSE; + } + }else if(Conf.SepEncoder == 2){ + DBG("Two devices!"); + const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath}; + for(int i = 0; i < 2; ++i){ + if(encfd[i] > -1) close(encfd[i]); + encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed); + if(encfd[i] < 0) return FALSE; + } + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; + if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ + for(int i = 0; i < 2; ++i){ + close(encfd[i]); + encfd[i] = -1; + } + return FALSE; + } + }else return FALSE; + DBG("Encoder opened, thread started"); + return TRUE; +} + +// return FALSE if failed +int openMount(){ + if(Conf.RunModel) goto create_thread; + if(mntfd > -1) close(mntfd); + DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); + mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed); + 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 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed +create_thread: + if(pthread_create(&mntthread, NULL, mountthread, NULL)){ + DBG("Can't create mount thread"); + if(!Conf.RunModel){ + close(mntfd); + mntfd = -1; + } + return FALSE; + } + DBG("Mount opened, thread started"); + return TRUE; +} + +// close all opened serial devices and quit threads +void closeSerial(){ + // TODO: close devices in "model" mode too! + if(Conf.RunModel) return; + if(mntfd > -1){ + DBG("Cancel mount thread"); + pthread_cancel(mntthread); + DBG("join mount thread"); + pthread_join(mntthread, NULL); + DBG("close mount fd"); + close(mntfd); + mntfd = -1; + } + if(encfd[0] > -1){ + DBG("Cancel encoder thread"); + pthread_cancel(encthread); + DBG("join encoder thread"); + pthread_join(encthread, NULL); + DBG("close encoder's fd"); + close(encfd[0]); + encfd[0] = -1; + if(Conf.SepEncoder == 2 && encfd[1] > -1){ + close(encfd[1]); + encfd[1] = -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); + //DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val); + //DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL)); + return MCC_E_OK; +} + +void setStat(axis_status_t Xstate, axis_status_t Ystate){ + DBG("set x/y state to %d/%d", Xstate, Ystate); + pthread_mutex_lock(&datamutex); + mountdata.Xstate = Xstate; + mountdata.Ystate = Ystate; + pthread_mutex_unlock(&datamutex); +} + +// 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){ + DBG("Wrong arguments or no mount fd"); + return FALSE; + } + clrmntbuf(); + if(out){ + if(out->len != (size_t)write(mntfd, out->buf, out->len)){ + DBG("written bytes not equal to need"); + return FALSE; + } + if(needeol){ + int g = write(mntfd, "\r", 1); // add EOL + (void) g; + } + usleep(50000); // add little pause so that the idiot has time to swallow + } + if(!in) return TRUE; + 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; + } + while(getmntbyte() > -1); + 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){ + if(Conf.RunModel) return -1; + pthread_mutex_lock(&mntmutex); + int ret = wr(out, in, 1); + pthread_mutex_unlock(&mntmutex); + return ret; +} +// send binary data - without EOL +int MountWriteReadRaw(const data_t *out, data_t *in){ + if(Conf.RunModel) return -1; + pthread_mutex_lock(&mntmutex); + int ret = wr(out, in, 0); + 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){ + if(Conf.RunModel) return FALSE; + 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"); +#ifdef EBUG + logscmd((SSscmd*)cmd); +#endif + if(!wr(dscmd, NULL, 1)) goto rtn; + }else if(len == sizeof(SSlcmd)){ + ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); + DBG("Long command"); +#ifdef EBUG + loglcmd((SSlcmd*)cmd); +#endif + if(!wr(dlcmd, NULL, 1)) goto rtn; + }else{ + goto rtn; + } + data_t d; + d.buf = cmd; + d.len = d.maxlen = len; + ret = wr(&d, NULL, 0); + DBG("%s", ret ? "SUCCESS" : "FAIL"); +rtn: + pthread_mutex_unlock(&mntmutex); + return ret; +} + +// short, long and config text-binary commands +// 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)); +} +// rw == 1 to write, 0 to read +int cmdC(SSconfig *conf, int rw){ + if(Conf.RunModel) return FALSE; + static data_t *wcmd = NULL, *rcmd = NULL; + int ret = FALSE; + // dummy buffer to clear trash in input + char ans[300]; + data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; + if(!wcmd) wcmd = cmd2dat(CMD_PROGFLASH); + if(!rcmd) rcmd = cmd2dat(CMD_DUMPFLASH); + pthread_mutex_lock(&mntmutex); + if(rw){ // write + if(!wr(wcmd, &a, 1)) goto rtn; + }else{ // read + data_t d; + d.buf = (uint8_t *) conf; + d.len = 0; d.maxlen = 0; + ret = wr(rcmd, &d, 1); + DBG("write command: %s", ret ? "TRUE" : "FALSE"); + if(!ret) goto rtn; + // make a huge pause for stupid SSII + usleep(100000); + d.len = 0; d.maxlen = sizeof(SSconfig); + ret = wr(rcmd, &d, 1); + DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen); + if(d.len != d.maxlen){ ret = FALSE; goto rtn; } + // simplest checksum + uint16_t sum = 0; + for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i]; + if(sum != conf->checksum){ + DBG("got sum: %u, need: %u", conf->checksum, sum); + ret = FALSE; + goto rtn; + } + } +rtn: + pthread_mutex_unlock(&mntmutex); + return ret; +} diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h new file mode 100644 index 0000000..45694b1 --- /dev/null +++ b/LibSidServo/serial.h @@ -0,0 +1,44 @@ +/* + * 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) + +data_t *cmd2dat(const char *cmd); +void data_free(data_t **x); +int openEncoder(); +int openMount(); +void closeSerial(); +mcc_errcodes_t getMD(mountdata_t *d); +void setStat(axis_status_t Xstate, axis_status_t Ystate); +int MountWriteRead(const data_t *out, data_t *in); +int MountWriteReadRaw(const data_t *out, data_t *in); +int cmdS(SSscmd *cmd); +int cmdL(SSlcmd *cmd); +int cmdC(SSconfig *conf, int rw); +void getXspeed(); +void getYspeed(); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h new file mode 100644 index 0000000..ed4feab --- /dev/null +++ b/LibSidServo/sidservo.h @@ -0,0 +1,261 @@ +/* + * 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 . + */ + +/* + * This file contains all need for external usage + */ + + +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +// minimal serial speed of mount device +#define MOUNT_BAUDRATE_MIN (1200) +// max speed interval, seconds +#define MCC_CONF_MAX_SPEEDINT (2.) +// minimal speed interval in parts of EncoderReqInterval +#define MCC_CONF_MIN_SPEEDC (3.) + + +// 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_E_AMOUNT // Just amount of errors +} mcc_errcodes_t; + +typedef struct{ + double P, I, D; +} PIDpar_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, ==2 if there's new version with two devices + char* EncoderXDevPath; // paths to new controller devices + char* EncoderYDevPath; + double EncodersDisagreement; // acceptable disagreement between motor and axis encoders + double MountReqInterval; // interval between subsequent mount requests (seconds) + double EncoderReqInterval; // interval between subsequent encoder requests (seconds) + double EncoderSpeedInterval; // interval between speed calculations + int RunModel; // == 1 if you want to use model instead of real mount + double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared) + double PIDRefreshDt; // normal PID refresh interval + double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps) + PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven) + PIDpar_t XPIDV; + PIDpar_t YPIDC; + PIDpar_t YPIDV; + double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees + double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg + double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1'' + int XEncZero; // encoders' zero position + int YEncZero; +} conf_t; + +// coordinates/speeds in degrees or d/s: X, Y +typedef struct{ + double X; double Y; +} coordpair_t; + +// coordinate/speed and time of last measurement +typedef struct{ + double val; + struct timespec t; +} coordval_t; + +typedef struct{ + coordval_t X; + coordval_t Y; +} coordval_pair_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 motrev :1; // If 1, the motor encoder is incremented in the opposite direction + uint8_t motpolarity :1; // If 1, the motor polarity is reversed + uint8_t encrev :1; // If 1, the axis encoder is reversed + uint8_t dragtrack :1; // If 1, we are in computerless Drag and Track mode + uint8_t trackplat :1; // If 1, we are in the tracking platform mode + uint8_t handpaden :1; // If 1, hand paddle is enabled + uint8_t newpad :1; // If 1, hand paddle is compatible with New hand paddle, which allows slewing in two directions and guiding + uint8_t guidemode :1; // If 1, we are in guide mode. The pan rate is added or subtracted from the current tracking rate +} xbits_t; + +typedef struct{ + uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction + uint8_t motpolarity :1; // If 1, the motor polarity is reversed + uint8_t encrev :1; // If 1, the axis encoder is reversed + /* If 1, we are in computerless Slew and Track mode + (no clutches; use handpad to slew; must be in Drag and Track mode too) */ + uint8_t slewtrack :1; + uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync + uint8_t digin :3; // Digital input from radio handpad receiver +} ybits_t; + +typedef struct{ + xbits_t XBits; + ybits_t YBits; + uint8_t ExtraBits; + uint16_t ain0; + uint16_t ain1; +} extradata_t; + +typedef enum{ + AXIS_STOPPED, + AXIS_SLEWING, + AXIS_POINTING, + AXIS_GUIDING, + AXIS_ERROR, +} axis_status_t; + +typedef struct{ + axis_status_t Xstate; + axis_status_t Ystate; + coordval_t motXposition; + coordval_t motYposition; + coordval_t encXposition; + coordval_t encYposition; + coordval_t encXspeed; // once per s + coordval_t encYspeed; + 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 + +// hardware axis configuration +typedef struct{ + double accel; // Default Acceleration, rad/s^2 + double backlash; // Backlash (???) + double errlimit; // Error Limit, rad + double propgain; // Proportional Gain (???) + double intgain; // Integral Gain (???) + double derivgain; // Derivative Gain (???) + double outplimit; // Output Limit, percent (0..100) + double currlimit; // Current Limit (A) + double intlimit; // Integral Limit (???) + // these params are taken from mount by text commands (don't save negative values - better save these marks in xybits + double motor_stepsperrev;// encoder's steps per revolution: motor and axis + double axis_stepsperrev; // negative sign of these values means reverse direction +} __attribute__((packed)) axis_config_t; + +// hardware configuration +typedef struct{ + axis_config_t Xconf; + xbits_t xbits; + axis_config_t Yconf; + ybits_t ybits; + uint8_t address; + double eqrate; // Equatorial Rate (???) + double eqadj; // Equatorial UpDown adjust (???) + double trackgoal; // Tracking Platform Goal (???) + double latitude; // Latitude, rad + uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev + uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev + uint32_t Ymetpr; // Azm Motor Ticks Per Rev + uint32_t Xmetpr; // Alt Motor Ticks Per Rev + double Xslewrate; // Alt/Dec Slew Rate (rad/s) + double Yslewrate; // Azm/RA Slew Rate (rad/s) + double Xpanrate; // Alt/Dec Pan Rate (rad/s) + double Ypanrate; // Azm/RA Pan Rate (rad/s) + double Xguiderate; // Alt/Dec Guide Rate (rad/s) + double Yguiderate; // Azm/RA Guide Rate (rad/s) + uint32_t baudrate; // Baud Rate (baud) + double locsdeg; // Local Search Degrees (rad) + double locsspeed; // Local Search Speed (rad/s) + double backlspd; // Backlash speed (rad/s) +} hardware_configuration_t; + +/* flags for slew function +typedef struct{ + uint32_t slewNguide : 1; // ==1 to guide after slewing +} slewflags_t; +*/ +// mount class +typedef struct{ + // TODO: on init/quit clear all XY-bits to default` + 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 (*slewTo)(const coordpair_t *target, slewflags_t flags); + mcc_errcodes_t (*correctTo)(const coordval_pair_t *target); + mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop + mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed + mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed + mcc_errcodes_t (*stop)(); // 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 + mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration + mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration + int (*currentT)(struct timespec *t); // current time + double (*timeFromStart)(); // amount of seconds from last init + double (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times + double (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time + mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis + mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//- + mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration +} mount_t; + +extern mount_t Mount; + +#ifdef __cplusplus +} +#endif diff --git a/LibSidServo/sidservo.pc.in b/LibSidServo/sidservo.pc.in new file mode 100644 index 0000000..431e00d --- /dev/null +++ b/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/LibSidServo/ssii.c b/LibSidServo/ssii.c new file mode 100644 index 0000000..2c88e52 --- /dev/null +++ b/LibSidServo/ssii.c @@ -0,0 +1,225 @@ +/* + * 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 "main.h" +#include "serial.h" +#include "ssii.h" + +int X_ENC_ZERO, Y_ENC_ZERO; +double X_MOT_STEPSPERREV = 1., Y_MOT_STEPSPERREV = 1., X_ENC_STEPSPERREV = 1., Y_ENC_STEPSPERREV = 1.; + +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; +} + +// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! +static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){ + if(*prev == INT32_MAX){ + *stat = AXIS_STOPPED; + DBG("START"); + }else if(*stat != AXIS_STOPPED){ + if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){ + *stat = AXIS_STOPPED; + DBG("AXIS stopped"); + } + }else if(*prev != cur){ + DBG("AXIS moving"); + *nstopped = 0; + } + *prev = cur; +} +// check for stopped/pointing states +static void ChkStopped(const SSstat *s, mountdata_t *m){ + static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates + static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state + chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); + chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate); +} + +/** + * @brief SSconvstat - convert stat from SSII format to human + * @param s (i) - just read data + * @param m (o) - output + * @param t - measurement time + */ +void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){ + if(!s || !m || !t) return; + m->motXposition.val = X_MOT2RAD(s->Xmot); + m->motYposition.val = Y_MOT2RAD(s->Ymot); + ChkStopped(s, m); + m->motXposition.t = m->motYposition.t = *t; + // fill encoder data from here, as there's no separate enc thread + if(!Conf.SepEncoder){ + m->encXposition.val = Xenc2rad(s->Xenc); + DBG("encx: %g", m->encXposition.val); + m->encYposition.val = Yenc2rad(s->Yenc); + m->encXposition.t = m->encYposition.t = *t; + getXspeed(); getYspeed(); + } + 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); +} + +// the same as SStextcmd, but not adding EOL - send raw 'cmd' +int SSrawcmd(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 MountWriteReadRaw(&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; +} + +/** + * @brief SSsetterI - integer setter + * @param cmd - command to send + * @param ival - value + * @return false if failed + */ +int SSsetterI(const char *cmd, int32_t ival){ + char buf[128]; + snprintf(buf, 127, "%s%" PRIi32, cmd, ival); + return SStextcmd(buf, NULL); +} + +int SSstop(int emerg){ + int i = 0; + const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX; + const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY; + for(; i < 10; ++i){ + if(!SStextcmd(cmdx, NULL)) continue; + if(SStextcmd(cmdy, NULL)) break; + } + if(i == 10) return FALSE; + return TRUE; +} + +// update motors' positions due to encoders' +mcc_errcodes_t updateMotorPos(){ + mountdata_t md = {0}; + if(Conf.RunModel) return MCC_E_OK; + double t0 = timefromstart(), t = 0.; + struct timespec curt; + DBG("start @ %g", t0); + do{ + t = timefromstart(); + if(!curtime(&curt)){ + usleep(10000); + continue; + } + //DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV); + if(MCC_E_OK == getMD(&md)){ + if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){ + DBG("Just started? t-t0 = %g!", t - t0); + usleep(10000); + continue; + } + if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK; + DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec); + mcc_errcodes_t OK = MCC_E_OK; + if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){ + DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val); + DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val)); + if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ + DBG("Xpos sync failed!"); + OK = MCC_E_FAILED; + }else DBG("Xpos sync OK, Dt=%g", t - t0); + } + if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){ + DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val); + if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ + DBG("Ypos sync failed!"); + OK = MCC_E_FAILED; + }else DBG("Ypos sync OK, Dt=%g", t - t0); + } + if(MCC_E_OK == OK){ + DBG("Encoders synced"); + return OK; + } + } + DBG("NO DATA; dt = %g", t - t0); + }while(t - t0 < 2.); + return MCC_E_FATAL; +} diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h new file mode 100644 index 0000000..39d65a0 --- /dev/null +++ b/LibSidServo/ssii.h @@ -0,0 +1,348 @@ +/* + * 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 . + */ + +/* + * This file contains stuff for sidereal-servo specific protocol + */ + +#pragma once + +#include +#include + +#include "sidservo.h" + +/*********** base commands ***********/ +// get/set X/Y in motsteps +#define CMD_MOTX "X" +#define CMD_MOTY "Y" +// set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags) +#define CMD_MOTXYS "S" +// reset current motor position to given value (and stop, if moving) +#define CMD_MOTXSET "XF" +#define CMD_MOTYSET "YF" +// acceleration (per each loop, max: 3900) +#define CMD_MOTXACCEL "XR" +#define CMD_MOTYACCEL "YR" +// PID regulator: +// P: 0..32767 +#define CMD_PIDPX "XP" +#define CMD_PIDPY "YP" +// I: 0..32767 +#define CMD_PIDIX "XI" +#define CMD_PIDIY "YI" +// limit of I (doesn't work): 0:24000 (WTF???) +#define CMD_PIDILX "XL" +#define CMD_PIDILY "YL" +// D: 0..32767 +#define CMD_PIDDX "XD" +#define CMD_PIDDY "YD" +// current position error +#define CMD_POSERRX "XE" +#define CMD_POSERRY "YE" +// max position error limit (X: E#, Y: e#) +#define CMD_POSERRLIMX "XEL" +#define CMD_POSERRLIMY "YEL" +// current PWM output: 0..255 (or set max PWM out) +#define CMD_PWMOUTX "XO" +#define CMD_PWMOUTY "YO" +// motor current *100 (or set current limit): 0..240 +#define CMD_MOTCURNTX "XC" +#define CMD_MOTCURNTY "YC" +// change axis to Manual mode and set the PWM output: -255:255 +#define CMD_MANUALPWMX "XM" +#define CMD_MANUALPWMY "YM" +// change axis to Auto mode +#define CMD_AUTOX "XA" +#define CMD_AUTOY "YA" +// get positioin in encoders' ticks or reset it to given value +#define CMD_ENCX "XZ" +#define CMD_ENCY "YZ" +// get/set speed (geter x: S#, getter y: s#) +#define CMD_SPEEDX "XS" +#define CMD_SPEEDY "YS" +// normal stop X/Y +#define CMD_STOPX "XN" +#define CMD_STOPY "YN" +// lower speed -> drag&track or slew&track +#define CMD_STOPTRACKX "XNT" +#define CMD_STOPTRACKY "YNT" +// emergency stop +#define CMD_EMSTOPX "XG" +#define CMD_EMSTOPY "YG" +// get/set X/Ybits +#define CMD_BITSX "XB" +#define CMD_BITSY "YB" + +/*********** getters/setters without "Y" variant ***********/ +// get handpad status (decimal) +#define CMD_HANDPAD "XK" +// get TCPU (deg F) +#define CMD_TCPU "XH" +// get firmware version *10 +#define CMD_FIRMVER "XV" +// get motor voltage *10 +#define CMD_MOTVOLTAGE "XJ" +// get/set current CPU clock (milliseconds) +#define CMD_MILLIS "XY" +// reset servo +#define CMD_RESET "XQ" +// clear to factory defaults +#define CMD_CLRDEFAULTS "XU" +// save configuration to flash ROM +#define CMD_WRITEFLASH "XW" +// read config from flash to RAM +#define CMD_READFLASH "XT" +// write to flash following full config (128 bytes + 2 bytes of checksum) +#define CMD_PROGFLASH "FC" +// read configuration (-//-) +#define CMD_DUMPFLASH "SC" +// get serial number +#define CMD_SERIAL "YV" + +/*********** extended commands ***********/ +// get/set latitute +#define CMD_LATITUDE "XXL" +// getters/setters of motor's encoders per rev +#define CMD_MEPRX "XXU" +#define CMD_MEPRY "XXV" +// -//- axis encoders +#define CMD_AEPRX "XXT" +#define CMD_AEPRY "XXZ" +// get/set slew rate +#define CMD_SLEWRATEX "XXA" +#define CMD_SLEWRATEY "XXB" +// get/set pan rate +#define CMD_PANRATEX "XXC" +#define CMD_PANRATEY "XXD" +// get/set platform tracking rate +#define CMD_PLATRATE "XXE" +// get/set platform up/down adjuster +#define CMD_PLATADJ "XXF" +// get/set platform goal +#define CMD_PLATGOAL "XXG" +// get/set guide rate +#define CMD_GUIDERATEX "XXH" +#define CMD_GUIDERATEY "XXI" +// get/set picservo timeout (seconds) +#define CMD_PICTMOUT "XXJ" +// get/set digital outputs of radio handpad +#define CMD_RADIODIGOUT "XXQ" +// get/set argo navis mode +#define CMD_ARGONAVIS "XXN" +// get/set local search distance +#define CMD_LOSCRCHDISTX "XXM" +#define CMD_LOSCRCHDISTY "XXO" +// get/set backlash +#define CMD_BACKLASHX "XXO" +#define CMD_BACKLASHY "XXP" + + +// get binary data of all statistics +#define CMD_GETSTAT "XXS" +// send short command +#define CMD_SHORTCMD "XXR" +// send long command +#define CMD_LONGCMD "YXR" + + +/*********** special ***********/ +// exit ASCII checksum mode +#define CMD_EXITACM "YXY0\r\xb8" +// controller status: +// X# Y# XZ# YZ# XC# YC# V# T# X[AM] Y[AM] K# +// X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits +#define CMD_GETSTATTEXT "\r" + +// Loop freq +#define SITECH_LOOP_FREQUENCY (1953.) + +// amount of consequent same coordinates to detect stop +#define MOTOR_STOPPED_CNT (19) + +// replace macros with global variables inited when config read +extern int X_ENC_ZERO, Y_ENC_ZERO; +extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV; + +// TODO: take it from settings? +// steps per revolution (SSI - x4 - for SSI) +// -> hwconf.Xconf.mot/enc_stepsperrev +//#define X_MOT_STEPSPERREV_SSI (13312000.) +// 13312000 / 4 = 3328000 +//#define X_MOT_STEPSPERREV (3328000.) +//#define Y_MOT_STEPSPERREV_SSI (17578668.) +// 17578668 / 4 = 4394667 +//#define Y_MOT_STEPSPERREV (4394667.) + +// encoder per revolution +//#define X_ENC_STEPSPERREV (67108864.) +//#define Y_ENC_STEPSPERREV (67108864.) +// encoder zero position +// -> conf.XEncZero/YEncZero +//#define X_ENC_ZERO (61245239) +//#define Y_ENC_ZERO (36999830) +// encoder reversed (no: +1) -> sign of ...stepsperrev +//#define X_ENC_SIGN (-1.) +//#define Y_ENC_SIGN (-1.) + + +// encoder position to radians and back +#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV)) +#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV)) +#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV))) +#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV))) + +// convert angle in radians to +-pi +static inline __attribute__((always_inline)) double ang2half(double ang){ + if(ang < -M_PI) ang += 2.*M_PI; + else if(ang > M_PI) ang -= 2.*M_PI; + return ang; +} +// convert to only positive: 0..2pi +static inline __attribute__((always_inline)) double ang2full(double ang){ + if(ang < 0.) ang += 2.*M_PI; + else if(ang > 2.*M_PI) ang -= 2.*M_PI; + return ang; +} + +// motor position to radians and back +#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV)) +#define Y_MOT2RAD(n) ang2half(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. * (SITECH_LOOP_FREQUENCY)) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY)) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY))) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY))) +// motor acceleration -//- +#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY)) +#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY)) +#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY))) +#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY))) + +// adder time to seconds vice versa +#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY)) +#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY)) + +// 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 + xbits_t XBits; // 18 + ybits_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 + +typedef struct{ + uint32_t accel; // Default Acceleration (0..3900) + uint32_t backlash; // Backlash (???) + uint16_t errlimit; // Error Limit (0..32767) + uint16_t propgain; // Proportional Gain (0..32767) + uint16_t intgain; // Integral Gain (0..32767) + uint16_t derivgain; // Derivative Gain (0..32767) + uint16_t outplimit; // Output Limit, 0xFF = 100.0 (0..255) + uint16_t currlimit; // Current Limit * 100 (0..240) + uint16_t intlimit; // Integral Limit (0..24000) +} __attribute__((packed)) AxeConfig; + +typedef struct{ + AxeConfig Xconf; + xbits_t xbits; + uint8_t unused0; + AxeConfig Yconf; + ybits_t ybits; + uint8_t unused1; + uint8_t address; + uint8_t unused2; + uint32_t eqrate; // Equatorial Rate (platform?) (???) + int32_t eqadj; // Equatorial UpDown adjust (???) + uint32_t trackgoal; // Tracking Platform Goal (???) + uint16_t latitude; // Latitude * 100, MSB FIRST!! + uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev, MSB FIRST!! + uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev, MSB FIRST!! + uint32_t Ymetpr; // Azm Motor Ticks Per Rev, MSB FIRST!! + uint32_t Xmetpr; // Alt Motor Ticks Per Rev, MSB FIRST!! + int32_t Xslewrate; // Alt/Dec Slew Rate (rates are negative in "through the pole" mode!!!) + int32_t Yslewrate; // Azm/RA Slew Rate + int32_t Xpanrate; // Alt/Dec Pan Rate + int32_t Ypanrate; // Azm/RA Pan Rate + int32_t Xguiderate; // Alt/Dec Guide Rate + int32_t Yguiderate; // Azm/RA Guide Rate + uint8_t unknown0; // R/A PEC Auto Sync Enable (if low bit = 1), or PicServo Comm Timeout?? + uint8_t unused3; + uint8_t baudrate; // Baud Rate?? + uint8_t unused4; + uint8_t specmode; // 1 = Enable Argo Navis, 2 = Enable Sky Commander + uint8_t unused5; + uint32_t locsdeg; // Local Search Degrees * 100 + uint32_t locsspeed; // Local Search Speed, arcsec per sec (???) + uint32_t backlspd; // Backlash speed + uint32_t pecticks; // RA/Azm PEC Ticks + uint16_t unused6; + uint16_t checksum; +} __attribute__((packed)) SSconfig; + +uint16_t SScalcChecksum(uint8_t *buf, int len); +void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t); +int SStextcmd(const char *cmd, data_t *answer); +int SSrawcmd(const char *cmd, data_t *answer); +int SSgetint(const char *cmd, int64_t *ans); +int SSsetterI(const char *cmd, int32_t ival); +int SSstop(int emerg); +mcc_errcodes_t updateMotorPos(); diff --git a/asibfm700_common.h b/asibfm700_common.h new file mode 100644 index 0000000..821531b --- /dev/null +++ b/asibfm700_common.h @@ -0,0 +1,26 @@ +#pragma once + +/* AstroSib FORK MOUNT FM-700 CONTROL LIBRARY */ + + +/* COMMON LIBRARY DEFINITIONS */ + + +#include +#include +#include +#include + + +namespace asibfm700 +{ + +static constexpr mcc::MccMountType asibfm700MountType = mcc::MccMountType::FORK_TYPE; + + +typedef mcc::ccte::erfa::MccCCTE_ERFA Asibfm700CCTE; +typedef mcc::impl::MccDefaultPCM Asibfm700PCM; +typedef mcc::impl::MccPZoneContainer Asibfm700PZoneContainer; +typedef mcc::utils::MccSpdlogLogger Asibfm700Logger; + +} // namespace asibfm700 diff --git a/asibfm700_configfile.h b/asibfm700_configfile.h new file mode 100644 index 0000000..d70775c --- /dev/null +++ b/asibfm700_configfile.h @@ -0,0 +1,870 @@ +#pragma once + +/**/ + +#include +#include +#include + +#include +#include +#include +#include + +#include "asibfm700_common.h" +#include "asibfm700_servocontroller.h" + +namespace asibfm700 +{ + + +/* A SIMPLE "KEYWORD - VALUE" HOLDER CLASS SUITABLE TO STORE SOME APPLICATION CONFIGURATION */ + + +// to follow std::variant requirements (not references, not array, not void) +template +concept config_record_valid_type_c = requires { !std::is_array_v && !std::is_void_v && !std::is_reference_v; }; + +// simple minimal-requirement configuration record class +template +struct simple_config_record_t { + std::string_view key; + T value; + std::vector comment; +}; + + +/* ASTOROSIB FM700 MOUNT CONFIGURATION CLASS */ + +// configuration description and its defaults +static auto Asibfm700MountConfigDefaults = std::make_tuple( + // main cycle period in millisecs + simple_config_record_t{"hardwarePollingPeriod", std::chrono::milliseconds{100}, {"main cycle period in millisecs"}}, + + /* geographic coordinates of the observation site */ + + // site latitude in degrees + simple_config_record_t{"siteLatitude", mcc::impl::MccAngle(43.646711_degs), {"site latitude in degrees"}}, + + // site longitude in degrees + simple_config_record_t{"siteLongitude", mcc::impl::MccAngle(41.440732_degs), {"site longitude in degrees"}}, + + // site elevation in meters + simple_config_record_t{"siteElevation", 2070.0, {"site elevation in meters"}}, + + /* celestial coordinate transformation */ + + // wavelength at which refraction is calculated (in mkm) + simple_config_record_t{"refractWavelength", 0.55, {"wavelength at which refraction is calculated (in mkm)"}}, + + // an empty filename means default precompiled string + simple_config_record_t{"leapSecondFilename", std::string(), {"an empty filename means default precompiled string"}}, + + // an empty filename means default precompiled string + simple_config_record_t{"bulletinAFilename", std::string(), {"an empty filename means default precompiled string"}}, + + /* pointing correction model */ + + // PCM default type + simple_config_record_t{"pcmType", + mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY, + {"PCM type:", "GEOMETRY - 'classic' geometry-based correction coefficients", + "GEOMETRY-BSPLINE - previous one and additional 2D B-spline corrections", + "BSPLINE - pure 2D B-spline corrections"}}, + + // PCM geometrical coefficients + simple_config_record_t{"pcmGeomCoeffs", + std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {"PCM geometrical coefficients"}}, + + // PCM B-spline degrees + simple_config_record_t{"pcmBsplineDegree", std::vector{3, 3}, {"PCM B-spline degrees"}}, + + // PCM B-spline knots along X-axis (HA-angle or azimuth). By default from 0 to 2*PI radians + // NOTE: The first and last values are interpretated as border knots!!! + // Thus the array length must be equal to or greater than 2! + simple_config_record_t{"pcmBsplineXknots", + std::vector{0.0, 0.6981317, 1.3962634, 2.0943951, 2.7925268, 3.4906585, 4.1887902, + 4.88692191, 5.58505361, 6.28318531}, + {"PCM B-spline knots along X-axis (HA-angle or azimuth). By default from 0 to 2*PI radians", + "NOTE: The first and last values are interpretated as border knots!!!", + " Thus the array length must be equal to or greater than 2!"}}, + + // PCM B-spline knots along Y-axis (declination or zenithal distance). By default from -PI/6 to PI/2 radians + // NOTE: The first and last values are interpretated as border knots!!! + // Thus the array length must be equal to or greater than 2! + simple_config_record_t{ + "pcmBsplineYknots", + std::vector{-0.52359878, -0.29088821, -0.05817764, 0.17453293, 0.40724349, 0.63995406, 0.87266463, + 1.10537519, 1.33808576, 1.57079633}, + {"PCM B-spline knots along Y-axis (declination or zenithal distance). By default from -PI/6 to PI/2 radians", + "NOTE: The first and last values are interpretated as border knots!!!", + " Thus the array length must be equal to or greater than 2!"}}, + + // PCM B-spline coeffs for along X-axis (HA-angle or azimuth) + simple_config_record_t{"pcmBsplineXcoeffs", + std::vector{}, + {"PCM B-spline coeffs for along X-axis (HA-angle)"}}, + + // PCM B-spline coeffs for along Y-axis (declination or zenithal distance) + simple_config_record_t{"pcmBsplineYcoeffs", + std::vector{}, + {"PCM B-spline coeffs for along Y-axis (declination angle)"}}, + + + /* slewing and tracking parameters */ + + // // arcseconds per second + // simple_config_record_t{"sideralRate", 15.0410686}, + + // timeout for telemetry updating in milliseconds + simple_config_record_t{"telemetryTimeout", + std::chrono::milliseconds(3000), + {"timeout for telemetry updating in milliseconds"}}, + + // minimal allowed time in seconds to prohibited zone + simple_config_record_t{"minTimeToPZone", + std::chrono::seconds(10), + {"minimal allowed time in seconds to prohibited zone"}}, + + // a time interval to update prohibited zones related quantities (millisecs) + simple_config_record_t{"updatingPZoneInterval", + std::chrono::milliseconds(5000), + {"a time interval to update prohibited zones related quantities (millisecs)"}}, + + // coordinates difference in arcsecs to stop slewing + simple_config_record_t{"slewToleranceRadius", 5.0, {"coordinates difference in arcsecs to stop slewing"}}, + + simple_config_record_t{"slewingTelemetryInterval", + std::chrono::milliseconds(100), + {"telemetry request interval (in millisecs) in slewing mode"}}, + + simple_config_record_t{"slewingPathFilename", + std::string(), + {"slewing trajectory filename", "if it is an empty - just skip saving"}}, + + // target-mount coordinate difference in arcsecs to start adjusting of slewing + simple_config_record_t{"adjustCoordDiff", + 50.0, + {"target-mount coordinate difference in arcsecs to start adjusting of slewing"}}, + + // minimum time in millisecs between two successive adjustments + simple_config_record_t{"adjustCycleInterval", + std::chrono::milliseconds(300), + {"minimum time in millisecs between two successive adjustments"}}, + + // slew process timeout in seconds + simple_config_record_t{"slewTimeout", std::chrono::seconds(3600), {"slew process timeout in seconds"}}, + + // a time shift into future to compute target position in future (UT1-scale time duration, millisecs) + simple_config_record_t{ + "timeShiftToTargetPoint", + std::chrono::milliseconds(10000), + {"a time shift into future to compute target position in future (UT1-scale time duration, millisecs)"}}, + + + simple_config_record_t{"trackingTelemetryInterval", + std::chrono::milliseconds(100), + {"telemetry request interval (in millisecs) in tracking mode"}}, + + + // minimum time in millisecs between two successive tracking corrections + simple_config_record_t{"trackingCycleInterval", + std::chrono::milliseconds(300), + {"minimum time in millisecs between two successive tracking corrections"}}, + + // maximal valid target-to-mount distance for tracking process (arcsecs) + // if current distance is greater than assume current mount coordinate as target point + simple_config_record_t{"trackingMaxCoordDiff", + 20.0, + {"maximal valid target-to-mount distance for tracking process (arcsecs)", + "if current distance is greater than assume current mount coordinate as target point"}}, + + simple_config_record_t{"trackingPathFilename", + std::string(), + {"tracking trajectory filename", "if it is an empty - just skip saving"}}, + + + /* prohibited zones */ + + // minimal altitude + simple_config_record_t{"pzMinAltitude", mcc::impl::MccAngle(10.0_degs), {"minimal altitude"}}, + + // HA-axis limit switch minimal value + simple_config_record_t{"pzLimitSwitchHAMin", + mcc::impl::MccAngle(-270.0_degs), + {"HA-axis limit switch minimal value"}}, + + // HA-axis limit switch maximal value + simple_config_record_t{"pzLimitSwitchHAMax", + mcc::impl::MccAngle(270.0_degs), + {"HA-axis limit switch maximal value"}}, + + // DEC-axis limit switch minimal value + simple_config_record_t{"pzLimitSwitchDecMin", + mcc::impl::MccAngle(-90.0_degs), + {"DEC-axis limit switch minimal value"}}, + + // DEC-axis limit switch maximal value + simple_config_record_t{"pzLimitSwitchDecMax", + mcc::impl::MccAngle(90.0_degs), + {"DEC-axis limit switch maximal value"}}, + + + /* hardware-related */ + + // hardware mode: 1 - model mode, otherwise real mode + simple_config_record_t{"RunModel", 0, {"hardware mode: 1 - model mode, otherwise real mode"}}, + + // mount serial device paths + simple_config_record_t{"MountDevPath", std::string("/dev/ttyUSB0"), {"mount serial device paths"}}, + + // mount serial device speed + simple_config_record_t{"MountDevSpeed", 19200, {"mount serial device speed"}}, + + // motor encoders serial device path + simple_config_record_t{"EncoderDevPath", std::string(""), {"motor encoders serial device path"}}, + + // X-axis encoder serial device path + simple_config_record_t{"EncoderXDevPath", std::string("/dev/encoderX0"), {"X-axis encoder serial device path"}}, + + // Y-axis encoder serial device path + simple_config_record_t{"EncoderYDevPath", std::string("/dev/encoderY0"), {"Y-axis encoder serial device path"}}, + + // encoders serial device speed + simple_config_record_t{"EncoderDevSpeed", 153000, {"encoders serial device speed"}}, + + // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices + simple_config_record_t{ + "SepEncoder", + 2, + {"==1 if encoder works as separate serial device, ==2 if there's new version with two devices"}}, + + + // mount polling interval in millisecs + simple_config_record_t{"MountReqInterval", std::chrono::milliseconds(100), {"mount polling interval in millisecs"}}, + + // encoders polling interval in millisecs + simple_config_record_t{"EncoderReqInterval", + std::chrono::milliseconds(1), + {"encoders polling interval in millisecs"}}, + + // mount axes rate calculation interval in millisecs + simple_config_record_t{"EncoderSpeedInterval", + std::chrono::milliseconds(50), + {"mount axes rate calculation interval in millisecs"}}, + + simple_config_record_t{"PIDMaxDt", + std::chrono::milliseconds(1000), + {"maximal PID refresh time interval in millisecs", + "NOTE: if PID data will be refreshed with interval longer than this value (e.g. user polls " + "encoder data too rarely)", + "then the PID 'expired' data will be cleared and new computing loop is started"}}, + + simple_config_record_t{"PIDRefreshDt", std::chrono::milliseconds(100), {"PID refresh interval"}}, + + simple_config_record_t{"PIDCycleDt", + std::chrono::milliseconds(5000), + {"PID I cycle time (analog of 'RC' for PID on opamps)"}}, + + + + // X-axis coordinate PID P,I,D-params + simple_config_record_t{"XPIDC", std::vector{0.5, 0.1, 0.2}, {"X-axis coordinate PID P,I,D-params"}}, + + // X-axis rate PID P,I,D-params + simple_config_record_t{"XPIDV", std::vector{0.09, 0.0, 0.05}, {"X-axis rate PID P,I,D-params"}}, + + // Y-axis coordinate PID P, I, D-params + simple_config_record_t{"YPIDC", std::vector{0.5, 0.1, 0.2}, {"Y-axis coordinate PID P, I, D-params"}}, + + // Y-axis rate PID P,I,D-params + simple_config_record_t{"YPIDV", std::vector{0.09, 0.0, 0.05}, {"Y-axis rate PID P,I,D-params"}}, + + + // maximal moving rate (degrees per second) along HA-axis (Y-axis of Sidereal servo microcontroller) + simple_config_record_t{ + "hwMaxRateHA", + mcc::impl::MccAngle(8.0_degs), + {"maximal moving rate (degrees per second) along HA-axis (Y-axis of Sidereal servo microcontroller)"}}, + + // maximal moving rate (degrees per second) along DEC-axis (X-axis of Sidereal servo microcontroller) + simple_config_record_t{ + "hwMaxRateDEC", + mcc::impl::MccAngle(10.0_degs), + {"maximal moving rate (degrees per second) along DEC-axis (X-axis of Sidereal servo microcontroller)"}}, + + simple_config_record_t{"MaxPointingErr", + mcc::impl::MccAngle(8.0_degs), + {"slewing-to-pointing mode angular limit in degrees"}}, + + simple_config_record_t{"MaxFinePointingErr", + mcc::impl::MccAngle(1.5_degs), + {"pointing-to-guiding mode angular limit in degrees"}}, + + simple_config_record_t{"MaxGuidingErr", + mcc::impl::MccAngle(0.5_arcsecs), + {"guiding 'good'-flag error cirle radius (mount-to-target distance) in degrees"}}, + + simple_config_record_t{"XEncZero", mcc::impl::MccAngle(0.0_degs), {"X-axis encoder zero-point in degrees"}}, + + simple_config_record_t{"YEncZero", mcc::impl::MccAngle(0.0_degs), {"Y-axis encoder zero-point in degrees"}} + +); + + +class Asibfm700MountConfig : public mcc::utils::KeyValueHolder +{ + using base_t = mcc::utils::KeyValueHolder; + +protected: + inline static auto deserializer = [](std::string_view str, VT& value) { + std::error_code ec{}; + + mcc::utils::MccSimpleDeserializer deser; + deser.setRangeDelim(base_t::VALUE_ARRAY_DELIM); + + if constexpr (std::is_arithmetic_v || mcc::traits::mcc_output_char_range || std::ranges::range || + mcc::traits::mcc_time_duration_c) { + // ec = base_t::defaultDeserializeFunc(str, value); + ec = deser(str, value); + } else if constexpr (std::same_as) { // assume here all angles are in degrees + double vd; + // ec = base_t::defaultDeserializeFunc(str, vd); + ec = deser(str, vd); + if (!ec) { + value = mcc::impl::MccAngle(vd, mcc::impl::MccDegreeTag{}); + } + } else if constexpr (std::same_as) { + std::string vstr; + // ec = base_t::defaultDeserializeFunc(str, vstr); + ec = deser(str, vstr); + + if (!ec) { + auto s = mcc::utils::trimSpaces(vstr); + + if (s == mcc::impl::MccDefaultPCMTypeString) { + value = mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY; + } else if (s == mcc::impl::MccDefaultPCMTypeString< + mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE>) { + value = mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY; + } else if (s == mcc::impl::MccDefaultPCMTypeString) { + value = mcc::impl::MccDefaultPCMType::PCM_TYPE_BSPLINE; + } else { + ec = std::make_error_code(std::errc::invalid_argument); + } + } + } else { + ec = std::make_error_code(std::errc::invalid_argument); + } + + return ec; + }; + + +public: + /* the most usefull config fields */ + + template + DT hardwarePollingPeriod() const + { + return std::chrono::duration_cast
( + getValue("hardwarePollingPeriod").value_or(std::chrono::milliseconds{})); + }; + + std::chrono::milliseconds hardwarePollingPeriod() const + { + return hardwarePollingPeriod(); + }; + + template + T siteLatitude() const + { + return static_cast(getValue("siteLatitude").value_or(mcc::impl::MccAngle{})); + }; + + mcc::impl::MccAngle siteLatitude() const + { + return siteLatitude(); + }; + + template + T siteLongitude() const + { + return static_cast(getValue("siteLongitude").value_or(mcc::impl::MccAngle{})); + }; + + mcc::impl::MccAngle siteLongitude() const + { + return siteLongitude(); + }; + + template + T siteElevation() const + requires std::is_arithmetic_v + { + return getValue("siteElevation").value_or(0.0); + } + + double siteElevation() const + { + return getValue("siteElevation").value_or(0.0); + }; + + template + T refractWavelength() const + requires std::is_arithmetic_v + { + return getValue("refractWavelength").value_or(0.0); + } + + double refractWavelength() const + { + return getValue("refractWavelength").value_or(0.0); + }; + + template + R leapSecondFilename() const + { + R r; + + std::string val = getValue("leapSecondFilename").value_or(""); + std::ranges::copy(val, std::back_inserter(r)); + + return r; + } + + std::string leapSecondFilename() const + { + return leapSecondFilename(); + }; + + template + R bulletinAFilename() const + { + R r; + std::string val = getValue("bulletinAFilename").value_or(""); + std::ranges::copy(val, std::back_inserter(r)); + + return r; + } + + std::string bulletinAFilename() const + { + return bulletinAFilename(); + }; + + + template + T pzMinAltitude() const + { + return static_cast(getValue("pzMinAltitude").value_or(mcc::impl::MccAngle{})); + }; + + mcc::impl::MccAngle pzMinAltitude() const + { + return pzMinAltitude(); + }; + + template + T pzLimitSwitchHAMin() const + { + return static_cast(getValue("pzLimitSwitchHAMin").value_or(mcc::impl::MccAngle{})); + }; + + mcc::impl::MccAngle pzLimitSwitchHAMin() const + { + return pzLimitSwitchHAMin(); + }; + + template + T pzLimitSwitchHAMax() const + { + return static_cast(getValue("pzLimitSwitchHAMax").value_or(mcc::impl::MccAngle{})); + }; + + mcc::impl::MccAngle pzLimitSwitchHAMax() const + { + return pzLimitSwitchHAMax(); + }; + + + AsibFM700ServoController::hardware_config_t servoControllerConfig() const + { + AsibFM700ServoController::hardware_config_t hw_cfg; + + hw_cfg.hwConfig = {}; + + hw_cfg.MountDevPath = getValue("MountDevPath").value_or(std::string{}); + hw_cfg.EncoderDevPath = getValue("EncoderDevPath").value_or(std::string{}); + hw_cfg.EncoderXDevPath = getValue("EncoderXDevPath").value_or(std::string{}); + hw_cfg.EncoderYDevPath = getValue("EncoderYDevPath").value_or(std::string{}); + + hw_cfg.devConfig.MountDevPath = hw_cfg.MountDevPath.data(); + hw_cfg.devConfig.EncoderDevPath = hw_cfg.EncoderDevPath.data(); + hw_cfg.devConfig.EncoderXDevPath = hw_cfg.EncoderXDevPath.data(); + hw_cfg.devConfig.EncoderYDevPath = hw_cfg.EncoderYDevPath.data(); + + hw_cfg.devConfig.RunModel = getValue("RunModel").value_or(int{}); + hw_cfg.devConfig.MountDevSpeed = getValue("MountDevSpeed").value_or(int{}); + hw_cfg.devConfig.EncoderDevSpeed = getValue("EncoderDevSpeed").value_or(int{}); + hw_cfg.devConfig.SepEncoder = getValue("SepEncoder").value_or(int{}); + + std::chrono::duration secs; // seconds as floating-point + + secs = getValue("MountReqInterval").value_or(std::chrono::milliseconds{}); + hw_cfg.devConfig.MountReqInterval = secs.count(); + + secs = getValue("EncoderReqInterval").value_or(std::chrono::milliseconds{}); + hw_cfg.devConfig.EncoderReqInterval = secs.count(); + + secs = getValue("EncoderSpeedInterval").value_or(std::chrono::milliseconds{}); + hw_cfg.devConfig.EncoderSpeedInterval = secs.count(); + + secs = getValue("PIDMaxDt").value_or(std::chrono::milliseconds{1000}); + hw_cfg.devConfig.PIDMaxDt = secs.count(); + + secs = getValue("PIDRefreshDt").value_or(std::chrono::milliseconds{100}); + hw_cfg.devConfig.PIDRefreshDt = secs.count(); + + secs = getValue("PIDCycleDt").value_or(std::chrono::milliseconds{5000}); + hw_cfg.devConfig.PIDCycleDt = secs.count(); + + + std::vector pid = getValue>("XPIDC").value_or(std::vector{}); + if (pid.size() > 2) { + hw_cfg.devConfig.XPIDC.P = pid[0]; + hw_cfg.devConfig.XPIDC.I = pid[1]; + hw_cfg.devConfig.XPIDC.D = pid[2]; + } + + pid = getValue>("XPIDV").value_or(std::vector{}); + if (pid.size() > 2) { + hw_cfg.devConfig.XPIDV.P = pid[0]; + hw_cfg.devConfig.XPIDV.I = pid[1]; + hw_cfg.devConfig.XPIDV.D = pid[2]; + } + + pid = getValue>("YPIDC").value_or(std::vector{}); + if (pid.size() > 2) { + hw_cfg.devConfig.YPIDC.P = pid[0]; + hw_cfg.devConfig.YPIDC.I = pid[1]; + hw_cfg.devConfig.YPIDC.D = pid[2]; + } + + pid = getValue>("YPIDV").value_or(std::vector{}); + if (pid.size() > 2) { + hw_cfg.devConfig.YPIDV.P = pid[0]; + hw_cfg.devConfig.YPIDV.I = pid[1]; + hw_cfg.devConfig.YPIDV.D = pid[2]; + } + + double ang = getValue("MaxPointingErr").value_or(mcc::impl::MccAngle(8.0_degs)); + hw_cfg.devConfig.MaxPointingErr = ang; + + ang = getValue("MaxFinePointingErr").value_or(mcc::impl::MccAngle(1.5_degs)); + hw_cfg.devConfig.MaxFinePointingErr = ang; + + ang = getValue("MaxGuidingErr").value_or(mcc::impl::MccAngle(0.5_arcsecs)); + hw_cfg.devConfig.MaxGuidingErr = ang; + + ang = getValue("XEncZero").value_or(mcc::impl::MccAngle(0.0_degs)); + hw_cfg.devConfig.XEncZero = ang; + + + ang = getValue("YEncZero").value_or(mcc::impl::MccAngle(0.0_degs)); + hw_cfg.devConfig.YEncZero = ang; + + + + return hw_cfg; + } + + + mcc::impl::MccSimpleMovementControlsParameters movingModelParams() const + { + static constexpr double arcsecs2rad = std::numbers::pi / 180.0 / 3600.0; // arcseconds to radians + + mcc::impl::MccSimpleMovementControlsParameters pars; + + auto get_value = [this](std::string_view name, VT& val) { + val = getValue(name).value_or(val); + }; + + pars.telemetryTimeout = + getValue("telemetryTimeout").value_or(pars.telemetryTimeout); + + pars.minTimeToPZone = getValue("minTimeToPZone").value_or(pars.minTimeToPZone); + + pars.updatingPZoneInterval = getValue("updatingPZoneInterval") + .value_or(pars.updatingPZoneInterval); + + pars.slewToleranceRadius = + getValue("slewToleranceRadius").value_or(pars.slewToleranceRadius) * + arcsecs2rad; + + get_value("slewingTelemetryInterval", pars.slewingTelemetryInterval); + + pars.slewRateX = getValue("hwMaxRateHA").value_or(pars.slewRateX); + pars.slewRateY = getValue("hwMaxRateDEC").value_or(pars.slewRateY); + + pars.adjustCoordDiff = + getValue("adjustCoordDiff").value_or(pars.adjustCoordDiff) * arcsecs2rad; + + pars.adjustCycleInterval = + getValue("adjustCycleInterval").value_or(pars.adjustCycleInterval); + + pars.slewTimeout = getValue("slewTimeout").value_or(pars.slewTimeout); + + pars.slewingPathFilename = + getValue("slewingPathFilename").value_or(std::string()); + + get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval); + + pars.timeShiftToTargetPoint = getValue("timeShiftToTargetPoint") + .value_or(pars.timeShiftToTargetPoint); + + pars.trackingCycleInterval = getValue("trackingCycleInterval") + .value_or(pars.trackingCycleInterval); + + pars.trackingMaxCoordDiff = + getValue("trackingMaxCoordDiff").value_or(pars.trackingMaxCoordDiff) * + arcsecs2rad; + + pars.trackingPathFilename = + getValue("trackingPathFilename").value_or(std::string()); + + return pars; + } + + Asibfm700PCM::pcm_data_t pcmData() const + { + Asibfm700PCM::pcm_data_t pcm_data; + + std::vector empty_vec; + + pcm_data.type = getValue("pcmType").value_or(pcm_data.type); + + pcm_data.siteLatitude = getValue("siteLatitude").value_or(pcm_data.siteLatitude); + + std::vector vec = getValue>("pcmGeomCoeffs").value_or(empty_vec); + if (vec.size() >= 9) { // must be 9 coefficients + pcm_data.geomCoefficients = {.zeroPointX = vec[0], + .zeroPointY = vec[1], + .collimationErr = vec[2], + .nonperpendErr = vec[3], + .misalignErr1 = vec[4], + .misalignErr2 = vec[5], + .tubeFlexure = vec[6], + .forkFlexure = vec[7], + .DECaxisFlexure = vec[8]}; + } + +#ifdef USE_BSPLINE_PCM + std::vector dd = getValue("pcmBsplineDegree").value_or(dd); + if (dd.size() >= 2) { + pcm_data.bspline.bsplDegreeX = dd[0] > 0 ? dd[0] : 3; + pcm_data.bspline.bsplDegreeY = dd[1] > 0 ? dd[1] : 3; + } + + vec = getValue>("pcmBsplineXknots").value_or(empty_vec); + // pid must contains interior and border (single point for each border) knots so minimal length must be 2 + if (vec.size() >= 2) { + // generate full knots array (with border knots) + size_t Nknots = vec.size() + pcm_data.bspline.bsplDegreeX * 2 - 2; + pcm_data.bspline.knotsX.resize(Nknots); + + for (size_t i = 0; i <= pcm_data.bspline.bsplDegreeX; ++i) { // border knots + pcm_data.bspline.knotsX[i] = vec[0]; + pcm_data.bspline.knotsX[Nknots - i - 1] = vec.back(); + } + for (size_t i = 0; i < (vec.size() - 2); ++i) { // interior knots + pcm_data.bspline.knotsX[i + pcm_data.bspline.bsplDegreeX] = vec[1 + i]; + } + } + + vec = getValue>("pcmBsplineYknots").value_or(empty_vec); + // pid must contains interior and border (single point for each border) knots so minimal length must be 2 + if (vec.size() >= 2) { + // generate full knots array (with border knots) + size_t Nknots = vec.size() + pcm_data.bspline.bsplDegreeY * 2 - 2; + pcm_data.bspline.knotsY.resize(Nknots); + + for (size_t i = 0; i <= pcm_data.bspline.bsplDegreeY; ++i) { // border knots + pcm_data.bspline.knotsY[i] = vec[0]; + pcm_data.bspline.knotsY[Nknots - i - 1] = vec.back(); + } + for (size_t i = 0; i < (vec.size() - 2); ++i) { // interior knots + pcm_data.bspline.knotsY[i + pcm_data.bspline.bsplDegreeY] = vec[1 + i]; + } + } + + // minimal allowed number of B-spline coefficients + size_t Ncoeffs = pcm_data.type == mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY + ? 0 + : (pcm_data.bspline.knotsX.size() - pcm_data.bspline.bsplDegreeX - 1) * + (pcm_data.bspline.knotsY.size() - pcm_data.bspline.bsplDegreeY - 1); + + vec = getValue>("pcmBsplineXcoeffs").value_or(empty_vec); + + if (vec.size() >= Ncoeffs) { + pcm_data.bspline.coeffsX.resize(Ncoeffs); + for (size_t i = 0; i < Ncoeffs; ++i) { + pcm_data.bspline.coeffsX[i] = vec[i]; + } + } + + vec = getValue>("pcmBsplineYcoeffs").value_or(empty_vec); + + if (vec.size() >= Ncoeffs) { + pcm_data.bspline.coeffsY.resize(Ncoeffs); + for (size_t i = 0; i < Ncoeffs; ++i) { + pcm_data.bspline.coeffsY[i] = vec[i]; + } + } +#endif + return pcm_data; + } + + + Asibfm700MountConfig() : base_t(Asibfm700MountConfigDefaults) {} + + ~Asibfm700MountConfig() = default; + + std::error_code load(const std::filesystem::path& path) + { + std::string buffer; + + std::error_code ec; + auto sz = std::filesystem::file_size(path, ec); + + if (!ec && sz) { + std::ifstream fst(path); + + try { + buffer.resize(sz); + + fst.read(buffer.data(), sz); + + fst.close(); + + ec = base_t::fromCharRange(buffer, deserializer); + if (!ec) { + // remove possible spaces in filenames + + std::string val = getValue("leapSecondFilename").value_or(""); + auto fname = mcc::utils::trimSpaces(val); + setValue("leapSecondFilename", fname); + + + val = getValue("bulletinAFilename").value_or(""); + fname = mcc::utils::trimSpaces(val); + setValue("bulletinAFilename", fname); + + val = getValue("MountDevPath").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("MountDevPath", fname); + + val = getValue("EncoderDevPath").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("EncoderDevPath", fname); + + val = getValue("EncoderXDevPath").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("EncoderXDevPath", fname); + + val = getValue("EncoderYDevPath").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("EncoderYDevPath", fname); + + val = getValue("slewingPathFilename").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("slewingPathFilename", fname); + + val = getValue("trackingPathFilename").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("trackingPathFilename", fname); + } + } catch (std::ios_base::failure const& ex) { + ec = ex.code(); + } catch (std::length_error const& ex) { + ec = std::make_error_code(std::errc::no_buffer_space); + } catch (std::bad_alloc const& ex) { + ec = std::make_error_code(std::errc::not_enough_memory); + } catch (...) { + ec = std::make_error_code(std::errc::operation_canceled); + } + } + + return ec; + } + + bool dumpDefaultsToFile(const std::filesystem::path& path) + { + std::ofstream fst(path); + if (!fst.is_open()) { + return false; + } + + fst << "#\n"; + fst << "# ASTROSIB FM-700 MOUNT CONFIGURATION\n" << "#\n"; + fst << "# (created at " << std::format("{:%FT%T UTC}", std::chrono::system_clock::now()) << ")\n"; + fst << "#\n"; + + auto wrec = [&fst, this]() { + fst << "\n"; + for (size_t i = 0; i < std::get(_keyValue).comment.size(); ++i) { + fst << "# " << std::get(_keyValue).comment[i] << "\n"; + } + fst << std::get(_keyValue).key << " = "; + auto v = std::get(_keyValue).value; + using v_t = std::remove_cvref_t; + + if constexpr (std::is_arithmetic_v || mcc::traits::mcc_char_range) { + fst << std::format("{}", v); + } else if constexpr (mcc::traits::mcc_time_duration_c) { + fst << std::format("{}", v.count()); + } else if constexpr (mcc::mcc_angle_c) { + fst << std::format("{}", mcc::impl::MccAngle(static_cast(v)).degrees()); + } else if constexpr (std::same_as) { + if (v == mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY) { + fst << mcc::impl::MccDefaultPCMTypeString; + } else if (v == mcc::impl::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) { + fst << mcc::impl::MccDefaultPCMTypeString; + } else if (v == mcc::impl::MccDefaultPCMType::PCM_TYPE_BSPLINE) { + fst << mcc::impl::MccDefaultPCMTypeString; + } + } else if constexpr (std::ranges::range && std::formattable, char>) { + size_t sz = std::ranges::size(v); + if (!sz) { + return; + } + --sz; + + auto it = v.begin(); + for (size_t j = 0; j < sz; ++j, ++it) { + fst << std::format("{}", *it) << base_t::VALUE_ARRAY_DELIM; + } + fst << std::format("{}", *it); + } else if constexpr (std::formattable) { + fst << std::format("{}", v); + } else { + static_assert(false, "INVALID TYPE!"); + } + + fst << "\n"; + }; + + [&wrec](std::index_sequence) { + (wrec.operator()(), ...); + }(std::make_index_sequence>()); + + fst.close(); + + return true; + }; +}; + + + +} // namespace asibfm700 diff --git a/asibfm700_mount.cpp b/asibfm700_mount.cpp new file mode 100644 index 0000000..788cbcf --- /dev/null +++ b/asibfm700_mount.cpp @@ -0,0 +1,337 @@ +#include "asibfm700_mount.h" + +#include +#include + +namespace asibfm700 +{ + + +/* CONSTRUCTOR AND DESTRUCTOR */ + +Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr logger) + : _servolController(config.servoControllerConfig()), + _pcm(config.pcmData()), + gm_class_t(std::make_tuple(&_servolController, &_pcm), + std::make_tuple(), + std::make_tuple(&_servolController, + this, + [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }), + std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)), + _mountConfig(config), + _mountConfigMutex(new std::mutex) +{ + gm_class_t::addMarkToPatternIdx("[ASIB-MOUNT]"); + + logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId()); + + initMount(); +} + +Asibfm700Mount::~Asibfm700Mount() +{ + logDebug("Delete Asibfm700Mount class instance ({})", this->getThreadId()); +} + + + +/* PUBIC METHODS */ + +Asibfm700Mount::error_t Asibfm700Mount::initMount() +{ + std::lock_guard lock{*_mountConfigMutex}; + + logInfo("Init AstroSib FM-700 mount with configuration:"); + logInfo(" site latitude: {}", _mountConfig.siteLatitude().sexagesimal()); + logInfo(" site longitude: {}", _mountConfig.siteLongitude().sexagesimal()); + logInfo(" site elevation: {} meters", _mountConfig.siteElevation()); + logInfo(" refraction wavelength: {} mkm", _mountConfig.refractWavelength()); + logInfo(" leap seconds filename: {}", _mountConfig.leapSecondFilename()); + logInfo(" IERS Bulletin A filename: {}", _mountConfig.bulletinAFilename()); + + logInfo(""); + logDebug("Delete previously defined prohobited zones"); + clearPZones(); + logInfo("Add prohibited zones ..."); + + logInfo(" Add MccAltLimitPZ zone: min alt = {}, lat = {} (pzone type: '{}')", + _mountConfig.pzMinAltitude().degrees(), _mountConfig.siteLatitude().degrees(), + "Minimal altitude prohibited zone"); + addPZone(mcc::impl::MccAltLimitPZ{_mountConfig.pzMinAltitude(), + _mountConfig.siteLatitude()}); + + logInfo(" Add MccAxisLimitSwitchPZ zone: min value = {}, max value = {} (pzone type: '{}')", + _mountConfig.pzLimitSwitchHAMin().degrees(), _mountConfig.pzLimitSwitchHAMax().degrees(), + "HA-axis limit switch"); + size_t pz_num = addPZone(mcc::impl::MccAxisLimitSwitchPZ{ + _mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), &_pcm}); + + logInfo("{} prohibited zones were added successfully", pz_num); + + auto mpars = _mountConfig.movingModelParams(); + + using secs_t = std::chrono::duration; + auto to_msecs = [](double secs) { + auto s = secs_t{secs}; + return std::chrono::duration_cast(s); + }; + + auto hw_cfg = _mountConfig.servoControllerConfig(); + logInfo(""); + logInfo("Hardware initialization ..."); + logInfo(" set hardware configuration:"); + logInfo(" RunModel: {}", hw_cfg.devConfig.RunModel == 1 ? "MODEL-MODE" : "REAL-MODE"); + logInfo(" mount dev path: {}", hw_cfg.MountDevPath); + logInfo(" encoder dev path: {}", hw_cfg.EncoderDevPath); + logInfo(" encoder X-dev path: {}", hw_cfg.EncoderXDevPath); + logInfo(" encoder Y-dev path: {}", hw_cfg.EncoderYDevPath); + + logInfo(" EncoderDevSpeed: {}", hw_cfg.devConfig.EncoderDevSpeed); + logInfo(" SepEncoder: {}", hw_cfg.devConfig.SepEncoder); + logInfo(" MountReqInterval: {}", to_msecs(hw_cfg.devConfig.MountReqInterval)); + logInfo(" EncoderReqInterval: {}", to_msecs(hw_cfg.devConfig.EncoderReqInterval)); + logInfo(" EncoderSpeedInterval: {}", to_msecs(hw_cfg.devConfig.EncoderSpeedInterval)); + logInfo(" PIDMaxDt: {}", to_msecs(hw_cfg.devConfig.PIDMaxDt)); + logInfo(" PIDRefreshDt: {}", to_msecs(hw_cfg.devConfig.PIDRefreshDt)); + logInfo(" PIDCycleDt: {}", to_msecs(hw_cfg.devConfig.PIDCycleDt)); + + logInfo(" XPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDC.P, hw_cfg.devConfig.XPIDC.I, + hw_cfg.devConfig.XPIDC.D); + logInfo(" XPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDV.P, hw_cfg.devConfig.XPIDV.I, + hw_cfg.devConfig.XPIDV.D); + logInfo(" YPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDC.P, hw_cfg.devConfig.YPIDC.I, + hw_cfg.devConfig.YPIDC.D); + logInfo(" YPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDV.P, hw_cfg.devConfig.YPIDV.I, + hw_cfg.devConfig.YPIDV.D); + logInfo(" XEncZero: {}", hw_cfg.devConfig.XEncZero); + logInfo(" YEncZero: {}", hw_cfg.devConfig.YEncZero); + + // actually, only set this->_hardwareConfig.devConfig part and paths!!! + // this->_hardwareConfig = hw_cfg; + _servolController.hardwareUpdateConfig(hw_cfg.devConfig); + + logInfo(""); + logInfo(" EEPROM data:"); + + if (hw_cfg.devConfig.RunModel != 1) { // load EEPROM only in REAL HARDWARE mode + // load EEPROM part + auto cfg_err = _servolController.hardwareUpdateConfig(); + if (cfg_err) { + errorLogging("Cannot load EEPROM data:", cfg_err); + return cfg_err; + } + + hw_cfg = _servolController.getHardwareConfig(); + + mcc::impl::MccAngle ang{hw_cfg.hwConfig.Yconf.accel}; // Sidereal defines HA-axis as Y-axis + + logInfo(" HA-axis accel: {} degs/s^2", ang.degrees()); + ang = hw_cfg.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis + logInfo(" DEC-axis accel: {} degs/s^2", ang.degrees()); + logInfo(" HA-axis backlash: {}", (double)hw_cfg.hwConfig.Yconf.backlash); + logInfo(" DEC-axis backlash: {}", (double)hw_cfg.hwConfig.Xconf.backlash); + + logInfo(" HA-axis encoder ticks per revolution: {}", + hw_cfg.hwConfig.Ysetpr); // Sidereal defines HA-axis as Y-axis + logInfo(" DEC-axis encoder ticks per revolution: {}", + hw_cfg.hwConfig.Xsetpr); // Sidereal defines DEC-axis as X-axis + logInfo(" HA-motor encoder ticks per revolution: {}", + hw_cfg.hwConfig.Ymetpr); // Sidereal defines HA-axis as Y-axis + logInfo(" DEC-motor encoder ticks per revolution: {}", + hw_cfg.hwConfig.Xmetpr); // Sidereal defines DEC-axis as X-axis + + ang = hw_cfg.hwConfig.Yslewrate; // Sidereal defines HA-axis as Y-axis + logInfo(" HA-axis slew rate: {} degs/s", ang.degrees()); + ang = hw_cfg.hwConfig.Xslewrate; // Sidereal defines DEC-axis as X-axis + logInfo(" DEC-axis slew rate: {} degs/s", ang.degrees()); + } else { + logWarn(" MODEL-MODE, no EEPROM data!"); + } + + logInfo(""); + logInfo("Setup slewing and tracking parameters ..."); + mpars.slewRateX = _mountConfig.getValue("hwMaxRateHA").value_or(0.0); + mpars.slewRateY = _mountConfig.getValue("hwMaxRateDEC").value_or(0.0); + if (hw_cfg.devConfig.RunModel != 1) { + mpars.brakingAccelX = hw_cfg.hwConfig.Yconf.accel; // Sidereal defines HA-axis as Y-axis + mpars.brakingAccelY = hw_cfg.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis + // + } else { // set model's default values + mpars.brakingAccelX = 0.165806; // Sidereal defines HA-axis as Y-axis + mpars.brakingAccelY = 0.219911; // Sidereal defines DEC-axis as X-axis + + hw_cfg.hwConfig.Xslewrate = mpars.slewRateY; + hw_cfg.hwConfig.Yslewrate = mpars.slewRateX; + hw_cfg.hwConfig.Xconf.accel = mpars.brakingAccelY; + hw_cfg.hwConfig.Yconf.accel = mpars.brakingAccelX; + } + + auto max_dt_intvl = _mountConfig.getValue("PIDMaxDt").value_or({}); + auto min_dt_intvl = _mountConfig.getValue("PIDRefreshDt").value_or({}); + + // check for polling interval consistency + auto intvl = mpars.slewingTelemetryInterval; + if (intvl > max_dt_intvl) { + mpars.slewingTelemetryInterval = max_dt_intvl; + logWarn( + " slewingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal " + "allowed one: {} ms", + intvl.count(), max_dt_intvl.count()); + } + if (intvl < min_dt_intvl) { + mpars.slewingTelemetryInterval = min_dt_intvl; + logWarn( + " slewingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal allowed " + "one: {} ms", + intvl.count(), min_dt_intvl.count()); + } + + intvl = mpars.trackingTelemetryInterval; + if (intvl > max_dt_intvl) { + mpars.trackingTelemetryInterval = max_dt_intvl; + logWarn( + " trackingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal " + "allowed one: {} ms", + intvl.count(), max_dt_intvl.count()); + } + if (intvl < min_dt_intvl) { + mpars.trackingTelemetryInterval = min_dt_intvl; + logWarn( + " trackingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal " + "allowed one: {} ms", + intvl.count(), min_dt_intvl.count()); + } + + + auto st_err = setMovementParams(mpars); + if (st_err) { + errorLogging(" An error occured while setting slewing parameters: ", st_err); + } else { + logInfo(" Max HA-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateX).degrees()); + logInfo(" Max DEC-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateY).degrees()); + logInfo(" HA-axis stop acceleration braking: {} degs/s^2", + mcc::impl::MccAngle(mpars.brakingAccelX).degrees()); + logInfo(" DEC-axis stop acceleration braking: {} degs/s^2", + mcc::impl::MccAngle(mpars.brakingAccelY).degrees()); + + logInfo(" Slewing telemetry polling interval: {} millisecs", mpars.slewingTelemetryInterval.count()); + } + // st_err = setMovementParams(_mountConfig.movingModelParams()); + // if (st_err) { + // errorLogging(" An error occured while setting tracking parameters: ", st_err); + // } else { + // logInfo(" Tracking telemetry polling interval: {} millisecs", mpars.trackingTelemetryInterval.count()); + // } + logInfo("Slewing and tracking parameters have been set successfully"); + + + // update Eddy's LibSidServo internal config + _servolController.hardwareUpdateConfig(hw_cfg.hwConfig); + + coordpair_t cp; + Mount.getMaxSpeed(&cp); + logInfo("Check mount max speed: {} {}", cp.Y, cp.X); + + _servolController.hardwareInit(); + + // call base class initMount method + auto hw_err = gm_class_t::initMount(); + // auto hw_err = base_gm_class_t::initMount(); + if (hw_err) { + errorLogging("", hw_err); + return hw_err; + } else { + logInfo("Hardware initialization was performed sucessfully!"); + } + + logInfo("ERFA engine initialization ..."); + + + auto ccte_state = mcc::impl::MccSkyPoint::cctEngine.getStateERFA(); + mcc::impl::MccSkyPoint::cctEngine.setStateERFA({.meteo = ccte_state.meteo, // just use of previous values + .wavelength = _mountConfig.refractWavelength(), + .lat = _mountConfig.siteLatitude(), + .lon = _mountConfig.siteLongitude(), + .elev = _mountConfig.siteElevation()}); + + // set ERFA state + // Asibfm700CCTE::engine_state_t ccte_state{ + // .meteo = Asibfm700CCTE::_currentState.meteo, // just use of previous values + // .wavelength = _mountConfig.refractWavelength(), + // .lat = _mountConfig.siteLatitude(), + // .lon = _mountConfig.siteLongitude(), + // .elev = _mountConfig.siteElevation()}; + + + if (_mountConfig.leapSecondFilename().size()) { // load leap seconds file + logInfo("Loading leap second file: '{}' ...", _mountConfig.leapSecondFilename()); + bool ok = ccte_state._leapSeconds.load(_mountConfig.leapSecondFilename()); + if (ok) { + logInfo("Leap second file was loaded successfully (expire date: {})", ccte_state._leapSeconds.expireDate()); + } else { + logError("Leap second file loading failed! Using hardcoded defauls (expire date: {})", + ccte_state._leapSeconds.expireDate()); + } + } else { + logInfo("Using hardcoded leap seconds defauls (expire date: {})", ccte_state._leapSeconds.expireDate()); + } + + if (_mountConfig.bulletinAFilename().size()) { // load IERS Bulletin A file + logInfo("Loading IERS Bulletin A file: '{}' ...", _mountConfig.bulletinAFilename()); + bool ok = ccte_state._bulletinA.load(_mountConfig.bulletinAFilename()); + if (ok) { + logInfo("IERS Bulletin A file was loaded successfully (date range: {} - {})", + ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end); + } else { + logError("IERS Bulletin A file loading failed! Using hardcoded defauls (date range: {} - {})", + ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end); + } + } else { + logInfo("Using hardcoded IERS Bulletin A defauls (date range: {} - {})", + ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end); + } + + + // setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod()); + setTelemetryDataTimeout(_mountConfig.movingModelParams().telemetryTimeout); + + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + telemetry_data_t tdata; + auto t_err = telemetryData(&tdata); + if (t_err) { + logError(gm_class_t::formatError(t_err, "Cannot update telemetry data: ")); + + return t_err; + } + + return mcc::impl::MccGenericMountErrorCode::ERROR_OK; +} + + +Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg) +{ + std::lock_guard lock{*_mountConfigMutex}; + + _mountConfig = cfg; + + auto hw_cfg = _mountConfig.servoControllerConfig(); + _servolController.hardwareUpdateConfig(hw_cfg.devConfig); + _servolController.hardwareUpdateConfig(hw_cfg.hwConfig); + + return AsibFM700ServoControllerErrorCode::ERROR_OK; +} + + +/* PROTECTED METHODS */ + +void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code& err) +{ + if (msg.empty()) { + logError("{}::{} ({})", err.category().name(), err.value(), err.message()); + } else { + logError("{}: {}::{} ({})", msg, err.category().name(), err.value(), err.message()); + } +} + +} // namespace asibfm700 diff --git a/asibfm700_mount.h b/asibfm700_mount.h new file mode 100644 index 0000000..d0373f7 --- /dev/null +++ b/asibfm700_mount.h @@ -0,0 +1,184 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "asibfm700_common.h" +#include "asibfm700_configfile.h" + +namespace asibfm700 +{ + + + +class Asibfm700Mount : public mcc::impl::MccGenericMount, + Asibfm700PZoneContainer, + mcc::impl::MccSimpleMovementControls, + Asibfm700Logger> +{ + typedef mcc::impl::MccGenericMount, + Asibfm700PZoneContainer, + mcc::impl::MccSimpleMovementControls, + Asibfm700Logger> + gm_class_t; + +public: + using gm_class_t::error_t; + + // using Asibfm700CCTE::setStateERFA; + // using Asibfm700CCTE::updateBulletinA; + // using Asibfm700CCTE::updateLeapSeconds; + // using Asibfm700CCTE::updateMeteoERFA; + + using gm_class_t::logCritical; + using gm_class_t::logDebug; + using gm_class_t::logError; + using gm_class_t::logInfo; + using gm_class_t::logWarn; + // using Asibfm700Logger::logCritical; + // using Asibfm700Logger::logDebug; + // using Asibfm700Logger::logError; + // using Asibfm700Logger::logInfo; + // using Asibfm700Logger::logWarn; + + // using Asibfm700PZoneContainer::addPZone; + + Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr logger); + + ~Asibfm700Mount(); + + Asibfm700Mount(Asibfm700Mount&&) = default; + Asibfm700Mount& operator=(Asibfm700Mount&&) = default; + + Asibfm700Mount(const Asibfm700Mount&) = delete; + Asibfm700Mount& operator=(const Asibfm700Mount&) = delete; + + error_t initMount(); + + error_t updateMountConfig(Asibfm700MountConfig const&); + Asibfm700MountConfig currentMountConfig(); + +protected: + Asibfm700MountConfig _mountConfig; + std::unique_ptr _mountConfigMutex; + + AsibFM700ServoController _servolController; + Asibfm700PCM _pcm; + + void errorLogging(const std::string&, const std::error_code&); +}; + + +/* +class Asibfm700Mount : public Asibfm700CCTE, + public Asibfm700PCM, + public mcc::MccGenericFsmMount> +{ + typedef mcc::MccGenericMount + gm_class_t; + + typedef mcc::MccGenericFsmMount> + base_gm_class_t; + +protected: + struct Asibfm700ErrorState : base_gm_class_t::MccGenericFsmMountBaseState { + static constexpr std::string_view ID{"ASIBFM700-MOUNT-ERROR-STATE"}; + + // void exit(MccGenericFsmMountErrorEvent& event) + // { + // event.mount()->logWarn("The mount already in error state!"); + // } + + void enter(MccGenericFsmMountErrorEvent& event) + { + enterLog(event); + + // event.mount()->logWarn("The mount already in error state!"); + auto err = event.eventData(); + event.mount()->logError("An error occured: {} [{} {}]", err.message(), err.value(), err.category().name()); + } + + void exit(mcc::fsm::traits::fsm_event_c auto& event) + { + exitLog(event); + } + + void enter(mcc::fsm::traits::fsm_event_c auto& event) + { + enterLog(event); + + // ... + } + + using transition_t = mcc::fsm::fsm_transition_table_t< + std::pair, + std::pair>, + std::pair>>; + }; + + + typedef base_gm_class_t::MccGenericFsmMountStartState Asibfm700StartState; + +public: + using base_gm_class_t::error_t; + + using Asibfm700CCTE::setStateERFA; + using Asibfm700CCTE::updateBulletinA; + using Asibfm700CCTE::updateLeapSeconds; + using Asibfm700CCTE::updateMeteoERFA; + + using Asibfm700Logger::logCritical; + using Asibfm700Logger::logDebug; + using Asibfm700Logger::logError; + using Asibfm700Logger::logInfo; + using Asibfm700Logger::logWarn; + + // using Asibfm700PZoneContainer::addPZone; + + Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr logger); + + ~Asibfm700Mount(); + + Asibfm700Mount(Asibfm700Mount&&) = default; + Asibfm700Mount& operator=(Asibfm700Mount&&) = default; + + Asibfm700Mount(const Asibfm700Mount&) = delete; + Asibfm700Mount& operator=(const Asibfm700Mount&) = delete; + + error_t initMount(); + + error_t updateMountConfig(Asibfm700MountConfig const&); + Asibfm700MountConfig currentMountConfig(); + +protected: + Asibfm700MountConfig _mountConfig; + std::unique_ptr _mountConfigMutex; + + void errorLogging(const std::string&, const std::error_code&); +}; +*/ + +// static_assert(mcc::mcc_position_controls_c, ""); +// static_assert(mcc::mcc_all_controls_c, ""); + +static_assert(mcc::mcc_generic_mount_c, ""); + +} // namespace asibfm700 diff --git a/asibfm700_netserver.cpp b/asibfm700_netserver.cpp new file mode 100644 index 0000000..3e7fef2 --- /dev/null +++ b/asibfm700_netserver.cpp @@ -0,0 +1,73 @@ +#include "asibfm700_netserver.h" + + +namespace asibfm700 +{ + +Asibfm700MountNetServer::Asibfm700MountNetServer(asio::io_context& ctx, + Asibfm700Mount& mount, + std::shared_ptr logger) + : base_t(ctx, mount, std::move(logger), Asibfm700Logger::LOGGER_DEFAULT_FORMAT) +{ + addMarkToPatternIdx("[ASIB-NETSERVER]"); + + // to avoid possible compiler optimization (one needs to catch 'mount' strictly by reference) + auto* mount_ptr = &mount; + + base_t::_handleMessageFunc = [mount_ptr, this](std::string_view command) { + // using mount_error_t = typename Asibfm700Mount::error_t; + std::error_code err{}; + + Asibfm700NetMessage input_msg; + using output_msg_t = Asibfm700NetMessage; + output_msg_t output_msg; + + auto nn = std::this_thread::get_id(); + + auto ec = parseMessage(command, input_msg); + + if (ec) { + output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, ec); + } else { + if (input_msg.withKey(ASIBFM700_COMMPROTO_KEYWORD_METEO_STR)) { + // what is operation type (set or get)? + if (input_msg.paramSize()) { // set operation + auto vp = input_msg.paramValue(0); + if (vp) { + // mount_ptr->updateMeteoERFA(vp.value()); + mcc::impl::MccSkyPoint::cctEngine.updateMeteoERFA(vp.value()); + + output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, input_msg.byteRepr()); + } else { + output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, vp.error()); + } + } else { // get operation + // output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, + // ASIBFM700_COMMPROTO_KEYWORD_METEO_STR, mount_ptr->getStateERFA().meteo); + output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, + ASIBFM700_COMMPROTO_KEYWORD_METEO_STR, + mcc::impl::MccSkyPoint::cctEngine.getStateERFA().meteo); + } + } else if (input_msg.withKey(ASIBFM700_COMMPROTO_KEYWORD_SITEGEO_STR)) { + mcc::impl::MccAngleLON lon{mcc::impl::MccSkyPoint::cctEngine.getStateERFA().lon}; + mcc::impl::MccAngleLAT lat{mcc::impl::MccSkyPoint::cctEngine.getStateERFA().lat}; + + // std::tuple lonlat{lon, lat}; + + output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, + ASIBFM700_COMMPROTO_KEYWORD_SITEGEO_STR, lon, lat); + } + + else { + // basic network message processing + output_msg = base_t::handleMessage(input_msg, mount_ptr); + } + } + + return output_msg.template byteRepr(); + }; +} + +Asibfm700MountNetServer::~Asibfm700MountNetServer() {} + +} // namespace asibfm700 diff --git a/asibfm700_netserver.h b/asibfm700_netserver.h new file mode 100644 index 0000000..0f1297c --- /dev/null +++ b/asibfm700_netserver.h @@ -0,0 +1,136 @@ +#pragma once + +#include +#include + +#include "asibfm700_common.h" +#include "asibfm700_mount.h" + + +// define serializer and deserializer for meteo parameters +namespace mcc::impl +{ + +template <> +struct MccSerializer : MccSerializerBase { + constexpr static std::string_view serializerName{"ASIBFM700-ERFA-METEO-SERIALIZER"}; + + template + error_t operator()(traits::mcc_output_char_range auto& output, + asibfm700::Asibfm700CCTE::meteo_t const& value, + ParamsT const& params = mcc_serialization_params_t{}) + { + std::vector vec{value.temperature, value.humidity, value.pressure}; + + auto err = MccSerializer{}(output, vec, params); + if (err) { + return mcc_deduced_err(err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER); + } + + return MccSerializerErrorCode::ERROR_OK; + } +}; + + +template <> +struct MccDeserializer : MccDeserializerBase { + static constexpr std::string_view deserializerName{"ASIBFM700-ERFA-METEO-DESERIALIZER"}; + + template + error_t operator()(traits::mcc_input_char_range auto const& input, + asibfm700::Asibfm700CCTE::meteo_t& value, + ParamsT const& params = mcc_serialization_params_t{}) + { + std::vector v; + + auto err = MccDeserializer{}(input, v, params); + if (err) { + return mcc_deduced_err(err, MccDeserializerErrorCode::ERROR_UNDERLYING_DESERIALIZER); + } + + if (v.size() < 3) { + return MccDeserializerErrorCode::ERROR_INVALID_SERIALIZED_VALUE; + } + + value.temperature = v[0]; + value.humidity = v[1]; + value.pressure = v[2]; + + return MccDeserializerErrorCode::ERROR_OK; + } +}; + +} // namespace mcc::impl + + + +namespace asibfm700 +{ + +namespace details +{ + +template +static constexpr auto merge_arrays(const std::array& arr1, const std::array& arr2) +{ + constexpr auto N = N1 + N2; + std::array res; + + for (size_t i = 0; i < N1; ++i) { + res[i] = arr1[i]; + } + + for (size_t i = N1; i < N; ++i) { + res[i] = arr2[i - N1]; + } + + return res; +} + +} // namespace details + +// meteo parameters (ambient air temperature, humidity and atmosperic pressure) +constexpr static std::string_view ASIBFM700_COMMPROTO_KEYWORD_METEO_STR{"METEO"}; +// current site geodetic longtude and latitude +constexpr static std::string_view ASIBFM700_COMMPROTO_KEYWORD_SITEGEO_STR{"SITEGEO"}; + +struct Asibfm700NetMessageValidKeywords { + static constexpr std::array NETMSG_VALID_KEYWORDS = details::merge_arrays( + mcc::network::MccNetMessageValidKeywords::NETMSG_VALID_KEYWORDS, + std::array{ASIBFM700_COMMPROTO_KEYWORD_METEO_STR, ASIBFM700_COMMPROTO_KEYWORD_SITEGEO_STR}); + + // hashes of valid keywords + static constexpr std::array NETMSG_VALID_KEYWORD_HASHES = [](std::index_sequence) { + return std::array{mcc::utils::FNV1aHash(NETMSG_VALID_KEYWORDS[Is])...}; + }(std::make_index_sequence()); + + constexpr static const size_t* isKeywordValid(std::string_view key) + { + const auto hash = mcc::utils::FNV1aHash(key); + + for (auto const& h : NETMSG_VALID_KEYWORD_HASHES) { + if (h == hash) { + return &h; + } + } + + return nullptr; + } +}; + + +template +using Asibfm700NetMessage = mcc::network::MccNetMessage; + + +class Asibfm700MountNetServer : public mcc::network::MccGenericMountNetworkServer +{ + using base_t = mcc::network::MccGenericMountNetworkServer; + +public: + Asibfm700MountNetServer(asio::io_context& ctx, Asibfm700Mount& mount, std::shared_ptr logger); + + ~Asibfm700MountNetServer(); +}; + +} // namespace asibfm700 diff --git a/asibfm700_netserver_endpoint.h b/asibfm700_netserver_endpoint.h new file mode 100644 index 0000000..6b9834e --- /dev/null +++ b/asibfm700_netserver_endpoint.h @@ -0,0 +1,507 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "mcc_traits.h" + +namespace asibfm700 +{ + +namespace utils +{ + +static constexpr bool charSubrangeCompare(const mcc::traits::mcc_char_view auto& what, + const mcc::traits::mcc_char_view auto& where, + bool case_insensitive = false) +{ + if (std::ranges::size(what) == std::ranges::size(where)) { + if (case_insensitive) { + auto f = std::ranges::search(where, + std::views::transform(what, [](const char& ch) { return std::tolower(ch); })); + return !f.empty(); + } else { + auto f = std::ranges::search(where, what); + return !f.empty(); + } + } + + return false; +} + +} // namespace utils + +/* + * Very simple various protocols endpoint parser and holder class + * + * endpoint: proto_mark://host_name:port_num/path + * where "part" is optional for all non-local protocol kinds; + * + * for local kind of protocols the endpoint must be given as: + * local://stream/PATH + * local://seqpacket/PATH + * local://serial/PATH + * where 'stream' and 'seqpacket' "host_name"-field marks the + * stream-type and seqpacket-type UNIX domain sockets protocols; + * 'serial' marks a serial (RS232/485) protocol. + * here, possible "port_num" field is allowed but ignored. + * + * NOTE: "proto_mark" and "host_name" (for local kind) fields are parsed in case-insensitive manner! + * + * EXAMPLES: tcp://192.168.70.130:3131 + * local://serial/dev/ttyS1 + * local://seqpacket/tmp/BM70_SERVER_SOCK + * + * + */ + + +class Asibfm700NetserverEndpoint +{ +public: + static constexpr std::string_view protoHostDelim = "://"; + static constexpr std::string_view hostPortDelim = ":"; + static constexpr std::string_view portPathDelim = "/"; + + enum proto_id_t : uint8_t { + PROTO_ID_LOCAL, + PROTO_ID_SEQLOCAL, + PROTO_ID_SERLOCAL, + PROTO_ID_TCP, + PROTO_ID_TLS, + PROTO_ID_UNKNOWN + }; + + static constexpr std::string_view protoMarkLocal{"local"}; // UNIX domain + static constexpr std::string_view protoMarkTCP{"tcp"}; // TCP + static constexpr std::string_view protoMarkTLS{"tls"}; // TLS + + static constexpr std::array validProtoMarks{protoMarkLocal, protoMarkTCP, protoMarkTLS}; + + + static constexpr std::string_view localProtoTypeStream{"stream"}; // UNIX domain stream + static constexpr std::string_view localProtoTypeSeqpacket{"seqpacket"}; // UNIX domain seqpacket + static constexpr std::string_view localProtoTypeSerial{"serial"}; // serial (RS232/485) + + static constexpr std::array validLocalProtoTypes{localProtoTypeStream, localProtoTypeSeqpacket, + localProtoTypeSerial}; + + + template + Asibfm700NetserverEndpoint(const R& ept) + { + fromRange(ept); + } + + Asibfm700NetserverEndpoint(const Asibfm700NetserverEndpoint& other) + { + copyInst(other); + } + + Asibfm700NetserverEndpoint(Asibfm700NetserverEndpoint&& other) + { + moveInst(std::move(other)); + } + + virtual ~Asibfm700NetserverEndpoint() = default; + + + Asibfm700NetserverEndpoint& operator=(const Asibfm700NetserverEndpoint& other) + { + copyInst(other); + + return *this; + } + + + Asibfm700NetserverEndpoint& operator=(Asibfm700NetserverEndpoint&& other) + { + moveInst(std::move(other)); + + return *this; + } + + template + requires std::ranges::contiguous_range + bool fromRange(const R& ept) + { + _isValid = false; + + // at least 'ws://a' (proto, proto-host delimiter and at least a single character of hostname) + if (std::ranges::size(ept) < 6) { + return _isValid; + } + + if constexpr (std::is_array_v>) { + _endpoint = ept; + } else { + _endpoint.clear(); + std::ranges::copy(ept, std::back_inserter(_endpoint)); + } + + auto found = std::ranges::search(_endpoint, protoHostDelim); + if (found.empty()) { + return _isValid; + } + + + ssize_t idx; + if ((idx = checkProtoMark(std::string_view{_endpoint.begin(), found.begin()})) < 0) { + return _isValid; + } + + _proto = validProtoMarks[idx]; + + _host = std::string_view{found.end(), _endpoint.end()}; + + auto f1 = std::ranges::search(_host, portPathDelim); + // std::string_view port_sv; + if (f1.empty() && isLocal()) { // no path, but it is mandatory for 'local'! + return _isValid; + } else { + _host = std::string_view(_host.begin(), f1.begin()); + + _path = std::string_view(f1.end(), &*_endpoint.end()); + + f1 = std::ranges::search(_host, hostPortDelim); + if (f1.empty() && !isLocal()) { // no port, but it is mandatory for non-local! + return _isValid; + } + + _portView = std::string_view(f1.end(), _host.end()); + if (_portView.size()) { + _host = std::string_view(_host.begin(), f1.begin()); + + if (!isLocal()) { + // convert port string to int + auto end_ptr = _portView.data() + _portView.size(); + + auto [ptr, ec] = std::from_chars(_portView.data(), end_ptr, _port); + if (ec != std::errc() || ptr != end_ptr) { + return _isValid; + } + } else { // ignore for local + _port = -1; + } + } else { + _port = -1; + } + + if (isLocal()) { // check for special values + idx = 0; + if (std::ranges::any_of(validLocalProtoTypes, [&idx, this](const auto& el) { + bool ok = utils::charSubrangeCompare(_host, el, true); + if (!ok) { + ++idx; + } + return ok; + })) { + _host = validLocalProtoTypes[idx]; + } else { + return _isValid; + } + } + } + + _isValid = true; + + return _isValid; + } + + + bool isValid() const + { + return _isValid; + } + + + auto endpoint() const + { + return _endpoint; + } + + template + R proto() const + { + return part(PROTO_PART); + } + + std::string_view proto() const + { + return proto(); + } + + template + R host() const + { + return part(HOST_PART); + } + + std::string_view host() const + { + return host(); + } + + int port() const + { + return _port; + } + + template + R portView() const + { + return part(PORT_PART); + } + + std::string_view portView() const + { + return portView(); + } + + template + R path(RR&& root_path) const + { + if (_path.empty()) { + if constexpr (mcc::traits::mcc_output_char_range) { + R res; + std::ranges::copy(std::forward(root_path), std::back_inserter(res)); + + return res; + } else { // can't add root path!!! + return part(PATH_PART); + } + } + + auto N = std::ranges::distance(root_path.begin(), root_path.end()); + + if (N) { + R res; + std::filesystem::path pt(root_path.begin(), root_path.end()); + + if (isLocal() && _path[0] == '\0') { + std::ranges::copy(std::string_view(" "), std::back_inserter(res)); + pt /= _path.substr(1); + std::ranges::copy(pt.string(), std::back_inserter(res)); + *res.begin() = '\0'; + } else { + pt /= _path; + std::ranges::copy(pt.string(), std::back_inserter(res)); + } + + return res; + } else { + return part(PATH_PART); + } + } + + template + std::string path(RR&& root_path) const + { + return path(std::forward(root_path)); + } + + template + R path() const + { + return part(PATH_PART); + } + + std::string_view path() const + { + return path(); + } + + + bool isLocal() const + { + return proto() == protoMarkLocal; + } + + bool isLocalStream() const + { + return host() == localProtoTypeStream; + } + + bool isLocalSerial() const + { + return host() == localProtoTypeSerial; + } + + bool isLocalSeqpacket() const + { + return host() == localProtoTypeSeqpacket; + } + + + bool isTCP() const + { + return proto() == protoMarkTCP; + } + + bool isTLS() const + { + return proto() == protoMarkTLS; + } + + + // add '\0' char (or replace special-meaning char/char-sequence) to construct UNIX abstract namespace + // endpoint path + template + Asibfm700NetserverEndpoint& makeAbstract(const T& mark = nullptr) + requires(mcc::traits::mcc_input_char_range || std::same_as, char> || + std::is_null_pointer_v>) + { + if (!(isLocalStream() || isLocalSeqpacket())) { // only local proto is valid! + return *this; + } + + if constexpr (std::is_null_pointer_v) { // just insert '\0' + auto it = _endpoint.insert(std::string::const_iterator(_path.begin()), '\0'); + _path = std::string_view(it, _endpoint.end()); + } else if constexpr (std::same_as, char>) { // replace a character (mark) + auto pos = std::distance(_endpoint.cbegin(), std::string::const_iterator(_path.begin())); + if (_endpoint[pos] == mark) { + _endpoint[pos] = '\0'; + } + } else { // replace a character range (mark) + if (std::ranges::equal(_path | std::views::take(std::ranges::size(mark), mark))) { + auto pos = std::distance(_endpoint.cbegin(), std::string::const_iterator(_path.begin())); + _endpoint.replace(pos, std::ranges::size(mark), 1, '\0'); + _path = std::string_view(_endpoint.begin() + pos, _endpoint.end()); + } + } + + return *this; + } + +protected: + std::string _endpoint; + std::string_view _proto, _host, _path, _portView; + int _port; + bool _isValid; + + + virtual ssize_t checkProtoMark(std::string_view proto_mark) + { + ssize_t idx = 0; + + // case-insensitive look-up + bool found = + std::ranges::any_of(Asibfm700NetserverEndpoint::validProtoMarks, [&idx, &proto_mark](const auto& el) { + bool ok = utils::charSubrangeCompare(proto_mark, el, true); + + if (!ok) { + ++idx; + } + + return ok; + }); + + return found ? idx : -1; + } + + enum EndpointPart { PROTO_PART, HOST_PART, PATH_PART, PORT_PART }; + + template + R part(EndpointPart what) const + { + R res; + + // if (!_isValid) { + // return res; + // } + + auto part = _proto; + + switch (what) { + case PROTO_PART: + part = _proto; + break; + case HOST_PART: + part = _host; + break; + case PATH_PART: + part = _path; + break; + case PORT_PART: + part = _portView; + break; + default: + break; + } + + if constexpr (std::ranges::view) { + return {part.begin(), part.end()}; + } else { + std::ranges::copy(part, std::back_inserter(res)); + } + + return res; + } + + void copyInst(const Asibfm700NetserverEndpoint& other) + { + if (&other != this) { + if (other._isValid) { + _isValid = other._isValid; + _endpoint = other._endpoint; + _proto = other._proto; + + std::iterator_traits::difference_type idx; + if (other.isLocal()) { // for 'local' host is one of static class constants + _host = other._host; + } else { + idx = std::distance(other._endpoint.c_str(), other._host.data()); + _host = std::string_view(_endpoint.c_str() + idx, other._host.size()); + } + + idx = std::distance(other._endpoint.c_str(), other._path.data()); + _path = std::string_view(_endpoint.c_str() + idx, other._path.size()); + + idx = std::distance(other._endpoint.c_str(), other._portView.data()); + _portView = std::string_view(_endpoint.c_str() + idx, other._portView.size()); + + _port = other._port; + } else { + _isValid = false; + _endpoint = std::string(); + _proto = std::string_view(); + _host = std::string_view(); + _path = std::string_view(); + _portView = std::string_view(); + _port = -1; + } + } + } + + + void moveInst(Asibfm700NetserverEndpoint&& other) + { + if (&other != this) { + if (other._isValid) { + _isValid = std::move(other._isValid); + _endpoint = std::move(other._endpoint); + _proto = other._proto; + _host = std::move(other._host); + _path = std::move(other._path); + _port = std::move(other._port); + _portView = std::move(other._portView); + } else { + _isValid = false; + _endpoint = std::string(); + _proto = std::string_view(); + _host = std::string_view(); + _path = std::string_view(); + _portView = std::string_view(); + _port = -1; + } + } + } +}; + +} // namespace asibfm700 diff --git a/asibfm700_netserver_main.cpp b/asibfm700_netserver_main.cpp new file mode 100644 index 0000000..b491bdb --- /dev/null +++ b/asibfm700_netserver_main.cpp @@ -0,0 +1,179 @@ +#include +#include + +#include +#include + +#include + +// #include +#include "asibfm700_netserver.h" +#include "mcc/mcc_netserver_endpoint.h" + + +int main(int argc, char* argv[]) +{ + /* COMMANDLINE OPTS */ + cxxopts::Options options(argv[0], "Astrosib (c) BM700 mount server\n"); + + options.allow_unrecognised_options(); + + options.add_options()("h,help", "Print usage"); + + options.add_options()("D,daemon", "Demonize server"); + + options.add_options()("l,log", "Log filename (use stdout and stderr for standard output and error stream)", + cxxopts::value()->default_value("")); + + options.add_options()("level", "Log level (see SPDLOG package description for valid values)", + cxxopts::value()->default_value("info")); + + options.add_options()("c,config", "Mount configuration filename (by default use of hardcoded one)", + cxxopts::value()->default_value("")); + + options.add_options()("dump", "Dump mount default configuration to file and exit", + cxxopts::value()->default_value("")); + + options.add_options()("M,model", "Ran server in model mode (explicitely set config.RunModel=1)"); + + options.add_options()( + "endpoints", + "endpoints server will be listening for. For 'local' endpoint the '@' symbol at the beginning of the path " + "means " + "abstract namespace socket.", + cxxopts::value>()->default_value("local://stream/@FM700_SERVER")); + + + options.positional_help("[endpoint0] [enpoint1] ... [endpointN]"); + options.parse_positional({"endpoints"}); + + asio::io_context ctx(8); + // asio::io_context ctx; + + + try { + auto opt_result = options.parse(argc, argv); + + if (opt_result["help"].count()) { + std::cout << options.help(); + std::cout << "\n"; + std::cout << "[endpoint0] [enpoint1] ... [endpointN] - endpoints server will be listening for. For 'local' " + "endpoint the '@' symbol at the beginning of the path " + "means abstract namespace socket (e.g. local://stream/@ASIBFM700_SERVER)." + << "\n"; + return 0; + } + + asibfm700::Asibfm700MountConfig mount_cfg; + + std::string fname = opt_result["dump"].as(); + if (fname.size()) { + bool ok = mount_cfg.dumpDefaultsToFile(fname); + if (!ok) { + return 255; + } + + return 0; + } else { + // just ignore + } + + auto logname = opt_result["log"].as(); + + auto logger = [&logname]() { + if (logname == "stdout") { + return spdlog::stdout_color_mt("console"); + } else if (logname == "stderr") { + return spdlog::stderr_color_mt("stderr"); + } else if (logname == "") { + return spdlog::null_logger_mt("FM700_SERVER_NULL_LOGGER"); + } else { + return spdlog::basic_logger_mt(logname, logname); + } + }(); + + std::string level_str = opt_result["level"].as(); + std::ranges::transform(level_str, level_str.begin(), [](const char& c) { return std::tolower(c); }); + + auto log_level = spdlog::level::from_str(level_str); + logger->set_level(log_level); + logger->flush_on(spdlog::level::trace); + + + logger->set_pattern("%v"); + int w = 90; + // const std::string fmt = std::format("{{:*^{}}}", w); + constexpr std::string_view fmt = "{:*^90}"; + logger->info("\n\n\n"); + logger->info(fmt, ""); + logger->info(fmt, " ASTROSIB FM700 MOUNT SERVER "); + auto zt = std::chrono::zoned_time(std::chrono::current_zone(), + std::chrono::floor(std::chrono::system_clock::now())); + logger->info(fmt, std::format(" {} ", zt)); + logger->info(fmt, ""); + logger->info("\n"); + + + logger->set_pattern("[%Y-%m-%d %T.%e][%l]: %v"); + std::string mount_cfg_fname = opt_result["config"].as(); + if (mount_cfg_fname.size()) { + logger->info("Try to load mount configuration from file: {}", mount_cfg_fname); + auto err = mount_cfg.load(mount_cfg_fname); + if (err) { + logger->error("Cannot load mount configuration (err = {})! Use of defaults!", err.message()); + } else { + logger->info("Mount configuration was loaded successfully!"); + } + logger->info("\n"); + } + + if (opt_result["model"].count()) { + mount_cfg.setValue("RunModel", 1); + } + + asibfm700::Asibfm700Mount mount(mount_cfg, logger); + asibfm700::Asibfm700MountNetServer server(ctx, mount, logger); + + server.setupSignals(); + + if (opt_result["daemon"].count()) { + server.daemonize(); + } + + // mcc::MccServerEndpoint epn(std::string_view("local://seqpacket/tmp/BM700_SERVER_SOCK")); + // mcc::MccServerEndpoint epn(std::string_view("local://stream/tmp/BM700_SERVER_SOCK")); + // mcc::MccServerEndpoint epn(std::string_view("local://stream/@tmp/BM700_SERVER_SOCK")); + // mcc::MccServerEndpoint epn(std::string_view("tcp://localhost:12345")); + + // asio::co_spawn(ctx, server.listen(epn), asio::detached); + + auto epnts = opt_result["endpoints"].as>(); + + for (auto& epnt : epnts) { + mcc::network::MccNetServerEndpoint ep(epnt); + + if (ep.isValid()) { + ep.makeAbstract('@'); + + asio::co_spawn(ctx, server.listen(ep), asio::detached); + } else { + std::cerr << "Unrecognized endpoint: '" << epnt << "'! Ignore!\n"; + } + } + + + // asio::thread_pool pool(5); + + // asio::post(pool, [&ctx]() { ctx.run(); }); + + // pool.join(); + ctx.run(); + + } catch (const std::system_error& ex) { + std::cerr << "An error occured: " << ex.code().message() << "\n"; + return ex.code().value(); + } catch (...) { + std::cerr << "Unhandled exceptions!\n"; + return 255; + } +} diff --git a/asibfm700_servocontroller.cpp b/asibfm700_servocontroller.cpp new file mode 100644 index 0000000..936afb1 --- /dev/null +++ b/asibfm700_servocontroller.cpp @@ -0,0 +1,266 @@ +#include "asibfm700_servocontroller.h" +#include + +namespace asibfm700 +{ + +const char* AsibFM700ServoControllerErrorCategory::name() const noexcept +{ + return "ASIBFM700-SERVOCONTROLLER-ERROR-CATEGORY"; +} + +std::string AsibFM700ServoControllerErrorCategory::message(int ec) const +{ + AsibFM700ServoControllerErrorCode err = static_cast(ec); + + switch (err) { + case AsibFM700ServoControllerErrorCode::ERROR_OK: + return "OK"; + case AsibFM700ServoControllerErrorCode::ERROR_FATAL: + return "LibSidServo fatal error"; + case AsibFM700ServoControllerErrorCode::ERROR_BADFORMAT: + return "LibSidServo wrong arguments of function"; + case AsibFM700ServoControllerErrorCode::ERROR_ENCODERDEV: + return "LibSidServo encoder device error or can't open"; + case AsibFM700ServoControllerErrorCode::ERROR_MOUNTDEV: + return "LibSidServo mount device error or can't open"; + case AsibFM700ServoControllerErrorCode::ERROR_FAILED: + return "LibSidServo failed to run command"; + case AsibFM700ServoControllerErrorCode::ERROR_NULLPTR: + return "nullptr argument"; + case AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT: + return "polling timeout"; + + default: + return "UNKNOWN"; + } +} + + +const AsibFM700ServoControllerErrorCategory& AsibFM700ServoControllerErrorCategory::get() +{ + static const AsibFM700ServoControllerErrorCategory constInst; + return constInst; +} + + + +AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {} + +AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController() +{ + _hardwareConfig = std::move(config); + + _hardwareConfig.devConfig.MountDevPath = const_cast(_hardwareConfig.MountDevPath.c_str()); + _hardwareConfig.devConfig.EncoderDevPath = const_cast(_hardwareConfig.EncoderDevPath.c_str()); + _hardwareConfig.devConfig.EncoderXDevPath = const_cast(_hardwareConfig.EncoderXDevPath.c_str()); + _hardwareConfig.devConfig.EncoderYDevPath = const_cast(_hardwareConfig.EncoderYDevPath.c_str()); +} + + +AsibFM700ServoController::~AsibFM700ServoController() {} + + +// AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop() +// { +// error_t err = static_cast(Mount.stop()); +// if (err) { +// return err; +// } + +// hardware_state_t hw_state; + +// auto start_tp = std::chrono::steady_clock::now(); + +// // poll hardware till stopped-state detected ... +// while (true) { +// err = hardwareGetState(&hw_state); +// if (err) { +// return err; +// } + +// if (hw_state.movementState == hardware_movement_state_t::HW_MOVE_STOPPED) { +// break; +// } + +// if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) { +// err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT; +// break; +// } + +// std::this_thread::sleep_for(_hardwareConfig.pollingInterval); +// } + +// return err; +// } + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit() +{ + return static_cast(Mount.init(&_hardwareConfig.devConfig)); +} + + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t const& state) +{ + std::lock_guard lock{*_setStateMutex}; + + if (state.movementState == hardware_movement_state_t::HW_MOVE_STOPPING) { // stop! + error_t err = static_cast(Mount.stop()); + if (err) { + return err; + } + + hardware_state_t hw_state; + + auto start_tp = std::chrono::steady_clock::now(); + + // poll hardware till stopped-state detected ... + while (true) { + err = hardwareGetState(&hw_state); + if (err) { + return err; + } + + if (hw_state.movementState == hardware_movement_state_t::HW_MOVE_STOPPED) { + break; + } + + if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) { + err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT; + break; + } + + std::this_thread::sleep_for(_hardwareConfig.pollingInterval); + } + + return err; + } + + // static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}}; + // static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0}; + + // cvalpair.X = {.val = state.Y, .t = tp}; + // cvalpair.Y = {.val = state.X, .t = tp}; + + // cpair.X = state.tagY; + // cpair.Y = state.tagX; + + // time point from sidservo library is 'double' number represented UNIXTIME with + // microseconds/nanoseconds precision + // double tp = std::chrono::duration(state.time_point.time_since_epoch()).count(); + + + // 2025-12-04: coordval_pair_t.X.t is now of type struct timespec + auto ns = std::chrono::duration_cast(state.XY.epoch().UTC().time_since_epoch()); + auto secs = std::chrono::floor(ns); + ns -= secs; + std::timespec tp{.tv_sec = secs.count(), .tv_nsec = ns.count()}; + + // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis + coordval_pair_t cvalpair{.X{.val = state.XY.y(), .t = tp}, .Y{.val = state.XY.x(), .t = tp}}; + + + // correctTo is asynchronous function!!! + // + // according to the Eddy's implementation of the LibSidServo library it is safe + // to pass the addresses of 'cvalpair' and 'cpair' automatic variables + // auto err = static_cast(Mount.correctTo(&cvalpair, &cpair)); + auto err = static_cast(Mount.correctTo(&cvalpair)); + + return err; +} + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state) +{ + if (state == nullptr) { + return AsibFM700ServoControllerErrorCode::ERROR_NULLPTR; + } + + using tp_t = decltype(state->XY.epoch().UTC()); + using d_t = tp_t::duration; + + mountdata_t mdata; + + error_t err = static_cast(Mount.getMountData(&mdata)); + if (!err) { + auto dr = std::chrono::duration_cast(std::chrono::seconds(mdata.encXposition.t.tv_sec) + + std::chrono::nanoseconds(mdata.encXposition.t.tv_nsec)); + + mcc::impl::MccCelestialCoordEpoch ep; + ep.fromTimePoint(tp_t{dr}); + + // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis + state->XY = mcc::impl::MccGenXY{mdata.encYposition.val, mdata.encXposition.val, ep}; + state->speedXY = mcc::impl::MccGenXY{mdata.encYspeed.val, mdata.encXspeed.val, ep}; + + if (mdata.Xstate == AXIS_ERROR || mdata.Ystate == AXIS_ERROR) { + state->movementState = hardware_movement_state_t::HW_MOVE_ERROR; + } else { + if (mdata.Xstate == AXIS_STOPPED) { + if (mdata.Ystate == AXIS_STOPPED) { + state->movementState = hardware_movement_state_t::HW_MOVE_STOPPED; + } else if (mdata.Ystate == AXIS_SLEWING) { + state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING; + } else if (mdata.Ystate == AXIS_POINTING) { + state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING; + } else if (mdata.Ystate == AXIS_GUIDING) { + state->movementState = hardware_movement_state_t::HW_MOVE_GUIDING; + } else { + state->movementState = hardware_movement_state_t::HW_MOVE_UNKNOWN; + } + } else if (mdata.Xstate == AXIS_SLEWING) { + state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING; + } else if (mdata.Xstate == AXIS_POINTING) { + if (mdata.Ystate == AXIS_SLEWING) { + state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING; + } else { + state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING; + } + } else if (mdata.Xstate == AXIS_GUIDING) { + if (mdata.Ystate == AXIS_SLEWING) { + state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING; + } else if (mdata.Ystate == AXIS_POINTING) { + state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING; + } else { + state->movementState = hardware_movement_state_t::HW_MOVE_GUIDING; + } + } else { + state->movementState = hardware_movement_state_t::HW_MOVE_UNKNOWN; + } + } + } + + return err; +} + + +void AsibFM700ServoController::hardwareUpdateConfig(conf_t cfg) +{ + _hardwareConfig.devConfig = std::move(cfg); + + _hardwareConfig.devConfig.MountDevPath = const_cast(_hardwareConfig.MountDevPath.c_str()); + _hardwareConfig.devConfig.EncoderDevPath = const_cast(_hardwareConfig.EncoderDevPath.c_str()); + _hardwareConfig.devConfig.EncoderXDevPath = const_cast(_hardwareConfig.EncoderXDevPath.c_str()); + _hardwareConfig.devConfig.EncoderYDevPath = const_cast(_hardwareConfig.EncoderYDevPath.c_str()); +} + + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig(hardware_configuration_t cfg) +{ + _hardwareConfig.hwConfig = std::move(cfg); + return static_cast(Mount.saveHWconfig(&_hardwareConfig.hwConfig)); +} + + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig() +{ + return static_cast(Mount.getHWconfig(&_hardwareConfig.hwConfig)); +} + + +AsibFM700ServoController::hardware_config_t AsibFM700ServoController::getHardwareConfig() const +{ + return _hardwareConfig; +} + +} // namespace asibfm700 diff --git a/asibfm700_servocontroller.h b/asibfm700_servocontroller.h new file mode 100644 index 0000000..eb9aa6d --- /dev/null +++ b/asibfm700_servocontroller.h @@ -0,0 +1,160 @@ +#pragma once + +// #include +// #include + +#include +#include + +#include "LibSidServo/sidservo.h" +#include "asibfm700_common.h" + + +namespace asibfm700 +{ + +/* error codes enum definition */ + +enum class AsibFM700ServoControllerErrorCode : int { + // error codes from sidservo library + ERROR_OK = MCC_E_OK, + ERROR_FATAL = MCC_E_FATAL, + ERROR_BADFORMAT = MCC_E_BADFORMAT, + ERROR_ENCODERDEV = MCC_E_ENCODERDEV, + ERROR_MOUNTDEV = MCC_E_MOUNTDEV, + ERROR_FAILED = MCC_E_FAILED, + // my codes ... + ERROR_POLLING_TIMEOUT, + ERROR_NULLPTR +}; + +// error category +struct AsibFM700ServoControllerErrorCategory : public std::error_category { + const char* name() const noexcept; + std::string message(int ec) const; + + static const AsibFM700ServoControllerErrorCategory& get(); +}; + + +static inline std::error_code make_error_code(AsibFM700ServoControllerErrorCode ec) +{ + return std::error_code(static_cast(ec), AsibFM700ServoControllerErrorCategory::get()); +} + +} // namespace asibfm700 + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + +namespace asibfm700 +{ + +class AsibFM700ServoController +{ +public: + static constexpr mcc::MccMountType hwMountType{mcc::MccMountType::FORK_TYPE}; + + static constexpr std::string_view hardwareName = "Sidereal-ServoControllerII"; + + typedef std::error_code error_t; + + enum class hardware_movement_state_t : int { + HW_MOVE_ERROR = -1, + HW_MOVE_STOPPED = 0, + HW_MOVE_STOPPING, + HW_MOVE_SLEWING, + HW_MOVE_ADJUSTING, + HW_MOVE_TRACKING, + HW_MOVE_GUIDING, + HW_MOVE_UNKNOWN + }; + + // typedef AsibFM700ServoControllerMovementState hardware_movement_state_t; + + struct hardware_state_t { + mcc::impl::MccGenXY XY{0.0, 0.0}, speedXY{0.0, 0.0}; + hardware_movement_state_t movementState{hardware_movement_state_t::HW_MOVE_STOPPED}; + }; + + + struct hardware_config_t { + // the 'char*' fields from conf_t: + // wrap it to std::string + std::string MountDevPath; + std::string EncoderDevPath; + std::string EncoderXDevPath; + std::string EncoderYDevPath; + + conf_t devConfig; // devices paths and PIDs parameters + hardware_configuration_t hwConfig; // EEPROM-located configuration + + std::chrono::milliseconds pollingInterval{300}; // hardware polling interval + std::chrono::milliseconds pollingTimeout{30000}; // hardware polling timeout + }; + + /* constructors and destructor */ + + AsibFM700ServoController(); + + AsibFM700ServoController(hardware_config_t config); + + AsibFM700ServoController(const AsibFM700ServoController&) = delete; + AsibFM700ServoController& operator=(const AsibFM700ServoController&) = delete; + + AsibFM700ServoController(AsibFM700ServoController&&) = default; + AsibFM700ServoController& operator=(AsibFM700ServoController&&) = default; + + virtual ~AsibFM700ServoController(); + + /* public methods */ + + error_t hardwareSetState(hardware_state_t const& state); + error_t hardwareGetState(hardware_state_t* state); + + error_t hardwareInit(); + + void hardwareUpdateConfig(conf_t cfg); + + // save config to EEPROM + error_t hardwareUpdateConfig(hardware_configuration_t cfg); + // load config from EEPROM + error_t hardwareUpdateConfig(); + + hardware_config_t getHardwareConfig() const; + +protected: + hardware_config_t _hardwareConfig; + + std::unique_ptr _setStateMutex; +}; + +} // namespace asibfm700 + +template <> +struct std::formatter + : std::formatter { + auto format(asibfm700::AsibFM700ServoController::hardware_movement_state_t e, auto& ctx) const + { + return formatter::format( + e == asibfm700::AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_ERROR ? "ERROR" + : e == asibfm700::AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED ? "STOPPED" + : e == asibfm700::AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING ? "STOPPING" + : e == asibfm700::AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_SLEWING ? "SLEWING" + : e == asibfm700::AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_TRACKING ? "TRACKING" + : "UNKNOWN", + ctx); + } +}; + + +static_assert(mcc::mcc_hardware_c, "!!!"); diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..923aac1 --- /dev/null +++ b/main.cpp @@ -0,0 +1,16 @@ +#include + +#include +#include + +using namespace std; + +int main() +{ + auto lg = spdlog::stdout_color_mt("STDOUT"); + mcc::utils::MccSpdlogLogger logger(lg); + + std::println("{}", logger.getThreadId()); + + return 0; +} diff --git a/mcc b/mcc deleted file mode 160000 index 7cc2c5e..0000000 --- a/mcc +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7cc2c5e2a33a5a99ce76a05570c5f4da98625a01