Compare commits

...

138 Commits

Author SHA1 Message Date
01261272b4 . 2026-03-11 17:18:21 +03:00
e61146db69 fixed stupid bug 2026-03-11 12:36:45 +03:00
2eb7b214b2 add more errors to model 2026-03-11 11:03:10 +03:00
27b1cecc7b enc 2026-03-11 10:05:42 +03:00
9e84306a94 poll test works @high speed 2026-02-09 22:00:51 +03:00
c0c36388db test of polling sensors 2026-01-26 22:25:20 +03:00
ed24d0b9e2 add records to AsibFM700 config file according to Eddy's commit 2026-01-26 18:23:03 +03:00
bccc7a9b29 1st working approach after last changes 2026-01-22 21:18:01 +03:00
873f292a11 fixed race bug 2026-01-21 23:20:58 +03:00
f7cb279841 change macros to config parameters 2026-01-20 23:18:36 +03:00
fd96dc395b ... 2026-01-16 12:23:58 +03:00
Timur A. Fatkhullin
0aa0113be3 ... 2026-01-15 23:21:28 +03:00
01d5657b1b ... 2026-01-15 19:11:16 +03:00
09cf5f9c19 ... 2026-01-14 19:15:18 +03:00
66c3c7e6c7 ... 2025-12-29 16:11:23 +03:00
fd2776084a ... 2025-12-25 16:18:54 +03:00
Timur A. Fatkhullin
bbc35b02bb ... 2025-12-22 23:02:41 +03:00
Timur A. Fatkhullin
8ce6ffc41c ... 2025-12-22 22:56:49 +03:00
54d6c25171 ... 2025-12-22 17:13:04 +03:00
2c7d563994 ... 2025-12-19 12:01:36 +03:00
bc12777f18 ... 2025-12-18 18:21:17 +03:00
7948321cce ... 2025-12-17 17:25:00 +03:00
b12c0ec521 ... 2025-12-16 17:53:50 +03:00
Timur A. Fatkhullin
d417c03f59 ... 2025-12-16 02:22:03 +03:00
a8dd511366 ... 2025-12-15 17:26:26 +03:00
a30729fb37 ... 2025-12-14 05:32:04 +03:00
a70339c55e ... 2025-12-14 00:25:03 +03:00
6fca94e571 ... 2025-12-12 17:21:11 +03:00
255c34dbb2 ... 2025-12-11 17:40:23 +03:00
3c0c719e37 ... 2025-12-10 17:24:00 +03:00
Timur A. Fatkhullin
0ddd633bc9 ... 2025-12-10 00:18:55 +03:00
28ecf307a8 add saving slewing trajectory in a file 2025-12-09 16:55:46 +03:00
57467ce48f ... 2025-12-08 17:34:59 +03:00
196ed3be1b change time from double to struct timespec 2025-12-08 13:31:23 +03:00
acd26edc9c add PID-related items in mount config
rewrite AsibFM700ServoController methods according to new time point
representation in LibSidServo
2025-12-04 18:11:41 +03:00
da9cd51e5c timespec 2025-12-04 11:10:27 +03:00
d868810048 cmake fixes 2025-12-03 18:14:34 +03:00
Timur A. Fatkhullin
7c9612c3a2 MccSimpleSlewingModel: the code has been rewritten in accordance with the
changes in Eddy's LibSidServo
2025-12-03 00:35:52 +03:00
54419b8e60 ... 2025-12-02 18:05:08 +03:00
7dfb0d5e9b ... 2025-12-02 18:02:57 +03:00
bbf7314592 . 2025-12-02 12:33:16 +03:00
6dde28e8d9 some fixes 2025-12-01 17:28:18 +03:00
9066b3f091 ... 2025-12-01 17:18:04 +03:00
c514d4adcc Merge branch 'main' of ssh://95.140.147.151:/home/git/mountcontrol 2025-12-01 10:07:45 +03:00
cca58e8ba9 ... 2025-12-01 02:49:01 +03:00
bf55a45cf9 ... 2025-11-27 18:01:40 +03:00
a825a6935b MccGenericNetworkServer: fix client session thread pool behavior in
destructor
2025-11-27 09:20:42 +03:00
43638f383f ran client sessions in separated thread pool 2025-11-26 18:01:34 +03:00
a42f6dbc98 ... 2025-11-25 08:49:43 +03:00
Timur A. Fatkhullin
acced75fa2 ... 2025-11-24 21:52:22 +03:00
e548451617 ... 2025-11-24 18:00:46 +03:00
e529265a63 ... 2025-11-21 12:33:49 +03:00
b2c27a6f5c ... 2025-11-21 02:02:35 +03:00
6214b82a6c ... 2025-11-19 12:02:43 +03:00
Timur A. Fatkhullin
c6b47d8ad6 ... 2025-11-18 22:36:08 +03:00
273f239abb ... 2025-11-18 18:51:01 +03:00
Timur A. Fatkhullin
14e583a244 ... 2025-11-17 23:42:04 +03:00
771619b832 ... 2025-11-17 18:04:40 +03:00
Timur A. Fatkhullin
e0c8d8f39b ... 2025-11-17 03:07:54 +03:00
Timur A. Fatkhullin
0ce4430668 ... 2025-11-16 10:34:17 +03:00
Timur A. Fatkhullin
e18066e4a6 add logging in MccSimpleSlewingModel class 2025-11-16 00:58:56 +03:00
1c774d2d69 Asibfm700Mount is now MccGenericMount (not MccGenericFsmMount)
fix mount initialization (add EEPROM reading, assign correponded
mount config items)
rewrite computing distance to pzones in slewing mode (add braking
aceleration)
add more informative errors description for serialization (network
protocol)
2025-11-15 16:01:42 +03:00
9e8a7a62c9 ... 2025-11-14 17:27:44 +03:00
1ea5fb623d fixed model for STOPPED state 2025-11-14 14:07:07 +03:00
078e3f38f2 ... 2025-11-14 12:23:39 +03:00
94fb4c6a48 ... 2025-11-13 17:56:51 +03:00
b3a257fab6 ... 2025-11-12 18:50:23 +03:00
08ad1e665b ... 2025-11-11 18:10:06 +03:00
90acf1ee8c fix axis switch limit pzone calculations 2025-11-10 16:06:44 +03:00
15cf04f164 ... 2025-11-03 18:27:43 +03:00
6fc0b8bb4e ... 2025-11-02 14:38:44 +03:00
c0f274cec0 fix compilation with GCC version<15 2025-11-02 11:59:23 +03:00
Edward Emelianov
511956531e fixed nanotime 2025-11-01 19:51:56 +03:00
3f108fcc13 ... 2025-11-01 17:53:24 +03:00
683da9739d change system time function to UNIX time 2025-11-01 14:59:37 +03:00
a7fbae47f0 ... 2025-11-01 11:57:49 +03:00
8a202bd38c ... 2025-10-31 17:40:33 +03:00
d69ea51b0c various Asibfm700MountConfig class fixes 2025-10-31 12:22:16 +03:00
a1fa54c636 fix ASIO and cxxopts libraries compile/link errors 2025-10-31 11:02:52 +03:00
Timur A. Fatkhullin
cb362c6e49 rewrite MccGenericMount and MccGenericFsmMount class creation
Asibfm700MountNetServer is now started
2025-10-31 01:30:24 +03:00
f2be52d17c ... add dump of config for Asibfm700MountConfig class 2025-10-30 16:11:23 +03:00
3682ccdda6 ... 2025-10-30 11:58:11 +03:00
Timur A. Fatkhullin
85259fc6ad ... compiled! 2025-10-30 01:01:52 +03:00
620f8ba136 ... 2025-10-29 19:26:20 +03:00
50e79aa0ae ... 2025-10-29 18:47:24 +03:00
6a72ead855 cleanups of commented code 2025-10-29 16:15:58 +03:00
bc300bb3de ... 2025-10-29 15:07:53 +03:00
78e4bb182c ... 2025-10-28 18:01:22 +03:00
Timur A. Fatkhullin
85dfa2e9a5 ... 2025-10-28 01:11:34 +03:00
Timur A. Fatkhullin
bdfc5dbc1c ... 2025-10-26 02:22:39 +03:00
Timur A. Fatkhullin
ec27cd981a ... 2025-10-25 19:26:42 +03:00
47c57dca72 ... 2025-10-24 12:16:44 +03:00
e6b4604bfa ... 2025-10-23 18:08:44 +03:00
412f038eb0 ... 2025-10-23 12:13:07 +03:00
Timur A. Fatkhullin
80ec2382ea ... 2025-10-22 23:52:14 +03:00
42a4349c76 ... 2025-10-22 17:55:43 +03:00
Timur A. Fatkhullin
e50fbfc57e ... 2025-10-21 22:35:45 +03:00
49a2e2f9c1 ... 2025-10-21 17:48:21 +03:00
fc64642bd6 ... 2025-10-20 00:36:00 +03:00
Timur A. Fatkhullin
cbe106fe95 ... 2025-10-11 23:02:43 +03:00
f618fb64cb ... 2025-10-09 17:43:40 +03:00
Timur A. Fatkhullin
04272b8e1d ... 2025-10-09 01:09:27 +03:00
e0e10395fb ... 2025-10-08 18:13:46 +03:00
Timur A. Fatkhullin
27dccfe7c0 ... 2025-10-07 23:51:58 +03:00
8b16ac79b8 ... 2025-10-06 17:52:41 +03:00
58d62d85b3 ... 2025-10-06 12:09:06 +03:00
Timur A. Fatkhullin
9c13def8be ... 2025-10-04 00:46:07 +03:00
5fe2788cd7 ... 2025-10-03 12:11:21 +03:00
962504ed98 ... 2025-10-02 19:21:13 +03:00
Timur A. Fatkhullin
4d7e830798 ... 2025-10-01 06:35:44 +03:00
Timur A. Fatkhullin
3d769d79eb ... 2025-10-01 06:26:50 +03:00
0b7261a431 ... 2025-09-30 18:55:14 +03:00
c5aa3dc495 ... 2025-09-29 19:02:04 +03:00
98c46c2b8c ... 2025-09-28 19:31:03 +03:00
d8fae31406 ... 2025-09-25 17:12:12 +03:00
4a9ecf8639 ... 2025-09-25 12:11:48 +03:00
Timur A. Fatkhullin
b8383c1375 ... 2025-09-25 00:08:08 +03:00
f729799335 ... 2025-09-24 18:23:17 +03:00
b1a48d2b77 ... 2025-09-24 12:36:02 +03:00
fedc324410 ... 2025-09-22 17:46:52 +03:00
Timur A. Fatkhullin
1a4d998141 ... 2025-09-21 23:16:03 +03:00
0f955b3c91 ... 2025-09-19 12:12:12 +03:00
f5039a329b ... 2025-09-18 17:44:23 +03:00
83b7e0d924 ... 2025-09-17 21:57:05 +03:00
1087e043a8 ... 2025-09-17 18:21:32 +03:00
281ceacf89 ... 2025-09-17 12:51:28 +03:00
4e3a50acba ... 2025-09-16 19:06:37 +03:00
732cd33947 ... 2025-09-16 18:35:39 +03:00
Timur A. Fatkhullin
bb41710645 ... 2025-09-14 23:14:09 +03:00
Timur A. Fatkhullin
0b084f44f6 ... 2025-09-13 23:50:02 +03:00
Timur A. Fatkhullin
92b1a3cfd5 ... 2025-09-13 23:46:38 +03:00
3ae2d41fc8 ... 2025-09-13 13:59:54 +03:00
c7dd816481 ... 2025-09-12 18:31:15 +03:00
5f802ff57e ... 2025-09-12 12:53:05 +03:00
8e8cb543ae ... 2025-09-11 18:23:39 +03:00
ab49f927fb ... 2025-09-10 18:07:22 +03:00
00354d9b41 ... 2025-09-10 12:24:06 +03:00
2478c1e8d2 remove guiding model
now it are only slewing and tracking states
2025-09-03 18:28:52 +03:00
77 changed files with 17328 additions and 2459 deletions

View File

@@ -2,15 +2,21 @@ cmake_minimum_required(VERSION 3.14)
#********************************************** #**********************************************
# Astrosib(c) BM-700 mount control software * # Astrosib(c) FM-700 mount control software *
#********************************************** #**********************************************
project(ASIB_BM700 LANGUAGES C CXX) project(ASIB_FM700 LANGUAGES C CXX)
set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
# #
# ******* C++ PART OF THE PROJECT ******* # ******* C++ PART OF THE PROJECT *******
add_subdirectory(cxx) set(EXAMPLES OFF CACHE BOOL "" FORCE)
# set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_BUILD_TYPE "Debug")
add_subdirectory(LibSidServo)
# add_subdirectory(cxx)
add_subdirectory(mcc) add_subdirectory(mcc)
add_subdirectory(asibfm700)

View File

@@ -0,0 +1,218 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="qlonglong">0</value>
</data>
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
<value type="bool" key="AutoTest.Framework.Boost">true</value>
<value type="bool" key="AutoTest.Framework.CTest">false</value>
<value type="bool" key="AutoTest.Framework.Catch">true</value>
<value type="bool" key="AutoTest.Framework.GTest">true</value>
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
</valuemap>
<value type="bool" key="AutoTest.ApplyFilter">false</value>
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">4</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap>
<value type="int" key="RcSync">0</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</value>
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">all</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">clean</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value>
</data>
<data>
<variable>Version</variable>
<value type="int">22</value>
</data>
</qtcreator>

View File

@@ -0,0 +1,218 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.0, 2025-11-27T17:22:09. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="qlonglong">0</value>
</data>
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
<value type="bool" key="AutoTest.Framework.Boost">true</value>
<value type="bool" key="AutoTest.Framework.CTest">false</value>
<value type="bool" key="AutoTest.Framework.Catch">true</value>
<value type="bool" key="AutoTest.Framework.GTest">true</value>
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
</valuemap>
<value type="bool" key="AutoTest.ApplyFilter">false</value>
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">4</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap>
<value type="int" key="RcSync">0</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</value>
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">all</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">clean</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value>
</data>
<data>
<variable>Version</variable>
<value type="int">22</value>
</data>
</qtcreator>

View File

@@ -84,49 +84,51 @@ typedef struct{
* @return calculated new speed or -1 for max speed * @return calculated new speed or -1 for max speed
*/ */
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){ double dt = timediff(&tagpos->t, &axis->position.t);
DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT); 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 return axis->speed.val; // data is too old or wrong
} }
double error = tagpos->val - axis->position.val, fe = fabs(error); double error = tagpos->val - axis->position.val, fe = fabs(error);
DBG("error: %g", error);
PIDController_t *pid = NULL; PIDController_t *pid = NULL;
switch(axis->state){ switch(axis->state){
case AXIS_SLEWING: case AXIS_SLEWING:
if(fe < MCC_MAX_POINTING_ERR){ if(fe < Conf.MaxPointingErr){
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
DBG("--> Pointing"); DBG("--> Pointing");
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else{ }else{
DBG("Slewing..."); DBG("Slewing...");
return -1.; // max speed for given axis return NAN; // max speed for given axis
} }
break; break;
case AXIS_POINTING: case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){ if(fe < Conf.MaxFinePointingErr){
axis->state = AXIS_GUIDING; axis->state = AXIS_GUIDING;
DBG("--> Guiding"); DBG("--> Guiding");
pid = pidpair->PIDV; pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){ }else if(fe > Conf.MaxPointingErr){
DBG("--> Slewing"); DBG("--> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return -1.; return NAN;
} else pid = pidpair->PIDC; } else pid = pidpair->PIDC;
break; break;
case AXIS_GUIDING: case AXIS_GUIDING:
pid = pidpair->PIDV; pid = pidpair->PIDV;
if(fe > MCC_MAX_GUIDING_ERR){ if(fe > Conf.MaxFinePointingErr){
DBG("--> Pointing"); DBG("--> Pointing");
axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){ }else if(fe < Conf.MaxGuidingErr){
DBG("At target"); DBG("At target");
// TODO: we can point somehow that we are at target or introduce new axis state // TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g", fe); }else DBG("Current error: %g", fe);
break; break;
case AXIS_STOPPED: // start pointing to target; will change speed next time case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!!"); DBG("AXIS STOPPED!!!! --> Slewing");
axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return -1.; return getspeed(tagpos, pidpair, axis);
case AXIS_ERROR: case AXIS_ERROR:
DBG("Can't move from erroneous state"); DBG("Can't move from erroneous state");
return 0.; return 0.;
@@ -135,16 +137,16 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
DBG("WTF? Where is a PID?"); DBG("WTF? Where is a PID?");
return axis->speed.val; return axis->speed.val;
} }
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){ double dtpid = timediff(&tagpos->t, &pid->prevT);
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
DBG("time diff too big: clear PID"); DBG("time diff too big: clear PID");
pid_clear(pid); pid_clear(pid);
} }
double dt = tagpos->t - pid->prevT; if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t; pid->prevT = tagpos->t;
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state); DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
double tagspeed = pid_calculate(pid, error, dt); double tagspeed = pid_calculate(pid, error, dtpid);
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
return tagspeed; // coordinate-based return tagspeed; // coordinate-based
} }
@@ -154,22 +156,23 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
* @param endpoint - stop point (some far enough point to stop in case of hang) * @param endpoint - stop point (some far enough point to stop in case of hang)
* @return error code * @return error code
*/ */
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){ mcc_errcodes_t correct2(const coordval_pair_t *target){
static PIDpair_t pidX = {0}, pidY = {0}; static PIDpair_t pidX = {0}, pidY = {0};
if(!pidX.PIDC){ if(!pidX.PIDC){
pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDC) return MCC_E_FATAL; if(!pidX.PIDC) return MCC_E_FATAL;
pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidX.PIDV) return MCC_E_FATAL; if(!pidX.PIDV) return MCC_E_FATAL;
} }
if(!pidY.PIDC){ if(!pidY.PIDC){
pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDC) return MCC_E_FATAL; if(!pidY.PIDC) return MCC_E_FATAL;
pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
if(!pidY.PIDV) return MCC_E_FATAL; if(!pidY.PIDV) return MCC_E_FATAL;
} }
mountdata_t m; mountdata_t m;
coordpair_t tagspeed; 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; if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
axisdata_t axis; axisdata_t axis;
DBG("state: %d/%d", m.Xstate, m.Ystate); DBG("state: %d/%d", m.Xstate, m.Ystate);
@@ -177,20 +180,42 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
axis.position = m.encXposition; axis.position = m.encXposition;
axis.speed = m.encXspeed; axis.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axis); tagspeed.X = getspeed(&target->X, &pidX, &axis);
if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X; if(isnan(tagspeed.X)){ // max speed
if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_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_status_t xstate = axis.state;
axis.state = m.Ystate; axis.state = m.Ystate;
axis.position = m.encYposition; axis.position = m.encYposition;
axis.speed = m.encYspeed; axis.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axis); tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y; if(isnan(tagspeed.Y)){ // max speed
if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_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; axis_status_t ystate = axis.state;
if(m.Xstate != xstate || m.Ystate != ystate){ if(m.Xstate != xstate || m.Ystate != ystate){
DBG("State changed"); DBG("State changed");
setStat(xstate, ystate); setStat(xstate, ystate);
} }
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y); coordpair_t endpoint;
return Mount.moveWspeed(endpoint, &tagspeed); // 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);
} }

View File

@@ -27,7 +27,7 @@ typedef struct {
double prev_error; // Previous error double prev_error; // Previous error
double integral; // Integral term double integral; // Integral term
double *pidIarray; // array for Integral double *pidIarray; // array for Integral
double prevT; // time of previous correction struct timespec prevT; // time of previous correction
size_t pidIarrSize; // it's size size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element size_t curIidx; // and index of current element
} PIDController_t; } PIDController_t;
@@ -37,4 +37,4 @@ void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid); void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt); double pid_calculate(PIDController_t *pid, double error, double dt);
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint); mcc_errcodes_t correct2(const coordval_pair_t *target);

View File

@@ -1,2 +1,3 @@
1. PID: slew2 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

View File

@@ -1,64 +0,0 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdlib.h>
#include "sidservo.h"
extern conf_t Conf;
// unused arguments of functions
#define _U_ __attribute__((__unused__))
// break absent in `case`
#define FALLTHRU __attribute__ ((fallthrough))
// and synonym for FALLTHRU
#define NOBREAKHERE __attribute__ ((fallthrough))
// weak functions
#define WEAK __attribute__ ((weak))
#ifndef DBL_EPSILON
#define DBL_EPSILON (2.2204460492503131e-16)
#endif
#ifndef FALSE
#define FALSE (0)
#endif
#ifndef TRUE
#define TRUE (1)
#endif
#ifdef EBUG
#include <stdio.h>
#define COLOR_RED "\033[1;31;40m"
#define COLOR_GREEN "\033[1;32;40m"
#define COLOR_OLD "\033[0;0;0m"
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n");} while(0)
#else // EBUG
#define FNAME()
#define DBG(...)
#endif // EBUG

View File

@@ -34,7 +34,9 @@ typedef struct{
static hardware_configuration_t HW = {0}; static hardware_configuration_t HW = {0};
static parameters G = {0}; static parameters G = {
.conffile = "servo.conf",
};
static sl_option_t cmdlnopts[] = { static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
@@ -53,7 +55,7 @@ static sl_option_t confopts[] = {
static void dumpaxis(char axis, axis_config_t *c){ static void dumpaxis(char axis, axis_config_t *c){
#define STRUCTPAR(p) (c)->p #define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0) #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) #define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel); DUMPD(accel);
DUMPD(backlash); DUMPD(backlash);
@@ -64,6 +66,8 @@ static void dumpaxis(char axis, axis_config_t *c){
DUMP(outplimit); DUMP(outplimit);
DUMP(currlimit); DUMP(currlimit);
DUMP(intlimit); DUMP(intlimit);
DUMP(motor_stepsperrev);
DUMP(axis_stepsperrev);
#undef DUMP #undef DUMP
#undef DUMPD #undef DUMPD
} }

View File

@@ -24,25 +24,32 @@
static conf_t Config = { static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0", .MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200, .MountDevSpeed = 19200,
.EncoderXDevPath = "/dev/encoderX0", .EncoderXDevPath = "/dev/encoder_X0",
.EncoderYDevPath = "/dev/encoderY0", .EncoderYDevPath = "/dev/encoder_Y0",
.EncoderDevSpeed = 153000, .EncoderDevSpeed = 153000,
.MountReqInterval = 0.1, .MountReqInterval = 0.1,
.EncoderReqInterval = 0.05, .EncoderReqInterval = 0.001,
.SepEncoder = 2, .SepEncoder = 2,
.EncoderSpeedInterval = 0.1, .EncoderSpeedInterval = 0.05,
.XPIDC.P = 0.8, .EncodersDisagreement = 1e-5, // 2''
.PIDMaxDt = 1.,
.PIDRefreshDt = 0.1,
.PIDCycleDt = 5.,
.XPIDC.P = 0.5,
.XPIDC.I = 0.1, .XPIDC.I = 0.1,
.XPIDC.D = 0.3, .XPIDC.D = 0.2,
.XPIDV.P = 1., .XPIDV.P = 0.09,
.XPIDV.I = 0.01, .XPIDV.I = 0.0,
.XPIDV.D = 0.2, .XPIDV.D = 0.05,
.YPIDC.P = 0.8, .YPIDC.P = 0.5,
.YPIDC.I = 0.1, .YPIDC.I = 0.1,
.YPIDC.D = 0.3, .YPIDC.D = 0.2,
.YPIDV.P = 0.5, .YPIDV.P = 0.09,
.YPIDV.I = 0.2, .YPIDV.I = 0.0,
.YPIDV.D = 0.5, .YPIDV.D = 0.05,
.MaxPointingErr = 0.13962634,
.MaxFinePointingErr = 0.026179939,
.MaxGuidingErr = 4.8481368e-7,
}; };
static sl_option_t opts[] = { static sl_option_t opts[] = {
@@ -50,13 +57,17 @@ static sl_option_t opts[] = {
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of 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"}, {"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"}, {"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"}, {"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
{"SepEncoder", NO_ARGS, 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)"}, {"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)"}, {"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"}, {"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"}, {"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)"}, {"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)"}, {"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)"}, {"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
@@ -69,6 +80,12 @@ static sl_option_t opts[] = {
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y 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)"}, {"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)"}, {"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 end_option
}; };
@@ -93,5 +110,19 @@ void dumpConf(){
} }
void confHelp(){ void confHelp(){
sl_showhelp(-1, opts); 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];
} }

View File

@@ -25,3 +25,4 @@
void confHelp(); void confHelp();
conf_t *readServoConf(const char *filename); conf_t *readServoConf(const char *filename);
void dumpConf(); void dumpConf();
const char *EcodeStr(mcc_errcodes_t e);

View File

@@ -23,6 +23,9 @@
#include "dump.h" #include "dump.h"
#include "simpleconv.h" #include "simpleconv.h"
// starting dump time (to conform different logs)
static struct timespec dumpT0 = {0};
#if 0 #if 0
// amount of elements used for encoders' data filtering // amount of elements used for encoders' data filtering
#define NFILT (10) #define NFILT (10)
@@ -59,6 +62,12 @@ static double filter(double val, int idx){
} }
#endif #endif
// return starting time of dump
void dumpt0(struct timespec *t){
if(t) *t = dumpT0;
}
/** /**
* @brief logmnt - log mount data into file * @brief logmnt - log mount data into file
* @param fcoords - file to dump * @param fcoords - file to dump
@@ -68,12 +77,12 @@ void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords) return; if(!fcoords) return;
//DBG("LOG %s", m ? "data" : "header"); //DBG("LOG %s", m ? "data" : "header");
if(!m){ // write header if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return; return;
} }else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
// write data // write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n", fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
m->encXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val), Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val), RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val), RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
m->millis); m->millis);
@@ -99,16 +108,17 @@ void dumpmoving(FILE *fcoords, double t, int N){
LOGWARN("Can't get mount data"); LOGWARN("Can't get mount data");
} }
uint32_t mdmillis = mdata.millis; uint32_t mdmillis = mdata.millis;
double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.; struct timespec encXt = mdata.encXposition.t;
int ctr = -1; int ctr = -1;
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val; double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
double t0 = Mount.currentT(); double t0 = Mount.timeFromStart();
while(Mount.currentT() - t0 < t && ctr < N){ while(Mount.timeFromStart() - t0 < t && ctr < N){
usleep(1000); usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.; //double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
if(tmsr == enct) continue; struct timespec msrt = mdata.encXposition.t;
enct = tmsr; if(msrt.tv_nsec == encXt.tv_nsec) continue;
encXt = msrt;
if(fcoords) logmnt(fcoords, &mdata); if(fcoords) logmnt(fcoords, &mdata);
if(mdata.millis == mdmillis) continue; if(mdata.millis == mdmillis) continue;
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val); //DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
@@ -119,7 +129,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
ctr = 0; ctr = 0;
}else ++ctr; }else ++ctr;
} }
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0); DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
} }
/** /**
@@ -130,17 +140,15 @@ void waitmoving(int N){
mountdata_t mdata; mountdata_t mdata;
int ctr = -1; int ctr = -1;
uint32_t millis = 0; uint32_t millis = 0;
double xlast = 0., ylast = 0.; //double xlast = 0., ylast = 0.;
DBG("Wait moving for %d stopped times", N);
while(ctr < N){ while(ctr < N){
usleep(10000); usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue; if(mdata.millis == millis) continue;
millis = mdata.millis; millis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
xlast = mdata.motXposition.val; else ++ctr;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
} }
} }

View File

@@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N); void waitmoving(int N);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc); int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles); void chk0(int ncycles);
void dumpt0(struct timespec *t);

View File

@@ -73,6 +73,7 @@ int main(int argc, char **argv){
conf_t *Config = readServoConf(G.conffile); conf_t *Config = readServoConf(G.conffile);
if(!Config){ if(!Config){
dumpConf(); dumpConf();
confHelp();
return 1; return 1;
} }
if(G.coordsoutput){ if(G.coordsoutput){

View File

@@ -139,8 +139,10 @@ static mcc_errcodes_t return2zero(){
short_command_t cmd = {0}; short_command_t cmd = {0};
DBG("Try to move to zero"); DBG("Try to move to zero");
cmd.Xmot = 0.; cmd.Ymot = 0.; cmd.Xmot = 0.; cmd.Ymot = 0.;
cmd.Xspeed = MCC_MAX_X_SPEED; coordpair_t maxspd;
cmd.Yspeed = MCC_MAX_Y_SPEED; if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
cmd.Xspeed = maxspd.X;
cmd.Yspeed = maxspd.Y;
/*cmd.xychange = 1; /*cmd.xychange = 1;
cmd.XBits = 100; cmd.XBits = 100;
cmd.YBits = 20;*/ cmd.YBits = 20;*/
@@ -216,7 +218,7 @@ int main(int argc, char **argv){
sleep(5); sleep(5);
// return to zero and wait // return to zero and wait
green("Return 2 zero and wait\n"); green("Return 2 zero and wait\n");
if(!return2zero()) ERRX("Can't return"); if(MCC_E_OK != return2zero()) ERRX("Can't return");
Wait(0., 0); Wait(0., 0);
Wait(0., 1); Wait(0., 1);
// wait moving ends // wait moving ends

View File

@@ -83,7 +83,7 @@ void waithalf(double t){
uint32_t millis = 0; uint32_t millis = 0;
double xlast = 0., ylast = 0.; double xlast = 0., ylast = 0.;
while(ctr < 5){ while(ctr < 5){
if(Mount.currentT() >= t) return; if(Mount.timeFromStart() >= t) return;
usleep(1000); usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue; if(mdata.millis == millis) continue;
@@ -110,16 +110,28 @@ int main(int argc, char **argv){
return 1; return 1;
} }
if(G.coordsoutput){ if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w"))) if(!(fcoords = fopen(G.coordsoutput, "w"))){
ERRX("Can't open %s", G.coordsoutput); WARNX("Can't open %s", G.coordsoutput);
return 1;
}
}else fcoords = stdout; }else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7"); if(G.Ncycles < 2){
WARNX("Ncycles should be >2");
return 1;
}
double absamp = fabs(G.amplitude); double absamp = fabs(G.amplitude);
if(absamp < 0.01 || absamp > 45.) if(absamp < 0.01 || absamp > 45.){
ERRX("Amplitude should be from 0.01 to 45 degrees"); WARNX("Amplitude should be from 0.01 to 45 degrees");
if(G.period < 0.1 || G.period > 900.) return 1;
ERRX("Period should be from 0.1 to 900s"); }
if(G.Nswings < 1) ERRX("Nswings should be more than 0"); 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); conf_t *Config = readServoConf(G.conffile);
if(!Config){ if(!Config){
dumpConf(); dumpConf();
@@ -146,24 +158,24 @@ int main(int argc, char **argv){
}else{ }else{
tagX = 0.; tagY = DEG2RAD(G.amplitude); tagX = 0.; tagY = DEG2RAD(G.amplitude);
} }
double t = Mount.currentT(), t0 = t; double t = Mount.timeFromStart(), t0 = t;
coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY}; coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
double divide = 2.; double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){ for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tag); Mount.moveTo(&tag);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
t += G.period / divide; t += G.period / divide;
divide = 1.; divide = 1.;
waithalf(t); waithalf(t);
DBG("Moved to +, t=%g", t-t0); DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
Mount.moveTo(&rtag); Mount.moveTo(&rtag);
t += G.period; t += G.period;
waithalf(t); waithalf(t);
DBG("Moved to -, t=%g", t-t0); DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.timeFromStart()-t0);
} }
green("Move to zero @ %g\n", Mount.currentT()); green("Move to zero @ %g\n", Mount.timeFromStart());
tag = (coordpair_t){0}; tag = (coordpair_t){0};
// be sure to move @ 0,0 // be sure to move @ 0,0
if(MCC_E_OK != Mount.moveTo(&tag)){ if(MCC_E_OK != Mount.moveTo(&tag)){

View File

@@ -91,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coordval_pair_t M, E; coordval_pair_t M, E;
if(!getPos(&M, &E)) ERRX("Can't get current position"); if(!getPos(&M, &E)) ERRX("Can't get current position");
printf("Current time: %.10f\n", Mount.timeFromStart());
if(G.coordsoutput){ if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
G.wait = 1; G.wait = 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w"))) if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput); ERRX("Can't open %s", G.coordsoutput);
logmnt(fcoords, NULL); logmnt(fcoords, NULL);
@@ -121,7 +120,11 @@ int main(int _U_ argc, char _U_ **argv){
} }
printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y); printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y); tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
Mount.moveTo(&tag); 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){ if(G.wait){
sleep(1); sleep(1);
waitmoving(G.Ncycles); waitmoving(G.Ncycles);
@@ -133,7 +136,9 @@ out:
if(G.coordsoutput) pthread_join(dthr, NULL); if(G.coordsoutput) pthread_join(dthr, NULL);
DBG("QUIT"); DBG("QUIT");
if(G.wait){ if(G.wait){
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); 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(); Mount.quit();
} }
return 0; return 0;

View File

@@ -44,6 +44,7 @@ typedef struct{
char *conffile; char *conffile;
} parameters; } parameters;
static conf_t *Config = NULL;
static FILE *fcoords = NULL, *errlog = NULL; static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr; static pthread_t dthr;
static parameters G = { static parameters G = {
@@ -96,35 +97,35 @@ static void runtraectory(traectory_fn tfn){
if(!tfn) return; if(!tfn) return;
coordval_pair_t telXY; coordval_pair_t telXY;
coordval_pair_t target; coordval_pair_t target;
coordpair_t traectXY, endpoint; coordpair_t traectXY;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax; double tlast = 0., tstart = Mount.timeFromStart();
double t0 = Mount.currentT(), tlast = 0.; long tlastXnsec = 0, tlastYnsec = 0;
double tlastX = 0., tlastY = 0.; struct timespec tcur, t0 = {0};
dumpt0(&t0);
while(1){ while(1){
if(!telpos(&telXY)){ if(!telpos(&telXY)){
WARNX("No next telescope position"); WARNX("No next telescope position");
return; return;
} }
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind if(!Mount.currentT(&tcur)) continue;
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t); if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
tlastX = telXY.X.t; tlastY = telXY.Y.t; DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
double t = Mount.currentT(); tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break; 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; if(!traectory_point(&traectXY, t)) break;
target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = t; target.X.t = target.Y.t = tcur;
// check whether we should change direction if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax; else{
else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax; //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax; DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax; //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); if(errlog)
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); 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));
//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) if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
tlast = t; tlast = t;
} }
WARNX("No next traectory point or emulation ends"); WARNX("No next traectory point or emulation ends");
@@ -150,7 +151,7 @@ int main(int argc, char **argv){
if(!(fcoords = fopen(G.coordsoutput, "w"))) if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput); ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout; }else fcoords = stdout;
conf_t *Config = readServoConf(G.conffile); Config = readServoConf(G.conffile);
if(!Config || G.dumpconf){ if(!Config || G.dumpconf){
dumpConf(); dumpConf();
return 1; return 1;

View File

@@ -1,4 +1,4 @@
Current configuration: # Current configuration
MountDevPath=/dev/ttyUSB0 MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200 MountDevSpeed=19200
EncoderDevPath=(null) EncoderDevPath=(null)

View File

@@ -41,7 +41,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
if(!f || !XY0) return FALSE; if(!f || !XY0) return FALSE;
cur_traectory = f; cur_traectory = f;
XYstart = *XY0; XYstart = *XY0;
tstart = Mount.currentT(); tstart = Mount.timeFromStart();
mountdata_t mdata; mountdata_t mdata;
int ntries = 0; int ntries = 0;
for(; ntries < 10; ++ntries){ for(; ntries < 10; ++ntries){
@@ -98,7 +98,7 @@ int Linear(coordpair_t *nextpt, double t){
int SinCos(coordpair_t *nextpt, double t){ int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt; coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI); 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); pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt; if(nextpt) *nextpt = pt;
return TRUE; return TRUE;
} }

View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. --> <!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
@@ -86,6 +86,7 @@
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/> <valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value> <value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap> </valuemap>
<value type="int" key="RcSync">0</value>
</valuemap> </valuemap>
</data> </data>
<data> <data>
@@ -110,8 +111,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap> </valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap> </valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@@ -123,8 +124,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap> </valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value> <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@@ -139,8 +140,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0"> <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value> <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@@ -164,6 +165,7 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value> <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value> <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap> </valuemap>
@@ -173,8 +175,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0"> <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0"> <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value> <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap> </valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value> <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@@ -198,6 +200,7 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value> <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value> <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value> <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value> <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap> </valuemap>
@@ -208,10 +211,6 @@
<variable>ProjectExplorer.Project.TargetCount</variable> <variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value> <value type="qlonglong">1</value>
</data> </data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">22</value>
</data>
<data> <data>
<variable>Version</variable> <variable>Version</variable>
<value type="int">22</value> <value type="int">22</value>

View File

@@ -22,6 +22,7 @@ examples/traectories.h
main.h main.h
movingmodel.c movingmodel.c
movingmodel.h movingmodel.h
polltest/main.c
ramp.c ramp.c
ramp.h ramp.h
serial.h serial.h

View File

@@ -25,6 +25,7 @@
#include <time.h> #include <time.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h>
#include "main.h" #include "main.h"
#include "movingmodel.h" #include "movingmodel.h"
@@ -32,40 +33,82 @@
#include "ssii.h" #include "ssii.h"
#include "PID.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}; conf_t Conf = {0};
// parameters for model // parameters for model
static movemodel_t *Xmodel, *Ymodel; 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 // radians, rad/sec, rad/sec^2
static limits_t // 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 = { Xlimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}}, .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
Ylimits = { Ylimits = {
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}} .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
; ;
static mcc_errcodes_t shortcmd(short_command_t *cmd); static mcc_errcodes_t shortcmd(short_command_t *cmd);
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
/** /**
* @brief nanotime - monotonic time from first run * @brief curtime - monotonic time from first run
* @return time in seconds * @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
*/ */
double nanotime(){ int curtime(struct timespec *t){
static struct timespec *start = NULL;
struct timespec now; struct timespec now;
if(!start){ if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
start = malloc(sizeof(struct timespec)); now.tv_sec += timeadder.tv_sec;
if(!start) return -1.; now.tv_nsec += timeadder.tv_nsec;
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.; 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.; if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
double sd = (double)now.tv_sec - (double)start->tv_sec;
return sd + nd;
} }
/** /**
* @brief quit - close all opened and return to default state * @brief quit - close all opened and return to default state
* TODO: close serial devices even in "model" mode
*/ */
static void quit(){ static void quit(){
if(Conf.RunModel) return; if(Conf.RunModel) return;
@@ -75,18 +118,19 @@ static void quit(){
DBG("Exit"); DBG("Exit");
} }
void getModData(coordval_pair_t *c){ void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
if(!c || !Xmodel || !Ymodel) return; if(!c || !Xmodel || !Ymodel) return;
double tnow = nanotime(); double tnow = timefromstart();
moveparam_t Xp, Yp; moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp); movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
//DBG("Xstate = %d", Xst); //DBG("Xstate = %d", Xst);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow); if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp); movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow); if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
c->X.t = c->Y.t = tnow; c->X = Xp.coord;
c->X.val = Xp.coord; c->Y = Yp.coord;
c->Y.val = Yp.coord; if(xst) *xst = Xst;
if(yst) *yst = Yst;
} }
/** /**
@@ -117,6 +161,8 @@ double LS_calc_slope(less_square_t *l, double x, double t){
if(!l) return 0.; if(!l) return 0.;
size_t idx = l->idx; size_t idx = l->idx;
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx]; double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
/*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
l->xsum, l->tsum, l->t2sum, l->xtsum);*/
double t2 = t * t, xt = x * t; double t2 = t * t, xt = x * t;
l->x[idx] = x; l->t2[idx] = t2; l->x[idx] = x; l->t2[idx] = t2;
l->t[idx] = t; l->xt[idx] = xt; l->t[idx] = t; l->xt[idx] = xt;
@@ -128,14 +174,13 @@ double LS_calc_slope(less_square_t *l, double x, double t){
l->xtsum += xt - oldxt; l->xtsum += xt - oldxt;
double n = (double)l->arraysz; double n = (double)l->arraysz;
double denominator = n * l->t2sum - l->tsum * l->tsum; 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.; if(fabs(denominator) < 1e-7) return 0.;
double numerator = n * l->xtsum - l->xsum * l->tsum; double numerator = n * l->xtsum - l->xsum * l->tsum;
//DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
// point: (sum_x - slope * sum_t) / n; // point: (sum_x - slope * sum_t) / n;
return (numerator / denominator); return (numerator / denominator);
} }
/** /**
* @brief init - open serial devices and do other job * @brief init - open serial devices and do other job
* @param c - initial configuration * @param c - initial configuration
@@ -144,15 +189,20 @@ double LS_calc_slope(less_square_t *l, double x, double t){
static mcc_errcodes_t init(conf_t *c){ static mcc_errcodes_t init(conf_t *c){
FNAME(); FNAME();
if(!c) return MCC_E_BADFORMAT; if(!c) return MCC_E_BADFORMAT;
if(!initstarttime()) return MCC_E_FAILED;
Conf = *c; Conf = *c;
mcc_errcodes_t ret = MCC_E_OK; mcc_errcodes_t ret = MCC_E_OK;
Xmodel = model_init(&Xlimits); Xmodel = model_init(&Xlimits);
Ymodel = model_init(&Ylimits); 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(Conf.RunModel){
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
return MCC_E_OK; return MCC_E_OK;
} }
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
DBG("Define mount device path and speed"); DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT; ret = MCC_E_BADFORMAT;
}else if(!openMount()){ }else if(!openMount()){
@@ -168,41 +218,47 @@ static mcc_errcodes_t init(conf_t *c){
ret = MCC_E_ENCODERDEV; ret = MCC_E_ENCODERDEV;
} }
} }
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ // TODO: read hardware configuration on init
DBG("Bad value of MountReqInterval");
ret = MCC_E_BADFORMAT;
}
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
DBG("Wrong speed interval"); DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT; ret = MCC_E_BADFORMAT;
} }
//uint8_t buf[1024];
//data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret; if(ret != MCC_E_OK) return ret;
return updateMotorPos(); // 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 // check coordinates (rad) and speeds (rad/s); return FALSE if failed
// TODO fix to real limits!!! // TODO fix to real limits!!!
static int chkX(double X){ static int chkX(double X){
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE; if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
return TRUE; return TRUE;
} }
static int chkY(double Y){ static int chkY(double Y){
if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE; if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
return TRUE; return TRUE;
} }
static int chkXs(double s){ static int chkXs(double s){
if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE; if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
return TRUE; return TRUE;
} }
static int chkYs(double s){ static int chkYs(double s){
if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE; if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
return TRUE; return TRUE;
} }
// set SLEWING state if axis was stopped later // set SLEWING state if axis was stopped
static void setslewingstate(){ static void setslewingstate(){
//FNAME(); //FNAME();
mountdata_t d; mountdata_t d;
@@ -218,19 +274,6 @@ static void setslewingstate(){
}else DBG("CAN't GET MOUNT DATA!"); }else DBG("CAN't GET MOUNT DATA!");
} }
/*
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
//if(Conf.RunModel) return ... ;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(AXIS_SLEWING, AXIS_SLEWING);
//...
return MCC_E_FAILED;
}
*/
/** /**
* @brief move2 - simple move to given point and stop * @brief move2 - simple move to given point and stop
* @param X - new X coordinate (radians: -pi..pi) or NULL * @param X - new X coordinate (radians: -pi..pi) or NULL
@@ -245,12 +288,13 @@ static mcc_errcodes_t move2(const coordpair_t *target){
DBG("x,y: %g, %g", target->X, target->Y); DBG("x,y: %g, %g", target->X, target->Y);
cmd.Xmot = target->X; cmd.Xmot = target->X;
cmd.Ymot = target->Y; cmd.Ymot = target->Y;
cmd.Xspeed = MCC_MAX_X_SPEED; cmd.Xspeed = Xlimits.max.speed;
cmd.Yspeed = MCC_MAX_Y_SPEED; cmd.Yspeed = Ylimits.max.speed;
mcc_errcodes_t r = shortcmd(&cmd); /*mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r; if(r != MCC_E_OK) return r;
setslewingstate(); setslewingstate();
return MCC_E_OK; return MCC_E_OK;*/
return shortcmd(&cmd);
} }
/** /**
@@ -279,6 +323,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
if(!target || !speed) return MCC_E_BADFORMAT; if(!target || !speed) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) 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; 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; if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0}; short_command_t cmd = {0};
cmd.Xmot = target->X; cmd.Xmot = target->X;
@@ -298,7 +343,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
static mcc_errcodes_t emstop(){ static mcc_errcodes_t emstop(){
FNAME(); FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
Xmodel->emergency_stop(Xmodel, curt); Xmodel->emergency_stop(Xmodel, curt);
Ymodel->emergency_stop(Ymodel, curt); Ymodel->emergency_stop(Ymodel, curt);
return MCC_E_OK; return MCC_E_OK;
@@ -310,7 +355,7 @@ static mcc_errcodes_t emstop(){
static mcc_errcodes_t stop(){ static mcc_errcodes_t stop(){
FNAME(); FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
Xmodel->stop(Xmodel, curt); Xmodel->stop(Xmodel, curt);
Ymodel->stop(Ymodel,curt); Ymodel->stop(Ymodel,curt);
return MCC_E_OK; return MCC_E_OK;
@@ -327,7 +372,7 @@ static mcc_errcodes_t stop(){
static mcc_errcodes_t shortcmd(short_command_t *cmd){ static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT; if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
moveparam_t param = {0}; moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed; param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
@@ -359,7 +404,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
static mcc_errcodes_t longcmd(long_command_t *cmd){ static mcc_errcodes_t longcmd(long_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT; if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = timefromstart();
moveparam_t param = {0}; moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed; param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
@@ -386,6 +431,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
if(!hwConfig) return MCC_E_BADFORMAT; if(!hwConfig) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED; if(Conf.RunModel) return MCC_E_FAILED;
SSconfig config; SSconfig config;
DBG("Read HW configuration");
if(!cmdC(&config, FALSE)) return MCC_E_FAILED; if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
// Convert acceleration (ticks per loop^2 to rad/s^2) // Convert acceleration (ticks per loop^2 to rad/s^2)
hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel); hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
@@ -425,8 +471,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
// Copy ticks per revolution // Copy ticks per revolution
hwConfig->Xsetpr = __bswap_32(config.Xsetpr); hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = __bswap_32(config.Ysetpr); hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4; hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
// Convert slew rates (ticks per loop to rad/s) // Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@@ -444,6 +490,30 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0); hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
// Convert backlash speed (ticks per loop to rad/s) // Convert backlash speed (ticks per loop to rad/s)
hwConfig->backlspd = X_MOTSPD2RS(config.backlspd); 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; return MCC_E_OK;
} }
@@ -499,17 +569,37 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.Ysetpr = __bswap_32(hwConfig->Ysetpr); config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
config.Xmetpr = __bswap_32(hwConfig->Xmetpr); config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
config.Ymetpr = __bswap_32(hwConfig->Ymetpr); config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
// todo - also write text params
// TODO - next // TODO - next
(void) config; (void) config;
return MCC_E_OK; 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 // init mount class
mount_t Mount = { mount_t Mount = {
.init = init, .init = init,
.quit = quit, .quit = quit,
.getMountData = getMD, .getMountData = getMD,
// .slewTo = slew2,
.moveTo = move2, .moveTo = move2,
.moveWspeed = move2s, .moveWspeed = move2s,
.setSpeed = setspeed, .setSpeed = setspeed,
@@ -519,7 +609,13 @@ mount_t Mount = {
.longCmd = longcmd, .longCmd = longcmd,
.getHWconfig = get_hwconf, .getHWconfig = get_hwconf,
.saveHWconfig = write_hwconf, .saveHWconfig = write_hwconf,
.currentT = nanotime, .currentT = curtime,
.timeFromStart = timefromstart,
.timeDiff = timediff,
.timeDiff0 = timediff0,
.correctTo = correct2, .correctTo = correct2,
.getMaxSpeed = maxspeed,
.getMinSpeed = minspeed,
.getAcceleration = acceleration,
}; };

View File

@@ -24,11 +24,16 @@
#include <stdlib.h> #include <stdlib.h>
#include "movingmodel.h"
#include "sidservo.h" #include "sidservo.h"
extern conf_t Conf; extern conf_t Conf;
double nanotime(); extern limits_t Xlimits, Ylimits;
void getModData(coordval_pair_t *c); 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{ typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
@@ -42,10 +47,6 @@ double LS_calc_slope(less_square_t *l, double x, double t);
// unused arguments of functions // unused arguments of functions
#define _U_ __attribute__((__unused__)) #define _U_ __attribute__((__unused__))
// break absent in `case`
#define FALLTHRU __attribute__ ((fallthrough))
// and synonym for FALLTHRU
#define NOBREAKHERE __attribute__ ((fallthrough))
// weak functions // weak functions
#define WEAK __attribute__ ((weak)) #define WEAK __attribute__ ((weak))

View File

@@ -60,9 +60,14 @@ movemodel_t *model_init(limits_t *l){
int model_move2(movemodel_t *model, moveparam_t *target, double t){ int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE; if(!target || !model) return FALSE;
//DBG("MOVE to %g at speed %g", target->coord, target->speed); DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
// only positive velocity // only positive velocity
if(target->speed < 0.) target->speed = -target->speed; 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 // don't mind about acceleration - user cannot set it now
return model->calculate(model, target, t); return model->calculate(model, target, t);
} }

View File

@@ -44,7 +44,7 @@ typedef struct{
typedef struct{ typedef struct{
moveparam_t min; moveparam_t min;
moveparam_t max; moveparam_t max;
double acceleration; //double acceleration;
} limits_t; } limits_t;
typedef enum{ typedef enum{

197
LibSidServo/polltest/main.c Normal file
View File

@@ -0,0 +1,197 @@
/*
* This file is part of the libsidservo project.
* Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <fcntl.h>
#include <poll.h>
#include <signal.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <usefull_macros.h>
// 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;
}

View File

@@ -23,12 +23,14 @@
#include "main.h" #include "main.h"
#include "ramp.h" #include "ramp.h"
/*
#ifdef EBUG #ifdef EBUG
#undef DBG #undef DBG
#define DBG(...) #define DBG(...)
#undef FNAME
#define FNAME()
#endif #endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT; static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){ static void emstop(movemodel_t *m, double _U_ t){

View File

@@ -20,6 +20,7 @@
#include <errno.h> #include <errno.h>
#include <fcntl.h> #include <fcntl.h>
#include <math.h> #include <math.h>
#include <poll.h>
#include <pthread.h> #include <pthread.h>
#include <signal.h> #include <signal.h>
#include <stdint.h> #include <stdint.h>
@@ -48,7 +49,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
// encoders thread and mount thread // encoders thread and mount thread
static pthread_t encthread, mntthread; static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
// encoders raw data // encoders raw data
typedef struct __attribute__((packed)){ typedef struct __attribute__((packed)){
uint8_t magick; uint8_t magick;
@@ -64,20 +65,12 @@ void getXspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); double dt = timediff0(&mountdata.encXposition.t);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t); double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ if(fabs(speed) < 1.5 * Xlimits.max.speed){
mountdata.encXspeed.val = speed; mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t; mountdata.encXspeed.t = mountdata.encXposition.t;
} }
pthread_mutex_unlock(&datamutex);
//DBG("Xspeed=%g", mountdata.encXspeed.val);
#if 0
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
lastXenc.val = mountdata.encXposition.val;
lastXenc.t = t;
#endif
} }
void getYspeed(){ void getYspeed(){
static less_square_t *ls = NULL; static less_square_t *ls = NULL;
@@ -85,19 +78,12 @@ void getYspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return; if(!ls) return;
} }
pthread_mutex_lock(&datamutex); double dt = timediff0(&mountdata.encYposition.t);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t); double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ if(fabs(speed) < 1.5 * Ylimits.max.speed){
mountdata.encYspeed.val = speed; mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t; mountdata.encYspeed.t = mountdata.encYposition.t;
} }
pthread_mutex_unlock(&datamutex);
#if 0
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
lastYenc.val = mountdata.encYposition.val;
lastYenc.t = t;
#endif
} }
/** /**
@@ -105,7 +91,8 @@ void getYspeed(){
* @param databuf - input buffer with 13 bytes of data * @param databuf - input buffer with 13 bytes of data
* @param t - time when databuf[0] got * @param t - time when databuf[0] got
*/ */
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
if(!t) return;
enc_t *edata = (enc_t*) databuf; enc_t *edata = (enc_t*) databuf;
/* /*
#ifdef EBUG #ifdef EBUG
@@ -140,18 +127,17 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
return; return;
} }
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
mountdata.encXposition.val = X_ENC2RAD(edata->encX); mountdata.encXposition.val = Xenc2rad(edata->encX);
mountdata.encYposition.val = Y_ENC2RAD(edata->encY); mountdata.encYposition.val = Yenc2rad(edata->encY);
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = t; mountdata.encXposition.t = *t;
mountdata.encYposition.t = t; mountdata.encYposition.t = *t;
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getXspeed(); getYspeed(); getXspeed(); getYspeed();
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI); //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);
} }
#if 0
/** /**
* @brief getencval - get uint64_t data from encoder * @brief getencval - get uint64_t data from encoder
* @param fd - encoder fd * @param fd - encoder fd
@@ -159,33 +145,53 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
* @param t - measurement time * @param t - measurement time
* @return amount of data read or 0 if problem * @return amount of data read or 0 if problem
*/ */
static int getencval(int fd, double *val, double *t){ static int getencval(int fd, double *val, struct timespec *t){
if(fd < 0) return FALSE; if(fd < 0){
DBG("Encoder fd < 0!");
return FALSE;
}
char buf[128]; char buf[128];
int got = 0, Lmax = 127; int got = 0, Lmax = 127;
double t0 = nanotime(); double t0 = timefromstart();
//DBG("start: %.6g", t0);
do{ do{
fd_set rfds; fd_set rfds;
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(fd, &rfds); FD_SET(fd, &rfds);
struct timeval tv = encRtmout; struct timeval tv = encRtmout;
int retval = select(fd + 1, &rfds, NULL, NULL, &tv); int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
if(!retval) continue; if(!retval){
//DBG("select()==0 - timeout, %.6g", timefromstart());
break;
}
if(retval < 0){ if(retval < 0){
if(errno == EINTR) continue; if(errno == EINTR){
DBG("EINTR");
continue;
}
DBG("select() < 0");
return 0; return 0;
} }
if(FD_ISSET(fd, &rfds)){ if(FD_ISSET(fd, &rfds)){
ssize_t l = read(fd, &buf[got], Lmax); ssize_t l = read(fd, &buf[got], Lmax);
if(l < 1) return 0; // disconnected ?? if(l < 1){
DBG("read() < 0");
return 0; // disconnected ??
}
got += l; Lmax -= l; got += l; Lmax -= l;
buf[got] = 0; buf[got] = 0;
} else continue; } else continue;
if(strchr(buf, '\n')) break; if(buf[got-1] == '\n') break; // got EOL as last symbol
}while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval); }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
if(got == 0) return 0; // WTF? if(got == 0){
//DBG("No data from encoder, tfs=%.6g", timefromstart());
return 0;
}
char *estr = strrchr(buf, '\n'); char *estr = strrchr(buf, '\n');
if(!estr) return 0; if(!estr){
DBG("No EOL");
return 0;
}
*estr = 0; *estr = 0;
char *bgn = strrchr(buf, '\n'); char *bgn = strrchr(buf, '\n');
if(bgn) ++bgn; if(bgn) ++bgn;
@@ -197,9 +203,11 @@ static int getencval(int fd, double *val, double *t){
return 0; // wrong number return 0; // wrong number
} }
if(val) *val = (double) data; if(val) *val = (double) data;
if(t) *t = t0; if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
return got; return got;
} }
#endif
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getencbyte(){ static int getencbyte(){
if(encfd[0] < 0) return -1; if(encfd[0] < 0) return -1;
@@ -261,8 +269,6 @@ static void clrmntbuf(){
if(mntfd < 0) return; if(mntfd < 0) return;
uint8_t byte; uint8_t byte;
fd_set rfds; fd_set rfds;
//double t0 = nanotime();
//int n = 0;
do{ do{
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(mntfd, &rfds); FD_SET(mntfd, &rfds);
@@ -276,10 +282,8 @@ static void clrmntbuf(){
if(FD_ISSET(mntfd, &rfds)){ if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1); ssize_t l = read(mntfd, &byte, 1);
if(l != 1) break; if(l != 1) break;
//++n;
} else break; } else break;
}while(1); }while(1);
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
} }
// main encoder thread (for separate encoder): read next data and make parsing // main encoder thread (for separate encoder): read next data and make parsing
@@ -287,7 +291,7 @@ static void *encoderthread1(void _U_ *u){
if(Conf.SepEncoder != 1) return NULL; if(Conf.SepEncoder != 1) return NULL;
uint8_t databuf[ENC_DATALEN]; uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0; int wridx = 0, errctr = 0;
double t = 0.; struct timespec tcur;
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){ while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
int b = getencbyte(); int b = getencbyte();
if(b == -2) ++errctr; if(b == -2) ++errctr;
@@ -298,13 +302,14 @@ static void *encoderthread1(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){ if((uint8_t)b == ENC_MAGICK){
// DBG("Got magic -> start filling packet"); // DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b; databuf[wridx++] = (uint8_t) b;
t = nanotime();
} }
continue; continue;
}else databuf[wridx++] = (uint8_t) b; }else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){ if(wridx == ENC_DATALEN){
parse_encbuf(databuf, t); if(curtime(&tcur)){
wridx = 0; parse_encbuf(databuf, &tcur);
wridx = 0;
}
} }
} }
if(encfd[0] > -1){ if(encfd[0] > -1){
@@ -314,53 +319,138 @@ static void *encoderthread1(void _U_ *u){
return NULL; return NULL;
} }
#define XYBUFSZ (128)
typedef struct{
char buf[XYBUFSZ+1];
int len;
} buf_t;
// write to buffer next data portion; return FALSE in case of error
static int readstrings(buf_t *buf, int fd){
if(!buf){DBG("Empty buffer"); return FALSE;}
int L = XYBUFSZ - buf->len;
if(L < 0){
DBG("buf not initialized!");
buf->len = 0;
}
if(L == 0){
DBG("buffer overfull: %d!", 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;
}
//DBG("read %d bytes from %d", L, fd);
int got = read(fd, &buf->buf[buf->len], L);
if(got < 0){
DBG("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 || buf->len > (XYBUFSZ+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;
}
// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0 // main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
static void *encoderthread2(void _U_ *u){ static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL; if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started"); DBG("Thread started");
struct pollfd pfds[2];
pfds[0].fd = encfd[0]; pfds[0].events = POLLIN;
pfds[1].fd = encfd[1]; pfds[1].events = POLLIN;
double t0[2], tstart;
buf_t strbuf[2] = {0};
long msrlast[2]; // last encoder data
double mtlast[2]; // last measurement time
asknext(encfd[0]); asknext(encfd[1]);
t0[0] = t0[1] = tstart = timefromstart();
int errctr = 0; int errctr = 0;
double t0 = nanotime(); do{ // main cycle
const char *req = "\n"; if(poll(pfds, 2, 0) < 0){
int need2ask = 0; // need or not to ask encoder for new data DBG("poll()");
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ break;
if(need2ask){
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
} }
double v, t; int got = 0;
if(getencval(encfd[0], &v, &t)){ for(int i = 0; i < 2; ++i){
pthread_mutex_lock(&datamutex); if(pfds[i].revents && POLLIN){
mountdata.encXposition.val = X_ENC2RAD(v); if(!readstrings(&strbuf[i], encfd[i])){
//DBG("encX(%g) = %g", t, mountdata.encXposition.val); ++errctr;
mountdata.encXposition.t = t; break;
pthread_mutex_unlock(&datamutex); }
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); }
getXspeed(); double curt = timefromstart();
if(getencval(encfd[1], &v, &t)){ if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
pthread_mutex_lock(&datamutex); if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
mountdata.encYposition.val = Y_ENC2RAD(v); if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
//DBG("encY(%g) = %g", t, mountdata.encYposition.val); pthread_mutex_lock(&datamutex);
mountdata.encYposition.t = t; if(i == 0){
pthread_mutex_unlock(&datamutex); mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); curtime(&mountdata.encXposition.t);
getYspeed(); /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
errctr = 0; msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
need2ask = 0; X_ENC_ZERO, X_ENC_STEPSPERREV);*/
} else { getXspeed();
if(need2ask) ++errctr; }else{
else need2ask = 1; mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
continue; curtime(&mountdata.encYposition.t);
getYspeed();
}
pthread_mutex_unlock(&datamutex);
}
if(!asknext(encfd[i])){
++errctr;
break;
}
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
++got;
} }
} else {
if(need2ask) ++errctr;
else need2ask = 1;
continue;
} }
while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); } if(got == 2) errctr = 0;
//DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval); }while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
t0 = nanotime(); DBG("\n\nEXIT: ERRCTR=%d", errctr);
}
DBG("ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){ if(encfd[i] > -1){
close(encfd[i]); close(encfd[i]);
@@ -386,33 +476,67 @@ void data_free(data_t **x){
*x = NULL; *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 // main mount thread
static void *mountthread(void _U_ *u){ static void *mountthread(void _U_ *u){
int errctr = 0; int errctr = 0;
uint8_t buf[2*sizeof(SSstat)]; uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf; SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata)); bzero(&mountdata, sizeof(mountdata));
double t0 = nanotime(); double t0 = timefromstart(), tstart = t0, tcur = t0;
static double oldmt = -100.; // old `millis measurement` time double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0; static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){ if(Conf.RunModel){
coordval_pair_t c; double Xprev = NAN, Yprev = NAN; // previous coordinates
// now change data int xcnt = 0, ycnt = 0;
getModData(&c); while(1){
pthread_mutex_lock(&datamutex); coordpair_t c;
double tnow = c.X.t; movestate_t xst, yst;
mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow; // now change data
mountdata.motXposition.val = mountdata.encXposition.val = c.X.val; getModData(&c, &xst, &yst);
mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val; struct timespec tnow;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
if(tnow - oldmt > Conf.MountReqInterval){ pthread_mutex_lock(&datamutex);
oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3); mountdata.encXposition.t = mountdata.encYposition.t = tnow;
oldmt = tnow; mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
}else mountdata.millis = oldmillis; mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
pthread_mutex_unlock(&datamutex); //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
getXspeed(); getYspeed(); if(tcur - oldmt > Conf.MountReqInterval){
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
t0 = nanotime(); 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);
getXspeed(); getYspeed();
pthread_mutex_unlock(&datamutex);
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = timefromstart();
}
} }
// data to get // data to get
data_t d = {.buf = buf, .maxlen = sizeof(buf)}; data_t d = {.buf = buf, .maxlen = sizeof(buf)};
@@ -421,31 +545,8 @@ static void *mountthread(void _U_ *u){
if(!cmd_getstat) goto failed; if(!cmd_getstat) goto failed;
while(mntfd > -1 && errctr < MAX_ERR_CTR){ while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status // read data to status
double t0 = nanotime(); struct timespec tcur;
#if 0 if(!curtime(&tcur)) continue;
// 127 milliseconds to get answer on X/Y commands!!!
int64_t ans;
int ctr = 0;
if(SSgetint(CMD_MOTX, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
tgot = nanotime();
if(SSgetint(CMD_MOTY, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
if(ctr == 2){
mountdata.millis = (uint32_t)(1e3 * tgot);
DBG("Got both coords; millis=%d", mountdata.millis);
}
#endif
// 80 milliseconds to get answer on GETSTAT // 80 milliseconds to get answer on GETSTAT
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG #ifdef EBUG
@@ -462,14 +563,13 @@ static void *mountthread(void _U_ *u){
errctr = 0; errctr = 0;
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
// now change data // now change data
SSconvstat(status, &mountdata, t0); SSconvstat(status, &mountdata, &tcur);
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("GOT FULL stat by %g", nanotime() - t0);
// allow writing & getters // allow writing & getters
do{ do{
usleep(500); usleep(500);
}while(nanotime() - t0 < Conf.MountReqInterval); }while(timefromstart() - t0 < Conf.MountReqInterval);
t0 = nanotime(); t0 = timefromstart();
} }
data_free(&cmd_getstat); data_free(&cmd_getstat);
failed: failed:
@@ -485,8 +585,15 @@ static int ttyopen(const char *path, speed_t speed){
int fd = -1; int fd = -1;
struct termios2 tty; struct termios2 tty;
DBG("Try to open %s @ %d", path, speed); DBG("Try to open %s @ %d", path, speed);
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1; if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; } 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_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
tty.c_iflag = 0; // don't do any changes in input stream 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_oflag = 0; // don't do any changes in output stream
@@ -495,7 +602,11 @@ static int ttyopen(const char *path, speed_t speed){
tty.c_ospeed = speed; tty.c_ospeed = speed;
//tty.c_cc[VMIN] = 0; // non-canonical mode //tty.c_cc[VMIN] = 0; // non-canonical mode
//tty.c_cc[VTIME] = 5; //tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; } 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); 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; } if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive // try to set exclusive
@@ -528,7 +639,7 @@ int openEncoder(){
if(encfd[i] < 0) return FALSE; if(encfd[i] < 0) return FALSE;
} }
encRtmout.tv_sec = 0; encRtmout.tv_sec = 0;
encRtmout.tv_usec = 1000; // 1ms encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
close(encfd[i]); close(encfd[i]);
@@ -575,6 +686,7 @@ create_thread:
// close all opened serial devices and quit threads // close all opened serial devices and quit threads
void closeSerial(){ void closeSerial(){
// TODO: close devices in "model" mode too!
if(Conf.RunModel) return; if(Conf.RunModel) return;
if(mntfd > -1){ if(mntfd > -1){
DBG("Cancel mount thread"); DBG("Cancel mount thread");
@@ -606,6 +718,8 @@ mcc_errcodes_t getMD(mountdata_t *d){
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
*d = mountdata; *d = mountdata;
pthread_mutex_unlock(&datamutex); 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; return MCC_E_OK;
} }
@@ -624,30 +738,24 @@ static int wr(const data_t *out, data_t *in, int needeol){
return FALSE; return FALSE;
} }
clrmntbuf(); clrmntbuf();
//double t0 = nanotime();
if(out){ if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need"); DBG("written bytes not equal to need");
return FALSE; return FALSE;
} }
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
if(needeol){ if(needeol){
int g = write(mntfd, "\r", 1); // add EOL int g = write(mntfd, "\r", 1); // add EOL
(void) g; (void) g;
} }
usleep(50000); // add little pause so that the idiot has time to swallow
} }
//DBG("sent by %g", nanotime() - t0);
//uint8_t buf[256];
//data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) return TRUE; if(!in) return TRUE;
//if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
in->len = 0; in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){ for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte(); int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b; in->buf[in->len++] = (uint8_t) b;
} }
//DBG("got %zd bytes by %g", in->len, nanotime() - t0);
while(getmntbyte() > -1); while(getmntbyte() > -1);
return TRUE; return TRUE;
} }
@@ -751,16 +859,23 @@ int cmdC(SSconfig *conf, int rw){
}else{ // read }else{ // read
data_t d; data_t d;
d.buf = (uint8_t *) conf; d.buf = (uint8_t *) conf;
d.len = 0; d.maxlen = sizeof(SSconfig); 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); ret = wr(rcmd, &d, 1);
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen); DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
if(d.len != d.maxlen) return FALSE; if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
// simplest checksum // simplest checksum
uint16_t sum = 0; uint16_t sum = 0;
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i]; for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
if(sum != conf->checksum){ if(sum != conf->checksum){
DBG("got sum: %u, need: %u", conf->checksum, sum); DBG("got sum: %u, need: %u", conf->checksum, sum);
return FALSE; ret = FALSE;
goto rtn;
} }
} }
rtn: rtn:

View File

@@ -32,38 +32,13 @@ extern "C"
#include <stdint.h> #include <stdint.h>
#include <sys/time.h> #include <sys/time.h>
// acceptable position error - 0.1'' // minimal serial speed of mount device
#define MCC_POSITION_ERROR (5e-7) #define MOUNT_BAUDRATE_MIN (1200)
// acceptable disagreement between motor and axis encoders - 2''
#define MCC_ENCODERS_ERROR (1e-7)
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
#define MCC_MAX_X_SPEED (0.174533)
#define MCC_MAX_Y_SPEED (0.139626)
// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
#define MCC_X_ACCELERATION (0.219911)
#define MCC_Y_ACCELERATION (0.165806)
// max speed interval, seconds // max speed interval, seconds
#define MCC_CONF_MAX_SPEEDINT (2.) #define MCC_CONF_MAX_SPEEDINT (2.)
// minimal speed interval in parts of EncoderReqInterval // minimal speed interval in parts of EncoderReqInterval
#define MCC_CONF_MIN_SPEEDC (3.) #define MCC_CONF_MIN_SPEEDC (3.)
// PID I cycle time (analog of "RC" for PID on opamps)
#define MCC_PID_CYCLE_TIME (5.)
// maximal PID refresh time interval (if larger all old data will be cleared)
#define MCC_PID_MAX_DT (1.)
// normal PID refresh interval
#define MCC_PID_REFRESH_DT (0.1)
// boundary conditions for axis state: "slewing/pointing/guiding"
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees
//#define MCC_MAX_POINTING_ERR (0.20943951)
//#define MCC_MAX_POINTING_ERR (0.08726646)
#define MCC_MAX_POINTING_ERR (0.13962634)
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
#define MCC_MAX_GUIDING_ERR (0.026179939)
// if error less than this value we suppose that target is captured and guiding is good: 0.1''
#define MCC_MAX_ATTARGET_ERR (4.8481368e-7)
// error codes // error codes
typedef enum{ typedef enum{
@@ -73,6 +48,7 @@ typedef enum{
MCC_E_ENCODERDEV, // encoder device error or can't open MCC_E_ENCODERDEV, // encoder device error or can't open
MCC_E_MOUNTDEV, // mount 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_FAILED, // failed to run command - protocol error
MCC_E_AMOUNT // Just amount of errors
} mcc_errcodes_t; } mcc_errcodes_t;
typedef struct{ typedef struct{
@@ -87,14 +63,23 @@ typedef struct{
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices 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* EncoderXDevPath; // paths to new controller devices
char* EncoderYDevPath; char* EncoderYDevPath;
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
double MountReqInterval; // interval between subsequent mount requests (seconds) double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds) double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
double EncoderSpeedInterval; // interval between speed calculations double EncoderSpeedInterval; // interval between speed calculations
int RunModel; // == 1 if you want to use model instead of real mount 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 XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
PIDpar_t XPIDV; PIDpar_t XPIDV;
PIDpar_t YPIDC; PIDpar_t YPIDC;
PIDpar_t YPIDV; 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; } conf_t;
// coordinates/speeds in degrees or d/s: X, Y // coordinates/speeds in degrees or d/s: X, Y
@@ -105,7 +90,7 @@ typedef struct{
// coordinate/speed and time of last measurement // coordinate/speed and time of last measurement
typedef struct{ typedef struct{
double val; double val;
double t; struct timespec t;
} coordval_t; } coordval_t;
typedef struct{ typedef struct{
@@ -206,6 +191,9 @@ typedef struct{
double outplimit; // Output Limit, percent (0..100) double outplimit; // Output Limit, percent (0..100)
double currlimit; // Current Limit (A) double currlimit; // Current Limit (A)
double intlimit; // Integral Limit (???) 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; } __attribute__((packed)) axis_config_t;
// hardware configuration // hardware configuration
@@ -247,7 +235,7 @@ typedef struct{
void (*quit)(); // deinit void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); // mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint); 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 (*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 (*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 (*setSpeed)(const coordpair_t *tagspeed); // set speed
@@ -257,7 +245,13 @@ typedef struct{
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long 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 (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
double (*currentT)(); // current time 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; } mount_t;
extern mount_t Mount; extern mount_t Mount;

View File

@@ -26,6 +26,13 @@
#include "serial.h" #include "serial.h"
#include "ssii.h" #include "ssii.h"
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
// defaults until read from controller
double X_MOT_STEPSPERREV = 13312000.,
Y_MOT_STEPSPERREV = 17578668.,
X_ENC_STEPSPERREV = 67108864.,
Y_ENC_STEPSPERREV = 67108864.;
uint16_t SScalcChecksum(uint8_t *buf, int len){ uint16_t SScalcChecksum(uint8_t *buf, int len){
uint16_t checksum = 0; uint16_t checksum = 0;
for(int i = 0; i < len; i++){ for(int i = 0; i < len; i++){
@@ -67,17 +74,18 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
* @param m (o) - output * @param m (o) - output
* @param t - measurement time * @param t - measurement time
*/ */
void SSconvstat(const SSstat *s, mountdata_t *m, double t){ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
if(!s || !m) return; if(!s || !m || !t) return;
m->motXposition.val = X_MOT2RAD(s->Xmot); m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot); m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m); ChkStopped(s, m);
m->motXposition.t = m->motYposition.t = t; m->motXposition.t = m->motYposition.t = *t;
// fill encoder data from here, as there's no separate enc thread // fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){ if(!Conf.SepEncoder){
m->encXposition.val = X_ENC2RAD(s->Xenc); m->encXposition.val = Xenc2rad(s->Xenc);
m->encYposition.val = Y_ENC2RAD(s->Yenc); DBG("encx: %g", m->encXposition.val);
m->encXposition.t = m->encYposition.t = t; m->encYposition.val = Yenc2rad(s->Yenc);
m->encXposition.t = m->encYposition.t = *t;
getXspeed(); getYspeed(); getXspeed(); getYspeed();
} }
m->keypad = s->keypad; m->keypad = s->keypad;
@@ -176,33 +184,39 @@ int SSstop(int emerg){
mcc_errcodes_t updateMotorPos(){ mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0}; mountdata_t md = {0};
if(Conf.RunModel) return MCC_E_OK; if(Conf.RunModel) return MCC_E_OK;
double t0 = nanotime(), t = 0.; double t0 = timefromstart(), t = 0.;
struct timespec curt;
DBG("start @ %g", t0); DBG("start @ %g", t0);
do{ do{
t = nanotime(); 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(MCC_E_OK == getMD(&md)){
if(md.encXposition.t == 0 || md.encYposition.t == 0){ if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
DBG("Just started, t-t0 = %g!", t - t0); DBG("Just started? t-t0 = %g!", t - t0);
sleep(1); usleep(10000);
DBG("t-t0 = %g", nanotime() - t0);
//usleep(10000);
continue; continue;
} }
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t); 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; mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); 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))){ if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!"); DBG("Xpos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0); }else DBG("Xpos sync OK, Dt=%g", t - t0);
} }
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!"); DBG("Ypos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Ypos sync OK, Dt=%g", nanotime() - t0); }else DBG("Ypos sync OK, Dt=%g", t - t0);
} }
if(MCC_E_OK == OK){ if(MCC_E_OK == OK){
DBG("Encoders synced"); DBG("Encoders synced");

View File

@@ -173,64 +173,74 @@
#define SITECH_LOOP_FREQUENCY (1953.) #define SITECH_LOOP_FREQUENCY (1953.)
// amount of consequent same coordinates to detect stop // amount of consequent same coordinates to detect stop
#define MOTOR_STOPPED_CNT (4) #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? // TODO: take it from settings?
// steps per revolution (SSI - x4 - for SSI) // steps per revolution (SSI - x4 - for SSI)
#define X_MOT_STEPSPERREV_SSI (13312000.) // -> hwconf.Xconf.mot/enc_stepsperrev
//#define X_MOT_STEPSPERREV_SSI (13312000.)
// 13312000 / 4 = 3328000 // 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV (3328000.) //#define X_MOT_STEPSPERREV (3328000.)
#define Y_MOT_STEPSPERREV_SSI (17578668.) //#define Y_MOT_STEPSPERREV_SSI (17578668.)
// 17578668 / 4 = 4394667 // 17578668 / 4 = 4394667
#define Y_MOT_STEPSPERREV (4394667.) //#define Y_MOT_STEPSPERREV (4394667.)
// encoder per revolution // encoder per revolution
#define X_ENC_STEPSPERREV (67108864.) //#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.) //#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position // encoder zero position
#define X_ENC_ZERO (61245239) // -> conf.XEncZero/YEncZero
#define Y_ENC_ZERO (36999830) //#define X_ENC_ZERO (61245239)
// encoder reversed (no: +1) //#define Y_ENC_ZERO (36999830)
#define X_ENC_SIGN (-1.) // encoder reversed (no: +1) -> sign of ...stepsperrev
#define Y_ENC_SIGN (-1.) //#define X_ENC_SIGN (-1.)
//#define Y_ENC_SIGN (-1.)
// encoder position to radians and back // encoder position to radians and back
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV) #define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) #define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) #define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) #define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
// convert angle in radians to +-pi // convert angle in radians to +-pi
static inline double ang2half(double ang){ static inline __attribute__((always_inline)) double ang2half(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < -M_PI) ang += 2.*M_PI; if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI; else if(ang > M_PI) ang -= 2.*M_PI;
return ang; return ang;
} }
// convert to only positive: 0..2pi // convert to only positive: 0..2pi
static inline double ang2full(double ang){ static inline __attribute__((always_inline)) double ang2full(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < 0.) ang += 2.*M_PI; if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang; return ang;
} }
// motor position to radians and back // motor position to radians and back
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) #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 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 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)) #define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
// motor speed in rad/s and back // motor speed in rad/s and back
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) #define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
#define Y_MOTSPD2RS(n) (Y_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 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)) #define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
// motor acceleration -//- // motor acceleration -//-
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) #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 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 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)) #define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
// adder time to seconds vice versa // adder time to seconds vice versa
#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY) #define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY))
#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) #define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY))
// encoder's tolerance (ticks) // encoder's tolerance (ticks)
#define YencTOL (25.) #define YencTOL (25.)
@@ -331,7 +341,7 @@ typedef struct{
} __attribute__((packed)) SSconfig; } __attribute__((packed)) SSconfig;
uint16_t SScalcChecksum(uint8_t *buf, int len); uint16_t SScalcChecksum(uint8_t *buf, int len);
void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t); void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
int SStextcmd(const char *cmd, data_t *answer); int SStextcmd(const char *cmd, data_t *answer);
int SSrawcmd(const char *cmd, data_t *answer); int SSrawcmd(const char *cmd, data_t *answer);
int SSgetint(const char *cmd, int64_t *ans); int SSgetint(const char *cmd, int64_t *ans);

88
asibfm700/CMakeLists.txt Normal file
View File

@@ -0,0 +1,88 @@
cmake_minimum_required(VERSION 3.14)
# set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
# find_package(ASIO QUIET CONFIG)
find_package(ASIO QUIET)
if (ASIO_FOUND)
message(STATUS "ASIO library was found in the host system")
else()
message(STATUS "ASIO library was not found! Try to download it!")
include(FetchContent)
include(ExternalProject)
FetchContent_Declare(asio_lib
SOURCE_DIR ${CMAKE_BINARY_DIR}/asio_lib
BINARY_DIR ${CMAKE_BINARY_DIR}
GIT_REPOSITORY "https://github.com/chriskohlhoff/asio"
GIT_TAG "asio-1-32-0"
GIT_SHALLOW TRUE
GIT_SUBMODULES ""
GIT_PROGRESS TRUE
)
FetchContent_MakeAvailable(asio_lib)
# FetchContent_GetProperties(asio_lib SOURCE_DIR asio_SOURCE_DIR)
set(ASIO_INSTALL_DIR ${CMAKE_BINARY_DIR}/asio_lib/asio)
find_package(ASIO)
endif()
find_package(cxxopts QUIET CONFIG)
if (cxxopts_FOUND)
message(STATUS "CXXOPTS library was found in the host system")
else()
message(STATUS "CXXOPTS library was not found! Try to download it!")
include(FetchContent)
include(ExternalProject)
FetchContent_Declare(cxxopts_lib
PREFIX ${CMAKE_BINARY_DIR}/cxxopts_lib
# SOURCE_DIR ${CMAKE_BINARY_DIR}/cxxopts_lib
# BINARY_DIR ${CMAKE_BINARY_DIR}
GIT_REPOSITORY "https://github.com/jarro2783/cxxopts.git"
GIT_TAG "v3.3.1"
GIT_SHALLOW TRUE
GIT_SUBMODULES ""
GIT_PROGRESS TRUE
OVERRIDE_FIND_PACKAGE
)
FetchContent_MakeAvailable(cxxopts_lib)
find_package(cxxopts CONFIG)
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
)
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 mcc ASIO::ASIO spdlog ERFA_LIB bsplines sidservo)
set(ASIBFM700_NETSERVER_APP asibfm700_netserver)
add_executable(${ASIBFM700_NETSERVER_APP} asibfm700_netserver_main.cpp)
target_link_libraries(${ASIBFM700_NETSERVER_APP} PRIVATE cxxopts::cxxopts ${ASIBFM700_LIB})
option(WITH_TESTS "Build tests" ON)
if (WITH_TESTS)
set(CFG_TEST_APP cfg_test)
add_executable(${CFG_TEST_APP} tests/cfg_test.cpp)
target_link_libraries(${CFG_TEST_APP} PRIVATE mcc)
enable_testing()
endif()

View File

@@ -0,0 +1,30 @@
#pragma once
/* AstroSib FORK MOUNT FM-700 CONTROL LIBRARY */
/* COMMON LIBRARY DEFINITIONS */
#include <mcc_moving_model_common.h>
#include <mcc_pcm.h>
#include <mcc_pzone_container.h>
#include <mcc_spdlog.h>
#include "mcc_ccte_erfa.h"
#include "mcc_slewing_model.h"
#include "mcc_tracking_model.h"
namespace asibfm700
{
static constexpr mcc::MccMountType asibfm700MountType = mcc::MccMountType::FORK_TYPE;
typedef mcc::ccte::erfa::MccCCTE_ERFA Asibfm700CCTE;
typedef mcc::MccDefaultPCM<asibfm700MountType> Asibfm700PCM;
typedef mcc::MccPZoneContainer<mcc::MccTimeDuration> Asibfm700PZoneContainer;
typedef mcc::utils::MccSpdlogLogger Asibfm700Logger;
typedef mcc::MccSimpleSlewingModel Asibfm700SlewingModel;
typedef mcc::MccSimpleTrackingModel Asibfm700TrackingModel;
} // namespace asibfm700

View File

@@ -0,0 +1,860 @@
#pragma once
/**/
#include <expected>
#include <filesystem>
#include <fstream>
#include <mcc_angle.h>
#include <mcc_moving_model_common.h>
#include <mcc_pcm.h>
#include <mcc_utils.h>
#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 <typename T>
concept config_record_valid_type_c = requires { !std::is_array_v<T> && !std::is_void_v<T> && !std::is_reference_v<T>; };
// simple minimal-requirement configuration record class
template <config_record_valid_type_c T>
struct simple_config_record_t {
std::string_view key;
T value;
std::vector<std::string_view> 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::MccAngle(43.646711_degs), {"site latitude in degrees"}},
// site longitude in degrees
simple_config_record_t{"siteLongitude", mcc::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::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<double>{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<size_t>{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<double>{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<double>{-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<double>{},
{"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<double>{},
{"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::MccAngle(10.0_degs), {"minimal altitude"}},
// HA-axis limit switch minimal value
simple_config_record_t{"pzLimitSwitchHAMin", mcc::MccAngle(-270.0_degs), {"HA-axis limit switch minimal value"}},
// HA-axis limit switch maximal value
simple_config_record_t{"pzLimitSwitchHAMax", mcc::MccAngle(270.0_degs), {"HA-axis limit switch maximal value"}},
// DEC-axis limit switch minimal value
simple_config_record_t{"pzLimitSwitchDecMin", mcc::MccAngle(-90.0_degs), {"DEC-axis limit switch minimal value"}},
// DEC-axis limit switch maximal value
simple_config_record_t{"pzLimitSwitchDecMax", mcc::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<double>{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<double>{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<double>{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<double>{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::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::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::MccAngle(8.0_degs),
{"slewing-to-pointing mode angular limit in degrees"}},
simple_config_record_t{"MaxFinePointingErr",
mcc::MccAngle(1.5_degs),
{"pointing-to-guiding mode angular limit in degrees"}},
simple_config_record_t{"MaxGuidingErr",
mcc::MccAngle(0.5_arcsecs),
{"guiding 'good'-flag error cirle radius (mount-to-target distance) in degrees"}},
simple_config_record_t{"XEncZero", mcc::MccAngle(0.0_degs), {"X-axis encoder zero-point in degrees"}},
simple_config_record_t{"YEncZero", mcc::MccAngle(0.0_degs), {"Y-axis encoder zero-point in degrees"}}
);
class Asibfm700MountConfig : public mcc::utils::KeyValueHolder<decltype(Asibfm700MountConfigDefaults)>
{
using base_t = mcc::utils::KeyValueHolder<decltype(Asibfm700MountConfigDefaults)>;
protected:
inline static auto deserializer = []<typename VT>(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<VT> || mcc::traits::mcc_output_char_range<VT> || std::ranges::range<VT> ||
mcc::traits::mcc_time_duration_c<VT>) {
// ec = base_t::defaultDeserializeFunc(str, value);
ec = deser(str, value);
} else if constexpr (std::same_as<VT, mcc::MccAngle>) { // assume here all angles are in degrees
double vd;
// ec = base_t::defaultDeserializeFunc(str, vd);
ec = deser(str, vd);
if (!ec) {
value = mcc::MccAngle(vd, mcc::MccDegreeTag{});
}
} else if constexpr (std::same_as<VT, mcc::MccDefaultPCMType>) {
std::string vstr;
// ec = base_t::defaultDeserializeFunc(str, vstr);
ec = deser(str, vstr);
if (!ec) {
auto s = mcc::utils::trimSpaces(vstr);
if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY>) {
value = mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY;
} else if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE>) {
value = mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY;
} else if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE>) {
value = mcc::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 <mcc::traits::mcc_time_duration_c DT>
DT hardwarePollingPeriod() const
{
return std::chrono::duration_cast<DT>(
getValue<std::chrono::milliseconds>("hardwarePollingPeriod").value_or(std::chrono::milliseconds{}));
};
std::chrono::milliseconds hardwarePollingPeriod() const
{
return hardwarePollingPeriod<std::chrono::milliseconds>();
};
template <mcc::mcc_angle_c T>
T siteLatitude() const
{
return static_cast<double>(getValue<mcc::MccAngle>("siteLatitude").value_or(mcc::MccAngle{}));
};
mcc::MccAngle siteLatitude() const
{
return siteLatitude<mcc::MccAngle>();
};
template <mcc::mcc_angle_c T>
T siteLongitude() const
{
return static_cast<double>(getValue<mcc::MccAngle>("siteLongitude").value_or(mcc::MccAngle{}));
};
mcc::MccAngle siteLongitude() const
{
return siteLongitude<mcc::MccAngle>();
};
template <typename T>
T siteElevation() const
requires std::is_arithmetic_v<T>
{
return getValue<double>("siteElevation").value_or(0.0);
}
double siteElevation() const
{
return getValue<double>("siteElevation").value_or(0.0);
};
template <typename T>
T refractWavelength() const
requires std::is_arithmetic_v<T>
{
return getValue<double>("refractWavelength").value_or(0.0);
}
double refractWavelength() const
{
return getValue<double>("refractWavelength").value_or(0.0);
};
template <mcc::traits::mcc_output_char_range R>
R leapSecondFilename() const
{
R r;
std::string val = getValue<std::string>("leapSecondFilename").value_or("");
std::ranges::copy(val, std::back_inserter(r));
return r;
}
std::string leapSecondFilename() const
{
return leapSecondFilename<std::string>();
};
template <mcc::traits::mcc_output_char_range R>
R bulletinAFilename() const
{
R r;
std::string val = getValue<std::string>("bulletinAFilename").value_or("");
std::ranges::copy(val, std::back_inserter(r));
return r;
}
std::string bulletinAFilename() const
{
return bulletinAFilename<std::string>();
};
template <mcc::mcc_angle_c T>
T pzMinAltitude() const
{
return static_cast<double>(getValue<mcc::MccAngle>("pzMinAltitude").value_or(mcc::MccAngle{}));
};
mcc::MccAngle pzMinAltitude() const
{
return pzMinAltitude<mcc::MccAngle>();
};
template <mcc::mcc_angle_c T>
T pzLimitSwitchHAMin() const
{
return static_cast<double>(getValue<mcc::MccAngle>("pzLimitSwitchHAMin").value_or(mcc::MccAngle{}));
};
mcc::MccAngle pzLimitSwitchHAMin() const
{
return pzLimitSwitchHAMin<mcc::MccAngle>();
};
template <mcc::mcc_angle_c T>
T pzLimitSwitchHAMax() const
{
return static_cast<double>(getValue<mcc::MccAngle>("pzLimitSwitchHAMax").value_or(mcc::MccAngle{}));
};
mcc::MccAngle pzLimitSwitchHAMax() const
{
return pzLimitSwitchHAMax<mcc::MccAngle>();
};
AsibFM700ServoController::hardware_config_t servoControllerConfig() const
{
AsibFM700ServoController::hardware_config_t hw_cfg;
hw_cfg.hwConfig = {};
hw_cfg.MountDevPath = getValue<std::string>("MountDevPath").value_or(std::string{});
hw_cfg.EncoderDevPath = getValue<std::string>("EncoderDevPath").value_or(std::string{});
hw_cfg.EncoderXDevPath = getValue<std::string>("EncoderXDevPath").value_or(std::string{});
hw_cfg.EncoderYDevPath = getValue<std::string>("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<int>("RunModel").value_or(int{});
hw_cfg.devConfig.MountDevSpeed = getValue<int>("MountDevSpeed").value_or(int{});
hw_cfg.devConfig.EncoderDevSpeed = getValue<int>("EncoderDevSpeed").value_or(int{});
hw_cfg.devConfig.SepEncoder = getValue<int>("SepEncoder").value_or(int{});
std::chrono::duration<double> secs; // seconds as floating-point
secs = getValue<std::chrono::milliseconds>("MountReqInterval").value_or(std::chrono::milliseconds{});
hw_cfg.devConfig.MountReqInterval = secs.count();
secs = getValue<std::chrono::milliseconds>("EncoderReqInterval").value_or(std::chrono::milliseconds{});
hw_cfg.devConfig.EncoderReqInterval = secs.count();
secs = getValue<std::chrono::milliseconds>("EncoderSpeedInterval").value_or(std::chrono::milliseconds{});
hw_cfg.devConfig.EncoderSpeedInterval = secs.count();
secs = getValue<std::chrono::milliseconds>("PIDMaxDt").value_or(std::chrono::milliseconds{1000});
hw_cfg.devConfig.PIDMaxDt = secs.count();
secs = getValue<std::chrono::milliseconds>("PIDRefreshDt").value_or(std::chrono::milliseconds{100});
hw_cfg.devConfig.PIDRefreshDt = secs.count();
secs = getValue<std::chrono::milliseconds>("PIDCycleDt").value_or(std::chrono::milliseconds{5000});
hw_cfg.devConfig.PIDCycleDt = secs.count();
std::vector<double> pid = getValue<std::vector<double>>("XPIDC").value_or(std::vector<double>{});
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<std::vector<double>>("XPIDV").value_or(std::vector<double>{});
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<std::vector<double>>("YPIDC").value_or(std::vector<double>{});
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<std::vector<double>>("YPIDV").value_or(std::vector<double>{});
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<mcc::MccAngle>("MaxPointingErr").value_or(mcc::MccAngle(8.0_degs));
hw_cfg.devConfig.MaxPointingErr = ang;
ang = getValue<mcc::MccAngle>("MaxFinePointingErr").value_or(mcc::MccAngle(1.5_degs));
hw_cfg.devConfig.MaxFinePointingErr = ang;
ang = getValue<mcc::MccAngle>("MaxGuidingErr").value_or(mcc::MccAngle(0.5_arcsecs));
hw_cfg.devConfig.MaxGuidingErr = ang;
ang = getValue<mcc::MccAngle>("XEncZero").value_or(mcc::MccAngle(0.0_degs));
hw_cfg.devConfig.XEncZero = ang;
ang = getValue<mcc::MccAngle>("YEncZero").value_or(mcc::MccAngle(0.0_degs));
hw_cfg.devConfig.YEncZero = ang;
return hw_cfg;
}
mcc::MccSimpleMovingModelParams movingModelParams() const
{
static constexpr double arcsecs2rad = std::numbers::pi / 180.0 / 3600.0; // arcseconds to radians
mcc::MccSimpleMovingModelParams pars;
auto get_value = [&pars, this]<typename VT>(std::string_view name, VT& val) {
val = getValue<VT>(name).value_or(val);
};
pars.telemetryTimeout =
getValue<decltype(pars.telemetryTimeout)>("telemetryTimeout").value_or(pars.telemetryTimeout);
pars.minTimeToPZone = getValue<decltype(pars.minTimeToPZone)>("minTimeToPZone").value_or(pars.minTimeToPZone);
pars.updatingPZoneInterval = getValue<decltype(pars.updatingPZoneInterval)>("updatingPZoneInterval")
.value_or(pars.updatingPZoneInterval);
pars.slewToleranceRadius =
getValue<decltype(pars.slewToleranceRadius)>("slewToleranceRadius").value_or(pars.slewToleranceRadius) *
arcsecs2rad;
get_value("slewingTelemetryInterval", pars.slewingTelemetryInterval);
pars.slewRateX = getValue<decltype(pars.slewRateX)>("hwMaxRateHA").value_or(pars.slewRateX);
pars.slewRateY = getValue<decltype(pars.slewRateY)>("hwMaxRateDEC").value_or(pars.slewRateY);
pars.adjustCoordDiff =
getValue<decltype(pars.adjustCoordDiff)>("adjustCoordDiff").value_or(pars.adjustCoordDiff) * arcsecs2rad;
pars.adjustCycleInterval =
getValue<decltype(pars.adjustCycleInterval)>("adjustCycleInterval").value_or(pars.adjustCycleInterval);
pars.slewTimeout = getValue<decltype(pars.slewTimeout)>("slewTimeout").value_or(pars.slewTimeout);
pars.slewingPathFilename =
getValue<decltype(pars.slewingPathFilename)>("slewingPathFilename").value_or(std::string());
get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval);
pars.timeShiftToTargetPoint = getValue<decltype(pars.timeShiftToTargetPoint)>("timeShiftToTargetPoint")
.value_or(pars.timeShiftToTargetPoint);
pars.trackingCycleInterval = getValue<decltype(pars.trackingCycleInterval)>("trackingCycleInterval")
.value_or(pars.trackingCycleInterval);
pars.trackingMaxCoordDiff =
getValue<decltype(pars.trackingMaxCoordDiff)>("trackingMaxCoordDiff").value_or(pars.trackingMaxCoordDiff) *
arcsecs2rad;
pars.trackingPathFilename =
getValue<decltype(pars.trackingPathFilename)>("trackingPathFilename").value_or(std::string());
return pars;
}
Asibfm700PCM::pcm_data_t pcmData() const
{
Asibfm700PCM::pcm_data_t pcm_data;
std::vector<double> empty_vec;
pcm_data.type = getValue<decltype(pcm_data.type)>("pcmType").value_or(pcm_data.type);
pcm_data.siteLatitude = getValue<mcc::MccAngle>("siteLatitude").value_or(pcm_data.siteLatitude);
std::vector<double> vec = getValue<std::vector<double>>("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]};
}
std::vector<size_t> dd = getValue<decltype(dd)>("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<std::vector<double>>("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<std::vector<double>>("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::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<std::vector<double>>("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<std::vector<double>>("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];
}
}
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<std::string>("leapSecondFilename").value_or("");
auto fname = mcc::utils::trimSpaces(val);
setValue("leapSecondFilename", fname);
val = getValue<std::string>("bulletinAFilename").value_or("");
fname = mcc::utils::trimSpaces(val);
setValue("bulletinAFilename", fname);
val = getValue<std::string>("MountDevPath").value_or(std::string{});
fname = mcc::utils::trimSpaces(val);
setValue("MountDevPath", fname);
val = getValue<std::string>("EncoderDevPath").value_or(std::string{});
fname = mcc::utils::trimSpaces(val);
setValue("EncoderDevPath", fname);
val = getValue<std::string>("EncoderXDevPath").value_or(std::string{});
fname = mcc::utils::trimSpaces(val);
setValue("EncoderXDevPath", fname);
val = getValue<std::string>("EncoderYDevPath").value_or(std::string{});
fname = mcc::utils::trimSpaces(val);
setValue("EncoderYDevPath", fname);
val = getValue<std::string>("slewingPathFilename").value_or(std::string{});
fname = mcc::utils::trimSpaces(val);
setValue("slewingPathFilename", fname);
val = getValue<std::string>("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]<size_t I>() {
fst << "\n";
for (size_t i = 0; i < std::get<I>(_keyValue).comment.size(); ++i) {
fst << "# " << std::get<I>(_keyValue).comment[i] << "\n";
}
fst << std::get<I>(_keyValue).key << " = ";
auto v = std::get<I>(_keyValue).value;
using v_t = std::remove_cvref_t<decltype(v)>;
if constexpr (std::is_arithmetic_v<v_t> || mcc::traits::mcc_char_range<v_t>) {
fst << std::format("{}", v);
} else if constexpr (mcc::traits::mcc_time_duration_c<v_t>) {
fst << std::format("{}", v.count());
} else if constexpr (mcc::mcc_angle_c<v_t>) {
fst << std::format("{}", mcc::MccAngle(static_cast<double>(v)).degrees());
} else if constexpr (std::same_as<v_t, mcc::MccDefaultPCMType>) {
if (v == mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY) {
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY>;
} else if (v == mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE>;
} else if (v == mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE) {
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE>;
}
} else if constexpr (std::ranges::range<v_t> && std::formattable<std::ranges::range_value_t<v_t>, 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<v_t, char>) {
fst << std::format("{}", v);
} else {
static_assert(false, "INVALID TYPE!");
}
fst << "\n";
};
[&wrec]<size_t... Is>(std::index_sequence<Is...>) {
(wrec.operator()<Is>(), ...);
}(std::make_index_sequence<std::tuple_size_v<decltype(Asibfm700MountConfigDefaults)>>());
fst.close();
return true;
};
};
} // namespace asibfm700

View File

@@ -0,0 +1,355 @@
#include "asibfm700_mount.h"
#include <mcc_pzone.h>
namespace asibfm700
{
/* CONSTRUCTOR AND DESTRUCTOR */
Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
: mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
.wavelength = config.refractWavelength(),
.lat = config.siteLatitude(),
.lon = config.siteLongitude(),
.elev = config.siteElevation()}),
Asibfm700PCM(config.pcmData()),
gm_class_t(std::make_tuple(config.servoControllerConfig()),
std::make_tuple(this),
std::make_tuple(),
std::make_tuple(this, Asibfm700Logger{logger}),
std::make_tuple(this, Asibfm700Logger{logger}),
std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
// base_gm_class_t(Asibfm700StartState{},
// std::make_tuple(config.servoControllerConfig()),
// std::make_tuple(this),
// std::make_tuple(),
// std::make_tuple(this),
// std::make_tuple(this),
// 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(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
// : mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
// .wavelength = config.refractWavelength(),
// .lat = config.siteLatitude(),
// .lon = config.siteLongitude(),
// .elev = config.siteElevation()}),
// Asibfm700PCM(config.pcmData()),
// base_gm_class_t(
// gm_class_t{AsibFM700ServoController{config.servoControllerConfig()}, mcc::MccTelemetry{this},
// Asibfm700PZoneContainer{}, mcc::MccSimpleSlewingModel{this}, mcc::MccSimpleTrackingModel{this},
// Asibfm700Logger{std::move(logger), Asibfm700Logger::LOGGER_DEFAULT_FORMAT}},
// Asibfm700StartState{}),
// _mountConfig(config),
// _mountConfigMutex(new std::mutex)
// {
// 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("Stop telemetry data updating");
stopInternalTelemetryDataUpdating();
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::MccAltLimitPZ<mcc::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
_mountConfig.siteLatitude(), this});
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::MccAxisLimitSwitchPZ<mcc::MccCoordKind::COORDS_KIND_HA>{
_mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), this});
logInfo("{} prohibited zones were added successfully", pz_num);
auto mpars = _mountConfig.movingModelParams();
using secs_t = std::chrono::duration<double>;
auto to_msecs = [](double secs) {
auto s = secs_t{secs};
return std::chrono::duration_cast<std::chrono::milliseconds>(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;
logInfo("");
logInfo(" EEPROM data:");
if (hw_cfg.devConfig.RunModel != 1) { // load EEPROM only in REAL HARDWARE mode
// load EEPROM part
auto cfg_err = this->hardwareUpdateConfig();
if (cfg_err) {
errorLogging("Cannot load EEPROM data:", cfg_err);
return cfg_err;
}
mcc::MccAngle ang{_hardwareConfig.hwConfig.Yconf.accel}; // Sidereal defines HA-axis as Y-axis
logInfo(" HA-axis accel: {} degs/s^2", ang.degrees());
ang = _hardwareConfig.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis
logInfo(" DEC-axis accel: {} degs/s^2", ang.degrees());
logInfo(" HA-axis backlash: {}", (double)_hardwareConfig.hwConfig.Yconf.backlash);
logInfo(" DEC-axis backlash: {}", (double)_hardwareConfig.hwConfig.Xconf.backlash);
logInfo(" HA-axis encoder ticks per revolution: {}",
_hardwareConfig.hwConfig.Ysetpr); // Sidereal defines HA-axis as Y-axis
logInfo(" DEC-axis encoder ticks per revolution: {}",
_hardwareConfig.hwConfig.Xsetpr); // Sidereal defines DEC-axis as X-axis
logInfo(" HA-motor encoder ticks per revolution: {}",
_hardwareConfig.hwConfig.Ymetpr); // Sidereal defines HA-axis as Y-axis
logInfo(" DEC-motor encoder ticks per revolution: {}",
_hardwareConfig.hwConfig.Xmetpr); // Sidereal defines DEC-axis as X-axis
ang = _hardwareConfig.hwConfig.Yslewrate; // Sidereal defines HA-axis as Y-axis
logInfo(" HA-axis slew rate: {} degs/s", ang.degrees());
ang = _hardwareConfig.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<mcc::MccAngle>("hwMaxRateHA").value_or(0.0);
mpars.slewRateY = _mountConfig.getValue<mcc::MccAngle>("hwMaxRateDEC").value_or(0.0);
if (hw_cfg.devConfig.RunModel != 1) {
mpars.brakingAccelX = _hardwareConfig.hwConfig.Yconf.accel; // Sidereal defines HA-axis as Y-axis
mpars.brakingAccelY = _hardwareConfig.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis
} else {
mpars.brakingAccelX = 0.165806; // Sidereal defines HA-axis as Y-axis
mpars.brakingAccelY = 0.219911; // Sidereal defines DEC-axis as X-axis
}
auto max_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDMaxDt").value_or({});
auto min_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("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 = setSlewingParams(mpars);
if (st_err) {
errorLogging(" An error occured while setting slewing parameters: ", st_err);
} else {
logInfo(" Max HA-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateX).degrees());
logInfo(" Max DEC-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateY).degrees());
logInfo(" HA-axis stop acceleration braking: {} degs/s^2", mcc::MccAngle(mpars.brakingAccelX).degrees());
logInfo(" DEC-axis stop acceleration braking: {} degs/s^2", mcc::MccAngle(mpars.brakingAccelY).degrees());
logInfo(" Slewing telemetry polling interval: {} millisecs", mpars.slewingTelemetryInterval.count());
}
st_err = setTrackingParams(_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");
// 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 ...");
// 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);
}
setStateERFA(std::move(ccte_state));
// setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod());
setTelemetryUpdateTimeout(_mountConfig.movingModelParams().telemetryTimeout);
startInternalTelemetryDataUpdating();
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
bool ok = isInternalTelemetryDataUpdating();
if (ok) {
logInfo("Start updating telemetry data ...");
mcc::MccTelemetryData tdata;
auto err = waitForTelemetryData(&tdata, _mountConfig.movingModelParams().telemetryTimeout);
if (err) {
logError("Cannot update telemetry data (err = {} [{}, {}])!", err.message(), err.value(),
err.category().name());
}
} else {
auto err = lastUpdateError();
logError("Cannot update telemetry data (err = {} [{}, {}])!", err.message(), err.value(),
err.category().name());
}
return mcc::MccGenericMountErrorCode::ERROR_OK;
}
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg)
{
std::lock_guard lock{*_mountConfigMutex};
_mountConfig = cfg;
auto hw_cfg = _mountConfig.servoControllerConfig();
hardwareUpdateConfig(hw_cfg.devConfig);
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

189
asibfm700/asibfm700_mount.h Normal file
View File

@@ -0,0 +1,189 @@
#pragma once
#include <mcc_generic_mount.h>
#include <mcc_pzone_container.h>
#include <mcc_slewing_model.h>
#include <mcc_spdlog.h>
#include <mcc_telemetry.h>
#include <mcc_tracking_model.h>
#include "asibfm700_common.h"
#include "asibfm700_configfile.h"
namespace asibfm700
{
class Asibfm700Mount : public Asibfm700CCTE,
public Asibfm700PCM,
public mcc::MccGenericMount<AsibFM700ServoController,
mcc::MccTelemetry,
Asibfm700PZoneContainer,
Asibfm700SlewingModel,
Asibfm700TrackingModel,
Asibfm700Logger>
{
typedef mcc::MccGenericMount<AsibFM700ServoController,
mcc::MccTelemetry,
Asibfm700PZoneContainer,
Asibfm700SlewingModel,
Asibfm700TrackingModel,
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<spdlog::logger> 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<std::mutex> _mountConfigMutex;
void errorLogging(const std::string&, const std::error_code&);
};
/*
class Asibfm700Mount : public Asibfm700CCTE,
public Asibfm700PCM,
public mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
mcc::MccTelemetry,
Asibfm700PZoneContainer,
mcc::MccSimpleSlewingModel,
mcc::MccSimpleTrackingModel,
Asibfm700Logger>>
{
typedef mcc::MccGenericMount<AsibFM700ServoController,
mcc::MccTelemetry,
Asibfm700PZoneContainer,
mcc::MccSimpleSlewingModel,
mcc::MccSimpleTrackingModel,
Asibfm700Logger>
gm_class_t;
typedef mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
mcc::MccTelemetry,
Asibfm700PZoneContainer,
mcc::MccSimpleSlewingModel,
mcc::MccSimpleTrackingModel,
Asibfm700Logger>>
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<MccGenericFsmMountErrorEvent, Asibfm700ErrorState>,
std::pair<MccGenericFsmMountInitEvent, MccGenericFsmMountInitState<Asibfm700ErrorState>>,
std::pair<MccGenericFsmMountIdleEvent, MccGenericFsmMountIdleState<Asibfm700ErrorState>>>;
};
typedef base_gm_class_t::MccGenericFsmMountStartState<Asibfm700ErrorState> 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<spdlog::logger> 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<std::mutex> _mountConfigMutex;
void errorLogging(const std::string&, const std::error_code&);
};
*/
static_assert(mcc::mcc_position_controls_c<Asibfm700Mount>, "");
static_assert(mcc::mcc_all_controls_c<Asibfm700Mount>, "");
static_assert(mcc::mcc_generic_mount_c<Asibfm700Mount>, "");
} // namespace asibfm700

View File

@@ -0,0 +1,59 @@
#include "asibfm700_netserver.h"
namespace asibfm700
{
Asibfm700MountNetServer::Asibfm700MountNetServer(asio::io_context& ctx,
Asibfm700Mount& mount,
std::shared_ptr<spdlog::logger> 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<handle_message_func_result_t>;
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<Asibfm700CCTE::meteo_t>(0);
if (vp) {
mount_ptr->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);
}
} else {
// basic network message processing
output_msg = base_t::handleMessage<output_msg_t>(input_msg, mount_ptr);
}
}
return output_msg.template byteRepr<typename base_t::handle_message_func_result_t>();
};
}
Asibfm700MountNetServer::~Asibfm700MountNetServer() {}
} // namespace asibfm700

View File

@@ -0,0 +1,146 @@
#pragma once
#include <mcc_netserver.h>
#include <mcc_netserver_proto.h>
#include "asibfm700_common.h"
#include "asibfm700_mount.h"
namespace asibfm700
{
namespace details
{
template <typename VT, size_t N1, size_t N2>
static constexpr auto merge_arrays(const std::array<VT, N1>& arr1, const std::array<VT, N2>& arr2)
{
constexpr auto N = N1 + N2;
std::array<VT, N> 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
constexpr static std::string_view ASIBFM700_COMMPROTO_KEYWORD_METEO_STR{"METEO"};
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});
// hashes of valid keywords
static constexpr std::array NETMSG_VALID_KEYWORD_HASHES = []<size_t... Is>(std::index_sequence<Is...>) {
return std::array{mcc::utils::FNV1aHash(NETMSG_VALID_KEYWORDS[Is])...};
}(std::make_index_sequence<NETMSG_VALID_KEYWORDS.size()>());
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 <mcc::traits::mcc_char_range BYTEREPR_T = std::string_view>
class Asibfm700NetMessage : public mcc::network::MccNetMessage<BYTEREPR_T, Asibfm700NetMessageValidKeywords>
{
protected:
using base_t = mcc::network::MccNetMessage<BYTEREPR_T, Asibfm700NetMessageValidKeywords>;
class serializer_t : public base_t::DefaultSerializer
{
public:
template <typename T, mcc::traits::mcc_output_char_range OR>
void operator()(const T& value, OR& bytes)
{
if constexpr (std::same_as<T, Asibfm700CCTE::meteo_t>) {
// serialize just like a vector
std::vector<double> meteo{value.temperature, value.humidity, value.pressure};
base_t::DefaultSerializer::operator()(meteo, bytes);
} else {
base_t::DefaultSerializer::operator()(value, bytes);
}
}
} _serializer;
class deserializer_t : public base_t::DefaultDeserializer
{
public:
template <mcc::traits::mcc_input_char_range IR, typename VT>
std::error_code operator()(IR&& bytes, VT& value) const
{
if constexpr (std::same_as<VT, Asibfm700CCTE::meteo_t>) {
// deserialize just like a vector
std::vector<double> v;
auto ec = base_t::DefaultDeserializer::operator()(std::forward<IR>(bytes), v);
if (ec) {
return ec;
}
if (v.size() < 3) {
return std::make_error_code(std::errc::invalid_argument);
}
value.temperature = v[0];
value.humidity = v[1];
value.pressure = v[2];
return {};
} else {
return base_t::DefaultDeserializer::operator()(std::forward<IR>(bytes), value);
}
}
} _deserializer;
public:
using base_t::base_t;
template <typename T>
std::expected<T, std::error_code> paramValue(size_t idx) const
{
return base_t::template paramValue<T>(idx, _deserializer);
}
template <mcc::traits::mcc_input_char_range KT, typename... PTs>
std::error_code construct(KT&& key, PTs&&... params)
requires mcc::traits::mcc_output_char_range<BYTEREPR_T>
{
return base_t::construct(_serializer, std::forward<KT>(key), std::forward<PTs>(params)...);
}
};
class Asibfm700MountNetServer : public mcc::network::MccGenericMountNetworkServer<Asibfm700Logger>
{
using base_t = mcc::network::MccGenericMountNetworkServer<Asibfm700Logger>;
public:
Asibfm700MountNetServer(asio::io_context& ctx, Asibfm700Mount& mount, std::shared_ptr<spdlog::logger> logger);
~Asibfm700MountNetServer();
};
} // namespace asibfm700

View File

@@ -0,0 +1,507 @@
#pragma once
#include <algorithm>
#include <array>
#include <charconv>
#include <cstdint>
#include <filesystem>
#include <ranges>
#include <string_view>
#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 <mcc::traits::mcc_input_char_range R>
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 <mcc::traits::mcc_input_char_range R>
requires std::ranges::contiguous_range<R>
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<std::remove_cvref_t<R>>) {
_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 <mcc::traits::mcc_view_or_output_char_range R>
R proto() const
{
return part<R>(PROTO_PART);
}
std::string_view proto() const
{
return proto<std::string_view>();
}
template <mcc::traits::mcc_view_or_output_char_range R>
R host() const
{
return part<R>(HOST_PART);
}
std::string_view host() const
{
return host<std::string_view>();
}
int port() const
{
return _port;
}
template <mcc::traits::mcc_view_or_output_char_range R>
R portView() const
{
return part<R>(PORT_PART);
}
std::string_view portView() const
{
return portView<std::string_view>();
}
template <mcc::traits::mcc_output_char_range R, mcc::traits::mcc_input_char_range RR = std::string_view>
R path(RR&& root_path) const
{
if (_path.empty()) {
if constexpr (mcc::traits::mcc_output_char_range<R>) {
R res;
std::ranges::copy(std::forward<RR>(root_path), std::back_inserter(res));
return res;
} else { // can't add root path!!!
return part<R>(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<R>(PATH_PART);
}
}
template <mcc::traits::mcc_input_char_range RR = std::string_view>
std::string path(RR&& root_path) const
{
return path<std::string, RR>(std::forward<RR>(root_path));
}
template <mcc::traits::mcc_view_or_output_char_range R>
R path() const
{
return part<R>(PATH_PART);
}
std::string_view path() const
{
return path<std::string_view>();
}
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 <typename T = std::nullptr_t>
Asibfm700NetserverEndpoint& makeAbstract(const T& mark = nullptr)
requires(mcc::traits::mcc_input_char_range<T> || std::same_as<std::remove_cv_t<T>, char> ||
std::is_null_pointer_v<std::remove_cv_t<T>>)
{
if (!(isLocalStream() || isLocalSeqpacket())) { // only local proto is valid!
return *this;
}
if constexpr (std::is_null_pointer_v<T>) { // 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<std::remove_cv_t<T>, 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 <mcc::traits::mcc_view_or_output_char_range R>
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<R>) {
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<const char*>::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

View File

@@ -0,0 +1,172 @@
#include <spdlog/sinks/basic_file_sink.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <cxxopts.hpp>
#include <iostream>
#include <asio/thread_pool.hpp>
#include <mcc_netserver_endpoint.h>
#include "asibfm700_netserver.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<std::string>()->default_value(""));
options.add_options()("level", "Log level (see SPDLOG package description for valid values)",
cxxopts::value<std::string>()->default_value("info"));
options.add_options()("c,config", "Mount configuration filename (by default use of hardcoded one)",
cxxopts::value<std::string>()->default_value(""));
options.add_options()("dump", "Dump mount default configuration to file and exit",
cxxopts::value<std::string>()->default_value(""));
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<std::vector<std::string>>()->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<std::string>();
if (fname.size()) {
bool ok = mount_cfg.dumpDefaultsToFile(fname);
if (!ok) {
return 255;
}
return 0;
} else {
// just ignore
}
auto logname = opt_result["log"].as<std::string>();
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::string>();
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::seconds>(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<std::string>();
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");
}
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<std::vector<std::string>>();
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;
}
}

View File

@@ -0,0 +1,292 @@
#include "asibfm700_servocontroller.h"
namespace asibfm700
{
const char* AsibFM700ServoControllerErrorCategory::name() const noexcept
{
return "ASIBFM700-SERVOCONTROLLER-ERROR-CATEGORY";
}
std::string AsibFM700ServoControllerErrorCategory::message(int ec) const
{
AsibFM700ServoControllerErrorCode err = static_cast<AsibFM700ServoControllerErrorCode>(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<char*>(_hardwareConfig.MountDevPath.c_str());
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
}
AsibFM700ServoController::~AsibFM700ServoController() {}
constexpr std::string_view AsibFM700ServoController::hardwareName() const
{
return "Sidereal-ServoControllerII";
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop()
{
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(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.moving_state == hardware_moving_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<AsibFM700ServoControllerErrorCode>(Mount.init(&_hardwareConfig.devConfig));
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state)
{
std::lock_guard lock{*_setStateMutex};
if (state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { // stop!
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(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.moving_state == hardware_moving_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<double>(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<std::chrono::nanoseconds>(state.time_point.time_since_epoch());
auto secs = std::chrono::floor<std::chrono::seconds>(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.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
// coordpair_t cpair{.X = state.endptY, .Y = state.endptX};
// 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<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair, &cpair));
auto err = static_cast<AsibFM700ServoControllerErrorCode>(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(hardware_state_t::time_point);
mountdata_t mdata;
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
if (!err) {
// time point from sidservo library is 'double' number represented UNIXTIME with
// microseconds/nanoseconds precision (must be equal for encXposition and encYposition)
// using secs_t = std::chrono::duration<double>;
// secs_t secs = secs_t{mdata.encXposition.t};
// state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
// 2025-12-04: coordval_pair_t.X.t is now of type struct timespec
auto dr = std::chrono::duration_cast<decltype(state->time_point)::duration>(
std::chrono::seconds(mdata.encXposition.t.tv_sec) + std::chrono::nanoseconds(mdata.encXposition.t.tv_nsec));
state->time_point = decltype(state->time_point){dr};
// if (mcc::utils::isEqual(secs.count(), 0.0)) { // model mode?
// state->time_point = decltype(state->time_point)::clock::now();
// } else {
// state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
// }
// WARNING: TEMPORARY (WAIT FOR Eddy fix its implementation of LibSidServo)!!!
// state->time_point = decltype(state->time_point)::clock::now();
// according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
state->X = mdata.encYposition.val;
state->Y = mdata.encXposition.val;
state->speedX = mdata.encYspeed.val;
state->speedY = mdata.encXspeed.val;
state->stateX = mdata.Ystate;
state->stateY = mdata.Xstate;
if (mdata.Xstate == AXIS_ERROR || mdata.Ystate == AXIS_ERROR) {
state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
} else {
if (mdata.Xstate == AXIS_STOPPED) {
if (mdata.Ystate == AXIS_STOPPED) {
state->moving_state = hardware_moving_state_t::HW_MOVE_STOPPED;
} else if (mdata.Ystate == AXIS_SLEWING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
} else if (mdata.Ystate == AXIS_POINTING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
} else if (mdata.Ystate == AXIS_GUIDING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
} else {
state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
}
} else if (mdata.Xstate == AXIS_SLEWING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
} else if (mdata.Xstate == AXIS_POINTING) {
if (mdata.Ystate == AXIS_SLEWING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
} else {
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
}
} else if (mdata.Xstate == AXIS_GUIDING) {
if (mdata.Ystate == AXIS_SLEWING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
} else if (mdata.Ystate == AXIS_POINTING) {
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
} else {
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
}
} else {
state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
}
}
}
return err;
}
void AsibFM700ServoController::hardwareUpdateConfig(conf_t cfg)
{
_hardwareConfig.devConfig = std::move(cfg);
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig(hardware_configuration_t cfg)
{
_hardwareConfig.hwConfig = std::move(cfg);
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.saveHWconfig(&_hardwareConfig.hwConfig));
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig()
{
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.getHWconfig(&_hardwareConfig.hwConfig));
}
AsibFM700ServoController::hardware_config_t AsibFM700ServoController::getHardwareConfig() const
{
return _hardwareConfig;
}
} // namespace asibfm700

View File

@@ -0,0 +1,144 @@
#pragma once
#include <mcc_defaults.h>
#include <mcc_generics.h>
#include "../LibSidServo/sidservo.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<int>(ec), AsibFM700ServoControllerErrorCategory::get());
}
} // namespace asibfm700
namespace std
{
template <>
class is_error_code_enum<asibfm700::AsibFM700ServoControllerErrorCode> : public true_type
{
};
} // namespace std
namespace asibfm700
{
class AsibFM700ServoController
{
public:
typedef std::error_code error_t;
enum class hardware_moving_state_t : int {
HW_MOVE_ERROR = -1,
HW_MOVE_STOPPED = 0,
HW_MOVE_SLEWING,
HW_MOVE_ADJUSTING,
HW_MOVE_TRACKING,
HW_MOVE_GUIDING,
HW_MOVE_UNKNOWN
};
struct hardware_state_t {
static constexpr mcc::MccCoordPairKind pair_kind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
mcc::MccTimePoint time_point;
double X, Y, speedX, speedY;
axis_status_t stateX, stateY; // Eddy's LibSidServo axis state
hardware_moving_state_t moving_state;
// endpoint: a point on the trajectory of movement behind the guidance point (X,Y), taking into account
// the movement vector (i.e. sign of movement speed)
// this point is needed as Sidereal controller commands require not only moving speed but
// also 'target' point (point at which mount will stop)
// double endptX, endptY;
};
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 */
constexpr std::string_view hardwareName() const;
error_t hardwareSetState(hardware_state_t state);
error_t hardwareGetState(hardware_state_t* state);
error_t hardwareStop();
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<std::mutex> _setStateMutex;
};
} // namespace asibfm700

View File

@@ -0,0 +1,69 @@
#include <iostream>
#include "../asibfm700_configfile.h"
template <typename VT>
struct rec_t {
std::string_view key;
VT value;
};
static std::string_view cfg_str = R"--(A = 11
B=3.3
# this is comment
C = WWWWWeeeWWWW
E = 10,20, 40, 32
)--";
int main()
{
auto desc = std::make_tuple(rec_t{"A", 1}, rec_t{"B", 2.2}, rec_t{"C", std::string("EEE")}, rec_t{"D", 3.3},
rec_t{"E", std::vector<int>{1, 2, 3}});
std::error_code err;
asibfm700::Asibfm700MountConfig acfg;
bool ok = acfg.dumpDefaultsToFile("/tmp/cfg.cfg");
if (!ok) {
std::cerr << "Cannot dump default configuration!\n";
exit(10);
}
auto ec = acfg.load("/tmp/cfg.cfg");
std::cout << "EC (load) = " << ec.message() << "\n";
std::cout << "refr w: " << acfg.refractWavelength() << "\n";
acfg.setValue("refractWavelength", 0.3);
auto e = acfg.getValue<double>("refractWavelength");
std::cout << "refr w: " << e.value_or(0.0) << "\n";
std::cout << "refr w: " << acfg.refractWavelength() << "\n";
mcc::utils::KeyValueHolder kvh(desc);
err = kvh.setValue("C", "ewlkjfde");
if (err) {
std::cout << "cannot set value: " << err.message() << "\n";
} else {
auto vs = kvh.getValue<std::string>("C");
std::cout << "kvh[C] = " << vs.value_or("<no value>") << "\n";
}
ec = kvh.fromCharRange(cfg_str);
if (ec) {
std::cout << "EC = " << ec.message() << "\n";
} else {
auto v3 = kvh.getValue<std::vector<int>>("E");
std::cout << "[";
for (auto& el : v3.value_or(std::vector<int>{0, 0, 0})) {
std::cout << el << " ";
}
std::cout << "]\n";
}
return 0;
}

View File

@@ -6,13 +6,22 @@ set(ASIO_FOUND FALSE)
find_package(Threads REQUIRED) find_package(Threads REQUIRED)
find_path(ASIO_DIR asio.hpp HINTS ${ASIO_INSTALL_DIR} PATH_SUFFIXES include) set(ASIO_INSTALL_DIR "" CACHE STRING "ASIO install dir")
set(ASIO_INSTALL_DIR_INTERNAL "" CACHE STRING "ASIO install dir")
if(NOT "${ASIO_INSTALL_DIR}" STREQUAL "${ASIO_INSTALL_DIR_INTERNAL}") # ASIO_INSTALL_DIR is given in command-line
unset(ASIO_INCLUDE_DIR CACHE)
find_path(ASIO_DIR asio.hpp HINTS ${ASIO_INSTALL_DIR} PATH_SUFFIXES include)
else() # in system path
find_path(ASIO_DIR asio.hpp PATH_SUFFIXES include)
endif()
if (NOT ASIO_DIR) if (NOT ASIO_DIR)
message(WARNING "Cannot find ASIO library headers!") message(WARNING "Cannot find ASIO library headers!")
set(ASIO_FOUND FALSE) set(ASIO_FOUND FALSE)
else() else()
message(STATUS "Found ASIO: TRUE (${ASIO_DIR})") message(STATUS "Found ASIO: (${ASIO_DIR})")
# ASIO is header-only library so it is IMPORTED target # ASIO is header-only library so it is IMPORTED target
add_library(ASIO::ASIO INTERFACE IMPORTED GLOBAL) add_library(ASIO::ASIO INTERFACE IMPORTED GLOBAL)

View File

@@ -52,7 +52,10 @@ namespace mcc
struct MccSimpleSlewModelCategory : public std::error_category { struct MccSimpleSlewModelCategory : public std::error_category {
MccSimpleSlewModelCategory() : std::error_category() {} MccSimpleSlewModelCategory() : std::error_category() {}
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; } const char* name() const noexcept
{
return "ADC_GENERIC_DEVICE";
}
std::string message(int ec) const std::string message(int ec) const
{ {
@@ -164,7 +167,7 @@ public:
} }
_stopRequested = other._stopRequested.load(); _stopRequested = other._stopRequested.load();
_slewFunc = std::move(_slewFunc); _slewFunc = std::move(other._slewFunc);
return *this; return *this;
}; };

View File

@@ -7,6 +7,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake") set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
find_package(Threads REQUIRED)
# ******* SPDLOG LIBRARY ******* # ******* SPDLOG LIBRARY *******
@@ -16,69 +17,77 @@ include(ExternalProject)
set(SPDLOG_USE_STD_FORMAT ON CACHE INTERNAL "Use of C++20 std::format") set(SPDLOG_USE_STD_FORMAT ON CACHE INTERNAL "Use of C++20 std::format")
set(SPDLOG_FMT_EXTERNAL OFF CACHE INTERNAL "Turn off external fmt library") set(SPDLOG_FMT_EXTERNAL OFF CACHE INTERNAL "Turn off external fmt library")
FetchContent_Declare(spdlog
# ExternalProject_Add(spdlog
# SOURCE_DIR ${CMAKE_BINARY_DIR}/spdlog_lib
# BINARY_DIR ${CMAKE_BINARY_DIR}/spdlog_lib/build
GIT_REPOSITORY "https://github.com/gabime/spdlog.git"
GIT_TAG "v1.15.1"
GIT_SHALLOW TRUE
GIT_SUBMODULES ""
GIT_PROGRESS TRUE
CMAKE_ARGS "-DSPDLOG_USE_STD_FORMAT=ON -DSPDLOG_FMT_EXTERNAL=OFF"
# CONFIGURE_COMMAND ""
# BUILD_COMMAND ""
# INSTALL_COMMAND ""
# UPDATE_COMMAND ""
# SOURCE_SUBDIR cmake # turn off building
OVERRIDE_FIND_PACKAGE
)
find_package(spdlog CONFIG) find_package(spdlog CONFIG)
if (NOT ${spdlog_FOUND})
FetchContent_Declare(spdlog
# ExternalProject_Add(spdlog
# SOURCE_DIR ${CMAKE_BINARY_DIR}/spdlog_lib
# BINARY_DIR ${CMAKE_BINARY_DIR}/spdlog_lib/build
GIT_REPOSITORY "https://github.com/gabime/spdlog.git"
GIT_TAG "v1.15.1"
GIT_SHALLOW TRUE
GIT_SUBMODULES ""
GIT_PROGRESS TRUE
CMAKE_ARGS "-DSPDLOG_USE_STD_FORMAT=ON -DSPDLOG_FMT_EXTERNAL=OFF"
# CONFIGURE_COMMAND ""
# BUILD_COMMAND ""
# INSTALL_COMMAND ""
# UPDATE_COMMAND ""
# SOURCE_SUBDIR cmake # turn off building
OVERRIDE_FIND_PACKAGE
)
find_package(spdlog CONFIG)
endif()
# ******* ERFA LIBRARY ******* # ******* ERFA LIBRARY *******
# ExternalProject_Add(erfalib ExternalProject_Add(erfalib
# PREFIX ${CMAKE_BINARY_DIR}/erfa_lib PREFIX ${CMAKE_BINARY_DIR}/erfa_lib
# GIT_REPOSITORY "https://github.com/liberfa/erfa.git" GIT_REPOSITORY "https://github.com/liberfa/erfa.git"
# GIT_TAG "v2.0.1" GIT_TAG "v2.0.1"
# UPDATE_COMMAND "" UPDATE_COMMAND ""
# PATCH_COMMAND "" PATCH_COMMAND ""
# # BINARY_DIR erfa_build # BINARY_DIR erfa_build
# # SOURCE_DIR erfa # SOURCE_DIR erfa
# # INSTALL_DIR # INSTALL_DIR
# LOG_CONFIGURE 1 LOG_CONFIGURE 1
# CONFIGURE_COMMAND meson setup --reconfigure -Ddefault_library=static -Dbuildtype=release CONFIGURE_COMMAND meson setup --reconfigure -Ddefault_library=static -Dbuildtype=release
# -Dprefix=${CMAKE_BINARY_DIR}/erfa_lib -Dlibdir= -Dincludedir= -Ddatadir= <SOURCE_DIR> -Dprefix=${CMAKE_BINARY_DIR}/erfa_lib -Dlibdir= -Dincludedir= -Ddatadir= <SOURCE_DIR>
# BUILD_COMMAND ninja -C <BINARY_DIR> BUILD_COMMAND ninja -C <BINARY_DIR>
# INSTALL_COMMAND meson install -C <BINARY_DIR> INSTALL_COMMAND meson install -C <BINARY_DIR>
# BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a
# ) )
add_library(ERFA_LIB STATIC IMPORTED) add_library(ERFA_LIB STATIC IMPORTED GLOBAL)
set_target_properties(ERFA_LIB PROPERTIES IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a) set_target_properties(ERFA_LIB PROPERTIES IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a)
add_dependencies(ERFA_LIB erfalib) add_dependencies(ERFA_LIB erfalib)
set(ERFA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/erfa_lib) set(ERFA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/erfa_lib)
# include_directories(${ERFA_INCLUDE_DIR}) # set(ERFA_LIBFILE ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a PARENT_SCOPE)
include_directories(${ERFA_INCLUDE_DIR})
message(STATUS ${ERFA_INCLUDE_DIR}) message(STATUS "ERFA INCLUDE DIR: " ${ERFA_INCLUDE_DIR})
add_subdirectory(bsplines) add_subdirectory(bsplines)
message(STATUS "BSPLINES_INCLUDE_DIR: " ${BSPLINES_INCLUDE_DIR}) message(STATUS "BSPLINES_INCLUDE_DIR: " ${BSPLINES_INCLUDE_DIR})
include_directories(${BSPLINES_INCLUDE_DIR}) include_directories(${BSPLINES_INCLUDE_DIR})
set(MCC_LIBRARY_SRC1 mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h set(MCC_LIBRARY_SRC mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h
mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h
mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h
mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_guiding_model.h mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_moving_model_common.h
mcc_moving_model_common.h) mcc_netserver_endpoint.h mcc_netserver.h mcc_netserver_proto.h)
list(APPEND MCC_LIBRARY_SRC1 mcc_spdlog.h) list(APPEND MCC_LIBRARY_SRC mcc_spdlog.h)
set(MCC_LIBRARY mcc)
add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC})
target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23)
target_compile_definitions(${MCC_LIBRARY} INTERFACE SPDLOG_USE_STD_FORMAT=1 SPDLOG_FMT_EXTERNAL=0)
target_link_libraries(${MCC_LIBRARY} INTERFACE spdlog Threads::Threads atomic ${ERFA_LIB})
target_include_directories(${MCC_LIBRARY} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR})
target_include_directories(${MCC_LIBRARY} INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/mcc>
)
set(MCC_LIBRARY1 mcc1)
add_library(${MCC_LIBRARY1} INTERFACE ${MCC_LIBRARY_SRC1})
target_compile_features(${MCC_LIBRARY1} INTERFACE cxx_std_23)
target_include_directories(${MCC_LIBRARY1} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR})
option(WITH_TESTS "Build tests" ON) option(WITH_TESTS "Build tests" ON)
@@ -88,5 +97,14 @@ if (WITH_TESTS)
target_include_directories(${CTTE_TEST_APP} PRIVATE ${ERFA_INCLUDE_DIR}) target_include_directories(${CTTE_TEST_APP} PRIVATE ${ERFA_INCLUDE_DIR})
target_link_libraries(${CTTE_TEST_APP} ERFA_LIB bsplines) target_link_libraries(${CTTE_TEST_APP} ERFA_LIB bsplines)
set(NETMSG_TESTS_APP netmsg_test)
add_executable(${NETMSG_TESTS_APP} tests/netmsg_test.cpp)
target_link_libraries(${NETMSG_TESTS_APP} mcc)
set(MCCCOORD_TEST_APP mcc_coord_test)
add_executable(${MCCCOORD_TEST_APP} tests/mcc_coord_test.cpp)
target_link_libraries(${MCCCOORD_TEST_APP} mcc ERFA_LIB)
enable_testing() enable_testing()
endif() endif()

View File

@@ -459,11 +459,31 @@ class MccAngleDEC_APP : public MccAngle
using MccAngle::MccAngle; using MccAngle::MccAngle;
}; };
class MccAngleRA_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleDEC_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleHA : public MccAngle class MccAngleHA : public MccAngle
{ {
using MccAngle::MccAngle; using MccAngle::MccAngle;
}; };
class MccAngleHA_APP : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleHA_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleAZ : public MccAngle class MccAngleAZ : public MccAngle
{ {
using MccAngle::MccAngle; using MccAngle::MccAngle;
@@ -504,6 +524,11 @@ class MccAngleLON : public MccAngle
using MccAngle::MccAngle; using MccAngle::MccAngle;
}; };
class MccAngleUnknown : public MccAngle
{
using MccAngle::MccAngle;
};
enum class MccCoordKind : size_t { enum class MccCoordKind : size_t {
COORDS_KIND_GENERIC = traits::mcc_type_hash<MccAngle>, COORDS_KIND_GENERIC = traits::mcc_type_hash<MccAngle>,
@@ -511,38 +536,147 @@ enum class MccCoordKind : size_t {
COORDS_KIND_DEC_ICRS = traits::mcc_type_hash<MccAngleDEC_ICRS>, COORDS_KIND_DEC_ICRS = traits::mcc_type_hash<MccAngleDEC_ICRS>,
COORDS_KIND_RA_APP = traits::mcc_type_hash<MccAngleRA_APP>, COORDS_KIND_RA_APP = traits::mcc_type_hash<MccAngleRA_APP>,
COORDS_KIND_DEC_APP = traits::mcc_type_hash<MccAngleDEC_APP>, COORDS_KIND_DEC_APP = traits::mcc_type_hash<MccAngleDEC_APP>,
COORDS_KIND_RA_OBS = traits::mcc_type_hash<MccAngleRA_OBS>,
COORDS_KIND_DEC_OBS = traits::mcc_type_hash<MccAngleDEC_OBS>,
COORDS_KIND_HA = traits::mcc_type_hash<MccAngleHA>, COORDS_KIND_HA = traits::mcc_type_hash<MccAngleHA>,
COORDS_KIND_HA_APP = traits::mcc_type_hash<MccAngleHA_APP>,
COORDS_KIND_HA_OBS = traits::mcc_type_hash<MccAngleHA_OBS>,
COORDS_KIND_AZ = traits::mcc_type_hash<MccAngleAZ>, COORDS_KIND_AZ = traits::mcc_type_hash<MccAngleAZ>,
COORDS_KIND_ZD = traits::mcc_type_hash<MccAngleZD>, COORDS_KIND_ZD = traits::mcc_type_hash<MccAngleZD>,
COORDS_KIND_ALT = traits::mcc_type_hash<MccAngleALT>, COORDS_KIND_ALT = traits::mcc_type_hash<MccAngleALT>,
COORDS_KIND_X = traits::mcc_type_hash<MccAngleX>, COORDS_KIND_X = traits::mcc_type_hash<MccAngleX>,
COORDS_KIND_Y = traits::mcc_type_hash<MccAngleY>, COORDS_KIND_Y = traits::mcc_type_hash<MccAngleY>,
COORDS_KIND_LAT = traits::mcc_type_hash<MccAngleLAT>, COORDS_KIND_LAT = traits::mcc_type_hash<MccAngleLAT>,
COORDS_KIND_LON = traits::mcc_type_hash<MccAngleLON> COORDS_KIND_LON = traits::mcc_type_hash<MccAngleLON>,
COORDS_KIND_UKNOWN = traits::mcc_type_hash<MccAngleUnknown>
}; };
enum class MccCoordPairKind : size_t { enum class MccCoordPairKind : size_t {
COORDS_KIND_GENERIC = traits::mcc_type_pair_hash<MccAngle, MccAngle>(), COORDS_KIND_GENERIC = traits::mcc_type_pair_hash<MccAngle, MccAngle>(),
COORDS_KIND_RADEC_ICRS = traits::mcc_type_pair_hash<MccAngleRA_ICRS, MccAngleDEC_ICRS>(), COORDS_KIND_RADEC_ICRS = traits::mcc_type_pair_hash<MccAngleRA_ICRS, MccAngleDEC_ICRS>(),
COORDS_KIND_RADEC_APP = traits::mcc_type_pair_hash<MccAngleRA_APP, MccAngleDEC_APP>(), COORDS_KIND_RADEC_APP = traits::mcc_type_pair_hash<MccAngleRA_APP, MccAngleDEC_APP>(),
COORDS_KIND_RADEC_OBS = traits::mcc_type_pair_hash<MccAngleRA_OBS, MccAngleDEC_OBS>(),
COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash<MccAngleHA, MccAngleDEC_APP>(), COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash<MccAngleHA, MccAngleDEC_APP>(),
// COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash<MccAngleHA_APP, MccAngleDEC_APP>(),
COORDS_KIND_HADEC_OBS = traits::mcc_type_pair_hash<MccAngleHA_OBS, MccAngleDEC_OBS>(),
COORDS_KIND_AZZD = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleZD>(), COORDS_KIND_AZZD = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleZD>(),
COORDS_KIND_AZALT = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleALT>(), COORDS_KIND_AZALT = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleALT>(),
COORDS_KIND_XY = traits::mcc_type_pair_hash<MccAngleX, MccAngleY>(), COORDS_KIND_XY = traits::mcc_type_pair_hash<MccAngleX, MccAngleY>(),
COORDS_KIND_LATLON = traits::mcc_type_pair_hash<MccAngleLAT, MccAngleLON>() COORDS_KIND_LONLAT = traits::mcc_type_pair_hash<MccAngleLON, MccAngleLAT>(),
COORDS_KIND_UNKNOWN = traits::mcc_type_pair_hash<MccAngleUnknown, MccAngleUnknown>()
};
template <MccCoordPairKind PK>
static constexpr bool mccIsObsCoordPairKind =
(PK == MccCoordPairKind::COORDS_KIND_RADEC_OBS || PK == MccCoordPairKind::COORDS_KIND_HADEC_OBS ||
PK == MccCoordPairKind::COORDS_KIND_AZZD || PK == MccCoordPairKind::COORDS_KIND_AZALT);
static constexpr bool mcc_is_obs_coordpair(MccCoordPairKind kind)
{
return kind == MccCoordPairKind::COORDS_KIND_RADEC_OBS || kind == MccCoordPairKind::COORDS_KIND_HADEC_OBS ||
kind == MccCoordPairKind::COORDS_KIND_AZZD || kind == MccCoordPairKind::COORDS_KIND_AZALT;
};
template <MccCoordPairKind PK>
static constexpr bool mccIsAppCoordPairKind =
(PK == MccCoordPairKind::COORDS_KIND_RADEC_APP || PK == MccCoordPairKind::COORDS_KIND_HADEC_APP);
static constexpr bool mcc_is_app_coordpair(MccCoordPairKind kind)
{
return kind == MccCoordPairKind::COORDS_KIND_RADEC_APP || kind == MccCoordPairKind::COORDS_KIND_HADEC_APP;
}; };
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_ICRS_STR = "RADEC-IRCS";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_APP_STR = "RADEC-APP";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_OBS_STR = "RADEC-OBS";
static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_APP_STR = "HADEC-APP";
static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_OBS_STR = "HADEC-OBS";
static constexpr std::string_view MCC_COORDPAIR_KIND_AZALT_STR = "AZALT";
static constexpr std::string_view MCC_COORDPAIR_KIND_AZZD_STR = "AZZD";
static constexpr std::string_view MCC_COORDPAIR_KIND_XY_STR = "XY";
static constexpr std::string_view MCC_COORDPAIR_KIND_LATLON_STR = "LATLON";
static constexpr std::string_view MCC_COORDPAIR_KIND_GENERIC_STR = "GENERIC";
static constexpr std::string_view MCC_COORDPAIR_KIND_UNKNOWN_STR = "UNKNOWN";
template <MccCoordPairKind KIND> template <MccCoordPairKind KIND>
static constexpr std::string_view MccCoordPairKindStr = static constexpr std::string_view MccCoordPairKindStr =
KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? "RADEC-IRCS" KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? "RADEC-APP" : KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? "HADEC-APP" : KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZALT ? "Azimuth-Altitude" : KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZZD ? "Azimuth-Zendist" : KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_XY ? "X-Y" : KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR
: KIND == MccCoordPairKind::COORDS_KIND_LATLON ? "Latitude-Longitude" : KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR
: "UNKNOWN"; : KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR
: KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR
: MCC_COORDPAIR_KIND_UNKNOWN_STR;
static constexpr std::string_view MccCoordPairKindToStr(MccCoordPairKind KIND)
{
return KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR
: KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR
: KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR
: MCC_COORDPAIR_KIND_UNKNOWN_STR;
}
template <mcc::traits::mcc_char_range R>
static constexpr MccCoordPairKind MccCoordStrToPairKind(R&& spair)
{
if constexpr (std::is_pointer_v<std::decay_t<R>>) {
return MccCoordStrToPairKind(std::string_view{spair});
}
const auto hash = mcc::utils::FNV1aHash(std::forward<R>(spair));
return hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_ICRS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_OBS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_OBS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZALT_STR) ? MccCoordPairKind::COORDS_KIND_AZALT
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZZD_STR) ? MccCoordPairKind::COORDS_KIND_AZZD
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_XY_STR) ? MccCoordPairKind::COORDS_KIND_XY
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_LATLON_STR) ? MccCoordPairKind::COORDS_KIND_LONLAT
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_GENERIC_STR) ? MccCoordPairKind::COORDS_KIND_GENERIC
: MccCoordPairKind::COORDS_KIND_UNKNOWN;
}
std::string MccAngleFancyString(std::convertible_to<MccAngle> auto const& ang,
std::format_string<double> val_fmt = "{}")
{
std::string s;
double abs_ang;
if constexpr (std::is_arithmetic_v<std::decay_t<decltype(ang)>>) {
abs_ang = std::abs(ang);
} else {
abs_ang = std::abs(MccAngle{ang});
}
if (abs_ang < 1.0_arcmins) {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcsecs());
s += " arcsecs";
} else if (abs_ang < 1.0_degs) {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcmins());
s += " arcmins";
} else {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.degrees());
s += " degs";
}
return s;
}
} // namespace mcc } // namespace mcc

View File

@@ -334,7 +334,9 @@ public:
*st = eraGst06a(ERFA_DJM0, ut1, ERFA_DJM0, tt); *st = eraGst06a(ERFA_DJM0, ut1, ERFA_DJM0, tt);
if (islocal) { if (islocal) {
*st += _currentState.lon; // *st += _currentState.lon;
// *st = MccAngle(*st + _currentState.lon).normalize<MccAngle::NORM_KIND_0_360>();
*st = eraAnp(*st + _currentState.lon);
} }
@@ -394,8 +396,20 @@ public:
// coordinates transformations // coordinates transformations
template <typename ResT>
error_t transformCoordinates(mcc_celestial_point_c auto from_pt, ResT* to_pt)
requires(mcc_eqt_hrz_coord_c<ResT> || mcc_celestial_point_c<ResT>)
{
if constexpr (mcc_eqt_hrz_coord_c<ResT>) {
return transformCoordinatesEQHR(std::move(from_pt), to_pt);
} else if constexpr (mcc_celestial_point_c<ResT>) {
return transformCoordinatesCP(std::move(from_pt), to_pt);
} else {
static_assert(false, "UNSUPPORTED TYPE!");
}
}
error_t transformCoordinates(mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt) error_t transformCoordinatesCP(mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt)
{ {
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK; error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
@@ -491,7 +505,8 @@ public:
// first, compute HA from CIO-based RA!!! // first, compute HA from CIO-based RA!!!
lst_eo(); lst_eo();
if (!ret) { if (!ret) {
ha = lst - from_pt.X + eo; ha = MccAngle(lst - from_pt.X + eo).normalize<MccAngle::NORM_KIND_0_360>();
// ha = MccAngle(lst - from_pt.X - eo).normalize<MccAngle::NORM_KIND_180_180>();
} else { } else {
return ret; return ret;
} }
@@ -519,6 +534,7 @@ public:
lst_eo(); lst_eo();
if (!ret) { if (!ret) {
to_pt->X = MccAngle(lst - from_pt.X + eo).normalize<MccAngle::NORM_KIND_0_360>(); to_pt->X = MccAngle(lst - from_pt.X + eo).normalize<MccAngle::NORM_KIND_0_360>();
// to_pt->X = MccAngle(lst - from_pt.X - eo).normalize<MccAngle::NORM_KIND_0_360>();
to_pt->Y = from_pt.Y; to_pt->Y = from_pt.Y;
} else { } else {
return ret; return ret;
@@ -543,6 +559,7 @@ public:
lst_eo(); lst_eo();
if (!ret) { if (!ret) {
to_pt->X = MccAngle(lst - to_pt->X + eo).normalize<MccAngle::NORM_KIND_0_360>(); to_pt->X = MccAngle(lst - to_pt->X + eo).normalize<MccAngle::NORM_KIND_0_360>();
// to_pt->X = MccAngle(lst - to_pt->X - eo).normalize<MccAngle::NORM_KIND_0_360>();
} }
} else { } else {
ret = MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; ret = MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
@@ -557,7 +574,7 @@ public:
} }
error_t transformCoordinates(mcc_celestial_point_c auto from_pt, mcc_eqt_hrz_coord_c auto* to_pt) error_t transformCoordinatesEQHR(mcc_celestial_point_c auto from_pt, mcc_eqt_hrz_coord_c auto* to_pt)
{ {
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK; error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
@@ -573,7 +590,24 @@ public:
// the main scenario: from ICRS to apparent // the main scenario: from ICRS to apparent
if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
to_pt->RA_ICRS = from_pt.X;
to_pt->DEC_ICRS = from_pt.Y;
return icrs2obs(from_pt, to_pt); return icrs2obs(from_pt, to_pt);
} else { // from apparent (2025-12-18: according to addition of RA/DEC_OCRS to mcc_eqt_hrz_coord_c)
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
ret = transformCoordinates(from_pt, &cpt); // to ICRS
if (ret) {
return ret;
}
to_pt->RA_ICRS = cpt.X;
to_pt->DEC_ICRS = cpt.Y;
// if (!ret) {
// ret = transformCoordinates(cpt, to_pt); // from ICRS to observed
// }
// return ret;
} }
// from apparent: copy corresponded coordinates and compute other ones (ignore to_pt->pair_kind) // from apparent: copy corresponded coordinates and compute other ones (ignore to_pt->pair_kind)

838
mcc/mcc_ccte_erfa_new.h Normal file
View File

@@ -0,0 +1,838 @@
#pragma once
#include <erfa.h>
#include <erfam.h>
#include <mutex>
#include "mcc_ccte_iers.h"
// #include "mcc_coord.h"
#include "mcc_generics.h"
namespace mcc::ccte::erfa
{
enum class MccCCTE_ERFAErrorCode : int {
ERROR_OK = 0,
ERROR_NULLPTR,
ERROR_INVALID_INPUT_ARG,
ERROR_julday_INVALID_YEAR,
ERROR_julday_INVALID_MONTH,
ERROR_julday_INVALID_DAY,
ERROR_UNSUPPORTED_COORD_PAIR,
ERROR_BULLETINA_OUT_OF_RANGE,
ERROR_LEAPSECONDS_OUT_OF_RANGE,
ERROR_DUBIOUS_YEAR,
ERROR_UNACCEPTABLE_DATE,
ERROR_UPDATE_LEAPSECONDS,
ERROR_UPDATE_BULLETINA,
ERROR_UNEXPECTED
};
} // namespace mcc::ccte::erfa
namespace std
{
template <>
class is_error_code_enum<mcc::ccte::erfa::MccCCTE_ERFAErrorCode> : public true_type
{
};
} // namespace std
namespace mcc::ccte::erfa
{
/* error category definition */
// error category
struct MccCCTE_ERFACategory : public std::error_category {
MccCCTE_ERFACategory() : std::error_category() {}
const char* name() const noexcept
{
return "CCTE-ERFA";
}
std::string message(int ec) const
{
MccCCTE_ERFAErrorCode err = static_cast<MccCCTE_ERFAErrorCode>(ec);
switch (err) {
case MccCCTE_ERFAErrorCode::ERROR_OK:
return "OK";
case MccCCTE_ERFAErrorCode::ERROR_NULLPTR:
return "input argument is the nullptr";
case MccCCTE_ERFAErrorCode::ERROR_INVALID_INPUT_ARG:
return "invalid argument";
case MccCCTE_ERFAErrorCode::ERROR_julday_INVALID_YEAR:
return "invalid year number";
case MccCCTE_ERFAErrorCode::ERROR_julday_INVALID_MONTH:
return "invalid month number";
case MccCCTE_ERFAErrorCode::ERROR_julday_INVALID_DAY:
return "invalid day number";
case MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
return "unsupported coordinate pair";
case MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE:
return "time point is out of range";
case MccCCTE_ERFAErrorCode::ERROR_LEAPSECONDS_OUT_OF_RANGE:
return "time point is out of range";
case MccCCTE_ERFAErrorCode::ERROR_DUBIOUS_YEAR:
return "dubious year";
case MccCCTE_ERFAErrorCode::ERROR_UNACCEPTABLE_DATE:
return "unacceptable date";
case MccCCTE_ERFAErrorCode::ERROR_UPDATE_LEAPSECONDS:
return "leap seconds update error";
case MccCCTE_ERFAErrorCode::ERROR_UPDATE_BULLETINA:
return "bulletin A update error";
case MccCCTE_ERFAErrorCode::ERROR_UNEXPECTED:
return "unexpected error value";
default:
return "UNKNOWN";
}
}
static const MccCCTE_ERFACategory& get()
{
static const MccCCTE_ERFACategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccCCTE_ERFAErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccCCTE_ERFACategory::get());
}
class MccCCTE_ERFA
{
static constexpr double PI_2 = std::numbers::pi / 2.0;
public:
static constexpr double DEFAULT_WAVELENGTH = 0.55; // default observed wavelength in mkm
typedef std::error_code error_t;
struct refract_model_t {
static constexpr std::string_view name()
{
return "ERFA";
}
double refa, refb;
};
// meteo parameters (to compute refraction)
struct meteo_t {
typedef double temp_t;
typedef double humid_t;
typedef double press_t;
temp_t temperature; // Temperature in C
humid_t humidity; // humidity in % ([0.0, 1.0])
press_t pressure; // atmospheric presure in hPa=mB
};
// celestial object addition parameters
struct obj_pars_t {
double pm_RA = 0.0; // rads/year
double pm_DEC = 0.0; // rads/year
double parallax; // in arcsecs
double radvel; // radial velocity (signed, km/s)
};
struct engine_state_t {
meteo_t meteo{.temperature = 0.0, .humidity = 0.5, .pressure = 1010.0};
double wavelength = DEFAULT_WAVELENGTH; // observed wavelength in mkm
double lat = 0.0; // site latitude
double lon = 0.0; // site longitude
double elev = 0.0; // site elevation (in meters)
mcc::ccte::iers::MccLeapSeconds _leapSeconds{};
mcc::ccte::iers::MccIersBulletinA _bulletinA{};
};
MccCCTE_ERFA() : _stateMutex(new std::mutex) {}
MccCCTE_ERFA(engine_state_t state) : _currentState(std::move(state)), _stateMutex(new std::mutex) {}
MccCCTE_ERFA(const MccCCTE_ERFA&) = delete;
MccCCTE_ERFA& operator=(const MccCCTE_ERFA&) = delete;
MccCCTE_ERFA(MccCCTE_ERFA&&) = default;
MccCCTE_ERFA& operator=(MccCCTE_ERFA&&) = default;
virtual ~MccCCTE_ERFA() = default;
std::string_view nameCCTE() const
{
return "ERFA-CCTE-ENGINE";
}
// engine state related methods
void setStateERFA(engine_state_t state)
{
std::lock_guard lock{*_stateMutex};
_currentState = std::move(state);
}
engine_state_t getStateERFA() const
{
std::lock_guard lock{*_stateMutex};
return _currentState;
}
void updateMeteoERFA(meteo_t meteo)
{
std::lock_guard lock{*_stateMutex};
_currentState.meteo = std::move(meteo);
}
error_t updateLeapSeconds(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '#')
{
std::lock_guard lock{*_stateMutex};
if (!_currentState._leapSeconds.load(stream, comment_sym)) {
return MccCCTE_ERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
}
return MccCCTE_ERFAErrorCode::ERROR_OK;
}
error_t updateLeapSeconds(traits::mcc_input_char_range auto const& filename, char comment_sym = '#')
{
std::lock_guard lock{*_stateMutex};
if (!_currentState._leapSeconds.load(filename, comment_sym)) {
return MccCCTE_ERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
}
return MccCCTE_ERFAErrorCode::ERROR_OK;
}
error_t updateBulletinA(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '*')
{
std::lock_guard lock{*_stateMutex};
if (!_currentState._bulletinA.load(stream, comment_sym)) {
return MccCCTE_ERFAErrorCode::ERROR_UPDATE_BULLETINA;
}
return MccCCTE_ERFAErrorCode::ERROR_OK;
}
error_t updateBulletinA(traits::mcc_input_char_range auto const& filename, char comment_sym = '*')
{
std::lock_guard lock{*_stateMutex};
if (!_currentState._bulletinA.load(filename, comment_sym)) {
return MccCCTE_ERFAErrorCode::ERROR_UPDATE_BULLETINA;
}
return MccCCTE_ERFAErrorCode::ERROR_OK;
}
// apparent sideral time (Greenwitch or local)
error_t apparentSideralTime(mcc_coord_epoch_c auto const& epoch, mcc_angle_c auto* st, bool islocal = false)
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
if (st == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
double ut1 = epoch.MJD();
double tt = epoch.MJD();
std::lock_guard lock{*_stateMutex};
auto dut1 = _currentState._bulletinA.DUT1(epoch.MJD());
if (dut1.has_value()) {
ut1 += std::chrono::duration_cast<real_days_t>(dut1.value()).count();
} else { // out of range
return MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE;
}
auto tai_utc = _currentState._leapSeconds[epoch.MJD()];
if (tai_utc.has_value()) {
tt += std::chrono::duration_cast<real_days_t>(tai_utc.value()).count();
} else {
return MccCCTE_ERFAErrorCode::ERROR_LEAPSECONDS_OUT_OF_RANGE;
}
auto tt_tai = _currentState._bulletinA.TT_TAI();
tt += std::chrono::duration_cast<real_days_t>(tt_tai).count();
*st = eraGst06a(ERFA_DJM0, ut1, ERFA_DJM0, tt);
if (islocal) {
*st = eraAnp(*st + _currentState.lon);
}
return ret;
}
// ICRS to observed
// returned azimuth is counted from the South through the West
error_t icrsToObs(mcc_angle_c auto const& ra_icrs,
mcc_angle_c auto const& dec_icrs,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto* ra_obs,
mcc_angle_c auto* dec_obs,
mcc_angle_c auto* ha_obs,
mcc_angle_c auto* az,
mcc_angle_c auto* zd,
obj_pars_t* obj_params = nullptr)
{
return icrsTo(true, ra_icrs, dec_icrs, epoch, ra_obs, dec_obs, ha_obs, az, zd, obj_params);
}
// error_t icrsToObs(MccSkyRADEC_ICRS const& radec_icrs,
// MccSkyRADEC_OBS* radec_obs,
// MccSkyAZZD* azzd,
// mcc_angle_c auto* ha_obs,
// obj_pars_t* obj_params = nullptr)
// {
// double ra_obs, dec_obs, az, zd, ha;
// auto err =
// icrsToObs(radec_icrs.x(), radec_icrs.y(), radec_icrs.epoch(), &ra_obs, &dec_obs, &ha, &az, &zd,
// obj_params);
// if (!err) {
// if (radec_obs) {
// radec_obs->setX(ra_obs);
// radec_obs->setY(dec_obs);
// }
// if (azzd) {
// azzd->setEpoch(radec_obs->epoch());
// azzd->setX(az);
// azzd->setY(zd);
// }
// if (ha_obs) {
// *ha_obs = ha;
// }
// }
// return err;
// };
// ICRS to apparent (in vacuo)
// returned azimuth is counted from the South through the West
error_t icrsToApp(mcc_angle_c auto const& ra_icrs,
mcc_angle_c auto const& dec_icrs,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto* ra_app,
mcc_angle_c auto* dec_app,
mcc_angle_c auto* ha_app,
mcc_angle_c auto* az,
mcc_angle_c auto* zd, // should be interpretated as zenithal distance corrected for refraction
obj_pars_t* obj_params = nullptr)
{
return icrsTo(false, ra_icrs, dec_icrs, epoch, ra_app, dec_app, ha_app, az, zd, obj_params);
}
// error_t icrsToApp(MccSkyRADEC_ICRS const& radec_icrs,
// MccSkyRADEC_OBS* radec_app,
// MccSkyAZZD* azzd,
// mcc_angle_c auto* ha_app,
// obj_pars_t* obj_params = nullptr)
// {
// double ra_app, dec_app, az, zd, ha;
// auto err =
// icrsToApp(radec_icrs.x(), radec_icrs.y(), radec_icrs.epoch(), &ra_app, &dec_app, &ha, &az, &zd,
// obj_params);
// if (!err) {
// if (radec_app) {
// radec_app->setX(ra_app);
// radec_app->setY(dec_app);
// }
// if (azzd) {
// azzd->setEpoch(radec_app->epoch());
// azzd->setX(az);
// azzd->setY(zd);
// }
// if (ha_app) {
// *ha_app = ha;
// }
// }
// return err;
// }
error_t obsToICRS(MccCoordPairKind obs_type,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto const& co_lon,
mcc_angle_c auto const& co_lat,
mcc_angle_c auto* ra_icrs,
mcc_angle_c auto* dec_icrs)
{
return toICRS(true, obs_type, epoch, co_lon, co_lat, ra_icrs, dec_icrs);
}
// error_t obsToICRS(mcc_coord_pair_c auto const& xy_obs, MccSkyRADEC_ICRS* radec_icrs)
// {
// double ra, dec;
// auto err = obsToICRS(xy_obs.pair_kind, xy_obs.epoch(), xy_obs.x(), xy_obs.y(), &ra, &dec);
// if (err) {
// return err;
// }
// if (radec_icrs) {
// radec_icrs->setX(ra);
// radec_icrs->setY(dec);
// }
// return err;
// }
error_t appToICRS(MccCoordPairKind app_type,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto const& co_lon,
mcc_angle_c auto const& co_lat,
mcc_angle_c auto* ra_icrs,
mcc_angle_c auto* dec_icrs)
{
return toICRS(false, app_type, epoch, co_lon, co_lat, ra_icrs, dec_icrs);
}
// error_t appToICRS(mcc_coord_pair_c auto const& xy_app, MccSkyRADEC_ICRS* radec_icrs)
// {
// double ra, dec;
// auto err = appToICRS(xy_app.pair_kind, xy_app.epoch(), xy_app.x(), xy_app.y(), &ra, &dec);
// if (!err) {
// if (radec_icrs) {
// radec_icrs->setX(ra);
// radec_icrs->setY(dec);
// }
// }
// return err;
// }
error_t equationOrigins(mcc_coord_epoch_c auto const& epoch, mcc_angle_c auto* eo)
{
if (eo == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
std::lock_guard lock{*_stateMutex};
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
double mjd = epoch.MJD();
auto tai_utc = _currentState._leapSeconds[mjd];
if (tai_utc.has_value()) {
double tt = mjd;
tt += std::chrono::duration_cast<real_days_t>(tai_utc.value()).count();
auto tt_tai = _currentState._bulletinA.TT_TAI();
tt += +std::chrono::duration_cast<real_days_t>(tt_tai).count();
*eo = eraEo06a(ERFA_DJM0, tt);
} else {
ret = MccCCTE_ERFAErrorCode::ERROR_LEAPSECONDS_OUT_OF_RANGE;
}
return ret;
}
// refraction
error_t refractionModel(refract_model_t* model)
{
if (model == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
std::lock_guard lock{*_stateMutex};
eraRefco(_currentState.meteo.pressure, _currentState.meteo.temperature, _currentState.meteo.humidity,
_currentState.wavelength, &model->refa, &model->refb);
return MccCCTE_ERFAErrorCode::ERROR_OK;
}
// Zobs must be observed zenithal distance (Zapp = Zobs + dZ -- corrected (in vacuo) zenithal distance)
template <typename ZAPP_T = std::nullptr_t>
error_t refractionCorrection(mcc_angle_c auto Zobs, mcc_angle_c auto* dZ, ZAPP_T Zapp = nullptr)
requires(std::is_null_pointer_v<ZAPP_T> ||
(std::is_pointer_v<ZAPP_T> && mcc_angle_c<std::remove_pointer_t<ZAPP_T>>))
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
if (dZ == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
refract_model_t rmodel;
ret = refractionModel(&rmodel);
if (!ret) {
ret = refractionCorrection(rmodel, Zobs, dZ, Zapp);
}
return ret;
}
// Zobs must be observed zenithal distance (Zapp = Zobs + dZ -- corrected (in vacuo) zenithal distance)
template <typename ZAPP_T = std::nullptr_t>
error_t refractionCorrection(const refract_model_t& rmodel,
mcc_angle_c auto Zobs,
mcc_angle_c auto* dZ,
ZAPP_T Zapp = nullptr)
requires(std::is_null_pointer_v<ZAPP_T> ||
(std::is_pointer_v<ZAPP_T> && mcc_angle_c<std::remove_pointer_t<ZAPP_T>>))
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
if (dZ == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
if (Zobs >= std::numbers::pi / 2.0) {
*dZ = 35.4 / 60.0 * std::numbers::pi / 180.0; // 35.4 arcminutes
} else {
auto tanZ = tan(Zobs);
*dZ = rmodel.refa * tanZ + rmodel.refb * tanZ * tanZ * tanZ;
}
if constexpr (!std::is_null_pointer_v<ZAPP_T>) {
*Zapp = Zobs + *dZ;
}
return ret;
}
// Zapp must be topocentric (in vacuo) zenithal distance (Zobs = Zapp - dZ -- observed, i.e. affected by refraction,
// zenithal distance)
template <typename ZOBS_T = std::nullptr_t>
error_t refractionReverseCorrection(mcc_angle_c auto Zapp, mcc_angle_c auto* dZ, ZOBS_T Zobs = nullptr)
requires(std::is_null_pointer_v<ZOBS_T> ||
(std::is_pointer_v<ZOBS_T> && mcc_angle_c<std::remove_pointer_t<ZOBS_T>>))
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
if (dZ == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
refract_model_t rmodel;
ret = refractionModel(&rmodel);
if (!ret) {
ret = refractionReverseCorrection(rmodel, Zapp, dZ, Zobs);
}
return ret;
}
// Zapp must be topocentric (in vacuo) zenithal distance (Zobs = Zapp - dZ -- observed, i.e. affected by refraction,
// zenithal distance)
template <typename ZOBS_T = std::nullptr_t>
error_t refractionReverseCorrection(const refract_model_t& rmodel,
mcc_angle_c auto Zapp,
mcc_angle_c auto* dZ,
ZOBS_T Zobs = nullptr)
requires(std::is_null_pointer_v<ZOBS_T> ||
(std::is_pointer_v<ZOBS_T> && mcc_angle_c<std::remove_pointer_t<ZOBS_T>>))
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
if (dZ == nullptr) {
return MccCCTE_ERFAErrorCode::ERROR_NULLPTR;
}
if (Zapp >= std::numbers::pi / 2.0) {
*dZ = 35.4 / 60.0 * std::numbers::pi / 180.0; // 35.4 arcminutes
} else {
auto tanZ = tan(Zapp);
auto tanZ2 = tanZ * tanZ;
auto b3 = 3.0 * rmodel.refb;
// with Newton-Raphson correction
*dZ = (rmodel.refa * tanZ + rmodel.refb * tanZ * tanZ2) /
(1.0 + rmodel.refa + tanZ2 * (rmodel.refa + b3) + b3 * tanZ2 * tanZ2);
}
if constexpr (!std::is_null_pointer_v<ZOBS_T>) {
*Zobs = Zapp - *dZ;
}
return ret;
}
/* helper mathods */
auto leapSecondsExpireDate() const
{
return _currentState._leapSeconds.expireDate();
}
auto leapSecondsExpireMJD() const
{
return _currentState._leapSeconds.expireMJD();
}
auto bulletinADateRange() const
{
return _currentState._bulletinA.dateRange();
}
auto bulletinADateRangeMJD() const
{
return _currentState._bulletinA.dateRangeMJD();
}
protected:
engine_state_t _currentState{};
std::unique_ptr<std::mutex> _stateMutex;
error_t icrsTo(bool observed, // true - observed, false - apparent
mcc_angle_c auto const& ra_icrs,
mcc_angle_c auto const& dec_icrs,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto* ra,
mcc_angle_c auto* dec,
mcc_angle_c auto* ha,
mcc_angle_c auto* az,
mcc_angle_c auto* zd,
obj_pars_t* obj_params = nullptr)
{
int err;
double r, d, h, a, z, eo;
double pressure = 0.0; // 0 for apparent coordinates type (see ERFA's refco.c: if pressure is zero then
// refraction is also zero)
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
std::lock_guard lock{*_stateMutex};
if (observed) {
pressure = _currentState.meteo.pressure;
}
auto dut1 = _currentState._bulletinA.DUT1(epoch.MJD());
if (!dut1.has_value()) {
return MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE;
}
auto pol_pos = _currentState._bulletinA.polarCoords(epoch.MJD());
if (!pol_pos.has_value()) {
return MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE;
}
// const auto arcsec2rad = std::numbers::pi / 180 / 3600;
const auto arcsec2rad = 1.0_arcsecs;
pol_pos->x *= arcsec2rad;
pol_pos->y *= arcsec2rad;
if (obj_params) {
err = eraAtco13(ra_icrs, dec_icrs, obj_params->pm_RA, obj_params->pm_DEC, obj_params->parallax,
obj_params->radvel, ERFA_DJM0, epoch.MJD(), dut1->count(), _currentState.lon,
_currentState.lat, _currentState.elev, pol_pos->x, pol_pos->y, pressure,
_currentState.meteo.temperature, _currentState.meteo.humidity, _currentState.wavelength, &a,
&z, &h, &d, &r, &eo);
} else {
err = eraAtco13(ra_icrs, dec_icrs, 0.0, 0.0, 0.0, 0.0, ERFA_DJM0, epoch.MJD(), dut1->count(),
_currentState.lon, _currentState.lat, _currentState.elev, pol_pos->x, pol_pos->y, pressure,
_currentState.meteo.temperature, _currentState.meteo.humidity, _currentState.wavelength, &a,
&z, &h, &d, &r, &eo);
}
if (err == 1) {
ret = MccCCTE_ERFAErrorCode::ERROR_DUBIOUS_YEAR;
} else if (err == -1) {
ret = MccCCTE_ERFAErrorCode::ERROR_UNACCEPTABLE_DATE;
}
if (ra) {
*ra = r;
}
if (dec) {
*dec = d;
}
if (ha) {
*ha = h;
}
if (az) {
// NOTE: according to definition of astronomical azimuth it is counted from the South through the West, but
// in the ERFA the azimuth is counted from the North through the East!!!
//
*az = MccAngle(a - std::numbers::pi).normalize<MccAngle::NORM_KIND_0_360>();
// *az = MccAngle(a + std::numbers::pi).normalize<MccAngle::NORM_KIND_0_360>();
}
if (zd) {
*zd = z;
}
return ret;
}
error_t toICRS(bool observed, // true - observed, false - apparent
MccCoordPairKind pair_type,
mcc_coord_epoch_c auto const& epoch,
mcc_angle_c auto const& co_lon,
mcc_angle_c auto const& co_lat,
mcc_angle_c auto* ra_icrs,
mcc_angle_c auto* dec_icrs)
{
error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK;
// check coordinate pair consistency
if (mcc_is_app_coordpair(pair_type) && observed) {
return MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
if (mcc_is_obs_coordpair(pair_type) && !observed) {
return MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
std::lock_guard lock{*_stateMutex};
auto dut1 = _currentState._bulletinA.DUT1(epoch.MJD());
if (!dut1.has_value()) {
return MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE;
}
auto pol_pos = _currentState._bulletinA.polarCoords(epoch.MJD());
if (!pol_pos.has_value()) {
return MccCCTE_ERFAErrorCode::ERROR_BULLETINA_OUT_OF_RANGE;
}
// const auto arcsec2rad = std::numbers::pi / 180 / 3600;
const auto arcsec2rad = 1.0_arcsecs;
pol_pos->x *= arcsec2rad;
pol_pos->y *= arcsec2rad;
std::string type;
double x, y, ra, dec;
double pressure = 0.0;
if (observed) {
pressure = _currentState.meteo.pressure;
}
switch (pair_type) {
case mcc::MccCoordPairKind::COORDS_KIND_AZZD:
// NOTE: according to definition of astronomical azimuth it is counted from the South through the West,
// but in the ERFA the azimuth is counted from the North through the East!!!
//
x = co_lon + std::numbers::pi;
y = co_lat;
type = "A";
break;
case mcc::MccCoordPairKind::COORDS_KIND_AZALT:
// NOTE: according to definition of astronomical azimuth it is counted from the South through the West,
// but in the ERFA the azimuth is counted from the North through the East!!!
//
x = co_lon + std::numbers::pi;
y = MccCCTE_ERFA::PI_2 - co_lat; // altitude to zenithal distance
type = "A";
break;
case mcc::MccCoordPairKind::COORDS_KIND_HADEC_OBS:
type = "H";
x = co_lon;
y = co_lat;
break;
case mcc::MccCoordPairKind::COORDS_KIND_RADEC_OBS:
type = "R";
x = co_lon;
y = co_lat;
break;
case mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP:
type = "H";
x = co_lon;
y = co_lat;
break;
case mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP:
type = "R";
x = co_lon;
y = co_lat;
break;
default:
return MccCCTE_ERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
};
int err =
eraAtoc13(type.c_str(), x, y, ERFA_DJM0, epoch.MJD(), dut1->count(), _currentState.lon, _currentState.lat,
_currentState.elev, pol_pos->x, pol_pos->y, pressure, _currentState.meteo.temperature,
_currentState.meteo.humidity, _currentState.wavelength, &ra, &dec);
if (err == 1) {
ret = MccCCTE_ERFAErrorCode::ERROR_DUBIOUS_YEAR;
} else if (err == -1) {
ret = MccCCTE_ERFAErrorCode::ERROR_UNACCEPTABLE_DATE;
}
if (ra) {
*ra_icrs = ra;
}
if (dec) {
*dec_icrs = dec;
}
return ret;
}
};
} // namespace mcc::ccte::erfa

View File

@@ -134,7 +134,12 @@ public:
bool load(traits::mcc_input_char_range auto const& filename, char comment_sym = '#') bool load(traits::mcc_input_char_range auto const& filename, char comment_sym = '#')
{ {
std::ifstream fst(filename); std::ifstream fst;
if constexpr (std::same_as<std::remove_cvref_t<decltype(filename)>, std::string>) {
fst.open(filename);
} else {
fst.open(std::string{filename.begin(), filename.end()});
}
bool ok = fst.is_open(); bool ok = fst.is_open();
if (!ok) { if (!ok) {
@@ -436,7 +441,12 @@ public:
bool load(traits::mcc_input_char_range auto const& filename, char comment_sym = '*') bool load(traits::mcc_input_char_range auto const& filename, char comment_sym = '*')
{ {
std::ifstream fst(filename); std::ifstream fst;
if constexpr (std::same_as<std::remove_cvref_t<decltype(filename)>, std::string>) {
fst.open(filename);
} else {
fst.open(std::string{filename.begin(), filename.end()});
}
bool ok = fst.is_open(); bool ok = fst.is_open();
if (!ok) { if (!ok) {

View File

@@ -15,10 +15,10 @@ static std::string MCC_DEFAULT_LEAP_SECONDS_FILE = R"--(
# Value of TAI-UTC in second valid beetween the initial value until # Value of TAI-UTC in second valid beetween the initial value until
# the epoch given on the next line. The last line reads that NO # the epoch given on the next line. The last line reads that NO
# leap second was introduced since the corresponding date # leap second was introduced since the corresponding date
# Updated through IERS Bulletin 70 issued in July 2025 # Updated through IERS Bulletin 71 issued in January 2026
# #
# #
# File expires on 28 June 2026 # File expires on 28 December 2026
# #
# #
# MJD Date TAI-UTC (s) # MJD Date TAI-UTC (s)
@@ -67,7 +67,7 @@ static std::string MCC_DEFAULT_IERS_BULLETIN_A_FILE = R"--(
* * * *
* Rapid Service/Prediction of Earth Orientation * * Rapid Service/Prediction of Earth Orientation *
********************************************************************** **********************************************************************
14 August 2025 Vol. XXXVIII No. 033 8 January 2026 Vol. XXXIX No. 002
______________________________________________________________________ ______________________________________________________________________
GENERAL INFORMATION: GENERAL INFORMATION:
MJD = Julian Date - 2 400 000.5 days MJD = Julian Date - 2 400 000.5 days
@@ -83,7 +83,7 @@ static std::string MCC_DEFAULT_IERS_BULLETIN_A_FILE = R"--(
* ANNOUNCEMENTS: * * ANNOUNCEMENTS: *
* * * *
* There will NOT be a leap second introduced in UTC * * There will NOT be a leap second introduced in UTC *
* at the end of December 2025. * * at the end of June 2026. *
* * * *
* The primary source for IERS Rapid Service/Prediction Center (RS/PC) * * The primary source for IERS Rapid Service/Prediction Center (RS/PC) *
* data products is the official IERS RS/PC website: * * data products is the official IERS RS/PC website: *
@@ -116,13 +116,47 @@ static std::string MCC_DEFAULT_IERS_BULLETIN_A_FILE = R"--(
IERS Rapid Service IERS Rapid Service
MJD x error y error UT1-UTC error MJD x error y error UT1-UTC error
" " " " s s " " " " s s
25 8 8 60895 0.21521 .00009 0.41904 .00009 0.069826 0.000016 26 1 2 61042 0.10962 .00009 0.33249 .00009 0.074152 0.000020
25 8 9 60896 0.21558 .00009 0.41771 .00009 0.070764 0.000016 26 1 3 61043 0.10827 .00009 0.33355 .00009 0.074367 0.000021
25 8 10 60897 0.21616 .00009 0.41640 .00009 0.071442 0.000017 26 1 4 61044 0.10690 .00009 0.33455 .00009 0.074487 0.000020
25 8 11 60898 0.21726 .00009 0.41513 .00009 0.071836 0.000016 26 1 5 61045 0.10554 .00009 0.33551 .00009 0.074361 0.000015
25 8 12 60899 0.21832 .00009 0.41407 .00009 0.071996 0.000017 26 1 6 61046 0.10404 .00009 0.33628 .00009 0.073991 0.000015
25 8 13 60900 0.21871 .00009 0.41314 .00009 0.072040 0.000013 26 1 7 61047 0.10253 .00009 0.33692 .00009 0.073470 0.000012
25 8 14 60901 0.21892 .00009 0.41223 .00009 0.072161 0.000011 26 1 8 61048 0.10121 .00009 0.33746 .00009 0.072861 0.000008
IERS Final Values
MJD x y UT1-UTC
" " s
25 11 2 60981 0.1750 0.3194 0.09220
25 11 3 60982 0.1735 0.3187 0.09112
25 11 4 60983 0.1718 0.3187 0.09005
25 11 5 60984 0.1699 0.3183 0.08916
25 11 6 60985 0.1679 0.3182 0.08854
25 11 7 60986 0.1659 0.3179 0.08822
25 11 8 60987 0.1640 0.3171 0.08814
25 11 9 60988 0.1630 0.3163 0.08819
25 11 10 60989 0.1625 0.3158 0.08820
25 11 11 60990 0.1615 0.3153 0.08801
25 11 12 60991 0.1602 0.3151 0.08763
25 11 13 60992 0.1583 0.3154 0.08711
25 11 14 60993 0.1564 0.3157 0.08644
25 11 15 60994 0.1545 0.3160 0.08566
25 11 16 60995 0.1530 0.3163 0.08494
25 11 17 60996 0.1511 0.3167 0.08433
25 11 18 60997 0.1491 0.3165 0.08390
25 11 19 60998 0.1474 0.3163 0.08366
25 11 20 60999 0.1456 0.3161 0.08357
25 11 21 61000 0.1437 0.3157 0.08363
25 11 22 61001 0.1419 0.3149 0.08381
25 11 23 61002 0.1398 0.3143 0.08403
25 11 24 61003 0.1380 0.3136 0.08426
25 11 25 61004 0.1373 0.3137 0.08431
25 11 26 61005 0.1360 0.3146 0.08417
25 11 27 61006 0.1351 0.3149 0.08377
25 11 28 61007 0.1343 0.3153 0.08305
25 11 29 61008 0.1334 0.3153 0.08209
25 11 30 61009 0.1325 0.3157 0.08097
25 12 1 61010 0.1316 0.3160 0.07984
_______________________________________________________________________ _______________________________________________________________________
@@ -130,387 +164,481 @@ static std::string MCC_DEFAULT_IERS_BULLETIN_A_FILE = R"--(
The following formulas will not reproduce the predictions given below, The following formulas will not reproduce the predictions given below,
but may be used to extend the predictions beyond the end of this table. but may be used to extend the predictions beyond the end of this table.
x = 0.1410 + 0.1141 cos A + 0.0905 sin A - 0.0377 cos C - 0.0594 sin C x = 0.1611 - 0.0626 cos A - 0.1221 sin A + 0.0025 cos C + 0.0585 sin C
y = 0.3821 + 0.0927 cos A - 0.1005 sin A - 0.0594 cos C + 0.0377 sin C y = 0.3836 - 0.1091 cos A + 0.0516 sin A + 0.0585 cos C - 0.0025 sin C
UT1-UTC = 0.0478 + 0.00010 (MJD - 60909) - (UT2-UT1) UT1-UTC = 0.0617 + 0.00005 (MJD - 61056) - (UT2-UT1)
where A = 2*pi*(MJD-60901)/365.25 and C = 2*pi*(MJD-60901)/435. where A = 2*pi*(MJD-61048)/365.25 and C = 2*pi*(MJD-61048)/435.
TAI-UTC(MJD 60902) = 37.0 TAI-UTC(MJD 61049) = 37.0
The accuracy may be estimated from the expressions: The accuracy may be estimated from the expressions:
S x,y = 0.00068 (MJD-60901)**0.80 S t = 0.00025 (MJD-60901)**0.75 S x,y = 0.00068 (MJD-61048)**0.80 S t = 0.00025 (MJD-61048)**0.75
Estimated accuracies are: Predictions 10 d 20 d 30 d 40 d Estimated accuracies are: Predictions 10 d 20 d 30 d 40 d
Polar coord's 0.004 0.007 0.010 0.013 Polar coord's 0.004 0.007 0.010 0.013
UT1-UTC 0.0014 0.0024 0.0032 0.0040 UT1-UTC 0.0014 0.0024 0.0032 0.0040
MJD x(arcsec) y(arcsec) UT1-UTC(sec) MJD x(arcsec) y(arcsec) UT1-UTC(sec)
2025 8 15 60902 0.2191 0.4112 0.07241 2026 1 9 61049 0.1001 0.3380 0.07228
2025 8 16 60903 0.2193 0.4101 0.07289 2026 1 10 61050 0.0993 0.3385 0.07180
2025 8 17 60904 0.2196 0.4090 0.07364 2026 1 11 61051 0.0985 0.3390 0.07149
2025 8 18 60905 0.2200 0.4078 0.07462 2026 1 12 61052 0.0978 0.3396 0.07136
2025 8 19 60906 0.2205 0.4066 0.07573 2026 1 13 61053 0.0971 0.3403 0.07142
2025 8 20 60907 0.2210 0.4054 0.07685 2026 1 14 61054 0.0964 0.3411 0.07168
2025 8 21 60908 0.2215 0.4042 0.07788 2026 1 15 61055 0.0957 0.3418 0.07209
2025 8 22 60909 0.2220 0.4029 0.07870 2026 1 16 61056 0.0949 0.3426 0.07260
2025 8 23 60910 0.2225 0.4018 0.07929 2026 1 17 61057 0.0942 0.3434 0.07313
2025 8 24 60911 0.2229 0.4006 0.07964 2026 1 18 61058 0.0935 0.3442 0.07360
2025 8 25 60912 0.2233 0.3994 0.07982 2026 1 19 61059 0.0928 0.3450 0.07391
2025 8 26 60913 0.2237 0.3982 0.07994 2026 1 20 61060 0.0921 0.3458 0.07400
2025 8 27 60914 0.2241 0.3970 0.08012 2026 1 21 61061 0.0915 0.3466 0.07380
2025 8 28 60915 0.2244 0.3957 0.08043 2026 1 22 61062 0.0909 0.3474 0.07332
2025 8 29 60916 0.2247 0.3945 0.08090 2026 1 23 61063 0.0903 0.3482 0.07264
2025 8 30 60917 0.2250 0.3932 0.08154 2026 1 24 61064 0.0897 0.3490 0.07185
2025 8 31 60918 0.2253 0.3919 0.08233 2026 1 25 61065 0.0892 0.3499 0.07110
2025 9 1 60919 0.2256 0.3907 0.08321 2026 1 26 61066 0.0886 0.3507 0.07051
2025 9 2 60920 0.2258 0.3894 0.08411 2026 1 27 61067 0.0881 0.3516 0.07015
2025 9 3 60921 0.2260 0.3881 0.08494 2026 1 28 61068 0.0876 0.3524 0.07004
2025 9 4 60922 0.2262 0.3868 0.08561 2026 1 29 61069 0.0872 0.3533 0.07016
2025 9 5 60923 0.2263 0.3855 0.08601 2026 1 30 61070 0.0867 0.3542 0.07040
2025 9 6 60924 0.2265 0.3842 0.08608 2026 1 31 61071 0.0863 0.3551 0.07062
2025 9 7 60925 0.2266 0.3829 0.08580 2026 2 1 61072 0.0858 0.3560 0.07069
2025 9 8 60926 0.2267 0.3816 0.08522 2026 2 2 61073 0.0855 0.3569 0.07052
2025 9 9 60927 0.2267 0.3803 0.08448 2026 2 3 61074 0.0851 0.3578 0.07011
2025 9 10 60928 0.2267 0.3790 0.08373 2026 2 4 61075 0.0847 0.3587 0.06952
2025 9 11 60929 0.2267 0.3777 0.08314 2026 2 5 61076 0.0844 0.3596 0.06884
2025 9 12 60930 0.2267 0.3764 0.08283 2026 2 6 61077 0.0841 0.3605 0.06818
2025 9 13 60931 0.2267 0.3751 0.08283 2026 2 7 61078 0.0838 0.3615 0.06763
2025 9 14 60932 0.2266 0.3738 0.08308 2026 2 8 61079 0.0835 0.3624 0.06723
2025 9 15 60933 0.2265 0.3724 0.08349 2026 2 9 61080 0.0832 0.3634 0.06700
2025 9 16 60934 0.2263 0.3711 0.08390 2026 2 10 61081 0.0830 0.3643 0.06696
2025 9 17 60935 0.2262 0.3698 0.08421 2026 2 11 61082 0.0828 0.3653 0.06706
2025 9 18 60936 0.2260 0.3685 0.08434 2026 2 12 61083 0.0826 0.3662 0.06727
2025 9 19 60937 0.2258 0.3672 0.08426 2026 2 13 61084 0.0824 0.3672 0.06753
2025 9 20 60938 0.2256 0.3659 0.08400 2026 2 14 61085 0.0822 0.3682 0.06774
2025 9 21 60939 0.2253 0.3646 0.08360 2026 2 15 61086 0.0821 0.3692 0.06784
2025 9 22 60940 0.2250 0.3633 0.08315 2026 2 16 61087 0.0820 0.3702 0.06774
2025 9 23 60941 0.2247 0.3620 0.08272 2026 2 17 61088 0.0819 0.3711 0.06738
2025 9 24 60942 0.2244 0.3607 0.08239 2026 2 18 61089 0.0818 0.3721 0.06675
2025 9 25 60943 0.2240 0.3595 0.08220 2026 2 19 61090 0.0818 0.3731 0.06590
2025 9 26 60944 0.2236 0.3582 0.08219 2026 2 20 61091 0.0817 0.3741 0.06493
2025 9 27 60945 0.2232 0.3569 0.08235 2026 2 21 61092 0.0817 0.3751 0.06395
2025 9 28 60946 0.2228 0.3556 0.08264 2026 2 22 61093 0.0817 0.3761 0.06311
2025 9 29 60947 0.2223 0.3544 0.08302 2026 2 23 61094 0.0818 0.3771 0.06250
2025 9 30 60948 0.2218 0.3531 0.08339 2026 2 24 61095 0.0818 0.3781 0.06215
2025 10 1 60949 0.2213 0.3519 0.08369 2026 2 25 61096 0.0819 0.3792 0.06203
2025 10 2 60950 0.2208 0.3506 0.08381 2026 2 26 61097 0.0820 0.3802 0.06204
2025 10 3 60951 0.2202 0.3494 0.08367 2026 2 27 61098 0.0821 0.3812 0.06206
2025 10 4 60952 0.2196 0.3482 0.08322 2026 2 28 61099 0.0823 0.3822 0.06196
2025 10 5 60953 0.2190 0.3470 0.08244 2026 3 1 61100 0.0824 0.3832 0.06165
2025 10 6 60954 0.2184 0.3458 0.08143 2026 3 2 61101 0.0826 0.3842 0.06111
2025 10 7 60955 0.2177 0.3446 0.08031 2026 3 3 61102 0.0828 0.3852 0.06037
2025 10 8 60956 0.2171 0.3434 0.07929 2026 3 4 61103 0.0831 0.3862 0.05951
2025 10 9 60957 0.2164 0.3422 0.07851 2026 3 5 61104 0.0833 0.3872 0.05864
2025 10 10 60958 0.2156 0.3410 0.07804 2026 3 6 61105 0.0836 0.3882 0.05787
2025 10 11 60959 0.2149 0.3399 0.07788 2026 3 7 61106 0.0839 0.3893 0.05726
2025 10 12 60960 0.2141 0.3387 0.07790 2026 3 8 61107 0.0842 0.3903 0.05686
2025 10 13 60961 0.2133 0.3376 0.07799 2026 3 9 61108 0.0846 0.3913 0.05667
2025 10 14 60962 0.2125 0.3365 0.07800 2026 3 10 61109 0.0849 0.3922 0.05667
2025 10 15 60963 0.2117 0.3354 0.07785 2026 3 11 61110 0.0853 0.3932 0.05681
2025 10 16 60964 0.2108 0.3343 0.07750 2026 3 12 61111 0.0857 0.3942 0.05703
2025 10 17 60965 0.2100 0.3332 0.07696 2026 3 13 61112 0.0861 0.3952 0.05725
2025 10 18 60966 0.2091 0.3321 0.07629 2026 3 14 61113 0.0866 0.3962 0.05737
2025 10 19 60967 0.2082 0.3311 0.07556 2026 3 15 61114 0.0870 0.3972 0.05732
2025 10 20 60968 0.2072 0.3300 0.07484 2026 3 16 61115 0.0875 0.3981 0.05703
2025 10 21 60969 0.2063 0.3290 0.07422 2026 3 17 61116 0.0880 0.3991 0.05647
2025 10 22 60970 0.2053 0.3280 0.07375 2026 3 18 61117 0.0886 0.4000 0.05564
2025 10 23 60971 0.2043 0.3270 0.07347 2026 3 19 61118 0.0891 0.4010 0.05462
2025 10 24 60972 0.2033 0.3260 0.07337 2026 3 20 61119 0.0897 0.4019 0.05355
2025 10 25 60973 0.2022 0.3250 0.07345 2026 3 21 61120 0.0903 0.4029 0.05256
2025 10 26 60974 0.2012 0.3241 0.07364 2026 3 22 61121 0.0909 0.4038 0.05180
2025 10 27 60975 0.2001 0.3231 0.07388 2026 3 23 61122 0.0916 0.4047 0.05132
2025 10 28 60976 0.1990 0.3222 0.07410 2026 3 24 61123 0.0922 0.4056 0.05111
2025 10 29 60977 0.1979 0.3213 0.07419 2026 3 25 61124 0.0929 0.4065 0.05107
2025 10 30 60978 0.1968 0.3204 0.07409 2026 3 26 61125 0.0936 0.4074 0.05108
2025 10 31 60979 0.1957 0.3196 0.07374 2026 3 27 61126 0.0943 0.4083 0.05100
2025 11 1 60980 0.1945 0.3187 0.07309 2026 3 28 61127 0.0950 0.4092 0.05075
2025 11 2 60981 0.1933 0.3179 0.07220 2026 3 29 61128 0.0958 0.4101 0.05028
2025 11 3 60982 0.1922 0.3171 0.07115 2026 3 30 61129 0.0966 0.4109 0.04959
2025 11 4 60983 0.1910 0.3163 0.07011 2026 3 31 61130 0.0974 0.4118 0.04876
2025 11 5 60984 0.1898 0.3155 0.06924 2026 4 1 61131 0.0982 0.4126 0.04787
2025 11 6 60985 0.1885 0.3147 0.06866 2026 4 2 61132 0.0990 0.4134 0.04702
2025 11 7 60986 0.1873 0.3140 0.06841 2026 4 3 61133 0.0998 0.4142 0.04629
2025 11 8 60987 0.1860 0.3133 0.06842 2026 4 4 61134 0.1007 0.4150 0.04574
2025 11 9 60988 0.1848 0.3126 0.06856 2026 4 5 61135 0.1016 0.4158 0.04540
2025 11 10 60989 0.1835 0.3119 0.06868 2026 4 6 61136 0.1025 0.4166 0.04526
2025 11 11 60990 0.1822 0.3112 0.06866 2026 4 7 61137 0.1034 0.4174 0.04528
2025 11 12 60991 0.1809 0.3106 0.06845 2026 4 8 61138 0.1044 0.4181 0.04540
2025 11 13 60992 0.1796 0.3100 0.06806 2026 4 9 61139 0.1053 0.4188 0.04556
2025 11 14 60993 0.1782 0.3094 0.06755 2026 4 10 61140 0.1063 0.4196 0.04566
2025 11 15 60994 0.1769 0.3088 0.06698 2026 4 11 61141 0.1073 0.4203 0.04563
2025 11 16 60995 0.1755 0.3082 0.06643 2026 4 12 61142 0.1083 0.4210 0.04539
2025 11 17 60996 0.1742 0.3077 0.06596 2026 4 13 61143 0.1093 0.4217 0.04489
2025 11 18 60997 0.1728 0.3072 0.06564 2026 4 14 61144 0.1103 0.4223 0.04411
2025 11 19 60998 0.1714 0.3067 0.06550 2026 4 15 61145 0.1114 0.4230 0.04311
2025 11 20 60999 0.1701 0.3062 0.06555 2026 4 16 61146 0.1124 0.4236 0.04198
2025 11 21 61000 0.1687 0.3058 0.06578 2026 4 17 61147 0.1135 0.4242 0.04088
2025 11 22 61001 0.1673 0.3053 0.06615 2026 4 18 61148 0.1146 0.4248 0.03995
2025 11 23 61002 0.1659 0.3049 0.06660 2026 4 19 61149 0.1157 0.4254 0.03929
2025 11 24 61003 0.1644 0.3045 0.06704 2026 4 20 61150 0.1168 0.4260 0.03893
2025 11 25 61004 0.1630 0.3042 0.06739 2026 4 21 61151 0.1180 0.4265 0.03881
2025 11 26 61005 0.1616 0.3038 0.06759 2026 4 22 61152 0.1191 0.4271 0.03879
2025 11 27 61006 0.1602 0.3035 0.06756 2026 4 23 61153 0.1203 0.4276 0.03874
2025 11 28 61007 0.1587 0.3032 0.06728 2026 4 24 61154 0.1214 0.4281 0.03855
2025 11 29 61008 0.1573 0.3029 0.06677 2026 4 25 61155 0.1226 0.4286 0.03815
2025 11 30 61009 0.1558 0.3027 0.06607 2026 4 26 61156 0.1238 0.4291 0.03757
2025 12 1 61010 0.1544 0.3024 0.06523 2026 4 27 61157 0.1250 0.4295 0.03685
2025 12 2 61011 0.1529 0.3022 0.06448 2026 4 28 61158 0.1262 0.4299 0.03606
2025 12 3 61012 0.1515 0.3020 0.06395 2026 4 29 61159 0.1275 0.4304 0.03532
2025 12 4 61013 0.1500 0.3019 0.06373 2026 4 30 61160 0.1287 0.4308 0.03468
2025 12 5 61014 0.1486 0.3017 0.06379 2026 5 1 61161 0.1300 0.4311 0.03422
2025 12 6 61015 0.1471 0.3016 0.06403 2026 5 2 61162 0.1312 0.4315 0.03397
2025 12 7 61016 0.1456 0.3015 0.06430 2026 5 3 61163 0.1325 0.4318 0.03394
2025 12 8 61017 0.1442 0.3015 0.06446 2026 5 4 61164 0.1338 0.4322 0.03410
2025 12 9 61018 0.1427 0.3014 0.06442 2026 5 5 61165 0.1350 0.4325 0.03439
2025 12 10 61019 0.1413 0.3014 0.06419 2026 5 6 61166 0.1363 0.4327 0.03475
2025 12 11 61020 0.1398 0.3014 0.06380 2026 5 7 61167 0.1376 0.4330 0.03509
2025 12 12 61021 0.1383 0.3014 0.06335 2026 5 8 61168 0.1389 0.4332 0.03536
2025 12 13 61022 0.1369 0.3015 0.06291 2026 5 9 61169 0.1402 0.4335 0.03547
2025 12 14 61023 0.1354 0.3015 0.06256 2026 5 10 61170 0.1416 0.4337 0.03538
2025 12 15 61024 0.1340 0.3016 0.06234 2026 5 11 61171 0.1429 0.4339 0.03505
2025 12 16 61025 0.1325 0.3017 0.06230 2026 5 12 61172 0.1442 0.4340 0.03452
2025 12 17 61026 0.1311 0.3019 0.06246 2026 5 13 61173 0.1456 0.4342 0.03383
2025 12 18 61027 0.1297 0.3020 0.06279 2026 5 14 61174 0.1469 0.4343 0.03311
2025 12 19 61028 0.1282 0.3022 0.06328 2026 5 15 61175 0.1482 0.4344 0.03249
2025 12 20 61029 0.1268 0.3024 0.06386 2026 5 16 61176 0.1496 0.4345 0.03211
2025 12 21 61030 0.1254 0.3027 0.06444 2026 5 17 61177 0.1509 0.4345 0.03204
2025 12 22 61031 0.1239 0.3029 0.06496 2026 5 18 61178 0.1523 0.4345 0.03225
2025 12 23 61032 0.1225 0.3032 0.06533 2026 5 19 61179 0.1537 0.4346 0.03262
2025 12 24 61033 0.1211 0.3035 0.06550 2026 5 20 61180 0.1550 0.4346 0.03301
2025 12 25 61034 0.1197 0.3038 0.06544 2026 5 21 61181 0.1564 0.4345 0.03328
2025 12 26 61035 0.1183 0.3041 0.06515 2026 5 22 61182 0.1577 0.4345 0.03336
2025 12 27 61036 0.1170 0.3045 0.06469 2026 5 23 61183 0.1591 0.4344 0.03325
2025 12 28 61037 0.1156 0.3049 0.06414 2026 5 24 61184 0.1605 0.4343 0.03299
2025 12 29 61038 0.1142 0.3053 0.06364 2026 5 25 61185 0.1618 0.4342 0.03268
2025 12 30 61039 0.1129 0.3057 0.06331 2026 5 26 61186 0.1632 0.4341 0.03241
2025 12 31 61040 0.1115 0.3062 0.06323 2026 5 27 61187 0.1646 0.4339 0.03224
2026 1 1 61041 0.1102 0.3066 0.06342 2026 5 28 61188 0.1659 0.4338 0.03225
2026 1 2 61042 0.1089 0.3071 0.06382 2026 5 29 61189 0.1673 0.4336 0.03246
2026 1 3 61043 0.1076 0.3077 0.06429 2026 5 30 61190 0.1686 0.4333 0.03289
2026 1 4 61044 0.1063 0.3082 0.06469 2026 5 31 61191 0.1700 0.4331 0.03352
2026 1 5 61045 0.1050 0.3087 0.06489 2026 6 1 61192 0.1713 0.4328 0.03429
2026 1 6 61046 0.1037 0.3093 0.06486 2026 6 2 61193 0.1727 0.4326 0.03515
2026 1 7 61047 0.1024 0.3099 0.06463 2026 6 3 61194 0.1740 0.4323 0.03602
2026 1 8 61048 0.1012 0.3105 0.06428 2026 6 4 61195 0.1754 0.4319 0.03683
2026 1 9 61049 0.0999 0.3112 0.06391 2026 6 5 61196 0.1767 0.4316 0.03751
2026 1 10 61050 0.0987 0.3118 0.06361 2026 6 6 61197 0.1781 0.4312 0.03801
2026 1 11 61051 0.0975 0.3125 0.06344 2026 6 7 61198 0.1794 0.4308 0.03831
2026 1 12 61052 0.0963 0.3132 0.06344 2026 6 8 61199 0.1807 0.4304 0.03841
2026 1 13 61053 0.0951 0.3139 0.06364 2026 6 9 61200 0.1820 0.4300 0.03835
2026 1 14 61054 0.0940 0.3147 0.06401 2026 6 10 61201 0.1833 0.4295 0.03823
2026 1 15 61055 0.0928 0.3154 0.06454 2026 6 11 61202 0.1846 0.4291 0.03815
2026 1 16 61056 0.0917 0.3162 0.06517 2026 6 12 61203 0.1859 0.4286 0.03824
2026 1 17 61057 0.0906 0.3170 0.06582 2026 6 13 61204 0.1872 0.4280 0.03860
2026 1 18 61058 0.0895 0.3178 0.06640 2026 6 14 61205 0.1885 0.4275 0.03924
2026 1 19 61059 0.0884 0.3186 0.06684 2026 6 15 61206 0.1897 0.4270 0.04009
2026 1 20 61060 0.0874 0.3195 0.06707 2026 6 16 61207 0.1910 0.4264 0.04101
2026 1 21 61061 0.0863 0.3203 0.06706 2026 6 17 61208 0.1922 0.4258 0.04185
2026 1 22 61062 0.0853 0.3212 0.06680 2026 6 18 61209 0.1935 0.4252 0.04250
2026 1 23 61063 0.0843 0.3221 0.06636 2026 6 19 61210 0.1947 0.4245 0.04293
2026 1 24 61064 0.0833 0.3230 0.06582 2026 6 20 61211 0.1959 0.4239 0.04317
2026 1 25 61065 0.0823 0.3239 0.06531 2026 6 21 61212 0.1971 0.4232 0.04333
2026 1 26 61066 0.0814 0.3249 0.06494 2026 6 22 61213 0.1983 0.4225 0.04352
2026 1 27 61067 0.0804 0.3259 0.06479 2026 6 23 61214 0.1995 0.4218 0.04381
2026 1 28 61068 0.0795 0.3268 0.06489 2026 6 24 61215 0.2007 0.4211 0.04428
2026 1 29 61069 0.0786 0.3278 0.06520 2026 6 25 61216 0.2018 0.4203 0.04496
2026 1 30 61070 0.0778 0.3288 0.06562 2026 6 26 61217 0.2030 0.4195 0.04586
2026 1 31 61071 0.0769 0.3298 0.06599 2026 6 27 61218 0.2041 0.4187 0.04695
2026 2 1 61072 0.0761 0.3309 0.06620 2026 6 28 61219 0.2052 0.4179 0.04820
2026 2 2 61073 0.0753 0.3319 0.06617 2026 6 29 61220 0.2063 0.4171 0.04955
2026 2 3 61074 0.0745 0.3330 0.06589 2026 6 30 61221 0.2074 0.4162 0.05092
2026 2 4 61075 0.0737 0.3341 0.06543 2026 7 1 61222 0.2085 0.4154 0.05224
2026 2 5 61076 0.0730 0.3351 0.06490 2026 7 2 61223 0.2095 0.4145 0.05344
2026 2 6 61077 0.0723 0.3362 0.06439 2026 7 3 61224 0.2106 0.4136 0.05447
2026 2 7 61078 0.0716 0.3373 0.06401 2026 7 4 61225 0.2116 0.4127 0.05530
2026 2 8 61079 0.0709 0.3385 0.06379 2026 7 5 61226 0.2126 0.4118 0.05595
2026 2 9 61080 0.0703 0.3396 0.06378 2026 7 6 61227 0.2136 0.4108 0.05644
2026 2 10 61081 0.0696 0.3407 0.06396 2026 7 7 61228 0.2146 0.4098 0.05686
2026 2 11 61082 0.0690 0.3419 0.06432 2026 7 8 61229 0.2155 0.4088 0.05730
2026 2 12 61083 0.0685 0.3431 0.06481 2026 7 9 61230 0.2164 0.4078 0.05787
2026 2 13 61084 0.0679 0.3442 0.06537 2026 7 10 61231 0.2174 0.4068 0.05866
2026 2 14 61085 0.0674 0.3454 0.06590 2026 7 11 61232 0.2183 0.4058 0.05971
2026 2 15 61086 0.0669 0.3466 0.06634 2026 7 12 61233 0.2191 0.4048 0.06099
2026 2 16 61087 0.0664 0.3478 0.06660 2026 7 13 61234 0.2200 0.4037 0.06238
2026 2 17 61088 0.0659 0.3490 0.06664 2026 7 14 61235 0.2208 0.4026 0.06374
2026 2 18 61089 0.0655 0.3503 0.06646 2026 7 15 61236 0.2217 0.4015 0.06490
2026 2 19 61090 0.0651 0.3515 0.06608 2026 7 16 61237 0.2225 0.4004 0.06580
2026 2 20 61091 0.0647 0.3527 0.06560 2026 7 17 61238 0.2232 0.3993 0.06643
2026 2 21 61092 0.0643 0.3540 0.06514 2026 7 18 61239 0.2240 0.3982 0.06686
2026 2 22 61093 0.0640 0.3552 0.06482 2026 7 19 61240 0.2247 0.3970 0.06720
2026 2 23 61094 0.0637 0.3564 0.06470 2026 7 20 61241 0.2255 0.3959 0.06757
2026 2 24 61095 0.0634 0.3577 0.06483 2026 7 21 61242 0.2262 0.3947 0.06804
2026 2 25 61096 0.0632 0.3590 0.06515 2026 7 22 61243 0.2268 0.3935 0.06865
2026 2 26 61097 0.0629 0.3602 0.06557 2026 7 23 61244 0.2275 0.3923 0.06942
2026 2 27 61098 0.0627 0.3615 0.06596 2026 7 24 61245 0.2281 0.3911 0.07035
2026 2 28 61099 0.0625 0.3628 0.06620 2026 7 25 61246 0.2287 0.3899 0.07139
2026 3 1 61100 0.0624 0.3641 0.06619 2026 7 26 61247 0.2293 0.3887 0.07253
2026 3 2 61101 0.0623 0.3654 0.06591 2026 7 27 61248 0.2299 0.3875 0.07369
2026 3 3 61102 0.0622 0.3666 0.06538 2026 7 28 61249 0.2304 0.3862 0.07483
2026 3 4 61103 0.0621 0.3679 0.06470 2026 7 29 61250 0.2309 0.3849 0.07589
2026 3 5 61104 0.0620 0.3692 0.06398 2026 7 30 61251 0.2314 0.3837 0.07681
2026 3 6 61105 0.0620 0.3705 0.06332 2026 7 31 61252 0.2319 0.3824 0.07755
2026 3 7 61106 0.0620 0.3718 0.06280 2026 8 1 61253 0.2323 0.3811 0.07812
2026 3 8 61107 0.0620 0.3731 0.06247 2026 8 2 61254 0.2327 0.3798 0.07854
2026 3 9 61108 0.0621 0.3744 0.06235 2026 8 3 61255 0.2331 0.3785 0.07889
2026 3 10 61109 0.0621 0.3757 0.06241 2026 8 4 61256 0.2335 0.3772 0.07925
2026 3 11 61110 0.0622 0.3770 0.06263 2026 8 5 61257 0.2338 0.3759 0.07972
2026 3 12 61111 0.0624 0.3783 0.06293 2026 8 6 61258 0.2342 0.3746 0.08039
2026 3 13 61112 0.0625 0.3796 0.06322 2026 8 7 61259 0.2344 0.3733 0.08130
2026 3 14 61113 0.0627 0.3809 0.06341 2026 8 8 61260 0.2347 0.3719 0.08241
2026 3 15 61114 0.0629 0.3821 0.06342 2026 8 9 61261 0.2350 0.3706 0.08365
2026 3 16 61115 0.0631 0.3834 0.06320 2026 8 10 61262 0.2352 0.3692 0.08486
2026 3 17 61116 0.0634 0.3847 0.06271 2026 8 11 61263 0.2354 0.3679 0.08590
2026 3 18 61117 0.0637 0.3860 0.06199 2026 8 12 61264 0.2355 0.3665 0.08667
2026 3 19 61118 0.0640 0.3873 0.06113 2026 8 13 61265 0.2357 0.3652 0.08710
2026 3 20 61119 0.0643 0.3885 0.06024 2026 8 14 61266 0.2358 0.3638 0.08724
2026 3 21 61120 0.0647 0.3898 0.05943 2026 8 15 61267 0.2359 0.3624 0.08725
2026 3 22 61121 0.0651 0.3911 0.05880 2026 8 16 61268 0.2359 0.3611 0.08727
2026 3 23 61122 0.0655 0.3923 0.05842 2026 8 17 61269 0.2360 0.3597 0.08741
2026 3 24 61123 0.0659 0.3936 0.05830 2026 8 18 61270 0.2360 0.3583 0.08772
2026 3 25 61124 0.0664 0.3948 0.05832 2026 8 19 61271 0.2360 0.3570 0.08823
2026 3 26 61125 0.0668 0.3961 0.05841 2026 8 20 61272 0.2359 0.3556 0.08889
2026 3 27 61126 0.0673 0.3973 0.05840 2026 8 21 61273 0.2359 0.3542 0.08975
2026 3 28 61127 0.0679 0.3985 0.05821 2026 8 22 61274 0.2358 0.3528 0.09075
2026 3 29 61128 0.0684 0.3997 0.05777 2026 8 23 61275 0.2356 0.3515 0.09173
2026 3 30 61129 0.0690 0.4010 0.05709 2026 8 24 61276 0.2355 0.3501 0.09267
2026 3 31 61130 0.0696 0.4022 0.05624 2026 8 25 61277 0.2353 0.3487 0.09354
2026 4 1 61131 0.0702 0.4034 0.05523 2026 8 26 61278 0.2351 0.3473 0.09418
2026 4 2 61132 0.0709 0.4045 0.05420 2026 8 27 61279 0.2349 0.3460 0.09450
2026 4 3 61133 0.0715 0.4057 0.05330 2026 8 28 61280 0.2346 0.3446 0.09461
2026 4 4 61134 0.0722 0.4069 0.05257 2026 8 29 61281 0.2344 0.3432 0.09454
2026 4 5 61135 0.0729 0.4080 0.05202 2026 8 30 61282 0.2340 0.3419 0.09433
2026 4 6 61136 0.0737 0.4092 0.05164 2026 8 31 61283 0.2337 0.3405 0.09408
2026 4 7 61137 0.0744 0.4103 0.05137 2026 9 1 61284 0.2334 0.3391 0.09392
2026 4 8 61138 0.0752 0.4114 0.05123 2026 9 2 61285 0.2330 0.3378 0.09389
2026 4 9 61139 0.0760 0.4125 0.05118 2026 9 3 61286 0.2326 0.3364 0.09406
2026 4 10 61140 0.0768 0.4136 0.05105 2026 9 4 61287 0.2321 0.3351 0.09437
2026 4 11 61141 0.0777 0.4147 0.05087 2026 9 5 61288 0.2317 0.3337 0.09483
2026 4 12 61142 0.0786 0.4158 0.05044 2026 9 6 61289 0.2312 0.3324 0.09533
2026 4 13 61143 0.0794 0.4169 0.04970 2026 9 7 61290 0.2306 0.3311 0.09572
2026 4 14 61144 0.0803 0.4179 0.04867 2026 9 8 61291 0.2301 0.3298 0.09587
2026 4 15 61145 0.0813 0.4189 0.04739 2026 9 9 61292 0.2295 0.3285 0.09576
2026 4 16 61146 0.0822 0.4200 0.04601 2026 9 10 61293 0.2289 0.3272 0.09541
2026 4 17 61147 0.0832 0.4210 0.04462 2026 9 11 61294 0.2283 0.3259 0.09492
2026 4 18 61148 0.0842 0.4219 0.04343 2026 9 12 61295 0.2277 0.3246 0.09452
2026 4 19 61149 0.0852 0.4229 0.04254 2026 9 13 61296 0.2270 0.3233 0.09420
2026 4 20 61150 0.0862 0.4239 0.04195 2026 9 14 61297 0.2263 0.3220 0.09410
2026 4 21 61151 0.0872 0.4248 0.04156 2026 9 15 61298 0.2256 0.3208 0.09416
2026 4 22 61152 0.0883 0.4258 0.04129 2026 9 16 61299 0.2248 0.3195 0.09433
2026 4 23 61153 0.0894 0.4267 0.04098 2026 9 17 61300 0.2241 0.3183 0.09465
2026 4 24 61154 0.0905 0.4276 0.04050 2026 9 18 61301 0.2233 0.3170 0.09510
2026 4 25 61155 0.0916 0.4284 0.03988 2026 9 19 61302 0.2225 0.3158 0.09562
2026 4 26 61156 0.0927 0.4293 0.03914 2026 9 20 61303 0.2216 0.3146 0.09615
2026 4 27 61157 0.0939 0.4302 0.03829 2026 9 21 61304 0.2208 0.3134 0.09657
2026 4 28 61158 0.0950 0.4310 0.03744 2026 9 22 61305 0.2199 0.3122 0.09690
2026 4 29 61159 0.0962 0.4318 0.03659 2026 9 23 61306 0.2190 0.3110 0.09704
2026 4 30 61160 0.0974 0.4326 0.03591 2026 9 24 61307 0.2180 0.3099 0.09702
2026 5 1 61161 0.0986 0.4334 0.03540 2026 9 25 61308 0.2171 0.3087 0.09681
2026 5 2 61162 0.0998 0.4341 0.03522 2026 9 26 61309 0.2161 0.3076 0.09642
2026 5 3 61163 0.1010 0.4348 0.03525 2026 9 27 61310 0.2151 0.3065 0.09591
2026 5 4 61164 0.1023 0.4356 0.03547 2026 9 28 61311 0.2141 0.3054 0.09537
2026 5 5 61165 0.1035 0.4363 0.03594 2026 9 29 61312 0.2130 0.3043 0.09501
2026 5 6 61166 0.1048 0.4369 0.03644 2026 9 30 61313 0.2119 0.3032 0.09489
2026 5 7 61167 0.1061 0.4376 0.03697 2026 10 1 61314 0.2109 0.3022 0.09500
2026 5 8 61168 0.1074 0.4382 0.03741 2026 10 2 61315 0.2097 0.3011 0.09526
2026 5 9 61169 0.1087 0.4389 0.03765 2026 10 3 61316 0.2086 0.3001 0.09568
2026 5 10 61170 0.1100 0.4395 0.03762 2026 10 4 61317 0.2075 0.2991 0.09602
2026 5 11 61171 0.1114 0.4400 0.03733 2026 10 5 61318 0.2063 0.2981 0.09613
2026 5 12 61172 0.1127 0.4406 0.03682 2026 10 6 61319 0.2051 0.2971 0.09602
2026 5 13 61173 0.1140 0.4411 0.03611 2026 10 7 61320 0.2039 0.2962 0.09573
2026 5 14 61174 0.1154 0.4416 0.03540 2026 10 8 61321 0.2027 0.2952 0.09526
2026 5 15 61175 0.1168 0.4421 0.03481 2026 10 9 61322 0.2014 0.2943 0.09470
2026 5 16 61176 0.1182 0.4426 0.03447 2026 10 10 61323 0.2001 0.2934 0.09420
2026 5 17 61177 0.1195 0.4431 0.03440 2026 10 11 61324 0.1989 0.2925 0.09382
2026 5 18 61178 0.1209 0.4435 0.03458 2026 10 12 61325 0.1976 0.2917 0.09360
2026 5 19 61179 0.1223 0.4439 0.03496 2026 10 13 61326 0.1962 0.2908 0.09354
2026 5 20 61180 0.1237 0.4443 0.03531 2026 10 14 61327 0.1949 0.2900 0.09367
2026 5 21 61181 0.1252 0.4447 0.03553 2026 10 15 61328 0.1935 0.2892 0.09395
2026 5 22 61182 0.1266 0.4450 0.03563 2026 10 16 61329 0.1922 0.2884 0.09436
2026 5 23 61183 0.1280 0.4453 0.03555 2026 10 17 61330 0.1908 0.2876 0.09484
2026 5 24 61184 0.1294 0.4456 0.03538 2026 10 18 61331 0.1894 0.2869 0.09525
2026 5 25 61185 0.1309 0.4459 0.03522 2026 10 19 61332 0.1880 0.2862 0.09550
2026 5 26 61186 0.1323 0.4462 0.03510 2026 10 20 61333 0.1865 0.2855 0.09558
2026 5 27 61187 0.1337 0.4464 0.03509 2026 10 21 61334 0.1851 0.2848 0.09543
2026 5 28 61188 0.1352 0.4466 0.03526 2026 10 22 61335 0.1836 0.2841 0.09503
2026 5 29 61189 0.1366 0.4468 0.03571 2026 10 23 61336 0.1821 0.2835 0.09443
2026 5 30 61190 0.1381 0.4469 0.03643 2026 10 24 61337 0.1807 0.2829 0.09372
2026 5 31 61191 0.1395 0.4471 0.03730 2026 10 25 61338 0.1792 0.2823 0.09295
2026 6 1 61192 0.1410 0.4472 0.03832 2026 10 26 61339 0.1776 0.2817 0.09229
2026 6 2 61193 0.1425 0.4473 0.03948 2026 10 27 61340 0.1761 0.2812 0.09186
2026 6 3 61194 0.1439 0.4474 0.04058 2026 10 28 61341 0.1746 0.2807 0.09174
2026 6 4 61195 0.1454 0.4474 0.04160 2026 10 29 61342 0.1730 0.2802 0.09188
2026 6 5 61196 0.1468 0.4474 0.04249 2026 10 30 61343 0.1715 0.2797 0.09217
2026 6 6 61197 0.1483 0.4475 0.04322 2026 10 31 61344 0.1699 0.2792 0.09250
2026 6 7 61198 0.1497 0.4474 0.04372 2026 11 1 61345 0.1683 0.2788 0.09272
2026 6 8 61199 0.1512 0.4474 0.04404 2026 11 2 61346 0.1667 0.2784 0.09267
2026 6 9 61200 0.1526 0.4473 0.04420 2026 11 3 61347 0.1651 0.2780 0.09243
2026 6 10 61201 0.1541 0.4472 0.04427 2026 11 4 61348 0.1635 0.2777 0.09199
2026 6 11 61202 0.1555 0.4471 0.04441 2026 11 5 61349 0.1619 0.2773 0.09144
2026 6 12 61203 0.1570 0.4470 0.04471 2026 11 6 61350 0.1603 0.2770 0.09088
2026 6 13 61204 0.1584 0.4468 0.04527 2026 11 7 61351 0.1587 0.2767 0.09046
2026 6 14 61205 0.1598 0.4467 0.04607 2026 11 8 61352 0.1571 0.2765 0.09016
2026 6 15 61206 0.1613 0.4465 0.04699 2026 11 9 61353 0.1554 0.2763 0.09004
2026 6 16 61207 0.1627 0.4462 0.04805 2026 11 10 61354 0.1538 0.2760 0.09011
2026 6 17 61208 0.1641 0.4460 0.04907 2026 11 11 61355 0.1521 0.2759 0.09033
2026 6 18 61209 0.1655 0.4457 0.04991 2026 11 12 61356 0.1505 0.2757 0.09067
2026 6 19 61210 0.1669 0.4454 0.05056 2026 11 13 61357 0.1488 0.2756 0.09106
2026 6 20 61211 0.1683 0.4451 0.05105 2026 11 14 61358 0.1471 0.2755 0.09148
2026 6 21 61212 0.1697 0.4448 0.05136 2026 11 15 61359 0.1455 0.2754 0.09178
2026 6 22 61213 0.1711 0.4444 0.05165 2026 11 16 61360 0.1438 0.2753 0.09198
2026 6 23 61214 0.1725 0.4440 0.05210 2026 11 17 61361 0.1421 0.2753 0.09203
2026 6 24 61215 0.1738 0.4436 0.05266 2026 11 18 61362 0.1405 0.2753 0.09194
2026 6 25 61216 0.1752 0.4432 0.05340 2026 11 19 61363 0.1388 0.2753 0.09161
2026 6 26 61217 0.1765 0.4428 0.05430 2026 11 20 61364 0.1371 0.2754 0.09115
2026 6 27 61218 0.1779 0.4423 0.05539 2026 11 21 61365 0.1354 0.2755 0.09063
2026 6 28 61219 0.1792 0.4418 0.05654 2026 11 22 61366 0.1338 0.2756 0.09016
2026 6 29 61220 0.1805 0.4413 0.05779 2026 11 23 61367 0.1321 0.2757 0.08988
2026 6 30 61221 0.1818 0.4408 0.05910 2026 11 24 61368 0.1304 0.2759 0.08982
2026 7 1 61222 0.1831 0.4402 0.06048 2026 11 25 61369 0.1287 0.2760 0.09000
2026 7 2 61223 0.1844 0.4397 0.06171 2026 11 26 61370 0.1271 0.2762 0.09038
2026 7 3 61224 0.1856 0.4391 0.06280 2026 11 27 61371 0.1254 0.2765 0.09073
2026 7 4 61225 0.1869 0.4385 0.06369 2026 11 28 61372 0.1237 0.2767 0.09096
2026 7 5 61226 0.1881 0.4378 0.06434 2026 11 29 61373 0.1221 0.2770 0.09110
2026 7 6 61227 0.1893 0.4372 0.06480 2026 11 30 61374 0.1204 0.2773 0.09103
2026 7 7 61228 0.1905 0.4365 0.06523 2026 12 1 61375 0.1188 0.2777 0.09070
2026 7 8 61229 0.1917 0.4358 0.06572 2026 12 2 61376 0.1171 0.2780 0.09027
2026 7 9 61230 0.1929 0.4351 0.06633 2026 12 3 61377 0.1155 0.2784 0.08987
2026 7 10 61231 0.1941 0.4344 0.06723 2026 12 4 61378 0.1139 0.2788 0.08951
2026 7 11 61232 0.1952 0.4336 0.06843 2026 12 5 61379 0.1122 0.2793 0.08932
2026 7 12 61233 0.1964 0.4329 0.06985 2026 12 6 61380 0.1106 0.2797 0.08929
2026 7 13 61234 0.1975 0.4321 0.07146 2026 12 7 61381 0.1090 0.2802 0.08941
2026 7 14 61235 0.1986 0.4313 0.07292 2026 12 8 61382 0.1074 0.2807 0.08977
2026 7 15 61236 0.1996 0.4304 0.07413 2026 12 9 61383 0.1058 0.2813 0.09031
2026 7 16 61237 0.2007 0.4296 0.07502 2026 12 10 61384 0.1042 0.2819 0.09092
2026 7 17 61238 0.2018 0.4288 0.07557 2026 12 11 61385 0.1026 0.2824 0.09153
2026 7 18 61239 0.2028 0.4279 0.07596 2026 12 12 61386 0.1011 0.2831 0.09210
2026 7 19 61240 0.2038 0.4270 0.07629 2026 12 13 61387 0.0995 0.2837 0.09261
2026 7 20 61241 0.2048 0.4261 0.07668 2026 12 14 61388 0.0980 0.2844 0.09296
2026 7 21 61242 0.2058 0.4252 0.07728 2026 12 15 61389 0.0964 0.2851 0.09308
2026 7 22 61243 0.2067 0.4242 0.07807 2026 12 16 61390 0.0949 0.2858 0.09298
2026 7 23 61244 0.2076 0.4233 0.07908 2026 12 17 61391 0.0934 0.2865 0.09271
2026 7 24 61245 0.2085 0.4223 0.08036 2026 12 18 61392 0.0919 0.2873 0.09223
2026 7 25 61246 0.2094 0.4213 0.08180 2026 12 19 61393 0.0904 0.2881 0.09183
2026 7 26 61247 0.2103 0.4203 0.08338 2026 12 20 61394 0.0889 0.2889 0.09151
2026 7 27 61248 0.2112 0.4193 0.08491 2026 12 21 61395 0.0875 0.2897 0.09140
2026 7 28 61249 0.2120 0.4183 0.08647 2026 12 22 61396 0.0860 0.2906 0.09146
2026 7 29 61250 0.2128 0.4173 0.08786 2026 12 23 61397 0.0846 0.2914 0.09172
2026 7 30 61251 0.2136 0.4162 0.08904 2026 12 24 61398 0.0832 0.2923 0.09217
2026 7 31 61252 0.2143 0.4151 0.08998 2026 12 25 61399 0.0818 0.2933 0.09256
2026 8 1 61253 0.2151 0.4141 0.09076 2026 12 26 61400 0.0804 0.2942 0.09281
2026 8 2 61254 0.2158 0.4130 0.09130 2026 12 27 61401 0.0791 0.2952 0.09278
2026 8 3 61255 0.2165 0.4119 0.09180 2026 12 28 61402 0.0777 0.2962 0.09251
2026 8 4 61256 0.2172 0.4108 0.09232 2026 12 29 61403 0.0764 0.2972 0.09213
2026 8 5 61257 0.2178 0.4096 0.09294 2026 12 30 61404 0.0751 0.2982 0.09173
2026 8 6 61258 0.2184 0.4085 0.09372 2026 12 31 61405 0.0738 0.2993 0.09140
2026 8 7 61259 0.2190 0.4074 0.09466 2027 1 1 61406 0.0725 0.3004 0.09123
2026 8 8 61260 0.2196 0.4062 0.09591 2027 1 2 61407 0.0713 0.3015 0.09132
2026 8 9 61261 0.2202 0.4050 0.09728 2027 1 3 61408 0.0700 0.3026 0.09160
2026 8 10 61262 0.2207 0.4039 0.09868 2027 1 4 61409 0.0688 0.3037 0.09200
2026 8 11 61263 0.2212 0.4027 0.09988 2027 1 5 61410 0.0676 0.3049 0.09264
2026 8 12 61264 0.2217 0.4015 0.10077 2027 1 6 61411 0.0665 0.3061 0.09344
2026 8 13 61265 0.2221 0.4003 0.10141 2027 1 7 61412 0.0653 0.3072 0.09426
2026 8 14 61266 0.2226 0.3991 0.10182 2027 1 8 61413 0.0642 0.3085 0.09509
These predictions are based on all announced leap seconds. These predictions are based on all announced leap seconds.
CELESTIAL POLE OFFSET SERIES:
NEOS Celestial Pole Offset Series
MJD dpsi error deps error
(msec. of arc)
61026 -114.45 1.33 -7.19 0.16
61027 -114.43 1.33 -7.42 0.16
61028 -114.48 1.33 -7.64 0.16
61029 -114.51 1.19 -7.67 0.18
61030 -114.39 1.19 -7.49 0.18
61031 -114.09 1.24 -7.26 0.16
61032 -113.74 1.35 -7.13 0.06
61033 -113.52 1.35 -7.11 0.06
IERS Celestial Pole Offset Final Series
MJD dpsi deps
(msec. of arc)
60981 -117.6 -9.0
60982 -117.8 -8.8
60983 -118.0 -8.6
60984 -117.8 -8.6
60985 -117.5 -8.8
60986 -117.1 -8.9
60987 -116.8 -8.7
60988 -116.9 -8.5
60989 -117.0 -8.6
60990 -116.9 -8.8
60991 -116.6 -8.8
60992 -116.5 -8.5
60993 -116.5 -8.0
60994 -116.6 -7.7
60995 -116.6 -7.6
60996 -116.4 -7.5
60997 -116.1 -7.5
60998 -115.6 -7.8
60999 -115.5 -8.3
61000 -115.9 -8.6
61001 -116.0 -8.6
61002 -115.9 -8.5
61003 -115.7 -8.3
61004 -115.4 -8.2
61005 -115.1 -8.2
61006 -114.7 -8.1
61007 -114.4 -7.9
61008 -114.3 -7.9
61009 -114.6 -7.8
61010 -115.2 -7.6
IAU2000A Celestial Pole Offset Series
MJD dX error dY error
(msec. of arc)
61026 0.512 0.531 -0.092 0.161
61027 0.519 0.531 -0.101 0.161
61028 0.523 0.531 -0.107 0.161
61029 0.527 0.474 -0.112 0.176
61030 0.530 0.474 -0.114 0.176
61031 0.533 0.494 -0.114 0.156
61032 0.536 0.536 -0.113 0.059
61033 0.539 0.536 -0.111 0.059
IAU2000A Celestial Pole Offset Final Series
MJD dX dY
(msec. of arc)
60981 0.40 0.03
60982 0.38 0.02
60983 0.37 0.00
60984 0.42 -0.05
60985 0.41 -0.05
60986 0.34 -0.00
60987 0.33 0.01
60988 0.36 -0.01
60989 0.41 -0.04
60990 0.46 -0.06
60991 0.45 -0.05
60992 0.42 -0.02
60993 0.39 0.01
60994 0.33 0.07
60995 0.29 0.12
60996 0.30 0.14
60997 0.38 0.10
60998 0.59 -0.02
60999 0.62 -0.11
61000 0.51 -0.14
61001 0.41 -0.14
61002 0.35 -0.10
61003 0.32 -0.06
61004 0.33 -0.03
61005 0.38 -0.07
61006 0.47 -0.10
61007 0.57 -0.11
61008 0.57 -0.10
61009 0.48 -0.08
61010 0.35 -0.06
)--"; )--";

233
mcc/mcc_configfile.h Normal file
View File

@@ -0,0 +1,233 @@
#pragma once
/* MOUNT CONTRO: COMPONENTS */
#include <concepts>
#include <cstddef>
#include <filesystem>
#include <fstream>
#include <unordered_map>
#include <variant>
#include "mcc_traits.h"
#include "mcc_utils.h"
namespace mcc
{
/*
* A VERY SIMPLE CONFIG-FILE READER-PARSER
*
* format:
* key = value1, value2, ...
*
* a line beginning from '#' symbol is a comment and will be skipped
* a line containing from only ' ' is skipped
*/
class MccConfigFile
{
public:
static constexpr char COMMENT_SYMBOL = '#';
static constexpr char KEY_VALUE_DELIM = '=';
static constexpr char VALUE_VALUE_DELIM = ',';
typedef std::variant<std::monostate, std::string, std::vector<std::string>> config_value_t;
typedef std::unordered_map<std::string, config_value_t> config_t;
// typedef std::unordered_map<std::string, std::variant<std::monostate, std::string, std::vector<std::string>>>
// config_t;
MccConfigFile(const config_t& init_config = {})
: _currentFilename(), _currentConfig(init_config) {
};
MccConfigFile(std::derived_from<std::basic_istream<char>> auto const& stream, const config_t& init_config = {})
: MccConfigFile(init_config)
{
load(stream);
}
MccConfigFile(traits::mcc_input_char_range auto const& filename, const config_t& init_config = {})
: MccConfigFile(init_config)
{
load(filename);
}
config_value_t& operator[](traits::mcc_input_char_range auto&& key)
{
if constexpr (std::convertible_to<std::decay_t<decltype(key)>, typename config_t::key_type>) {
return _currentConfig[key];
} else {
typename config_t::key_type skey;
std::ranges::copy(std::forward<decltype(key)>(key), std::back_inserter(skey));
return _currentConfig[key];
}
}
const config_value_t& operator[](traits::mcc_input_char_range auto&& key) const
{
return const_cast<const MccConfigFile&>(*this).operator[](std::forward<decltype(key)>(key));
}
std::error_code load(std::derived_from<std::basic_istream<char>> auto& stream)
{
_currentConfig.clear();
// read line-by-line
for (std::string line; std::getline(stream, line);) {
if (line.empty()) {
continue;
}
auto sv = utils::trimSpaces(line, utils::TrimType::TRIM_LEFT);
if (sv.size()) {
if (sv[0] == COMMENT_SYMBOL) { // comment string
continue;
}
auto it = std::ranges::find(sv, KEY_VALUE_DELIM);
if (it == sv.begin()) { // empty key! skip!
continue;
}
auto key = utils::trimSpaces(std::string_view{sv.begin(), it}, utils::TrimType::TRIM_RIGHT);
std::string skey = {key.begin(), key.end()};
if (it == sv.end()) { // only key
_currentConfig[skey] = {};
} else { // key and value
auto vals = utils::trimSpaces(std::string_view{it + 1, sv.end()});
auto it_dlm = std::ranges::find(vals, VALUE_VALUE_DELIM);
if (it_dlm == vals.end()) { // scalar value
_currentConfig[skey] = std::string{vals.begin(), vals.end()};
} else { // vector of values
std::vector<std::string> elems;
// while (it_dlm != vals.end()) {
// auto el = utils::trimSpaces(std::string_view{vals.begin(), it_dlm});
// elems.emplace_back(el.begin(), el.end());
// vals = {it_dlm + 1, vals.end()};
// it_dlm = std::ranges::find(vals, VALUE_VALUE_DELIM);
// }
for (const auto& el :
std::views::split(vals, VALUE_VALUE_DELIM) |
std::views::transform([](const auto& sv) { return utils::trimSpaces(sv); })) {
elems.emplace_back(el.begin(), el.end());
}
_currentConfig[skey] = elems;
}
}
} else { // the string contains from only spaces
continue;
}
}
return {};
}
std::error_code load(traits::mcc_input_char_range auto const& filename)
{
std::filesystem::path pt(filename.begin(), filename.end());
// first, check file existence
std::error_code ec;
auto pt_cn = std::filesystem::canonical(pt, ec);
if (ec) {
return ec;
}
std::ifstream fin(pt_cn);
if (!fin.is_open()) {
return std::make_error_code(std::errc::no_such_file_or_directory);
// return std::make_error_code(std::errc::permission_denied);
}
_currentFilename = pt_cn.string();
load(fin);
fin.close();
return {};
}
std::error_code save() const
{
std::ofstream fst(_currentFilename, std::ios::trunc);
if (!fst.is_open()) {
return std::make_error_code(std::errc::no_such_file_or_directory);
}
const std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
fst << "#\n";
fst << "# Created by MccConfigFile class (" << std::put_time(std::localtime(&now), "[%F %T]") << ")\n";
fst << "#\n";
for (auto& [key, value] : _currentConfig) {
fst << key;
if (auto v_str = std::get_if<1>(&value)) {
fst << " = " << *v_str;
} else if (auto v_vec = std::get_if<2>(&value)) {
fst << " = " << (*v_vec)[0];
for (size_t i = 1; i < v_vec->size(); ++i) {
fst << ", " << (*v_vec)[i];
}
} // std::monostate here
fst << "\n";
}
fst.close();
return {};
}
std::error_code save(traits::mcc_input_char_range auto const& filename)
{
if (std::distance(filename.begin(), filename.end()) == 0) {
return std::make_error_code(std::errc::no_such_file_or_directory);
}
std::ranges::copy(filename, std::back_inserter(_currentFilename));
return save();
}
std::string currentFilename() const
{
return _currentFilename;
}
const config_t& config() const
{
return _currentConfig;
}
// config_t& config()
// {
// return _currentConfig;
// }
private:
std::string _currentFilename;
config_t _currentConfig;
};
} // namespace mcc

619
mcc/mcc_coord.h Normal file
View File

@@ -0,0 +1,619 @@
#pragma once
#include "mcc_angle.h"
#include "mcc_ccte_erfa_new.h"
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include <erfa.h>
namespace mcc
{
template <mcc_angle_c CO_LON_T, mcc_angle_c CO_LAT_T>
class MccCoordPair
{
public:
typedef CO_LON_T x_t;
typedef CO_LAT_T y_t;
static constexpr MccCoordPairKind pairKind =
!(std::derived_from<CO_LON_T, MccAngle> ||
std::derived_from<CO_LAT_T, MccAngle>) // unknown type (possibly just double or float)
? MccCoordPairKind::COORDS_KIND_GENERIC
: (std::same_as<CO_LON_T, MccAngle> || std::same_as<CO_LAT_T, MccAngle>) // one of the types is MccAngle
? MccCoordPairKind::COORDS_KIND_GENERIC
// ICRS RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_ICRS> && std::same_as<CO_LAT_T, MccAngleDEC_ICRS>)
? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
// apparent RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_APP> && std::same_as<CO_LAT_T, MccAngleDEC_APP>)
? MccCoordPairKind::COORDS_KIND_RADEC_APP
// observed RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_OBS> && std::same_as<CO_LAT_T, MccAngleDEC_OBS>)
? MccCoordPairKind::COORDS_KIND_RADEC_OBS
// apparent HA and DEC
: (std::same_as<CO_LON_T, MccAngleHA_APP> && std::same_as<CO_LAT_T, MccAngleDEC_APP>)
? MccCoordPairKind::COORDS_KIND_HADEC_APP
// observed HA and DEC
: (std::same_as<CO_LON_T, MccAngleHA_OBS> && std::same_as<CO_LAT_T, MccAngleDEC_OBS>)
? MccCoordPairKind::COORDS_KIND_HADEC_OBS
// apparent AZ and ZD
: (std::same_as<CO_LON_T, MccAngleAZ> && std::same_as<CO_LAT_T, MccAngleZD>)
? MccCoordPairKind::COORDS_KIND_AZZD
// apparent AZ and ALT
: (std::same_as<CO_LON_T, MccAngleAZ> && std::same_as<CO_LAT_T, MccAngleALT>)
? MccCoordPairKind::COORDS_KIND_AZALT
// general purpose X and Y
: (std::same_as<CO_LON_T, MccAngleX> && std::same_as<CO_LAT_T, MccAngleY>)
? MccCoordPairKind::COORDS_KIND_XY
// geographical longitude and latitude
: (std::same_as<CO_LON_T, MccAngleLON> && std::same_as<CO_LAT_T, MccAngleLAT>)
? MccCoordPairKind::COORDS_KIND_LONLAT
: MccCoordPairKind::COORDS_KIND_UNKNOWN;
template <mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
MccCoordPair(CO_LON_T const& x, CO_LAT_T const& y, EpT const& epoch = EpT::now()) : _x(x), _y(y), _epoch(epoch)
{
}
MccCoordPair(const MccCoordPair&) = default;
MccCoordPair(MccCoordPair&&) = default;
MccCoordPair& operator=(const MccCoordPair&) = default;
MccCoordPair& operator=(MccCoordPair&&) = default;
virtual ~MccCoordPair() = default;
CO_LON_T x() const
{
return _x;
}
CO_LAT_T y() const
{
return _y;
}
MccCelestialCoordEpoch epoch() const
{
return _epoch;
}
double MJD() const
{
return _epoch.MJD();
}
// for something like:
// auto [ra, dec, epoch] = coord_pair;
operator std::tuple<CO_LON_T, CO_LAT_T, MccCelestialCoordEpoch>() const
{
return {_x, _y, _epoch};
}
void setX(const CO_LON_T& x)
{
_x = x;
}
void setY(const CO_LAT_T& y)
{
_y = y;
}
void setEpoch(mcc_coord_epoch_c auto const& ep)
{
_epoch = ep;
}
protected:
CO_LON_T _x;
CO_LAT_T _y;
MccCelestialCoordEpoch _epoch;
};
template <typename T>
concept mcc_coord_pair_c = requires {
requires mcc_angle_c<typename T::x_t>;
requires mcc_angle_c<typename T::y_t>;
requires std::derived_from<T, MccCoordPair<typename T::x_t, typename T::y_t>>;
};
/* predefined coordinate pairs */
template <mcc_angle_c CO_LON_T, mcc_angle_c CO_LAT_T>
requires(std::derived_from<CO_LON_T, MccAngle> && std::derived_from<CO_LAT_T, MccAngle>)
class MccNamedCoordPair : public MccCoordPair<CO_LON_T, CO_LAT_T>
{
public:
MccNamedCoordPair() : MccCoordPair<CO_LON_T, CO_LAT_T>(CO_LON_T{0.0}, CO_LAT_T{0.0}, MccCelestialCoordEpoch::now())
{
}
template <typename CxT, typename CyT, mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
requires(std::is_arithmetic_v<CxT> && std::is_arithmetic_v<CyT>)
MccNamedCoordPair(CxT const& x, CyT const& y, EpT const& epoch = EpT::now())
: MccCoordPair<CO_LON_T, CO_LAT_T>(CO_LON_T{(double)x}, CO_LAT_T{(double)y}, epoch)
{
}
template <mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
MccNamedCoordPair(MccAngle const& x, MccAngle const& y, EpT const& epoch = EpT::now())
: MccCoordPair<CO_LON_T, CO_LAT_T>(CO_LON_T{(double)x}, CO_LAT_T{(double)y}, epoch)
{
}
MccNamedCoordPair(const MccNamedCoordPair&) = default;
MccNamedCoordPair(MccNamedCoordPair&&) = default;
MccNamedCoordPair& operator=(const MccNamedCoordPair&) = default;
MccNamedCoordPair& operator=(MccNamedCoordPair&&) = default;
virtual ~MccNamedCoordPair() = default;
};
struct MccSkyRADEC_ICRS : MccNamedCoordPair<MccAngleRA_ICRS, MccAngleDEC_ICRS> {
template <typename CxT, typename CyT>
requires(std::is_arithmetic_v<CxT> && std::is_arithmetic_v<CyT>)
MccSkyRADEC_ICRS(CxT const& x, CyT const& y)
: MccNamedCoordPair<MccAngleRA_ICRS, MccAngleDEC_ICRS>(x, y, MccCelestialCoordEpoch{})
{
}
MccSkyRADEC_ICRS(MccAngle const& x, MccAngle const& y) : MccSkyRADEC_ICRS((double)x, (double)y) {}
// ignore epoch setting (it is always J2000.0)
void setEpoch(mcc_coord_epoch_c auto const&)
{
static_assert(false, "CANNOT SET EPOCH FOR ICRS-KIND COORDINATE PAIR!!!");
}
};
struct MccSkyRADEC_APP : MccNamedCoordPair<MccAngleRA_APP, MccAngleDEC_APP> {
using MccNamedCoordPair<MccAngleRA_APP, MccAngleDEC_APP>::MccNamedCoordPair;
};
typedef MccNamedCoordPair<MccAngleRA_OBS, MccAngleDEC_OBS> MccSkyRADEC_OBS;
// struct MccSkyRADEC_OBS : MccNamedCoordPair<MccAngleRA_OBS, MccAngleDEC_OBS> {
// using MccNamedCoordPair<MccAngleRA_OBS, MccAngleDEC_OBS>::MccNamedCoordPair;
// };
struct MccSkyHADEC_APP : MccNamedCoordPair<MccAngleHA_APP, MccAngleDEC_APP> {
using MccNamedCoordPair<MccAngleHA_APP, MccAngleDEC_APP>::MccNamedCoordPair;
};
struct MccSkyHADEC_OBS : MccNamedCoordPair<MccAngleHA_OBS, MccAngleDEC_OBS> {
using MccNamedCoordPair<MccAngleHA_OBS, MccAngleDEC_OBS>::MccNamedCoordPair;
};
struct MccSkyAZZD : MccNamedCoordPair<MccAngleAZ, MccAngleZD> {
using MccNamedCoordPair<MccAngleAZ, MccAngleZD>::MccNamedCoordPair;
};
struct MccSkyAZALT : MccNamedCoordPair<MccAngleAZ, MccAngleALT> {
using MccNamedCoordPair<MccAngleAZ, MccAngleALT>::MccNamedCoordPair;
};
struct MccGenXY : MccNamedCoordPair<MccAngleX, MccAngleY> {
using MccNamedCoordPair<MccAngleX, MccAngleY>::MccNamedCoordPair;
};
struct MccGeoLONLAT : MccNamedCoordPair<MccAngleLON, MccAngleLAT> {
using MccNamedCoordPair<MccAngleLON, MccAngleLAT>::MccNamedCoordPair;
};
struct mcc_skypoint_interface_t {
virtual ~mcc_skypoint_interface_t() = default;
// template <std::derived_from<mcc_skypoint_interface_t> SelfT, mcc_angle_c XT, mcc_angle_c YT>
// SelfT& from(this SelfT&& self, XT&& x, YT&& y)
// {
// return std::forward<SelfT>(self).from(std::forward<XT>(x), std::forward<YT>(y));
// }
template <std::derived_from<mcc_skypoint_interface_t> SelfT, mcc_coord_pair_c PT>
auto from(this SelfT&& self, PT&& cpair)
{
return std::forward<SelfT>(self).from(std::forward<PT>(cpair));
}
template <std::derived_from<mcc_skypoint_interface_t> SelfT, mcc_coord_pair_c PT>
auto operator=(this SelfT&& self, PT&& cpair)
{
return std::forward<SelfT>(self).operator=(std::forward<PT>(cpair));
}
template <std::derived_from<mcc_skypoint_interface_t> SelfT, mcc_coord_pair_c PT, mcc_coord_pair_c... PTs>
auto to(this SelfT&& self, PT& cpair, PTs&... cpairs)
{
return std::forward<SelfT>(self).to(cpair, cpairs...);
}
template <std::derived_from<mcc_skypoint_interface_t> SelfT, mcc_coord_pair_c PT>
operator PT(this SelfT&& self)
{
return std::forward<SelfT>(self).operator PT();
}
};
template <typename T>
concept mcc_skypoint_c = std::derived_from<T, mcc_skypoint_interface_t> && requires(T t) {
typename T::meteo_t;
{ T::meteo(std::declval<typename T::meteo_t const&>()) };
};
/* MCC-LIBRARY DEFAULT GENERIC SKY POINT CLASS IMPLEMENTATION */
template <typename CCTE_T>
class MccGenericSkyPoint : public mcc_skypoint_interface_t
{
public:
typedef CCTE_T ccte_t;
static constexpr double MJD0 = 2400000.5;
inline static CCTE_T cctEngine{}; // celestial coordinates transformation engine
MccGenericSkyPoint() {}
template <mcc_coord_pair_c PT>
MccGenericSkyPoint(const PT& coord_pair) : MccGenericSkyPoint()
{
auto self = from(coord_pair);
}
MccGenericSkyPoint(const MccGenericSkyPoint&) = default;
MccGenericSkyPoint(MccGenericSkyPoint&&) = default;
MccGenericSkyPoint& operator=(const MccGenericSkyPoint&) = default;
MccGenericSkyPoint& operator=(MccGenericSkyPoint&&) = default;
virtual ~MccGenericSkyPoint() = default;
MccCelestialCoordEpoch epoch() const
{
return _epoch;
}
template <mcc_coord_pair_c PT>
MccGenericSkyPoint& from(const PT& coord_pair)
{
_x = coord_pair.x();
_y = coord_pair.y();
_pairKind = coord_pair.pairKind;
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
_epoch = MccCelestialCoordEpoch(); // J2000.0
} else {
_epoch.fromMJD(coord_pair.MJD());
}
return *this;
}
MccGenericSkyPoint& operator=(mcc_coord_pair_c auto const& coord_pair)
{
return from(coord_pair);
}
template <mcc_coord_pair_c PT, mcc_coord_pair_c... PTs>
auto to(PT& cpair, PTs&... cpairs) const
{
toHelper(cpair);
if constexpr (sizeof...(PTs)) {
to(cpairs...);
}
}
template <mcc_coord_pair_c PT>
operator PT()
{
PT res;
to(res);
return res;
}
protected:
double _x{0.0}, _y{0.0};
MccCoordPairKind _pairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
MccCelestialCoordEpoch _epoch{}; // J2000.0
template <mcc_coord_pair_c PT>
auto toHelper(PT& cpair) const
{
static constexpr double half_pi = std::numbers::pi / 2.0;
// HA, DEC to AZ, ALT (AZ from the South through the West)
auto hadec2azalt = [](double ha, double dec, double phi, double& az, double& alt) {
const auto cos_phi = std::cos(phi), sin_phi = std::sin(phi);
const auto cos_dec = std::cos(dec), sin_dec = std::sin(dec);
const auto cos_ha = std::cos(ha), sin_ha = std::sin(ha);
auto x = sin_phi * cos_dec * cos_ha - cos_phi * sin_dec;
auto y = -cos_dec * sin_ha;
auto z = cos_phi * cos_dec * cos_ha + sin_phi * sin_dec;
auto xx = x * x, yy = y * y;
decltype(x) r;
if (xx < yy) {
r = yy * sqrt(1.0 + xx / yy);
} else {
r = xx * sqrt(1.0 + yy / xx);
}
az = utils::isEqual(r, 0.0) ? 0.0 : std::atan2(y, x);
if (az < 0.0) {
az += std::numbers::pi * 2.0; // to range of [0, 2*PI]
}
alt = std::atan2(z, r);
};
// AZ, ALT to HA, DEC (AZ from the South through the West)
auto azalt2hadec = [](double az, double alt, double phi, double& ha, double& dec) {
const auto cos_phi = std::cos(phi), sin_phi = std::sin(phi);
const auto cos_az = std::cos(az), sin_az = std::sin(az);
const auto cos_alt = std::cos(alt), sin_alt = std::sin(alt);
auto x = sin_phi * cos_alt * cos_az + cos_phi * sin_alt;
auto y = cos_alt * sin_az;
auto z = -cos_phi * cos_alt * cos_az + sin_phi * sin_alt;
auto xx = x * x, yy = y * y;
decltype(x) r;
if (xx < yy) {
r = yy * sqrt(1.0 + xx / yy);
} else {
r = xx * sqrt(1.0 + yy / xx);
}
ha = utils::isEqual(r, 0.0) ? 0.0 : std::atan2(y, x);
dec = std::atan2(z, r);
};
typename CCTE_T::error_t ccte_err;
double phi = cctEngine.getStateERFA().lat;
double ra_icrs, dec_icrs, ra, dec, ha, az, zd, alt, lst, eo;
static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_GENERIC, "UNSUPPORTED SKY POINT TRANSFORMATION!");
static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_UNKNOWN, "UNSUPPORTED SKY POINT TRANSFORMATION!");
if (_pairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS &&
PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // from ICRS to ICRS - just copy and exit
cpair = PT(typename PT::x_t(_x), typename PT::y_t(_y));
return;
}
// just copy coordinates and exit
if (_pairKind == PT::pairKind && utils::isEqual(_epoch.MJD(), cpair.MJD())) {
// cpair = PT(typename PT::x_t(_x), typename PT::y_t(_y), _epoch);
cpair.setX(_x);
cpair.setY(_y);
return;
}
// if epochs are not the same then
// 1) convert stored coordinates to ICRS ones
// 2) convert from the computed ICRS coordinates to required ones
MccCoordPairKind pkind = _pairKind;
if (!utils::isEqual(_epoch.MJD(), cpair.MJD())) { // convert stored pair to ICRS one (ra_icrs, dec_icrs)
if (_pairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
pkind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
if (mcc_is_obs_coordpair(_pairKind)) {
ccte_err = cctEngine.obsToICRS(_pairKind, _epoch, _x, _y, &ra_icrs, &dec_icrs);
} else if (mcc_is_app_coordpair(_pairKind)) {
ccte_err = cctEngine.appToICRS(_pairKind, _epoch, _x, _y, &ra_icrs, &dec_icrs);
} else { // unsupported transformation!!!
return;
}
if (ccte_err) {
return;
}
} else {
ra_icrs = _x;
dec_icrs = _y;
}
}
// here, from APP or OBS to ICRS and exit
if (pkind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS &&
PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
cpair = PT(typename PT::x_t(ra_icrs), typename PT::y_t(dec_icrs));
return;
}
// here, the input coordinates and stored one are at the same epoch
ccte_err = cctEngine.equationOrigins(cpair.epoch(), &eo);
if (ccte_err) {
return;
}
ccte_err = cctEngine.apparentSideralTime(cpair.epoch(), &lst, true);
if (ccte_err) {
return;
}
if (pkind == MccCoordPairKind::COORDS_KIND_RADEC_APP || pkind == MccCoordPairKind::COORDS_KIND_RADEC_OBS) {
ra = _x;
dec = _y;
} else if (pkind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
pkind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) {
ha = _x;
dec = _y;
} else if (pkind == MccCoordPairKind::COORDS_KIND_AZZD) {
az = _x;
zd = _y;
} else if (pkind == MccCoordPairKind::COORDS_KIND_AZALT) {
az = _x;
alt = _y;
}
// else if (pkind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
// ra_icrs = _x;
// dec_icrs = _y;
// } else { // unsupported transformation!!!
// return;
// }
// coordinate transformation lambda (possibly recursive!!!)
auto comp_func = [&](this auto&& self, MccCoordPairKind cp_kind) -> void {
if (cp_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
if constexpr (mccIsAppCoordPairKind<PT::pairKind>) {
ccte_err = cctEngine.icrsToApp(ra_icrs, dec_icrs, cpair.epoch(), &ra, &dec, &ha, &az, &zd);
} else if constexpr (mccIsObsCoordPairKind<PT::pairKind>) {
ccte_err = cctEngine.icrsToObs(ra_icrs, dec_icrs, cpair.epoch(), &ra, &dec, &ha, &az, &zd);
} else {
static_assert(true, "UNSUPPORTED SKY POINT TRANSFORMATION!");
}
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_OBS) {
cpair.setX(ra);
cpair.setY(dec);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) {
cpair.setX(ha);
cpair.setY(dec);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_AZZD) {
cpair.setX(az);
cpair.setY(zd);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_AZALT) {
cpair.setX(az);
cpair.setY(half_pi - zd);
} else {
static_assert(true, "UNSUPPORTED SKY POINT TRANSFORMATION!");
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_AZZD) {
zd = half_pi - alt;
cpair.setX(az);
cpair.setY(zd);
} else {
if constexpr (mccIsAppCoordPairKind<PT::pairKind>) {
// correct for refraction: alt -= dz_refr
double dZ;
ccte_err = cctEngine.refractionCorrection(half_pi - alt, &dZ);
alt -= dZ;
}
azalt2hadec(az, alt, phi, ha, dec);
cpair.setY(dec);
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
ra = lst + eo - ha;
cpair.setX(ra);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_OBS) {
ra = lst + eo - ha;
cpair.setX(ra);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
cpair.setX(ha);
} else if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) {
cpair.setX(ha);
} else { // unsupported transformation!!!
return;
}
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
alt = half_pi - zd;
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_AZALT) {
cpair.setX(az);
cpair.setY(alt);
} else {
self(MccCoordPairKind::COORDS_KIND_AZALT);
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) {
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_OBS) {
ra = lst + eo - ha;
cpair.setX(ra);
cpair.setY(dec);
} else {
hadec2azalt(ha, dec, phi, az, alt);
if constexpr (mccIsAppCoordPairKind<PT::pairKind>) { // RADEC_APP, HADEC_APP
self(MccCoordPairKind::COORDS_KIND_AZALT);
} else { // AZALT, AZZD
cpair.setX(az);
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_AZZD) {
zd = half_pi - alt;
cpair.setY(zd);
} else {
cpair.setY(alt);
}
}
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
ra = lst + eo - ha;
cpair.setX(ra);
cpair.setY(dec);
} else {
hadec2azalt(ha, dec, phi, az, alt);
if constexpr (mccIsObsCoordPairKind<PT::pairKind>) { // RADEC_OBS, HADEC_OBS, AZALT, AZZD
// correct for refraction: alt += dz_refr
double dZ;
ccte_err = cctEngine.refractionReverseCorrection(half_pi - alt, &dZ);
alt += dZ;
self(MccCoordPairKind::COORDS_KIND_AZALT);
}
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_RADEC_OBS) {
ha = lst + eo - ra;
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) {
cpair.setX(ha);
cpair.setY(dec);
} else {
self(MccCoordPairKind::COORDS_KIND_HADEC_OBS);
}
} else if (cp_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
ha = lst + eo - ra;
if constexpr (PT::pairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
cpair.setX(ha);
cpair.setY(dec);
} else {
self(MccCoordPairKind::COORDS_KIND_HADEC_APP);
}
}
};
comp_func(pkind); // ran transformation
}
};
/* MCC-LIBRARY DEFAULT SKY POINT CLASS WITH ERFA-LIBRARY BASED ENGINE */
typedef MccGenericSkyPoint<ccte::erfa::MccCCTE_ERFA> MccSkyPoint;
} // end namespace mcc

File diff suppressed because it is too large Load Diff

View File

@@ -52,7 +52,7 @@ struct MccFiniteStateMachineCategory : public std::error_category {
const char* name() const noexcept const char* name() const noexcept
{ {
return "ADC_GENERIC_DEVICE"; return "MCC-FSM-ERROR-CATEGORY";
} }
std::string message(int ec) const std::string message(int ec) const
@@ -317,6 +317,7 @@ protected:
std::vector<std::function<void(const MccFiniteStateMachine*)>> _destroyFunc{}; std::vector<std::function<void(const MccFiniteStateMachine*)>> _destroyFunc{};
std::string_view _currentStateID; std::string_view _currentStateID;
std::string_view _previousStateID;
std::vector<std::string_view> _stateID{}; std::vector<std::string_view> _stateID{};
std::vector<std::string_view> _eventID{}; std::vector<std::string_view> _eventID{};
@@ -367,7 +368,7 @@ protected:
public: public:
template <traits::fsm_state_c InitStateT> template <traits::fsm_state_c InitStateT>
constexpr MccFiniteStateMachine(InitStateT) : _currentStateID(InitStateT::ID) constexpr MccFiniteStateMachine(InitStateT) : _currentStateID(InitStateT::ID), _previousStateID(InitStateT::ID)
{ {
using states_t = deduce_states_t<InitStateT>; using states_t = deduce_states_t<InitStateT>;
auto states = std::make_shared<states_t>(); auto states = std::make_shared<states_t>();
@@ -396,10 +397,6 @@ public:
[p_event, states, currentState, this]<traits::fsm_state_c curr_state_t>(curr_state_t*) { [p_event, states, currentState, this]<traits::fsm_state_c curr_state_t>(curr_state_t*) {
using to_state_t = curr_state_t::transition_t::template find_state_by_event_t<EvT>; using to_state_t = curr_state_t::transition_t::template find_state_by_event_t<EvT>;
if constexpr (std::holds_alternative<to_state_t>(*currentState)) {
// ?!!!! from self
}
if constexpr (!std::is_void_v<to_state_t>) { if constexpr (!std::is_void_v<to_state_t>) {
std::lock_guard lock(_transitionMutex); std::lock_guard lock(_transitionMutex);
@@ -421,6 +418,7 @@ public:
p_event->onTransit(); p_event->onTransit();
} }
_previousStateID = curr_state_t::ID;
*currentState = &std::get<to_state_t>(*states); *currentState = &std::get<to_state_t>(*states);
_currentStateID = to_state_t::ID; _currentStateID = to_state_t::ID;
@@ -529,6 +527,11 @@ public:
return _currentStateID; return _currentStateID;
} }
std::string_view previousStateID() const
{
return _previousStateID;
}
// returns IDs of all deduced unique states // returns IDs of all deduced unique states

File diff suppressed because it is too large Load Diff

View File

@@ -64,18 +64,10 @@ static consteval bool mccIsAltAzMount(const MccMountType type)
}; };
enum class MccProhibitedZonePolicy : int {
// enum MccCoordPairKind : size_t { PZ_POLICY_STOP, // stop mount near the zone
// COORDS_KIND_GENERIC, PZ_POLICY_FLIP // flip mount, e.g., near the meridian, near HA-axis encoder limit switch
// COORDS_KIND_RADEC_ICRS, };
// COORDS_KIND_RADEC_APP,
// COORDS_KIND_HADEC_APP,
// COORDS_KIND_AZZD,
// COORDS_KIND_AZALT,
// COORDS_KIND_XY,
// COORDS_KIND_LATLON
// };
/* GENERIC LOGGER CLASS CONCEPT */ /* GENERIC LOGGER CLASS CONCEPT */
@@ -86,6 +78,7 @@ concept mcc_logger_c = requires(T t, const T t_const) {
{ t.logDebug(std::declval<const std::string&>()) }; { t.logDebug(std::declval<const std::string&>()) };
{ t.logWarn(std::declval<const std::string&>()) }; { t.logWarn(std::declval<const std::string&>()) };
{ t.logInfo(std::declval<const std::string&>()) }; { t.logInfo(std::declval<const std::string&>()) };
{ t.logTrace(std::declval<const std::string&>()) };
}; };
@@ -94,6 +87,7 @@ struct MccNullLogger {
void logDebug(const std::string&) {} void logDebug(const std::string&) {}
void logWarn(const std::string&) {} void logWarn(const std::string&) {}
void logInfo(const std::string&) {} void logInfo(const std::string&) {}
void logTrace(const std::string&) {}
}; };
@@ -148,7 +142,8 @@ static constexpr void mcc_tp2tp(const T1& from_tp1, T2& to_tp)
/* JULIAN DAY CLASS CONCEPT */ /* JULIAN DAY CLASS CONCEPT */
template <typename T> template <typename T>
concept mcc_julday_c = mcc_fp_type_like_c<T> && requires(const T v) { concept mcc_julday_c = mcc_fp_type_like_c<T> || requires(const T v) {
// concept mcc_julday_c = mcc_fp_type_like_c<T> && requires(const T v) {
// modified Julian Day // modified Julian Day
{ v.MJD() } -> std::convertible_to<double>; { v.MJD() } -> std::convertible_to<double>;
// comparison operators // comparison operators
@@ -156,10 +151,60 @@ concept mcc_julday_c = mcc_fp_type_like_c<T> && requires(const T v) {
}; };
/* CELESTIAL COORDINATES EPOCH CLASS CONCEPT */
struct mcc_coord_epoch_interface_t {
static constexpr double MJD0 = 2400000.5;
template <std::derived_from<mcc_coord_epoch_interface_t> SelfT, traits::mcc_input_char_range IR>
bool fromCharRange(this SelfT&& self, IR&& str)
{
return std::forward<decltype(self)>(self).fromCharRange(std::forward<IR>(str));
}
template <std::derived_from<mcc_coord_epoch_interface_t> SelfT, typename ClockT, typename DurT>
bool fromTimePoint(this SelfT&& self, std::chrono::time_point<ClockT, DurT>&& tp)
{
return std::forward<decltype(self)>(self).fromTimePoint(std::forward<decltype(tp)>(tp));
}
template <std::derived_from<mcc_coord_epoch_interface_t> SelfT, typename VT>
bool fromMJD(this SelfT&& self, VT&& mjd)
requires std::is_arithmetic_v<VT>
{
return std::forward<decltype(self)>(self).fromMJD(std::forward<VT>(mjd));
}
template <std::derived_from<mcc_coord_epoch_interface_t> SelfT, traits::mcc_time_duration_c DT>
SelfT& operator+=(this SelfT& self, DT&& dt)
{
return std::forward<decltype(self)>(self).operator+=(std::forward<DT>(dt));
}
template <std::derived_from<mcc_coord_epoch_interface_t> SelfT, traits::mcc_time_duration_c DT>
SelfT& operator-=(this SelfT& self, DT&& dt)
{
return std::forward<decltype(self)>(self).operator-=(std::forward<DT>(dt));
}
};
template <typename T>
concept mcc_coord_epoch_c = std::derived_from<T, mcc_coord_epoch_interface_t> && requires(T t1, T t2, const T t_const) {
{ t_const.MJD() } -> std::convertible_to<double>;
{ t_const.UTC() } -> traits::mcc_systime_c;
{ t_const.JEpoch() } -> traits::mcc_output_char_range;
{ t1 <=> t2 };
{ T::now() } -> std::same_as<T>;
};
/* ERROR CLASS CONCEPT */ /* ERROR CLASS CONCEPT */
template <typename T> template <typename T>
concept mcc_error_c = std::default_initializable<T> && (std::convertible_to<T, bool> || requires(const T t) { concept mcc_error_c = std::default_initializable<T> && (std::convertible_to<T, bool> || requires(const T t) {
{ t.value() } -> std::formattable<char>;
{ t.operator bool() }; { t.operator bool() };
(bool)T() == false; // default constucted value must be a "non-error"! (bool)T() == false; // default constucted value must be a "non-error"!
}); });
@@ -175,6 +220,17 @@ static constexpr ErrT mcc_deduce_error(const DErrT& err, const ErrT& default_err
} }
} }
template <typename ErrT, typename DErrT>
static constexpr std::error_code mcc_deduce_error_code(const DErrT& err, const ErrT& default_err)
requires(std::is_error_code_enum_v<ErrT> || std::same_as<ErrT, std::error_code>)
{
if constexpr (std::is_error_code_enum_v<DErrT> || std::same_as<DErrT, std::error_code>) {
return err;
} else {
return default_err;
}
}
/* ATMOSPHERIC REFRACTION MODEL CLASS CONCEPT */ /* ATMOSPHERIC REFRACTION MODEL CLASS CONCEPT */
@@ -189,7 +245,8 @@ concept mcc_refract_model_c = requires(const T t_const) {
template <typename T> template <typename T>
concept mcc_celestial_point_c = requires(T t) { concept mcc_celestial_point_c = requires(T t) {
requires std::same_as<decltype(t.pair_kind), MccCoordPairKind>; // type of given coordinate pair requires std::same_as<std::remove_const_t<decltype(t.pair_kind)>,
MccCoordPairKind>; // type of given coordinate pair (it may be even static constexpr value)
requires mcc_time_point_c<decltype(t.time_point)>; // time point for given coordinates requires mcc_time_point_c<decltype(t.time_point)>; // time point for given coordinates
@@ -225,6 +282,9 @@ static constexpr void mcc_copy_celestial_point(mcc_celestial_point_c auto const&
template <typename T> template <typename T>
concept mcc_eqt_hrz_coord_c = mcc_celestial_point_c<T> && requires(T t) { concept mcc_eqt_hrz_coord_c = mcc_celestial_point_c<T> && requires(T t) {
requires mcc_angle_c<decltype(t.RA_ICRS)>; // ICRS right ascention
requires mcc_angle_c<decltype(t.DEC_ICRS)>; // ICRS declination
requires mcc_angle_c<decltype(t.RA_APP)>; // right ascension requires mcc_angle_c<decltype(t.RA_APP)>; // right ascension
requires mcc_angle_c<decltype(t.DEC_APP)>; // declination requires mcc_angle_c<decltype(t.DEC_APP)>; // declination
requires mcc_angle_c<decltype(t.HA)>; // hour angle requires mcc_angle_c<decltype(t.HA)>; // hour angle
@@ -255,6 +315,9 @@ static constexpr void mcc_copy_eqt_hrz_coord(mcc_eqt_hrz_coord_c auto const& fro
to_pt->X = (double)from_pt.X; to_pt->X = (double)from_pt.X;
to_pt->Y = (double)from_pt.Y; to_pt->Y = (double)from_pt.Y;
to_pt->RA_ICRS = (double)from_pt.RA_ICRS;
to_pt->DEC_ICRS = (double)from_pt.DEC_ICRS;
to_pt->RA_APP = (double)from_pt.RA_APP; to_pt->RA_APP = (double)from_pt.RA_APP;
to_pt->DEC_APP = (double)from_pt.DEC_APP; to_pt->DEC_APP = (double)from_pt.DEC_APP;
@@ -265,6 +328,14 @@ static constexpr void mcc_copy_eqt_hrz_coord(mcc_eqt_hrz_coord_c auto const& fro
} }
// nullptr_t or pointer to celestial/equatorial and horizontal coordinates class
template <typename T>
concept mcc_coord_pointer_or_nullptr =
std::is_null_pointer_v<T> ||
(std::is_pointer_v<std::decay_t<T>> && (mcc_celestial_point_c<std::remove_pointer_t<std::decay_t<T>>> ||
mcc_eqt_hrz_coord_c<std::remove_pointer_t<std::decay_t<T>>>));
/* CELESTIAL COORDINATES TRANSFORMATION ENGINE */ /* CELESTIAL COORDINATES TRANSFORMATION ENGINE */
@@ -291,6 +362,20 @@ struct mcc_CCTE_interface_t {
return std::forward<SelfT>(self).timepointToAppSideral(std::move(jd), st, islocal); return std::forward<SelfT>(self).timepointToAppSideral(std::move(jd), st, islocal);
} }
// EQUATION OF THE ORIGINS (ERA-GST)
template <std::derived_from<mcc_CCTE_interface_t> SelfT>
RetT equationOrigins(this SelfT&& self, mcc_time_point_c auto const& tp, mcc_angle_c auto* eo)
{
return std::forward<SelfT>(self).equationOrigins(std::move(tp), eo);
}
template <std::derived_from<mcc_CCTE_interface_t> SelfT>
RetT equationOrigins(this SelfT&& self, mcc_julday_c auto const& jd, mcc_angle_c auto* eo)
{
return std::forward<SelfT>(self).equationOrigins(std::move(jd), eo);
}
// NOTE: ASSUMING THE AZINUTH IS COUNTED FROM THE SOUTH THROUGH THE WEST!!! // NOTE: ASSUMING THE AZINUTH IS COUNTED FROM THE SOUTH THROUGH THE WEST!!!
template <std::derived_from<mcc_CCTE_interface_t> SelfT> template <std::derived_from<mcc_CCTE_interface_t> SelfT>
RetT transformCoordinates(this SelfT&& self, mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt) RetT transformCoordinates(this SelfT&& self, mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt)
@@ -424,27 +509,28 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
{ t_const.hardwareName() } -> std::formattable<char>; { t_const.hardwareName() } -> std::formattable<char>;
// the 'T' class must contain static constexpr member of 'MccMountType' type // the 'T' class must contain static constexpr member of 'MccMountType' type
requires std::same_as<decltype(T::mountType), const MccMountType>; // requires std::same_as<decltype(T::mountType), const MccMountType>;
[]() { // []() {
static constexpr MccMountType val = T::mountType; // static constexpr MccMountType val = T::mountType;
return val; // return val;
}(); // to ensure 'mountType' can be used in compile-time context // }(); // to ensure 'mountType' can be used in compile-time context
// a type that defines at least HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING // a type that defines at least HW_MOVE_ERROR, HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING
// and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a // and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a
// possible tunning of hardware hardwareSetState-related commands and detect stop-state // possible tunning of hardware hardwareSetState-related commands and detect stop end error states from hardware
// //
// e.g. an implementations can be as follows: // e.g. an implementations can be as follows:
// enum class hardware_moving_state_t: int {HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, // enum class hardware_moving_state_t: int {HW_MOVE_ERROR = -1, HW_MOVE_STOPPED = 0, HW_MOVE_SLEWING,
// HW_MOVE_TRACKING, HW_MOVE_GUIDING} // HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING}
// //
// struct hardware_moving_state_t { // struct hardware_moving_state_t {
// uint16_t HW_MOVE_STOPPED = 0; // static constexpr uint16_t HW_MOVE_STOPPED = 0;
// uint16_t HW_MOVE_SLEWING = 111; // static constexpr uint16_t HW_MOVE_SLEWING = 111;
// uint16_t HW_MOVE_ADJUSTING = 222; // static constexpr uint16_t HW_MOVE_ADJUSTING = 222;
// uint16_t HW_MOVE_TRACKING = 333; // static constexpr uint16_t HW_MOVE_TRACKING = 333;
// uint16_t HW_MOVE_GUIDING = 444; // static constexpr uint16_t HW_MOVE_GUIDING = 444;
// static constexpr uint16_t HW_MOVE_ERROR = 555;
// } // }
requires requires(typename T::hardware_moving_state_t type) { requires requires(typename T::hardware_moving_state_t type) {
[]() { []() {
@@ -464,6 +550,9 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
// hardware was asked for guiding // hardware was asked for guiding
// (small corrections to align actual mount position with target celestial point) // (small corrections to align actual mount position with target celestial point)
static constexpr auto v4 = T::hardware_moving_state_t::HW_MOVE_GUIDING; static constexpr auto v4 = T::hardware_moving_state_t::HW_MOVE_GUIDING;
// to detect possible hardware error
static constexpr auto v5 = T::hardware_moving_state_t::HW_MOVE_ERROR;
}(); }();
}; };
@@ -480,7 +569,7 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
requires mcc_angle_c<decltype(state.speedX)>; // moving speed along co-longitude coordinate requires mcc_angle_c<decltype(state.speedX)>; // moving speed along co-longitude coordinate
requires mcc_angle_c<decltype(state.speedY)>; // moving speed along co-latitude coordinate requires mcc_angle_c<decltype(state.speedY)>; // moving speed along co-latitude coordinate
requires std::same_as<typename T::hardware_moving_state_t, decltype(state.moving_type)>; requires std::same_as<typename T::hardware_moving_state_t, decltype(state.moving_state)>;
}; };
// set hardware state: // set hardware state:
@@ -498,66 +587,19 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
/* AUXILIARY COORDINATE-TRANSFORMATON CLASS CONCEPT */
// a concept of class that consist of the full set of coordinate transformation mount control components
// (celestial coordinate transformation engine, mount hardware encoders readings and pointing correction model)
// the set of methods of this class is enough to transform coordinates from ICRS to hardware and back
template <typename T>
concept mcc_coord_trfm_controls_c = mcc_ccte_c<T> && mcc_hardware_c<T> && mcc_PCM_c<T>;
/* MOUNT TELEMETRY DATA CLASS CONCEPT */ /* MOUNT TELEMETRY DATA CLASS CONCEPT */
template <typename T>
concept mcc_pointing_target_coord_c = mcc_eqt_hrz_coord_c<T> && requires(T t) {
requires mcc_angle_c<decltype(t.RA_ICRS)>; // ICRS right ascention
requires mcc_angle_c<decltype(t.DEC_ICRS)>; // ICRS declination
};
static constexpr void mcc_copy_pointing_target_coord(mcc_pointing_target_coord_c auto const& from_pt,
mcc_pointing_target_coord_c auto* to_pt)
{
if (to_pt == nullptr) {
return;
}
using from_pt_t = std::remove_cvref_t<decltype(from_pt)>;
using to_pt_t = std::remove_cvref_t<decltype(*to_pt)>;
if constexpr (std::derived_from<to_pt_t, from_pt_t> && std::copyable<to_pt_t>) {
*to_pt = from_pt;
return;
}
to_pt->pair_kind = from_pt.pair_kind;
to_pt->time_point =
std::chrono::time_point_cast<typename decltype(to_pt->time_point)::duration>(from_pt.time_point);
to_pt->X = (double)from_pt.X;
to_pt->Y = (double)from_pt.Y;
to_pt->RA_ICRS = (double)from_pt.RA_ICRS;
to_pt->DEC_ICRS = (double)from_pt.DEC_ICRS;
to_pt->RA_APP = (double)from_pt.RA_APP;
to_pt->DEC_APP = (double)from_pt.DEC_APP;
to_pt->HA = (double)from_pt.HA;
to_pt->AZ = (double)from_pt.AZ;
to_pt->ZD = (double)from_pt.ZD;
to_pt->ALT = (double)from_pt.ALT;
}
template <typename T> template <typename T>
concept mcc_telemetry_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializable<T> && requires(T t) { concept mcc_telemetry_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializable<T> && requires(T t) {
// user entered target coordinates
requires mcc_celestial_point_c<decltype(t.entered_target)>;
// target target coordinates // target target coordinates
requires mcc_pointing_target_coord_c<decltype(t.target)>; requires mcc_eqt_hrz_coord_c<decltype(t.target)>;
// t.X and t.Y (from mcc_celestial_point_c) are encoder coordinates // t.X and t.Y (from mcc_celestial_point_c) are encoder coordinates
// t.* from mcc_eqt_hrz_coord_c are apparent mount pointing coordinates // t.* from mcc_eqt_hrz_coord_c are current mount coordinates
requires mcc_angle_c<decltype(t.speedX)>; // speed along X from hardware encoder requires mcc_angle_c<decltype(t.speedX)>; // speed along X from hardware encoder
requires mcc_angle_c<decltype(t.speedY)>; // speed along Y from hardware encoder requires mcc_angle_c<decltype(t.speedY)>; // speed along Y from hardware encoder
@@ -567,6 +609,34 @@ concept mcc_telemetry_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializa
// atmospheric refraction correction for current zenithal distance // atmospheric refraction correction for current zenithal distance
requires mcc_angle_c<decltype(t.refCorr)>; // for current .ZD requires mcc_angle_c<decltype(t.refCorr)>; // for current .ZD
// equation of the origins (ERA-GST)
requires mcc_angle_c<decltype(t.EO)>;
};
// minimal library-wide telemetry data
template <typename T>
concept mcc_tlm_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializable<T> && requires(T t) {
// user entered target coordinates
requires mcc_celestial_point_c<decltype(t.entered_target)>;
// target target coordinates
requires mcc_eqt_hrz_coord_c<decltype(t.target)>;
// t.X and t.Y (from mcc_celestial_point_c) are encoder coordinates
// t.* from mcc_eqt_hrz_coord_c are current mount coordinates
requires mcc_PCM_result_c<decltype(t.pcm)>; // PCM correction
// atmospheric refraction correction for current zenithal distance
requires mcc_angle_c<decltype(t.refCorr)>; // for current .ZD
// equation of the origins (ERA-GST)
requires mcc_angle_c<decltype(t.EO)>;
// local sideral time
requires mcc_angle_c<decltype(t.LST)>;
}; };
@@ -594,6 +664,9 @@ static constexpr void mcc_copy_telemetry_data(mcc_telemetry_data_c auto const& f
to_pt->speedX = (double)from_pt.speedX; to_pt->speedX = (double)from_pt.speedX;
to_pt->speedY = (double)from_pt.speedY; to_pt->speedY = (double)from_pt.speedY;
to_pt->RA_ICRS = (double)from_pt.RA_ICRS;
to_pt->DEC_ICRS = (double)from_pt.DEC_ICRS;
to_pt->RA_APP = (double)from_pt.RA_APP; to_pt->RA_APP = (double)from_pt.RA_APP;
to_pt->DEC_APP = (double)from_pt.DEC_APP; to_pt->DEC_APP = (double)from_pt.DEC_APP;
@@ -607,7 +680,9 @@ static constexpr void mcc_copy_telemetry_data(mcc_telemetry_data_c auto const& f
to_pt->refCorr = (double)from_pt.refCorr; to_pt->refCorr = (double)from_pt.refCorr;
mcc_copy_pointing_target_coord(from_pt.target, &to_pt->target); mcc_copy_eqt_hrz_coord(from_pt.target, &to_pt->target);
mcc_copy_celestial_point(from_pt.entered_target, &to_pt->entered_target);
} }
@@ -654,7 +729,7 @@ struct mcc_telemetry_interface_t {
template <std::derived_from<mcc_telemetry_interface_t> SelfT> template <std::derived_from<mcc_telemetry_interface_t> SelfT>
RetT setPointingTarget(this SelfT&& self, mcc_celestial_point_c auto pt) RetT setPointingTarget(this SelfT&& self, mcc_celestial_point_c auto pt)
{ {
return std::forward<SelfT>(self).telemetryData(std::move(pt)); return std::forward<SelfT>(self).setPointingTarget(std::move(pt));
} }
@@ -683,6 +758,24 @@ template <typename T>
concept mcc_telemetry_c = std::derived_from<T, mcc_telemetry_interface_t<typename T::error_t>>; concept mcc_telemetry_c = std::derived_from<T, mcc_telemetry_interface_t<typename T::error_t>>;
// template <typename T, typename VT>
// concept mcc_retval_c = requires(T t) {
// //
// []<mcc_error_c ErrT>(std::expected<VT, ErrT>) {}(t);
// };
template <typename T>
concept mcc_telemetry_controls_c = requires(T t) {
requires mcc_telemetry_data_c<typename T::telemetry_data_t>;
{ t.telemetryData(std::declval<typename T::telemetry_data_t*>()) } -> mcc_error_c;
// { t.telemetryData() } -> mcc_retval_c<typename T::telemetry_data_t>;
{
t.setPointingTarget(std::declval<decltype(std::declval<typename T::telemetry_data_t>().entered_target)>())
} -> mcc_error_c;
};
/* PROHIBITED ZONE CLASS CONCEPT */ /* PROHIBITED ZONE CLASS CONCEPT */
@@ -745,12 +838,12 @@ concept mcc_prohibited_zone_c =
std::derived_from<T, mcc_pzone_interface_t<typename T::error_t>> && requires(const T t_const) { std::derived_from<T, mcc_pzone_interface_t<typename T::error_t>> && requires(const T t_const) {
{ t_const.name() } -> std::formattable<char>; { t_const.name() } -> std::formattable<char>;
requires requires(typename T::pzone_policy_t pp) { // the 'T' class must contain static constexpr member of 'MccMountType' type
[]() { requires std::same_as<decltype(T::pzPolicy), const MccProhibitedZonePolicy>;
static constexpr auto v1 = T::pzone_policy_t::STOP_POLICY; []() {
static constexpr auto v2 = T::pzone_policy_t::FLIP_POLICY; static constexpr MccProhibitedZonePolicy val = T::pzPolicy;
}(); return val;
}; }(); // to ensure 'pzPolicy' can be used in compile-time context
}; };
@@ -834,25 +927,36 @@ concept mcc_pzone_container_c = std::derived_from<T, mcc_pzone_container_interfa
template <typename T> template <typename T>
concept mcc_slewing_model_c = requires(T t) { concept mcc_slewing_model_c = requires(T t) {
requires mcc_error_c<typename T::error_t>; // requires mcc_error_c<typename T::error_t>;
// a class of slewing process parameters // a class of slewing process parameters
requires requires(typename T::slewing_params_t pars) { typename T::slewing_params_t;
// slew mount to target and stop // requires requires(typename T::slewing_params_t pars) {
requires std::convertible_to<decltype(pars.slewAndStop), bool>; // // slew mount to target and stop
}; // requires std::convertible_to<decltype(pars.slewAndStop), bool>;
// };
{ t.slewToTarget() } -> std::same_as<typename T::error_t>; // { t.slewToTarget() } -> std::same_as<typename T::error_t>;
{ t.stopSlewing() } -> std::same_as<typename T::error_t>; // { t.stopSlewing() } -> std::same_as<typename T::error_t>;
{ t.setSlewingParams(std::declval<typename T::slewing_params_t>()) } -> std::same_as<typename T::error_t>; // the method signature:
// slewToTarget(bool slew_and_stop)
// slew_and_stop == true, slew mount and stop
// slew_and_stop == false, slew mount and track
{ t.slewToTarget(std::declval<bool>()) } -> mcc_error_c;
{ t.stopSlewing() } -> mcc_error_c;
{ t.setSlewingParams(std::declval<typename T::slewing_params_t>()) } -> mcc_error_c;
// { t.setSlewingParams(std::declval<typename T::slewing_params_t>()) } -> std::same_as<typename T::error_t>;
{ t.getSlewingParams() } -> std::same_as<typename T::slewing_params_t>; { t.getSlewingParams() } -> std::same_as<typename T::slewing_params_t>;
// { t.slewingCurrentError() } -> mcc_error_c;
}; };
template <typename T> template <typename T>
concept mcc_tracking_model_c = requires(T t) { concept mcc_tracking_model_c = requires(T t) {
requires mcc_error_c<typename T::error_t>; // requires mcc_error_c<typename T::error_t>;
// a class of tracking process parameters // a class of tracking process parameters
requires requires(typename T::tracking_params_t pars) { requires requires(typename T::tracking_params_t pars) {
@@ -860,35 +964,55 @@ concept mcc_tracking_model_c = requires(T t) {
requires mcc_angle_c<decltype(pars.trackSpeedY)>; requires mcc_angle_c<decltype(pars.trackSpeedY)>;
}; };
{ t.trackTarget() } -> std::same_as<typename T::error_t>; // { t.trackTarget() } -> std::same_as<typename T::error_t>;
{ t.stopTracking() } -> std::same_as<typename T::error_t>; // { t.stopTracking() } -> std::same_as<typename T::error_t>;
{ t.trackTarget() } -> mcc_error_c;
{ t.stopTracking() } -> mcc_error_c;
{ t.setTrackingParams(std::declval<typename T::tracking_params_t>()) } -> std::same_as<typename T::error_t>; { t.setTrackingParams(std::declval<typename T::tracking_params_t>()) } -> mcc_error_c;
// { t.setTrackingParams(std::declval<typename T::tracking_params_t>()) } -> std::same_as<typename T::error_t>;
{ t.getTrackingParams() } -> std::same_as<typename T::tracking_params_t>; { t.getTrackingParams() } -> std::same_as<typename T::tracking_params_t>;
}; };
// template <typename T>
// concept mcc_guiding_model_c = requires(T t) {
// requires mcc_error_c<typename T::error_t>;
// // a class of guiding process parameters
// requires requires(typename T::guiding_params_t pars) {
// // guide along both mount axis
// requires std::convertible_to<decltype(pars.dualAxisGuiding), bool>;
// };
// { t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.setGuidingParams(std::declval<typename T::guiding_params_t>()) } -> std::same_as<typename T::error_t>;
// { t.getGuidingParams() } -> std::same_as<typename T::guiding_params_t>;
// };
// a concept of a class of mount moving control methods
template <typename T> template <typename T>
concept mcc_guiding_model_c = requires(T t) { concept mcc_moving_controls_c = requires(T t) {
requires mcc_error_c<typename T::error_t>; typename T::moving_params_t;
// a class of guiding process parameters { t.slewToTarget(std::declval<bool>()) } -> mcc_error_c;
requires requires(typename T::guiding_params_t pars) { { t.trackTarget() } -> mcc_error_c;
// guide along both mount axis
requires std::convertible_to<decltype(pars.dualAxisGuiding), bool>;
};
{ t.startGuidingTarget() } -> std::same_as<typename T::error_t>; { t.setMovingParams(std::declval<typename T::moving_params_t>()) } -> mcc_error_c;
{ t.stopGuidingTarget() } -> std::same_as<typename T::error_t>; { t.getMovingParams() } -> std::same_as<typename T::moving_params_t>;
{ t.setGuidingParams(std::declval<typename T::guiding_params_t>()) } -> std::same_as<typename T::error_t>; { t.stopMount() } -> mcc_error_c;
{ t.getGuidingParams() } -> std::same_as<typename T::guiding_params_t>;
}; };
/* GENERIC MOUNT CLASS CONCEPT */ /* GENERIC MOUNT CLASS CONCEPT */
// a class containing mount position related controls // a concept of class that consist of the full set of coordinate transformation mount control components
// (celestial coordinate transformation engine, mount hardware encoders readings and pointing correction model)
// the set of methods of this class is enough to transform coordinates from ICRS to hardware and back
template <typename T> template <typename T>
concept mcc_position_controls_c = mcc_ccte_c<T> && mcc_hardware_c<T> && mcc_PCM_c<T>; concept mcc_position_controls_c = mcc_ccte_c<T> && mcc_hardware_c<T> && mcc_PCM_c<T>;
@@ -897,26 +1021,59 @@ template <typename T>
concept mcc_all_controls_c = mcc_position_controls_c<T> && mcc_telemetry_c<T> && mcc_pzone_container_c<T>; concept mcc_all_controls_c = mcc_position_controls_c<T> && mcc_telemetry_c<T> && mcc_pzone_container_c<T>;
// generic mount:
// 1) telemetry-related methods
// 2) prohibited zones related methods
// 3) slewing and tracking, stop and init mount methods
template <typename T> template <typename T>
concept mcc_generic_mount_c = mcc_telemetry_c<T> && mcc_pzone_container_c<T> && requires(T t) { concept mcc_generic_mount_c = mcc_telemetry_c<T> && mcc_pzone_container_c<T> && requires(T t, const T t_const) {
requires mcc_error_c<typename T::error_t>; // slew mount to target (it is assumed that the target coordinates are determined in the telemetry data)
{ t.slewToTarget(std::declval<bool>()) };
// slew mount to target (target coordinates were defined in telemetry data)
{ t.slewToTarget() } -> std::same_as<typename T::error_t>;
// track target, i.e., the mount moves with celestial speed // track target, i.e., the mount moves with celestial speed
{ t.trackTarget() } -> std::same_as<typename T::error_t>; { t.trackTarget() };
{ t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
{ t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
// stop any movement // stop any movement
{ t.stopMount() } -> std::same_as<typename T::error_t>; { t.stopMount() };
// init mount // init mount
{ t.initMount() }; { t.initMount() };
//
// minimal set of mount status mnemonic constants
//
requires requires(typename T::mount_status_t type) {
requires std::formattable<
std::conditional_t<std::is_enum_v<typename T::mount_status_t>,
std::underlying_type_t<typename T::mount_status_t>, typename T::mount_status_t>,
char>;
[]() {
// mount initialization
static constexpr auto v1 = T::mount_status_t::INITIALIZATION;
// IDLE
static constexpr auto v2 = T::mount_status_t::IDLE;
// mount is in state after the stopMount() method invoking
static constexpr auto v3 = T::mount_status_t::STOPPED;
// mount is slewing (move to given celestial point)
static constexpr auto v4 = T::mount_status_t::SLEWING;
// mount is adjusting its position in the end of slewing
// (adjusting actual mount position to align with target celestial point at the end of slewing process)
static constexpr auto v5 = T::mount_status_t::ADJUSTING;
// mount is tracking
static constexpr auto v6 = T::mount_status_t::TRACKING;
// an error occured while mount operation
static constexpr auto v7 = T::mount_status_t::ERROR;
}();
};
{ t_const.mountStatus() } -> std::same_as<typename T::mount_status_t>;
}; };
@@ -932,6 +1089,4 @@ concept mcc_generic_fsm_mount_c = mcc_generic_mount_c<T> && std::derived_from<T,
template <typename T> template <typename T>
concept mcc_generic_fsm_log_mount_c = concept mcc_generic_fsm_log_mount_c =
mcc_generic_mount_c<T> && mcc_logger_c<T> && std::derived_from<T, fsm::MccFiniteStateMachine>; mcc_generic_mount_c<T> && mcc_logger_c<T> && std::derived_from<T, fsm::MccFiniteStateMachine>;
} // namespace mcc } // namespace mcc

1215
mcc/mcc_generics_new.h Normal file

File diff suppressed because it is too large Load Diff

View File

@@ -1,302 +0,0 @@
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* SIMPLE GUIDING MODEL IMPLEMENTATION */
#include "mcc_defaults.h"
#include "mcc_moving_model_common.h"
namespace mcc
{
enum class MccSimpleGuidingModelErrorCode : int {
ERROR_OK,
ERROR_CCTE,
ERROR_HW_GETSTATE,
ERROR_HW_SETSTATE,
ERROR_PCM_COMP,
ERROR_GET_TELEMETRY,
ERROR_DIST_TELEMETRY,
ERROR_DIFF_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_TIMEOUT,
ERROR_UNEXPECTED_AXIS_RATES,
ERROR_STOPPED
};
} // namespace mcc
namespace std
{
template <>
class is_error_code_enum<mcc::MccSimpleGuidingModelErrorCode> : public true_type
{
};
} // namespace std
namespace mcc
{
class MccSimpleGuidingModel
{
public:
typedef std::error_code error_t;
typedef MccSimpleMovingModelParams guiding_params_t;
template <mcc_all_controls_c CONTROLS_T>
MccSimpleGuidingModel(CONTROLS_T* controls)
: _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
{
_guidingFunc = [controls, this]() -> error_t {
typename CONTROLS_T::hardware_state_t hw_state;
MccTelemetryData tdata;
MccEqtHrzCoords intsc_coords;
MccCelestialPoint target_in_future_pt;
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
// double dist, dx, dy;
auto t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
}
bool no_intersects = false;
// function to update the closest prohibited zone intersect point
auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state,
this]() -> error_t {
// compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.HA)) {
intsc_coords.X = intsc_coords.HA;
intsc_coords.Y = intsc_coords.DEC_APP;
} else {
no_intersects = true;
// intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
// intsc_coords.Y = tdata.DEC_APP;
}
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.AZ)) {
intsc_coords.X = intsc_coords.AZ;
intsc_coords.Y = intsc_coords.ZD;
} else {
no_intersects = true;
}
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
// MccPCMResult pcm_inv_res;
// // endpoint of the mount moving
// auto pcm_err = controls->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
// if (pcm_err) {
// return mcc_deduce_error<error_t>(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
// }
// if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
// hw_state.speedX = _currentParams.trackSpeedX;
// hw_state.speedY = _currentParams.trackSpeedY;
// }
};
auto target_point = [&, this](MccCelestialPoint* point) {
auto dt = std::chrono::duration<double>{tdata.HA} +
_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
_currentParams.timeShiftToTargetPoint);
// point in +time_dist future
MccCelestialPoint pt{
.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP,
.X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize<MccAngle::NORM_KIND_0_360>(),
.Y = tdata.DEC_APP};
mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);
point->time_point = pt.time_point;
// check for prohibited zone
if (std::isfinite(intsc_coords.HA)) {
bool through_pzone =
(intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone
through_pzone &=
(intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone
if (through_pzone) {
pt.X = intsc_coords.HA;
}
}
auto ret = controls->transformCoordinates(std::move(pt), point);
if (ret) {
return mcc_deduce_error<error_t>(ret, MccSimpleGuidingModelErrorCode::ERROR_CCTE);
} else {
MccPCMResult pcm_inv_res;
// endpoint of the mount moving
auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
}
mcc_tp2tp(tdata.time_point, hw_state.time_point);
}
return MccSimpleGuidingModelErrorCode::ERROR_OK;
};
auto pz_err = update_pzones_ipoint();
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING;
{
std::lock_guard lock{*_currentParamsMutex};
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
return mcc_deduce_error(ccte_err, MccSimpleGuidingModelErrorCode::ERROR_CCTE);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
hw_state.speedX = _currentParams.trackSpeedX;
hw_state.speedY = _currentParams.trackSpeedY;
}
}
// move mount
auto hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
}
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
while (*_stopGuiding) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (*_stopGuiding) {
break;
}
// control prohibited zones
if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopGuiding) {
break;
}
{
std::lock_guard lock{*_currentParamsMutex};
auto now = std::chrono::steady_clock::now();
if ((now - last_corr_tp) < _currentParams.guidingMinInterval) {
continue;
}
// update prohibited zones intersection point
if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
pz_err = update_pzones_ipoint();
if (pz_err) {
return mcc_deduce_error<error_t>(
pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
}
// compute new target-in-future point
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
return mcc_deduce_error(ccte_err, MccSimpleGuidingModelErrorCode::ERROR_CCTE);
}
}
// send corrections
hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING;
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
}
}
return MccSimpleGuidingModelErrorCode::ERROR_OK;
};
}
MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default;
MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default;
MccSimpleGuidingModel(const MccSimpleGuidingModel&) = delete;
MccSimpleGuidingModel& operator=(const MccSimpleGuidingModel&) = delete;
error_t startGuidingTarget()
{
*_stopGuiding = false;
return _guidingFunc();
}
error_t stoptGuidingTarget()
{
*_stopGuiding = true;
return MccSimpleGuidingModelErrorCode::ERROR_OK;
}
protected:
std::function<error_t()> _guidingFunc{};
std::unique_ptr<std::atomic_bool> _stopGuiding;
guiding_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{};
};
} // namespace mcc

814
mcc/mcc_moving_controls.h Normal file
View File

@@ -0,0 +1,814 @@
#pragma once
#include <fstream>
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"
namespace mcc
{
enum class MccSimpleMovingControlsErrorCode : int {
ERROR_OK,
ERROR_HW_GETSTATE,
ERROR_HW_SETSTATE,
ERROR_PCM_COMP,
ERROR_GET_TELEMETRY,
ERROR_DIST_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP,
ERROR_TARGET_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_TIMEOUT,
ERROR_ALREADY_SLEW,
ERROR_ALREADY_STOPPED,
ERROR_STOPPED
};
} // namespace mcc
namespace std
{
template <>
class is_error_code_enum<mcc::MccSimpleMovingControlsErrorCode> : public true_type
{
};
} // namespace std
namespace mcc
{
// error category
struct MccSimpleMovingControlsCategory : public std::error_category {
MccSimpleMovingControlsCategory() : std::error_category() {}
const char* name() const noexcept
{
return "SIMPLE-SLEWING-MODEL";
}
std::string message(int ec) const
{
MccSimpleMovingControlsErrorCode err = static_cast<MccSimpleMovingControlsErrorCode>(ec);
switch (err) {
case MccSimpleMovingControlsErrorCode::ERROR_OK:
return "OK";
case MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE:
return "cannot get hardware state";
case MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE:
return "cannot set hardware state";
case MccSimpleMovingControlsErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
case MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY:
return "cannot get telemetry";
case MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY:
return "cannot get target-to-mount-position distance";
case MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP:
return "pzone container computation error";
case MccSimpleMovingControlsErrorCode::ERROR_TARGET_IN_PZONE:
return "target is in prohibited zone";
case MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE:
return "near prohibited zone";
case MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT:
return "a timeout occured while slewing";
case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_SLEW:
return "already slewing";
case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED:
return "slewing is already stopped";
case MccSimpleMovingControlsErrorCode::ERROR_STOPPED:
return "slewing was stopped";
default:
return "UNKNOWN";
}
}
static const MccSimpleMovingControlsCategory& get()
{
static const MccSimpleMovingControlsCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccSimpleMovingControlsCategory::get());
}
class MccSimpleMovingControls
{
static constexpr auto DEG90INRADS = std::numbers::pi / 2.0;
class PathFile
{
public:
PathFile(const std::string& filename = "") : _filename(filename), _st() {}
void setFilename(const std::string& filename)
{
_filename = filename;
}
std::string getFilename() const
{
return _filename;
}
~PathFile()
{
save();
}
friend PathFile& operator<<(PathFile& pf, auto&& v)
{
pf._st << std::forward<decltype(v)>(v);
return pf;
}
bool save()
{
std::fstream fst;
if (_filename.empty()) {
return false;
}
if (_st.str().empty()) { // nothing to save
return true;
}
fst.open(_filename);
if (!fst.is_open()) {
return false;
}
fst << _st.str();
_st.str("");
_filename.clear();
return true;
}
private:
std::string _filename;
std::istringstream _st;
};
public:
typedef std::error_code error_t;
typedef MccSimpleMovingModelParams moving_params_t;
enum Mode { MOVING_MODE_SLEW, MOVING_MODE_TRACK, MOVING_MODE_ERROR };
// typedef std::CallbackFuncTion<void(Mode mode)> mode_switch_callback_t;
// protected:
// constexpr static auto defaultModeSwitchCallback = [](Mode) {};
// public:
template <mcc_generic_mount_c MountT,
std::invocable<typename MountT::mount_status_t> CallbackFuncT =
decltype([](typename MountT::mount_status_t) {})>
MccSimpleMovingControls(
MountT* mount,
CallbackFuncT&& mode_switch_callback = [](typename MountT::mount_status_t) {})
: _stopMoving(new std::atomic_bool), _currentParamsMutex(new std::mutex), _lastError(new std::atomic<error_t>)
{
auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state) {
mount->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
auto hw_err = mount->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
}
mount->logDebug(" the 'hardwareSetState' method performed successfully!");
return MccSimpleMovingControlsErrorCode::ERROR_OK;
};
auto check_pzones = [mount, this](MccTelemetryData const& tdata, double min_time_to_pzone_in_secs,
double braking_accelX, double braking_accelY) {
bool in_zone;
std::vector<bool> in_zone_vec;
MccCelestialPoint cpt;
auto distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
mount->logTrace(
std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first),
mcc::MccAngleFancyString(distXY.second)));
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
// and check them for getting into the prohibited zones
if constexpr (mccIsEquatorialMount(MountT::mountType)) {
cpt.X = tdata.HA + distXY.first;
cpt.Y = tdata.DEC_APP + distXY.second;
if (cpt.Y > DEG90INRADS) {
cpt.Y = DEG90INRADS;
}
if (cpt.Y < -DEG90INRADS) {
cpt.Y = -DEG90INRADS;
}
} else if constexpr (mccIsAltAzMount(MountT::mountType)) {
cpt.X = tdata.AZ + distXY.first;
cpt.Y = tdata.ZD + distXY.second;
if (cpt.Y < 0.0) {
cpt.Y = 0.0;
}
if (cpt.Y > std::numbers::pi) {
cpt.Y = std::numbers::pi;
}
}
mcc_tp2tp(tdata.time_point, cpt.time_point);
mount->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s",
mcc::MccAngleFancyString(tdata.speedX),
mcc::MccAngleFancyString(tdata.speedY)));
in_zone_vec.clear();
auto pz_err = mount->inPZone(cpt, &in_zone, &in_zone_vec);
if (pz_err) {
return mcc_deduce_error_code(pz_err, MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
size_t i = 0;
for (; i < in_zone_vec.size(); ++i) {
if (in_zone_vec[i]) {
break;
}
}
mount->logError("target point is near prohibited zone (zone index: {})! Entered target coordinates:",
i);
mount->logError(std::format(
" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
mount->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal(),
mcc::MccAngle{tdata.ALT}.sexagesimal()));
mount->logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(),
mcc::MccAngle{tdata.Y}.sexagesimal()));
return MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE;
}
return MccSimpleMovingControlsErrorCode::ERROR_OK;
};
auto log_pos = [mount, this](typename MountT::hardware_state_t const& hw_state, MccTelemetryData const& tdata) {
if constexpr (mccIsEquatorialMount(MountT::mountType)) {
mount->logTrace(std::format(" current target: HA = {}, DEC = {}",
mcc::MccAngle(tdata.target.HA).sexagesimal(true),
mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
mount->logTrace(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle(tdata.HA).sexagesimal(true),
mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
_pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.HA << " "
<< tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP << " "
<< (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
<< (int)hw_state.moving_state << "\n";
} else if constexpr (mccIsAltAzMount(MountT::mountType)) {
mount->logTrace(std::format(" target: AZ = {}, ZD = {}",
mcc::MccAngle(tdata.target.AZ).sexagesimal(),
mcc::MccAngle(tdata.target.ZD).sexagesimal()));
mount->logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
mcc::MccAngle(tdata.ZD).sexagesimal()));
_pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.AZ << " "
<< tdata.target.ZD << " " << tdata.AZ << " " << tdata.ZD << " "
<< (tdata.target.AZ - tdata.AZ) << " " << (tdata.target.ZD - tdata.ZD) << " "
<< (int)hw_state.moving_state << "\n";
}
};
*_stopMoving = true;
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;
using cb_func_t = std::function<void(typename MountT::mount_status_t)>;
auto cb_sptr = std::shared_ptr<cb_func_t>(new cb_func_t(std::forward<CallbackFuncT>(mode_switch_callback)));
/* stop moving function */
_stopMovingFunc = [mount, this]() {
typename MountT::hardware_state_t hw_state;
hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED;
*_stopMoving = true;
*_lastError = send_to_hardware(hw_state);
};
/* slewing function */
_slewingFunc = [mount, cb_sptr, send_to_hardware, check_pzones, log_pos, this](bool slew_and_stop) {
double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs;
{
// std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
braking_accelX = std::numeric_limits<double>::min();
} else {
braking_accelX = std::abs(_currentParams.brakingAccelX);
}
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
braking_accelY = std::numeric_limits<double>::min();
} else {
braking_accelY = std::abs(_currentParams.brakingAccelY);
}
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file
_pathFile.setFilename(_currentParams.slewingPathFilename);
} else {
mount->logError("Slewing path filename is empty! Do not save it!");
}
}
mount->logInfo(
std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK")));
mount->logInfo(std::format(" slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
if (!slew_and_stop) {
mount->logInfo(std::format(" slewing tolerance radius: {} arcsecs",
mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
}
mount->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
mount->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
mount->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
_pathFile << "# \n";
_pathFile << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
_pathFile << "# Config:\n";
_pathFile << "# slewing tolerance radius: "
<< mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs() << " arcsecs\n";
_pathFile << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
_pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n";
typename MountT::error_t t_err;
MccTelemetryData tdata;
{
std::lock_guard lock{*_currentParamsMutex};
t_err = mount->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
return;
}
}
auto last_hw_time = tdata.time_point;
// bool in_zone;
// std::vector<bool> in_zone_vec;
// MccCelestialPoint cpt;
// if constexpr (mccIsEquatorialMount(MountT::mountType)) {
// cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
// } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
// cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
// } else {
// static_assert(false, "UNKNOWN MOUNT TYPE!");
// }
typename MountT::hardware_state_t hw_state;
auto hw_err = mount->hardwareGetState(&hw_state);
if (hw_err) {
*_stopMoving = true;
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
return;
}
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
{
std::lock_guard lock{*_currentParamsMutex};
hw_state.speedX = _currentParams.slewRateX;
hw_state.speedY = _currentParams.slewRateY;
}
hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;
// start slewing ...
error_t err = send_to_hardware(hw_state);
if (err) {
*_lastError = err;
mount->logError(std::format("start slewing: an error occured while sending hardware state: {} {} {}",
err.value(), err.category().name(), err.message()));
return;
}
*cb_sptr(MountT::mount_status_t::SLEWING); // send the status to the mount
double dist;
std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
start_slewing_tp = std::chrono::steady_clock::now();
last_adjust_tp = start_slewing_tp;
std::pair<double, double> distXY;
bool tag_var_coord = true;
if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
tag_var_coord = false;
}
auto start_point = tdata.time_point; // needed for trajectory file
// main loop (simply monitors the current position taking into account the prohibited zones, as well as the
// timeout of the entire process)
while (!*_stopMoving) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
*_lastError =
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
break;
}
last_hw_time = tdata.time_point;
}
hw_err = mount->hardwareGetState(&hw_state);
if (hw_err) {
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
break;
}
log_pos(hw_state, tdata);
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
break;
}
err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (err) {
*_lastError = err;
break;
}
{
std::lock_guard lock{*_currentParamsMutex};
if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
mount->logError("slewing process timeout!");
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT;
break;
}
}
if (slew_and_stop && !tag_var_coord) { // just wait for mount to be stopped
if (hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED) {
mount->logInfo("mount moving state is STOPPED - exit!");
break;
}
} else {
if (last_hw_time == tdata.time_point) {
mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
continue;
}
last_hw_time = tdata.time_point;
t_err = mount->targetToMountDist(&dist);
if (t_err) {
*_lastError =
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
break;
}
mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
if ((dist <= _currentParams.slewToleranceRadius) &&
(hw_state.moving_state ==
MountT::hardware_moving_state_t::HW_MOVE_GUIDING)) { // stop slewing and exit from
// cycle
mount->logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");
if (slew_and_stop) {
stopMount();
}
break;
}
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
break;
// return MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
}
// resend new position since target coordinates are changed in time
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
err = send_to_hardware(hw_state);
if (err) {
*_lastError = err;
break;
}
}
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
break;
}
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
}
*_stopMoving = true;
mount->logInfo("Slewing finished");
err = *_lastError;
mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
_pathFile.save();
// get final position
if (!err) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
*_lastError =
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
return;
}
}
t_err = mount->targetToMountDist(&dist);
if (t_err) {
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
return;
}
log_pos(hw_state, tdata);
mount->logDebug(std::format(" target-to-mount distance {}", mcc::MccAngleFancyString(dist)));
if (!slew_and_stop) { // start tracking
_trackingFunc();
} else {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;
}
}
};
/* tracking function */
_trackingFunc = [mount, cb_sptr, check_pzones, send_to_hardware, log_pos, this]() {
double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs;
{
// std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
braking_accelX = std::numeric_limits<double>::min();
} else {
braking_accelX = std::abs(_currentParams.brakingAccelX);
}
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
braking_accelY = std::numeric_limits<double>::min();
} else {
braking_accelY = std::abs(_currentParams.brakingAccelY);
}
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.trackingPathFilename.empty()) { // open slewing trajectory file
_pathFile.setFilename(_currentParams.trackingPathFilename);
} else {
mount->logError("Tracking path filename is empty! Do not save it!");
}
}
mount->logInfo("Start tracking");
mount->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
mount->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
mount->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
_pathFile << "# \n";
_pathFile << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n";
_pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n";
typename MountT::hardware_state_t hw_state;
error_t err;
MccTelemetryData tdata;
double dist;
auto last_hw_time = tdata.time_point;
*cb_sptr(MountT::mount_status_t::TRACKING); // send the status to the mount
while (!*_stopMoving) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
auto t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
*_lastError =
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
break;
}
last_hw_time = tdata.time_point;
}
auto hw_err = mount->hardwareGetState(&hw_state);
if (hw_err) {
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
break;
}
log_pos(hw_state, tdata);
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
break;
}
err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (err) {
*_lastError = err;
break;
}
if (last_hw_time == tdata.time_point) {
mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
continue;
}
last_hw_time = tdata.time_point;
auto t_err = mount->targetToMountDist(&dist);
if (t_err) {
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
break;
}
mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
// resend new position since target coordinates are changed in time
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
err = send_to_hardware(hw_state);
if (err) {
*_lastError = err;
break;
}
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
break;
}
// sleep here
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
}
*_stopMoving = true;
mount->logInfo("Tracking finished");
err = *_lastError;
mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
_pathFile.save();
};
}
virtual ~MccSimpleMovingControls()
{
*_stopMoving = true;
}
error_t slewToTarget(bool slew_and_stop = false)
{
return *_lastError;
}
error_t trackTarget()
{
return *_lastError;
}
error_t stopMount()
{
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED;
} else {
_stopMovingFunc();
}
return *_lastError;
}
error_t setMovingParams(moving_params_t params)
{
std::lock_guard lock{*_currentParamsMutex};
_currentParams = std::move(params);
return MccSimpleMovingControlsErrorCode::ERROR_OK;
}
moving_params_t getMovingParams() const
{
std::lock_guard lock{*_currentParamsMutex};
return _currentParams;
}
error_t mountMovingLastError() const
{
return *_lastError;
}
protected:
std::function<void(bool)> _slewingFunc{};
std::function<void()> _trackingFunc{};
std::function<void()> _stopMovingFunc{};
std::unique_ptr<std::atomic_bool> _stopMoving;
std::unique_ptr<std::mutex> _currentParamsMutex;
moving_params_t _currentParams{};
std::unique_ptr<std::atomic<error_t>> _lastError;
PathFile _pathFile{};
};
} // namespace mcc

View File

@@ -21,7 +21,7 @@ struct MccSimpleMovingModelParams {
static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second
// timeout to telemetry updating // timeout to telemetry updating
std::chrono::seconds telemetryTimeout{3}; std::chrono::milliseconds telemetryTimeout{3000};
// minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
std::chrono::seconds minTimeToPZone{10}; std::chrono::seconds minTimeToPZone{10};
@@ -37,28 +37,51 @@ struct MccSimpleMovingModelParams {
// coordinates difference to stop slewing (in radians) // coordinates difference to stop slewing (in radians)
double slewToleranceRadius{5.0_arcsecs}; double slewToleranceRadius{5.0_arcsecs};
// telemetry request interval
std::chrono::milliseconds slewingTelemetryInterval{100};
// target-mount coordinate difference to start adjusting of slewing (in radians) // target-mount coordinate difference to start adjusting of slewing (in radians)
double adjustCoordDiff{slewToleranceRadius * 10.0}; double adjustCoordDiff{slewToleranceRadius * 10.0};
// slew process timeout // slew process timeout
std::chrono::seconds slewTimeout{3600}; std::chrono::seconds slewTimeout{3600};
double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) double slewRateX{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) double slewRateY{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments
double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// braking acceleration after execution of mount stopping command (in rads/s^2)
// it must be given as non-negative value!!!
double brakingAccelX{0.0};
double brakingAccelY{0.0};
// slewing trajectory file. if empty - just skip saving
std::string slewingPathFilename{};
// ******* tracking mode ******* // ******* tracking mode *******
// telemetry request interval
std::chrono::milliseconds trackingTelemetryInterval{100};
double trackSpeedX{}; double trackSpeedX{};
double trackSpeedY{}; double trackSpeedY{};
std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections
bool dualAxisTracking{true}; // mount must be of an equatorial type: false means guiding along only HA-axis
// time shift into future to compute target position in future (UT1-scale time duration) // time shift into future to compute target position in future (UT1-scale time duration)
std::chrono::duration<double> timeShiftToTargetPoint{10.0}; std::chrono::milliseconds timeShiftToTargetPoint{10000};
// maximal target-to-mount difference for tracking process (in arcsecs)
// it it is greater then the current mount coordinates are used as target one
double trackingMaxCoordDiff{20.0};
// tracking trajectory file. if empty - just skip saving
std::string trackingPathFilename{};
// ******* guiding mode ******* // ******* guiding mode *******
@@ -68,9 +91,52 @@ struct MccSimpleMovingModelParams {
}; };
// calculate the distances along the X and Y axes that the mount will travel at the current speed in a given time,
// taking into account the braking acceleration
//
// WARNING:
// It is assumed that the given braking accelerations are non-negative,
// while speeds may have a sign according to motion direction.
// The latter means that returned distances can be negative!
std::pair<double, double> mcc_compute_distance(mcc_telemetry_data_c auto const& tdata,
double time_in_secs,
mcc_angle_c auto const& braking_accelX,
mcc_angle_c auto const& braking_accelY)
{
std::pair<double, double> res;
// first, check if the mount stops before time_in_secs
//
// the time to stop mount: (V_ini - a*t) = 0 => t = V_ini/a
//
// the traveled path: s = V_ini*t - a*t*t/2, V_ini - initial speed, a - braking accel, t - the time
//
// time to stop mount with given current speed and braking acceleration
double tx_stop = std::abs(tdata.speedX) / braking_accelX;
double ty_stop = std::abs(tdata.speedY) / braking_accelY;
double tx = time_in_secs;
double ty = time_in_secs;
if (std::isfinite(tx_stop) && (time_in_secs > tx_stop)) {
tx = tx_stop;
}
if (std::isfinite(ty_stop) && (time_in_secs > ty_stop)) {
ty = ty_stop;
}
// here, one must take into account the sign of the speed!!!
res.first = tdata.speedX * tx - std::copysign(braking_accelX, tdata.speedX) * tx * tx / 2.0;
res.second = tdata.speedY * ty - std::copysign(braking_accelY, tdata.speedY) * ty * ty / 2.0;
return res;
}
template <mcc_pzone_container_c PZoneContT> template <mcc_pzone_container_c PZoneContT>
bool mcc_is_near_pzones(PZoneContT* pz_cont, bool mcc_is_near_pzones(PZoneContT* pz_cont,
mcc_telemetry_c auto const& tdata, mcc_telemetry_data_c auto const& tdata,
traits::mcc_time_duration_c auto const& min_timeto, traits::mcc_time_duration_c auto const& min_timeto,
typename PZoneContT::error_t& err) typename PZoneContT::error_t& err)
{ {
@@ -95,7 +161,7 @@ bool mcc_is_near_pzones(PZoneContT* pz_cont,
template <mcc_pzone_container_c PZoneContT> template <mcc_pzone_container_c PZoneContT>
typename PZoneContT::error_t mcc_find_closest_pzone(PZoneContT* pz_cont, typename PZoneContT::error_t mcc_find_closest_pzone(PZoneContT* pz_cont,
mcc_telemetry_c auto const& tdata, mcc_telemetry_data_c auto const& tdata,
mcc_eqt_hrz_coord_c auto* closest_coords) mcc_eqt_hrz_coord_c auto* closest_coords)
{ {
using res_t = std::decay_t<decltype(*closest_coords)>; using res_t = std::decay_t<decltype(*closest_coords)>;

1245
mcc/mcc_netserver.h Normal file

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,512 @@
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* NETWORK SERVER ENDPOINT CLASS IMPLEMENTATION */
#include <algorithm>
#include <array>
#include <charconv>
#include <cstdint>
#include <filesystem>
#include <ranges>
#include <string_view>
#include "mcc_traits.h"
namespace mcc::network
{
namespace utils
{
static constexpr bool mcc_char_range_compare(const traits::mcc_char_view auto& what,
const 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 "path" 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 MccNetServerEndpoint
{
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 <traits::mcc_input_char_range R>
MccNetServerEndpoint(const R& ept)
{
fromRange(ept);
}
MccNetServerEndpoint(const MccNetServerEndpoint& other)
{
copyInst(other);
}
MccNetServerEndpoint(MccNetServerEndpoint&& other)
{
moveInst(std::move(other));
}
virtual ~MccNetServerEndpoint() = default;
MccNetServerEndpoint& operator=(const MccNetServerEndpoint& other)
{
copyInst(other);
return *this;
}
MccNetServerEndpoint& operator=(MccNetServerEndpoint&& other)
{
moveInst(std::move(other));
return *this;
}
template <traits::mcc_input_char_range R>
requires std::ranges::contiguous_range<R>
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<std::remove_cvref_t<R>>) {
_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::mcc_char_range_compare(_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 <traits::mcc_view_or_output_char_range R>
R proto() const
{
return part<R>(PROTO_PART);
}
std::string_view proto() const
{
return proto<std::string_view>();
}
template <traits::mcc_view_or_output_char_range R>
R host() const
{
return part<R>(HOST_PART);
}
std::string_view host() const
{
return host<std::string_view>();
}
int port() const
{
return _port;
}
template <traits::mcc_view_or_output_char_range R>
R portView() const
{
return part<R>(PORT_PART);
}
std::string_view portView() const
{
return portView<std::string_view>();
}
template <traits::mcc_output_char_range R, traits::mcc_input_char_range RR = std::string_view>
R path(RR&& root_path) const
{
if (_path.empty()) {
if constexpr (traits::mcc_output_char_range<R>) {
R res;
std::ranges::copy(std::forward<RR>(root_path), std::back_inserter(res));
return res;
} else { // can't add root path!!!
return part<R>(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<R>(PATH_PART);
}
}
template <traits::mcc_input_char_range RR = std::string_view>
std::string path(RR&& root_path) const
{
return path<std::string, RR>(std::forward<RR>(root_path));
}
template <traits::mcc_view_or_output_char_range R>
R path() const
{
return part<R>(PATH_PART);
}
std::string_view path() const
{
return path<std::string_view>();
}
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 <typename T = std::nullptr_t>
MccNetServerEndpoint& makeAbstract(const T& mark = nullptr)
requires(traits::mcc_input_char_range<T> || std::same_as<std::remove_cv_t<T>, char> ||
std::is_null_pointer_v<std::remove_cv_t<T>>)
{
if (!(isLocalStream() || isLocalSeqpacket())) { // only local proto is valid!
return *this;
}
if constexpr (std::is_null_pointer_v<T>) { // 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<std::remove_cv_t<T>, 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(MccNetServerEndpoint::validProtoMarks, [&idx, &proto_mark](const auto& el) {
bool ok = utils::mcc_char_range_compare(proto_mark, el, true);
if (!ok) {
++idx;
}
return ok;
});
return found ? idx : -1;
}
enum EndpointPart { PROTO_PART, HOST_PART, PATH_PART, PORT_PART };
template <traits::mcc_view_or_output_char_range R>
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<R>) {
return {part.begin(), part.end()};
} else {
std::ranges::copy(part, std::back_inserter(res));
}
return res;
}
void copyInst(const MccNetServerEndpoint& other)
{
if (&other != this) {
if (other._isValid) {
_isValid = other._isValid;
_endpoint = other._endpoint;
_proto = other._proto;
std::iterator_traits<const char*>::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(MccNetServerEndpoint&& 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 mcc::network

821
mcc/mcc_netserver_proto.h Normal file
View File

@@ -0,0 +1,821 @@
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* BASIC NETWORK PROTOCOL DEFINITIONS */
#include <algorithm>
#include <string_view>
#include "mcc_angle.h"
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_utils.h"
namespace mcc::network
{
/*
* The network protocol is the ASCII-based, case-sensitive textual protocol.
* The "client-server" communication is performed through messages.
* The message is a minimal unit of this communication.
* The model of network communication is a simple "client-server" one, i.e.,
* client asks - server responds.
*
* network communication message format:
* <keyword>[[<key-param-delim>]<param1>[<param-param-delim>][<param2>]...]<stop-seq>
*
* where
* <keyword> - mandatory message keyword (one or more ASCII symbols)
* <key-param-delim>
*
* e.g.
* "TARGET 12:23:45.56 00:32:21.978\n"
*/
/* low-level network message format definitions */
static constexpr std::string_view MCC_COMMPROTO_STOP_SEQ = "\n";
static constexpr std::string_view MCC_COMMPROTO_KEYPARAM_DELIM_SEQ = " ";
static constexpr std::string_view MCC_COMMPROTO_PARAMPARAM_DELIM_SEQ = ";";
static constexpr std::string_view MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ = ",";
/* server special keywords */
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR = "ACK"; // ACK
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR = "ERROR"; // mount operational error
// pre-defined errors
static constexpr std::string_view MCC_COMMPROTO_SERVER_ERROR_INVKEY_STR = "INVKEY"; // invalid keyword
static constexpr std::string_view MCC_COMMPROTO_SERVER_ERROR_INVPAR_STR = "INVPAR"; // invalid parameter
/* server control keywords */
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_RESTART_SERVER_STR = "RESTART"; // restart server
/* BELOW IS ONE OF THE PROTOCOL OPTIONS CORRESPONDING MCC_GENERIC_MOUNT_C CONCEPT */
/* predefined parameters */
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_RADEC_ICRS = "RADEC_ICRS"; // ICRS RA and DEC
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_RADEC = "RADEC"; // apparent RA and DEC
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_HADEC = "HADEC"; // apparent HA and DEC
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_AZZD = "AZZD"; // azimuth and zenithal distance
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_AZALT = "AZALT"; // azimuth and altitude
static constexpr std::string_view MCC_COMMPROTO_COORD_KIND_XY = "XY"; // hardware (encoder) coordinates
// static constexpr MccCoordPairKind mcc_str2pairkind(std::string_view spair)
// {
// return spair == MCC_COMMPROTO_COORD_KIND_RADEC_ICRS ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
// : spair == MCC_COMMPROTO_COORD_KIND_RADEC ? MccCoordPairKind::COORDS_KIND_RADEC_APP
// : spair == MCC_COMMPROTO_COORD_KIND_HADEC ? MccCoordPairKind::COORDS_KIND_HADEC_APP
// : spair == MCC_COMMPROTO_COORD_KIND_AZZD ? MccCoordPairKind::COORDS_KIND_AZZD
// : spair == MCC_COMMPROTO_COORD_KIND_AZALT ? MccCoordPairKind::COORDS_KIND_AZALT
// : spair == MCC_COMMPROTO_COORD_KIND_XY ? MccCoordPairKind::COORDS_KIND_XY
// : MccCoordPairKind::COORDS_KIND_GENERIC;
// }
template <mcc::traits::mcc_char_range R>
static constexpr MccCoordPairKind mcc_str2pairkind(R&& spair)
{
if constexpr (std::is_pointer_v<std::decay_t<R>>) {
return mcc_str2pairkind(std::string_view{spair});
}
const auto hash = mcc::utils::FNV1aHash(std::forward<R>(spair));
return hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_RADEC_ICRS) ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
: hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_RADEC) ? MccCoordPairKind::COORDS_KIND_RADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_HADEC) ? MccCoordPairKind::COORDS_KIND_HADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_AZZD) ? MccCoordPairKind::COORDS_KIND_AZZD
: hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_AZALT) ? MccCoordPairKind::COORDS_KIND_AZALT
: hash == mcc::utils::FNV1aHash(MCC_COMMPROTO_COORD_KIND_XY) ? MccCoordPairKind::COORDS_KIND_XY
: MccCoordPairKind::COORDS_KIND_GENERIC;
}
static constexpr std::string_view mcc_pairkind2str(MccCoordPairKind kind)
{
return kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COMMPROTO_COORD_KIND_RADEC_ICRS
: kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COMMPROTO_COORD_KIND_RADEC
: kind == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COMMPROTO_COORD_KIND_HADEC
: kind == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COMMPROTO_COORD_KIND_AZZD
: kind == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COMMPROTO_COORD_KIND_AZALT
: kind == MccCoordPairKind::COORDS_KIND_XY ? MCC_COMMPROTO_COORD_KIND_XY
: "UNKNOWN";
}
/* keywords */
// NOTE: THE COORDINATES AND TIME-RELATED QUANTITIES CAN BE EXPRESSED IN THE TWO FORMATS:
// 1) fixed-point real number, e.g. 123.43987537359 or -0.09775
// 2) sexagesimal number, e.g. 10:43:43.12 or -123:54:12.435
//
// IN THE FIRST CASE ALL NUMBERS MUST BE INTERPRETATED AS DEGREES,
// IN THE SECOND CASE NUMBERS MUST BE INTERPRETATED ACCORDING TO ITS TYPE:
// ALL TIME-RELATED QUANTITIES AND RA/HA COORDINATES MUST BE EXPRESSED
// IN FORMAT 'HOURS:MINUTES:SECONDS', WHILE DEC/ALT/AZ/ZD COORDINATES MUST
// BE EXPRESSED AS '+/-DEGREES:ARCMINUTES:ARCSECONDS'
//
// USER-ENTERED (FROM NETWORK CLIENTS) COORDINATE PAIR CAN BE PROVIDED IN A MIXED FORM, I.E.,
// 12.34436658678 10:32:11.432 or
// 10:32:11.432 12.34436658678
//
// SERVER-RESPONDED COORDINATES ARE ALWAYS IN THE SAME FORMAT, SEXAGESIMAL OR FIXED-POINT
//
// format of output coordinates:
// "COORDFMT FMT-type\n"
// e.g.:
// "COORDFMT SGM\n"
// "COORDFMT\n"
//
// server must return "ACK" or "ERROR INVPAR" in the case of 'set'-operation and
// "ACK COORDFMT FMT-type" in the case of 'get'-operation
// e.g.:
// "COORDFMT FIX\n" -> "ACK\n"
// "COORDFMT SXT\n" -> "ERROR INVPAR\n" (invalid parameter of format type)
// "COORDFMT\n" -> "ACK COORDFMT FIX\n"
//
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_COORDFMT_STR = "COORDFMT";
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_COORDFMT_SEXGM_STR = "SGM"; // sexagesimal
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_COORDFMT_FIXED_STR = "FIX"; // fixed point
// precision (number of decimal places) of returned coordinates:
// "COORDPREC seconds-prec arcseconds-prec\n"
// seconds-prec - precision of hour-based coordinates (RA and HA) or time-related quantities
// arcseconds-prec - precision of degree-based coordinates (DEC, AZ, ZD, ALT)
// precision must be given as non-negative integer number
// e.g.
// "COORDPREC 2,1\n" (output sexagesimal RA=12:34:56.67, DEC=32:54:21.9)
//
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_COORDPREC_STR = "COORDPREC";
// set/get target coordinates
// "TARGET X-coord Y-coord XY-kind\n", if 'XY-kind' is omitted then one should assume RADEC_ICRS
// e.g.:
// "TARGET 12.7683487 10:23:09.75 AZZD\n"
// "TARGET HADEC\n"
// "TARGET\n"
//
// server must return "ACK" or "ERROR INVPAR" in the case of 'set'-operation and
// "ACK TARGET X-coord Y-coord XY-kind" in the case of 'get'-operation
// e.g.:
// "TARGET 12.7683487 10:23:09.75 AZZD\n" -> "ACK\n"
// "TARGET 12.7683487 10:23:09.75 AZZE\n" -> "ERROR INVPAR\n" (invalid parameter of coordinates pair kind)
//
// "TARGET HADEC\n" -> "ACK TARGET 20:21:56.32 -01:32:34.2 HADEC\n"
// "TARGET\n" -> "ACK TARGET 20:21:56.32 -01:32:34.2 RADEC_ICRS\n"
//
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TARGET_STR = "TARGET";
// get mount coordinates:
// "MOUNT coord-kind", if 'coord-kind' is omitted then coordinates are according to mount type,
// i.e., HADEC for equathorial-type mount and AZZD for alt-azimuthal one
// e.g.:
// "MOUNT RADEC\n" (get current apparent RA and DEC mount coordinates)
//
// server must return "ACK MOUNT X-coord Y-coord XY-kind" or "ERROR INVPAR"
// e.g.
// "MOUNT AZALT\n" -> "ACK MOUNT 1.2332325 54.23321312 AZALT\n"
// "MOUNT AZAL\n" -> "ERROR INVPAR\n" (invalid parameter of coordinates pair kind)
// "MOUNT\n" -> "ACK MOUNT 1.2332325 54.23321312 AZZD\n" for alt-azimuthal mount
// "MOUNT\n" -> "ACK MOUNT 1.2332325 54.23321312 HADEC\n" for equathorial mount
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_MOUNT_STR = "MOUNT";
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TELEMETRY_STR = "TELEMETRY";
// init mount
// "INIT\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_INIT_STR = "INIT";
// stop any movements
// "STOP\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_STOP_STR = "STOP";
// slew mount and track target:
// "SLEW\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_SLEW_STR = "SLEW";
// slew mount and stop:
// "MOVE\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_MOVE_STR = "MOVE";
// track target
// "TRACK\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TRACK_STR = "TRACK";
// get mount status
// "STATUS\n"
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_STATUS_STR = "STATUS";
// valid keywords
static constexpr std::array MCC_COMMPROTO_VALID_KEYS = {
MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, MCC_COMMPROTO_KEYWORD_COORDFMT_STR,
MCC_COMMPROTO_KEYWORD_COORDPREC_STR, MCC_COMMPROTO_KEYWORD_TARGET_STR, MCC_COMMPROTO_KEYWORD_MOUNT_STR,
MCC_COMMPROTO_KEYWORD_TELEMETRY_STR, MCC_COMMPROTO_KEYWORD_INIT_STR, MCC_COMMPROTO_KEYWORD_STOP_STR,
MCC_COMMPROTO_KEYWORD_SLEW_STR, MCC_COMMPROTO_KEYWORD_MOVE_STR, MCC_COMMPROTO_KEYWORD_TRACK_STR,
MCC_COMMPROTO_KEYWORD_STATUS_STR};
// hashes of valid keywords
static constexpr std::array MCC_COMMPROTO_VALID_KEYS_HASH = []<size_t... Is>(std::index_sequence<Is...>) {
return std::array{mcc::utils::FNV1aHash(MCC_COMMPROTO_VALID_KEYS[Is])...};
}(std::make_index_sequence<MCC_COMMPROTO_VALID_KEYS.size()>());
template <typename T>
concept mcc_netmsg_valid_keys_c = requires(T t) {
// std::array of valid message keywords
[]<size_t N>(std::array<std::string_view, N>) {
// to ensure T::NETMSG_VALID_KEYWORDS can be used as compile-time constant
static constexpr auto v0 = T::NETMSG_VALID_KEYWORDS[0];
return v0;
}(T::NETMSG_VALID_KEYWORDS);
// std::array of valid message keywords hashes
[]<size_t N>(std::array<size_t, N>) {
// to ensure T::NETMSG_VALID_KEYWORD_HASHES can be used as compile-time constant
static constexpr auto v0 = T::NETMSG_VALID_KEYWORD_HASHES[0];
return v0;
}(T::NETMSG_VALID_KEYWORD_HASHES);
requires T::NETMSG_VALID_KEYWORDS.size() == T::NETMSG_VALID_KEYWORD_HASHES.size();
};
struct MccNetMessageValidKeywords {
static constexpr std::array NETMSG_VALID_KEYWORDS = {
MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR,
MCC_COMMPROTO_KEYWORD_COORDFMT_STR, MCC_COMMPROTO_KEYWORD_COORDPREC_STR,
MCC_COMMPROTO_KEYWORD_TARGET_STR, MCC_COMMPROTO_KEYWORD_MOUNT_STR,
MCC_COMMPROTO_KEYWORD_TELEMETRY_STR, MCC_COMMPROTO_KEYWORD_INIT_STR,
MCC_COMMPROTO_KEYWORD_STOP_STR, MCC_COMMPROTO_KEYWORD_SLEW_STR,
MCC_COMMPROTO_KEYWORD_MOVE_STR, MCC_COMMPROTO_KEYWORD_TRACK_STR,
MCC_COMMPROTO_KEYWORD_STATUS_STR};
// hashes of valid keywords
static constexpr std::array NETMSG_VALID_KEYWORD_HASHES = []<size_t... Is>(std::index_sequence<Is...>) {
return std::array{mcc::utils::FNV1aHash(NETMSG_VALID_KEYWORDS[Is])...};
}(std::make_index_sequence<NETMSG_VALID_KEYWORDS.size()>());
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;
}
};
static_assert(mcc_netmsg_valid_keys_c<MccNetMessageValidKeywords>, "");
template <typename T>
concept mcc_netmessage_c = requires(T t) { T(); };
template <mcc::traits::mcc_char_range BYTEREPR_T = std::string_view,
mcc_netmsg_valid_keys_c BASE_T = MccNetMessageValidKeywords>
class MccNetMessage
{
protected:
class DefaultDeserializer : protected mcc::utils::MccSimpleDeserializer
{
protected:
using base_t = mcc::utils::MccSimpleDeserializer;
inline static mcc::MccCelestialPointDeserializer _cpDeserializer{MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ};
inline static mcc::MccEqtHrzCoordsDeserializer _eqhrDeserializer{MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ};
inline static mcc::MccTelemetryDataDeserializer _telemetryDeserializer{MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ};
public:
DefaultDeserializer() : base_t(MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ) {}
template <traits::mcc_input_char_range IR, typename VT>
std::error_code operator()(IR&& bytes, VT& value) const
{
if constexpr (mcc_telemetry_data_c<VT>) {
return _telemetryDeserializer(std::forward<IR>(bytes), value);
} else if constexpr (mcc_eqt_hrz_coord_c<VT>) {
return _eqhrDeserializer(std::forward<IR>(bytes), value);
} else if constexpr (mcc_celestial_point_c<VT>) {
return _cpDeserializer(std::forward<IR>(bytes), value);
} else if constexpr (std::same_as<VT, MccCoordPairKind>) {
value = MccCoordStrToPairKind(bytes);
if (value == MccCoordPairKind::COORDS_KIND_UNKNOWN) {
// return std::make_error_code(std::errc::invalid_argument);
return MccCoordinateConvErrorCode::ERROR_INVALID_CPAIR;
}
} else if constexpr (std::same_as<VT, MccCoordinateSerializer::SerializedCoordFormat>) {
std::string v;
auto ec = (*this)(std::forward<IR>(bytes), v);
if (ec) {
return ec;
}
if (v.compare(MCC_COMMPROTO_KEYWORD_COORDFMT_SEXGM_STR) == 0) {
value = MccCoordinateSerializer::SerializedCoordFormat::CFMT_SGM;
} else if (v.compare(MCC_COMMPROTO_KEYWORD_COORDFMT_FIXED_STR) == 0) {
value = MccCoordinateSerializer::SerializedCoordFormat::CFMT_DEGREES;
} else {
// return std::make_error_code(std::errc::invalid_argument);
return MccCoordinateConvErrorCode::ERROR_INVALID_COORD_FMT;
}
} else if constexpr (std::same_as<VT, MccCoordinateSerializer::SexagesimalCoordPrec>) {
std::vector<int64_t> v;
auto ec = (*this)(std::forward<IR>(bytes), v);
if (ec) {
// return ec;
return MccCoordinateConvErrorCode::ERROR_INVALID_COORD_PREC;
}
auto hprec = v[0];
value.hour_prec = hprec > 0 ? (hprec < std::numeric_limits<decltype(value.hour_prec)>::max()
? hprec
: std::numeric_limits<decltype(value.hour_prec)>::max())
: 2;
if (v.size() == 1) {
value.deg_prec = 1;
} else {
auto dprec = v[1];
value.deg_prec = dprec > 0 ? dprec < std::numeric_limits<decltype(value.deg_prec)>::max()
? dprec
: std::numeric_limits<decltype(value.deg_prec)>::max()
: 1;
}
} else {
return base_t::operator()(std::forward<IR>(bytes), value);
}
return {};
}
};
class DefaultSerializer
{
friend class MccNetMessage;
MccCoordinateSerializer::SerializedCoordFormat _coordFmt{};
MccCoordinateSerializer::SexagesimalCoordPrec _coordPrec{};
public:
template <typename T, traits::mcc_output_char_range OR>
void operator()(const T& value, OR& bytes)
{
if constexpr (std::is_arithmetic_v<T>) {
std::format_to(std::back_inserter(bytes), "{}", value);
} else if constexpr (std::convertible_to<T, std::string>) {
std::ranges::copy(static_cast<std::string>(value), std::back_inserter(bytes));
} else if constexpr (std::constructible_from<std::string, T>) {
std::ranges::copy(std::string(value), std::back_inserter(bytes));
} else if constexpr (traits::mcc_char_range<T>) {
std::ranges::copy(std::string(value.begin(), value.end()), std::back_inserter(bytes));
// } else if constexpr (std::same_as<T, MccCoordPairKind>) {
// std::ranges::copy(mcc_pairkind2str(value), std::back_inserter(bytes));
} else if constexpr (traits::mcc_time_duration_c<T>) {
(*this)(value.count(), bytes);
} else if constexpr (mcc_telemetry_data_c<T>) {
static MccTelemetryDataSerializer sr;
sr.setDelimiter(MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ);
sr.setFormat(_coordFmt);
sr.setPrecision(_coordPrec);
sr(value, bytes);
} else if constexpr (mcc_eqt_hrz_coord_c<T>) {
static MccEqtHrzCoordsSerializer sr;
sr.setDelimiter(MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ);
sr.setFormat(_coordFmt);
sr.setPrecision(_coordPrec);
sr(value, bytes);
} else if constexpr (mcc_celestial_point_c<T>) {
static MccCelestialPointSerializer sr;
sr.setDelimiter(MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ);
sr.setFormat(_coordFmt);
sr.setPrecision(_coordPrec);
sr(value, bytes);
} else if constexpr (std::ranges::range<T>) {
auto sz = std::ranges::size(value);
if (sz == 0) {
return;
}
(*this)(*value.begin(), bytes); // the first element
if (sz > 1) {
for (auto const& el : value | std::views::drop(1)) {
std::ranges::copy(MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ, std::back_inserter(bytes));
(*this)(el, bytes);
}
}
} else if constexpr (std::same_as<T, std::error_code>) {
std::format_to(std::back_inserter(bytes), "{}{}{}{}{}", value.value(),
MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ, value.message(), MCC_COMMPROTO_RANGEPARAM_DELIM_SEQ,
value.category().name());
} else if constexpr (std::formattable<T, char>) {
std::format_to(std::back_inserter(bytes), "{}", value);
} else {
static_assert(false, "UNSUPPORTED TYPE!!!");
}
}
};
public:
typedef BASE_T valid_keys_t;
typedef BYTEREPR_T byte_repr_t;
enum MccNetMessageError { ERROR_OK, ERROR_EMPTY_MESSAGE, ERROR_INVALID_KEYWORD, ERROR_EMPTY_KEYWORD };
MccNetMessage() = default;
template <traits::mcc_input_char_range KT, typename... PTs>
MccNetMessage(KT&& key, PTs&&... params)
requires traits::mcc_output_char_range<BYTEREPR_T>
{
construct(_defaultSerializer, std::forward<KT>(key), std::forward<PTs>(params)...);
}
template <traits::mcc_input_char_range R>
constexpr MccNetMessage(const R& msg)
requires traits::mcc_input_char_range<BYTEREPR_T>
{
fromCharRange(msg);
}
// constexpr MccNetMessage(const BYTEREPR_T& msg)
// requires traits::mcc_input_char_range<BYTEREPR_T>
// {
// fromCharRange(msg);
// }
virtual ~MccNetMessage() = default;
template <traits::mcc_input_char_range KT>
constexpr bool withKey(const KT& key) const
{
if constexpr (std::is_pointer_v<std::decay_t<KT>>) {
return withKey(std::string_view{key});
}
return mcc::utils::FNV1aHash(key) == _keywordHash;
}
template <traits::mcc_view_or_output_char_range R>
R keyword() const
{
if constexpr (traits::mcc_char_view<R>) {
return R{_keyword.begin(), _keyword.end()};
} else {
R r;
std::ranges::copy(_keyword, std::back_inserter(r));
return r;
}
}
std::string_view keyword() const
{
return _keyword;
}
size_t paramSize() const
{
return _params.size();
}
template <std::ranges::range R>
R params(size_t start_idx = 0, size_t Nelemes = std::numeric_limits<size_t>::max()) const
requires(traits::mcc_view_or_output_char_range<R> || traits::mcc_range_of_char_range<R>)
{
if (start_idx >= _params.size()) {
return R{};
}
auto stop_idx = start_idx + Nelemes - 1;
if (stop_idx >= _params.size()) {
stop_idx = _params.size() - 1;
}
if constexpr (traits::mcc_range_of_char_range<R>) { // returm parameters as array
using el_t = std::ranges::range_value_t<R>;
R r;
if constexpr (traits::mcc_char_view<el_t> || traits::mcc_output_char_range<el_t>) {
for (size_t i = start_idx; i <= stop_idx; ++i) {
auto& el = _params[i];
std::back_inserter(r) = el_t{el.begin(), el.end()};
}
} else {
static_assert(false, "UNSUPPORTED RANGE TYPE!!!");
}
return r;
} else {
if constexpr (traits::mcc_char_view<R>) { // return joined parameters as a single char-range
return R{_params[start_idx].begin(), _params[stop_idx].end()};
} else {
R r;
std::ranges::copy(std::string_view{_params[start_idx].begin(), _params[stop_idx].end()},
std::back_inserter(r));
return r;
}
}
}
std::string_view params(size_t start_idx = 0, size_t Nelemes = std::numeric_limits<size_t>::max()) const
{
return params<std::string_view>(start_idx, Nelemes);
}
template <traits::mcc_view_or_output_char_range R>
R param(size_t idx) const
{
if (idx >= _params.size()) {
return {};
}
if constexpr (traits::mcc_char_view<R>) {
return R{_params[idx].begin(), _params[idx].end()};
} else {
R r;
std::ranges::copy(_params[idx], std::back_inserter(r));
return r;
}
}
std::string_view param(size_t idx) const
{
if (idx >= _params.size()) {
return {};
}
return _params[idx];
}
template <typename T, typename DeserFuncT>
std::expected<T, std::error_code> paramValue(size_t idx, DeserFuncT&& deser_func) const
{
if (idx >= _params.size()) {
return std::unexpected{std::make_error_code(std::errc::value_too_large)};
}
T val;
auto ec = std::forward<DeserFuncT>(deser_func)(_params[idx], val);
if (ec) {
return std::unexpected(ec);
} else {
return val;
}
}
template <typename T>
std::expected<T, std::error_code> paramValue(size_t idx) const
{
return paramValue<T>(idx, _defaultDeserializer);
}
template <traits::mcc_view_or_output_char_range R>
R byteRepr() const
{
if constexpr (traits::mcc_char_view<R>) {
return R{_msgBuffer.begin(), _msgBuffer.end()};
} else {
R r;
std::ranges::copy(_msgBuffer, std::back_inserter(r));
return r;
}
}
std::string_view byteRepr() const
{
return byteRepr<std::string_view>();
}
template <traits::mcc_input_char_range KT, typename... PTs>
std::error_code construct(KT&& key, PTs&&... params)
requires traits::mcc_output_char_range<BYTEREPR_T>
{
return construct(_defaultSerializer, std::forward<KT>(key), std::forward<PTs>(params)...);
}
//
// serializing function SerFuncT - a callable with the signature:
// template<typename T, mcc_output_char_range R>
// void ser_func(const T& val, R&& buffer)
//
template <typename SerFuncT, traits::mcc_input_char_range KT, typename... PTs>
std::error_code construct(SerFuncT&& ser_func, KT&& key, PTs&&... params)
requires(traits::mcc_output_char_range<BYTEREPR_T> &&
!traits::mcc_input_char_range<std::remove_cvref_t<SerFuncT>>)
{
if constexpr (std::is_pointer_v<std::decay_t<KT>>) {
return construct(std::forward<SerFuncT>(ser_func), std::string_view(key), std::forward<PTs>(params)...);
}
if (!std::ranges::size(key)) {
return std::make_error_code(std::errc::invalid_argument);
}
auto r = valid_keys_t::isKeywordValid(key);
if (!r) {
return std::make_error_code(std::errc::not_supported);
}
_keywordHash = *r;
_msgBuffer = BYTEREPR_T{};
std::ranges::copy(std::forward<KT>(key), std::back_inserter(_msgBuffer));
// _keyword = {_msgBuffer.begin(), _msgBuffer.end()};
size_t key_idx = std::distance(_msgBuffer.begin(), _msgBuffer.end());
std::vector<size_t> par_idx;
_params.clear();
if constexpr (sizeof...(PTs)) {
std::ranges::copy(MCC_COMMPROTO_KEYPARAM_DELIM_SEQ, std::back_inserter(_msgBuffer));
convertFunc(std::forward<SerFuncT>(ser_func), par_idx, std::forward<PTs>(params)...);
for (size_t i = 0; i < par_idx.size(); i += 2) {
_params.emplace_back(_msgBuffer.begin() + par_idx[i], _msgBuffer.begin() + par_idx[i + 1]);
}
}
_keyword = std::string_view{_msgBuffer.begin(), _msgBuffer.begin() + key_idx};
return {};
}
template <traits::mcc_input_char_range R>
constexpr MccNetMessageError fromCharRange(const R& r)
{
if constexpr (std::is_pointer_v<std::decay_t<R>>) {
return fromCharRange(std::string_view(r));
}
if (std::ranges::size(r) == 0) {
return ERROR_EMPTY_MESSAGE;
}
std::string_view key;
// auto prev_msg_buff = _msgBuffer;
if constexpr (traits::mcc_output_char_range<BYTEREPR_T>) {
_msgBuffer = BYTEREPR_T{};
std::ranges::copy(r, std::back_inserter(_msgBuffer));
} else {
_msgBuffer = {std::begin(r), std::end(r)};
}
auto found = std::ranges::search(_msgBuffer, MCC_COMMPROTO_KEYPARAM_DELIM_SEQ);
if (found.empty()) { // only keyword
key = mcc::utils::trimSpaces(std::string_view{_msgBuffer.begin(), _msgBuffer.end()});
} else {
key = mcc::utils::trimSpaces(std::string_view{_msgBuffer.begin(), found.begin()});
}
auto kv = valid_keys_t::isKeywordValid(key);
if (!kv) {
// _msgBuffer = prev_msg_buff; // restore previous netmessage state
return ERROR_INVALID_KEYWORD;
}
_keywordHash = *kv;
_keyword = key;
if (!found.empty()) { // params ...
_params.clear();
auto pr =
std::views::split(std::string_view{found.end(), _msgBuffer.end()}, MCC_COMMPROTO_PARAMPARAM_DELIM_SEQ);
for (auto const& p : pr) {
_params.emplace_back(p.begin(), p.end());
}
}
return ERROR_OK;
}
protected:
size_t _keywordHash{};
std::string_view _keyword{};
std::vector<std::string_view> _params{};
BYTEREPR_T _msgBuffer{};
inline static DefaultDeserializer _defaultDeserializer{};
DefaultSerializer _defaultSerializer{};
template <typename T, typename... Ts>
void convertFunc(std::vector<size_t>& idx, const T& par, const Ts&... pars)
{
if constexpr (std::same_as<T, MccCoordinateSerializer::SerializedCoordFormat>) {
_defaultSerializer._coordFmt = par;
if constexpr (sizeof...(Ts)) {
convertFunc(idx, pars...);
}
} else if constexpr (std::same_as<T, MccCoordinateSerializer::SexagesimalCoordPrec>) {
_defaultSerializer._coordPrec = par;
if constexpr (sizeof...(Ts)) {
convertFunc(idx, pars...);
}
} else {
convertFunc(_defaultSerializer, idx, par, pars...);
// idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
// _defaultSerializer(par, _msgBuffer);
// idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
// if constexpr (sizeof...(Ts)) {
// std::ranges::copy(MCC_COMMPROTO_PARAMPARAM_DELIM_SEQ, std::back_inserter(_msgBuffer));
// convertFunc(idx, pars...);
// }
}
};
template <typename SerFuncT, typename T, typename... Ts>
void convertFunc(SerFuncT&& ser_func, std::vector<size_t>& idx, const T& par, const Ts&... pars)
requires(!std::same_as<std::remove_cvref_t<SerFuncT>, std::vector<size_t>>)
{
if constexpr (std::derived_from<std::remove_cvref_t<SerFuncT>, DefaultSerializer>) {
if constexpr (std::same_as<T, MccCoordinateSerializer::SerializedCoordFormat>) {
// _defaultSerializer._coordFmt = par;
ser_func._coordFmt = par;
} else if constexpr (std::same_as<T, MccCoordinateSerializer::SexagesimalCoordPrec>) {
_defaultSerializer._coordPrec = par;
} else {
idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
std::forward<SerFuncT>(ser_func)(par, _msgBuffer);
idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
if constexpr (sizeof...(Ts)) {
std::ranges::copy(MCC_COMMPROTO_PARAMPARAM_DELIM_SEQ, std::back_inserter(_msgBuffer));
}
}
} else {
idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
std::forward<SerFuncT>(ser_func)(par, _msgBuffer);
idx.emplace_back(std::distance(_msgBuffer.begin(), _msgBuffer.end()));
if constexpr (sizeof...(Ts)) {
std::ranges::copy(MCC_COMMPROTO_PARAMPARAM_DELIM_SEQ, std::back_inserter(_msgBuffer));
}
}
if constexpr (sizeof...(Ts)) {
convertFunc(std::forward<SerFuncT>(ser_func), idx, pars...);
}
}
};
static_assert(MccNetMessage<std::string, MccNetMessageValidKeywords>{"ACK"}.withKey("ACK"));
static_assert(MccNetMessage{"ACK"}.withKey("ACK"));
} // namespace mcc::network

View File

@@ -9,6 +9,7 @@
#include <mutex> #include <mutex>
#include "bsplines/mcc_bsplines.h" #include "bsplines/mcc_bsplines.h"
#include "mcc_defaults.h"
#include "mcc_generics.h" #include "mcc_generics.h"
namespace mcc namespace mcc
@@ -87,6 +88,13 @@ namespace mcc
// PCM_TYPE_BSPLINE - pure 2D B-spline corrections // PCM_TYPE_BSPLINE - pure 2D B-spline corrections
enum class MccDefaultPCMType { PCM_TYPE_GEOMETRY, PCM_TYPE_GEOMETRY_BSPLINE, PCM_TYPE_BSPLINE }; enum class MccDefaultPCMType { PCM_TYPE_GEOMETRY, PCM_TYPE_GEOMETRY_BSPLINE, PCM_TYPE_BSPLINE };
template <MccDefaultPCMType TYPE>
static constexpr std::string_view MccDefaultPCMTypeString =
TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY ? "GEOMETRY"
: TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "GEOMETRY-BSPLINE"
: TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "BSPLINE"
: "UNKNOWN";
template <MccMountType MOUNT_TYPE> template <MccMountType MOUNT_TYPE>
class MccDefaultPCM : public mcc_PCM_interface_t<std::error_code> class MccDefaultPCM : public mcc_PCM_interface_t<std::error_code>
{ {
@@ -97,7 +105,7 @@ public:
typedef double coord_t; typedef double coord_t;
// "classic" geometric PEC coefficients // "classic" geometric PCM coefficients
struct pcm_geom_coeffs_t { struct pcm_geom_coeffs_t {
typedef double coeff_t; typedef double coeff_t;
@@ -186,9 +194,9 @@ public:
// apparent_X = encoder_X + res.pcmX // apparent_X = encoder_X + res.pcmX
// apparent_Y = encoder_Y + res.pcmY // apparent_Y = encoder_Y + res.pcmY
// so, input x and y are assumed to be mount axis encoder coordinates // so, input x and y are assumed to be mount axis encoder coordinates
template <typename T = std::nullptr_t> template <typename T>
error_t computePCM(mcc_celestial_point_c auto pt, mcc_PCM_c auto* res, T* app_pt = nullptr) error_t computePCM(mcc_celestial_point_c auto pt, mcc_PCM_result_c auto* res, T* app_pt = nullptr)
requires(mcc_celestial_point_c<T> || mcc_eqt_hrz_coord_c<T> || std::same_as<T, std::nullptr_t>) requires(mcc_celestial_point_c<T> || mcc_eqt_hrz_coord_c<T>)
{ {
if (res == nullptr) { if (res == nullptr) {
return MccDefaultPCMErrorCode::ERROR_NULLPTR; return MccDefaultPCMErrorCode::ERROR_NULLPTR;
@@ -267,7 +275,7 @@ public:
static_assert(false, "UNSUPPORTED"); static_assert(false, "UNSUPPORTED");
} }
if constexpr (!std::is_null_pointer_v<T>) { if (app_pt != nullptr) {
if constexpr (mcc_eqt_hrz_coord_c<T>) { if constexpr (mcc_eqt_hrz_coord_c<T>) {
if constexpr (mccIsEquatorialMount(mountType)) { if constexpr (mccIsEquatorialMount(mountType)) {
app_pt->HA = pt.X + res->pcmX; app_pt->HA = pt.X + res->pcmX;
@@ -293,7 +301,19 @@ public:
requires(mcc_celestial_point_c<T> || mcc_eqt_hrz_coord_c<T>) requires(mcc_celestial_point_c<T> || mcc_eqt_hrz_coord_c<T>)
{ {
// for small corrections only!!! // for small corrections only!!!
auto ret = computePCM(std::move(app_pt), result); if constexpr (mcc_eqt_hrz_coord_c<T>) {
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {
app_pt.X = app_pt.HA;
app_pt.Y = app_pt.DEC_APP;
} else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
app_pt.X = app_pt.AZ;
app_pt.Y = app_pt.ZD;
}
}
MccCelestialPoint cp;
auto ret = computePCM(app_pt, result, &cp);
// auto ret = computePCM(std::move(app_pt), result, hw_pt);
if (ret) { if (ret) {
return ret; return ret;
} }

View File

@@ -12,7 +12,7 @@ namespace mcc
{ {
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM }; enum class MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP };
} // namespace mcc } // namespace mcc
@@ -56,6 +56,8 @@ struct MccAltLimitPZCategory : public std::error_category {
return "input argument os nullptr"; return "input argument os nullptr";
case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM: case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM:
return "coordinate transformation error"; return "coordinate transformation error";
case MccAltLimitPZErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
default: default:
return "UNKNOWN"; return "UNKNOWN";
} }
@@ -85,6 +87,8 @@ protected:
static constexpr auto pi2 = std::numbers::pi * 2.0; static constexpr auto pi2 = std::numbers::pi * 2.0;
public: public:
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_STOP;
typedef std::error_code error_t; typedef std::error_code error_t;
MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine) MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine)
@@ -551,4 +555,251 @@ protected:
} }
}; };
/* co-longitude axis (HA or AZ) limit switch prohibited zone */
template <MccCoordKind AXIS_KIND>
class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t<std::error_code>
{
public:
static_assert(AXIS_KIND == MccCoordKind::COORDS_KIND_AZ || AXIS_KIND == MccCoordKind::COORDS_KIND_HA,
"UNSUPPORTED AXIS TYPE!");
typedef std::error_code error_t;
static constexpr MccCoordKind axisKind = AXIS_KIND;
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_FLIP;
//
// min_limit_val and max_limit_val are hardware encoder angles in radians!
//
MccAxisLimitSwitchPZ(mcc_angle_c auto const& min_limit_val,
mcc_angle_c auto const& max_limit_val,
mcc_position_controls_c auto* controls)
: _minLimit(min_limit_val), _maxLimit(max_limit_val)
{
_transformCoordinates = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
if (to_pt == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
auto err = controls->transformCoordinates(from_pt, to_pt);
if (!err) {
return MccAltLimitPZErrorCode::ERROR_OK;
}
return mcc_deduce_error_code(err, MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM);
};
_transformCoordinatesEqtHrzCoords = [controls](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t {
if (to_pt == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
auto err = controls->transformCoordinates(from_pt, to_pt);
if (!err) {
return MccAltLimitPZErrorCode::ERROR_OK;
}
if (std::same_as<decltype(err), error_t>) {
return err;
} else {
return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM;
}
};
_computePCM = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
MccPCMResult inv_res;
auto err = controls->computeInversePCM(std::move(from_pt), &inv_res, to_pt);
if (err) {
return mcc_deduce_error_code(err, MccAltLimitPZErrorCode::ERROR_PCM_COMP);
}
return MccAltLimitPZErrorCode::ERROR_OK;
};
}
consteval std::string_view name() const
{
return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE"
: axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE"
: "UKNOWN";
}
template <typename InputT>
error_t inPZone(InputT coords, bool* result)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
if (result == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
*result = (coords.X > _maxLimit) || (coords.X < _minLimit);
// *result = (coords.X < _maxLimit) && (coords.X > _minLimit);
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
// *result = (coords.X < _maxLimit) && (coords.X > _minLimit);
*result = (coords.X > _maxLimit) || (coords.X < _minLimit);
} else { // here one needs transform input coordinates to hardware encoder ones
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
*result = (pt.X > _maxLimit) || (pt.X < _minLimit);
// *result = (pt.X < _maxLimit) && (pt.X > _minLimit);
}
}
return MccAltLimitPZErrorCode::ERROR_OK;
}
// time to reach maximal limit
template <typename InputT>
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
using res_t = std::remove_cvref_t<decltype(*res_time)>;
using period_t = typename res_t::period;
double time_ang;
if (res_time == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
}
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else {
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
time_ang = (_maxLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
}
}
std::chrono::nanoseconds ns{
static_cast<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
period_t rat;
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
return MccAltLimitPZErrorCode::ERROR_OK;
}
// time to reach minimal limit
template <typename InputT>
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
using res_t = std::remove_cvref_t<decltype(*res_time)>;
using period_t = typename res_t::period;
double time_ang;
if (res_time == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
}
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else {
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
time_ang = (_minLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
}
}
std::chrono::nanoseconds ns{
static_cast<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
period_t rat;
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
return MccAltLimitPZErrorCode::ERROR_OK;
}
template <typename InputT, typename ResultT>
error_t intersectPZone(InputT coords, ResultT* point)
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
{
if (point == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
point->X = _minLimit;
// if constexpr (mcc_eqt_hrz_coord_c<ResultT>) {
// point->X = _minLimit;
// } else { // mcc_celestial_point_c
// point->X = _minLimit;
// }
return MccAltLimitPZErrorCode::ERROR_OK;
}
protected:
double _minLimit, _maxLimit;
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _transformCoordinates{};
std::function<error_t(MccCelestialPoint, MccEqtHrzCoords*)> _transformCoordinatesEqtHrzCoords{};
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _computePCM{};
error_t getHWCoords(MccCelestialPoint from_pt, MccCelestialPoint* to_pt)
{
error_t ret = MccAltLimitPZErrorCode::ERROR_OK;
if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
to_pt->X = from_pt.X;
to_pt->Y = from_pt.Y;
} else { // here one needs transform input coordinates to hardware encoder ones
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
}
mcc_tp2tp(from_pt.time_point, to_pt->time_point);
ret = _transformCoordinates(std::move(from_pt), to_pt);
if (!ret) {
ret = _computePCM(*to_pt, to_pt);
}
}
return ret;
}
};
} // namespace mcc } // namespace mcc

View File

@@ -1,7 +1,5 @@
#pragma once #pragma once
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */ /* MOUNT CONTROL COMPONENTS LIBRARY */
@@ -115,78 +113,120 @@ public:
{ {
auto sptr = std::make_shared<decltype(zone)>(std::move(zone)); auto sptr = std::make_shared<decltype(zone)>(std::move(zone));
_inZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, bool* res) { _inZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, bool* res) -> error_t {
auto ret = sptr->inPZone(pt, res);
if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INZONE_FUNC);
}
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_OK);
});
_inZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, bool* res) -> error_t {
auto ret = sptr->inPZone(pt, res); auto ret = sptr->inPZone(pt, res);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INZONE_FUNC)); if (ret) {
}); return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INZONE_FUNC);
}
_inZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, bool* res) { return MccPZoneContainerErrorCode::ERROR_OK;
auto ret = sptr->inPZone(pt, res);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INZONE_FUNC));
}); });
_timeToZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) { _timeToZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) -> error_t {
auto ret = sptr->timeToPZone(pt, res_time); auto ret = sptr->timeToPZone(pt, res_time);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_timeToZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) { _timeToZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) -> error_t {
auto ret = sptr->timeToPZone(pt, res_time); auto ret = sptr->timeToPZone(pt, res_time);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_timeFromZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) { _timeFromZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) -> error_t {
auto ret = sptr->timeFromPZone(pt, res_time); auto ret = sptr->timeFromPZone(pt, res_time);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_timeFromZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) { _timeFromZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) -> error_t {
auto ret = sptr->timeFromPZone(pt, res_time); auto ret = sptr->timeFromPZone(pt, res_time);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
// _intersectZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, MccCelestialPoint* res_pt) { // _intersectZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, MccCelestialPoint* res_pt) {
// auto ret = sptr->intersectPZone(pt, res_pt); // auto ret = sptr->intersectPZone(pt, res_pt);
// return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); // return mcc_deduce_error_code(ret,
// mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC));
// }); // });
// _intersectZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, MccCelestialPoint* res_pt) { // _intersectZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, MccCelestialPoint* res_pt) {
// auto ret = sptr->intersectPZone(pt, res_pt); // auto ret = sptr->intersectPZone(pt, res_pt);
// return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); // return mcc_deduce_error_code(ret,
// mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC));
// }); // });
_intersectZoneFuncCPT2CPT.emplace_back([sptr](const MccCelestialPoint& pt, MccCelestialPoint* res_pt) { _intersectZoneFuncCPT2CPT.emplace_back(
[sptr](const MccCelestialPoint& pt, MccCelestialPoint* res_pt) -> error_t {
auto ret = sptr->intersectPZone(pt, res_pt);
if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
});
_intersectZoneFuncEHC2CPT.emplace_back([sptr](const MccEqtHrzCoords& pt, MccCelestialPoint* res_pt) -> error_t {
auto ret = sptr->intersectPZone(pt, res_pt); auto ret = sptr->intersectPZone(pt, res_pt);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_intersectZoneFuncEHC2CPT.emplace_back([sptr](const MccEqtHrzCoords& pt, MccCelestialPoint* res_pt) { _intersectZoneFuncCPT2EHC.emplace_back([sptr](const MccCelestialPoint& pt, MccEqtHrzCoords* res_pt) -> error_t {
auto ret = sptr->intersectPZone(pt, res_pt); auto ret = sptr->intersectPZone(pt, res_pt);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_intersectZoneFuncCPT2EHC.emplace_back([sptr](const MccCelestialPoint& pt, MccEqtHrzCoords* res_pt) { _intersectZoneFuncEHC2EHC.emplace_back([sptr](const MccEqtHrzCoords& pt, MccEqtHrzCoords* res_pt) -> error_t {
auto ret = sptr->intersectPZone(pt, res_pt); auto ret = sptr->intersectPZone(pt, res_pt);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); if (ret) {
return mcc_deduce_error_code(ret, MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC);
}
return MccPZoneContainerErrorCode::ERROR_OK;
}); });
_intersectZoneFuncEHC2EHC.emplace_back([sptr](const MccEqtHrzCoords& pt, MccEqtHrzCoords* res_pt) {
auto ret = sptr->intersectPZone(pt, res_pt);
return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC));
});
return _inZoneFuncCPT.size(); return _inZoneFuncCPT.size();
} }
@@ -217,8 +257,8 @@ public:
} }
template <typename InputT> template <typename InputT, std::ranges::output_range<bool> ResT = std::vector<bool>>
error_t inPZone(InputT coords, bool* common_result, std::ranges::output_range<bool> auto* result = nullptr) error_t inPZone(InputT coords, bool* common_result, ResT* result = nullptr)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{ {
if (common_result == nullptr) { if (common_result == nullptr) {
@@ -384,7 +424,7 @@ public:
std::ranges::advance(ptr, i); std::ranges::advance(ptr, i);
error_t ret; error_t ret;
if constexpr (mcc_eqt_hrz_coord_c<ResultT>) { if constexpr (mcc_eqt_hrz_coord_c<std::ranges::range_value_t<ResultT>>) {
MccEqtHrzCoords pt; MccEqtHrzCoords pt;
mcc_tp2tp(ptr->time_point, pt.time_point); mcc_tp2tp(ptr->time_point, pt.time_point);

View File

@@ -6,6 +6,8 @@
/* SIMPLE SLEWING MODEL IMPLEMENTATION */ /* SIMPLE SLEWING MODEL IMPLEMENTATION */
#include <fstream>
#include "mcc_defaults.h" #include "mcc_defaults.h"
#include "mcc_generics.h" #include "mcc_generics.h"
#include "mcc_moving_model_common.h" #include "mcc_moving_model_common.h"
@@ -20,12 +22,12 @@ enum class MccSimpleSlewingModelErrorCode : int {
ERROR_PCM_COMP, ERROR_PCM_COMP,
ERROR_GET_TELEMETRY, ERROR_GET_TELEMETRY,
ERROR_DIST_TELEMETRY, ERROR_DIST_TELEMETRY,
ERROR_DIFF_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP, ERROR_PZONE_CONTAINER_COMP,
ERROR_IN_PZONE, ERROR_TARGET_IN_PZONE,
ERROR_NEAR_PZONE, ERROR_NEAR_PZONE,
ERROR_TIMEOUT, ERROR_TIMEOUT,
ERROR_UNEXPECTED_AXIS_RATES, ERROR_ALREADY_SLEW,
ERROR_ALREADY_STOPPED,
ERROR_STOPPED ERROR_STOPPED
}; };
@@ -47,274 +49,633 @@ class is_error_code_enum<mcc::MccSimpleSlewingModelErrorCode> : public true_type
namespace mcc namespace mcc
{ {
// error category
struct MccSimpleSlewingModelCategory : public std::error_category {
MccSimpleSlewingModelCategory() : std::error_category() {}
const char* name() const noexcept
{
return "SIMPLE-SLEWING-MODEL";
}
std::string message(int ec) const
{
MccSimpleSlewingModelErrorCode err = static_cast<MccSimpleSlewingModelErrorCode>(ec);
switch (err) {
case MccSimpleSlewingModelErrorCode::ERROR_OK:
return "OK";
case MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE:
return "cannot get hardware state";
case MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE:
return "cannot set hardware state";
case MccSimpleSlewingModelErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
case MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY:
return "cannot get telemetry";
case MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY:
return "cannot get target-to-mount-position distance";
case MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP:
return "pzone container computation error";
case MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE:
return "target is in prohibited zone";
case MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE:
return "near prohibited zone";
case MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT:
return "a timeout occured while slewing";
case MccSimpleSlewingModelErrorCode::ERROR_ALREADY_SLEW:
return "already slewing";
case MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED:
return "slewing is already stopped";
case MccSimpleSlewingModelErrorCode::ERROR_STOPPED:
return "slewing was stopped";
default:
return "UNKNOWN";
}
}
static const MccSimpleSlewingModelCategory& get()
{
static const MccSimpleSlewingModelCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccSimpleSlewingModelErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccSimpleSlewingModelCategory::get());
}
/* /*
The target celestial point must be set in telemetry->target The target celestial point must be set in telemetry->target
*/ */
class MccSimpleSlewingModel class MccSimpleSlewingModel
{ {
static constexpr auto DEG90INRADS = std::numbers::pi / 2.0;
public: public:
typedef std::error_code error_t; typedef std::error_code error_t;
typedef MccSimpleMovingModelParams slewing_params_t; typedef MccSimpleMovingModelParams slewing_params_t;
// struct slewing_params_t { template <mcc_all_controls_c CONTROLS_T, mcc_logger_c LoggerT = MccNullLogger>
// bool slewAndStop{false}; // slew to target and stop mount MccSimpleSlewingModel(CONTROLS_T* controls, LoggerT logger)
: _stopSlewing(new std::atomic_bool()),
// std::chrono::seconds telemetryTimeout{3}; _currentParamsMutex(new std::mutex),
_lastError(new std::atomic<error_t>())
// // minimal time to prohibited zone at current speed. if it is lesser then exit with error
// std::chrono::seconds minTimeToPZone{10};
// // target-mount coordinate difference to start adjusting of slewing (in radians)
// double adjustCoordDiff{10.0_degs};
// // coordinates difference to stop slewing (in radians)
// double slewToleranceRadius{5.0_arcsecs};
// // slew process timeout
// std::chrono::seconds slewTimeout{3600};
// double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// };
template <mcc_telemetry_data_c TelemetryT, mcc_hardware_c HardwareT, mcc_pzone_container_c PZoneContT>
MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
: _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
{ {
_slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t { std::ostringstream os;
// first, check target coordinates os << std::this_thread::get_id();
typename TelemetryT::error_t t_err;
logger.logDebug(std::format("Create MccSimpleSlewingModel class instance (thread: {})", os.str()));
*_stopSlewing = true;
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_OK;
_checkTargetFunc = [controls, logger, this]() mutable -> error_t {
typename CONTROLS_T::error_t t_err;
MccTelemetryData tdata;
bool in_zone;
std::vector<bool> in_zone_vec;
// t_err = controls->telemetryData(&tdata);
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata);
if (t_err) {
return *_lastError =
mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (t_err) {
return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
auto pz_err = controls->inPZone(tdata.target, &in_zone, &in_zone_vec);
if (pz_err) {
return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
size_t i = 0;
for (; i < in_zone_vec.size(); ++i) {
if (in_zone_vec[i]) {
break;
}
}
logger.logError("target point is in prohibited zone (zone index: {})! Entered target coordinates:", i);
logger.logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
mcc::MccAngle{tdata.target.ALT}.sexagesimal()));
logger.logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.target.X}.sexagesimal(),
mcc::MccAngle{tdata.target.Y}.sexagesimal()));
return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
}
return MccSimpleSlewingModelErrorCode::ERROR_OK;
};
_slewingFunc = [controls, logger = std::move(logger), this](bool slew_and_stop) mutable -> error_t {
// reset error
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_OK;
double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs;
bool store_path = false;
std::ofstream fst;
using path_tp_t = std::chrono::duration<double>; // seconds represented as double
{
// std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
braking_accelX = std::numeric_limits<double>::min();
} else {
braking_accelX = std::abs(_currentParams.brakingAccelX);
}
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
braking_accelY = std::numeric_limits<double>::min();
} else {
braking_accelY = std::abs(_currentParams.brakingAccelY);
}
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file
fst.open(_currentParams.slewingPathFilename);
if (fst.is_open()) {
store_path = true;
} else {
logger.logError(std::format("Cannot open slewing path file: {}! Do not save it!",
_currentParams.slewingPathFilename));
}
}
}
logger.logInfo(
std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK")));
logger.logInfo(std::format(" slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
if (!slew_and_stop) {
logger.logInfo(std::format(" slewing tolerance radius: {} arcsecs",
mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
}
logger.logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
logger.logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
logger.logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
if (store_path) {
fst << "# \n";
fst << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
fst << "# Config:\n";
fst << "# slewing tolerance radius: " << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
<< " arcsecs\n";
fst << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
fst << "# \n";
fst << "# Format (time is in seconds, coordinates are in radians): \n";
fst << "# <time-since-start> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n";
}
typename CONTROLS_T::error_t t_err;
MccTelemetryData tdata; MccTelemetryData tdata;
{ {
std::lock_guard lock{*_currentParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
t_err = telemetry->telemetryData(&tdata); t_err = controls->telemetryData(&tdata);
if (t_err) { if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); return *_lastError =
mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
} }
} }
auto last_hw_time = tdata.time_point;
bool in_zone; bool in_zone;
auto pz_err = pz_cont->inPZone(tdata.target, &in_zone); std::vector<bool> in_zone_vec;
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
MccCelestialPoint cpt; MccCelestialPoint cpt;
mcc_tp2tp(tdata.time_point, cpt.time_point);
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else { } else {
static_assert(false, "UNKNOWN MOUNT TYPE!"); static_assert(false, "UNKNOWN MOUNT TYPE!");
} }
std::vector<MccCelestialPoint> isct_pt(pz_cont->sizePZones, cpt); typename CONTROLS_T::hardware_state_t hw_state;
pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopSlewing) { auto hw_err = controls->hardwareGetState(&hw_state);
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
typename HardwareT::hardware_state_t hw_state;
auto hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) { if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); *_stopSlewing = true;
return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
} }
hw_state.X = (double)tdata.target.X; hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y; hw_state.Y = (double)tdata.target.Y;
// hw_state.endptX = (double)tdata.target.X;
// hw_state.endptY = (double)tdata.target.Y;
{ {
std::lock_guard lock{*_currentParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
hw_state.speedX = _currentParams.slewXRate; hw_state.speedX = _currentParams.slewRateX;
hw_state.speedY = _currentParams.slewYRate; hw_state.speedY = _currentParams.slewRateY;
} }
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING; hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_SLEWING;
if (*_stopSlewing) { if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; logger.logDebug("slewing was stopped!");
return *_lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
} }
// start slewing // start slewing
hw_err = hardware->hardwareSetState(hw_state); logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
if (hw_err) { mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
auto start_point = tdata.time_point; // needed for trajectory file
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
logger.logDebug(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
if (store_path) {
fst << std::chrono::duration_cast<path_tp_t>(tdata.time_point - start_point).count() << " "
<< tdata.target.HA << " " << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP
<< " " << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
<< (int)hw_state.moving_state << "\n";
}
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal()));
} }
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
*_stopSlewing = true;
return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
// double dist, dx, dy;
double dist;
std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp; std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
mcc_tp2tp(hw_state.time_point, start_slewing_tp); start_slewing_tp = std::chrono::steady_clock::now();
last_adjust_tp = start_slewing_tp;
double dist, dx, dy, sinY, rate2, xrate; std::pair<double, double> distXY;
std::chrono::duration<double> dtx, dty; // seconds in double bool tag_var_coord = true;
bool adjust_mode = false; if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate; tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
tag_var_coord = false;
}
while (true) {
// main loop (simply monitors the current position taking into account the prohibited zones, as well as the
// timeout of the entire process)
while (!*_stopSlewing) {
// wait for updated telemetry data // wait for updated telemetry data
{ {
std::lock_guard lock{*_currentParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) { if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); *_lastError = mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
} }
} }
if (*_stopSlewing) { if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; *_lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
break;
// return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
} }
// compute time to prohibited zones at current speed distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
for (auto const& pt : isct_pt) {
if (std::isfinite(pt.X) && std::isfinite(pt.Y)) {
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
// sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
dx = pt.X - tdata.HA;
dy = pt.Y - tdata.DEC_APP;
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
// sinY = sin(tdata.ZD);
dx = pt.X - tdata.AZ;
dy = pt.Y - tdata.ZD;
}
// if (utils::isEqual(sinY, 0.0)) { logger.logTrace(
// dtx = decltype(dtx){std::numeric_limits<double>::infinity()}; std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
// rate2 = std::numeric_limits<double>::infinity(); min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first),
// } else { mcc::MccAngleFancyString(distXY.second)));
// xrate = tdata.speedX * sinY;
// dtx = decltype(dtx){std::abs(dx / xrate)};
// }
dtx = decltype(dtx){std::abs(dx / tdata.speedX)};
dty = decltype(dty){std::abs(dy / tdata.speedY)};
{ // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
std::lock_guard lock{*_currentParamsMutex}; // and check them for getting into the prohibited zones
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
cpt.X = tdata.HA + distXY.first;
cpt.Y = tdata.DEC_APP + distXY.second;
if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) { // cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs;
return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; // cpt.Y = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
} if (cpt.Y > DEG90INRADS) {
cpt.Y = DEG90INRADS;
}
if (cpt.Y < -DEG90INRADS) {
cpt.Y = -DEG90INRADS;
}
logger.logTrace(std::format(" current target: HA = {}, DEC = {}",
mcc::MccAngle(tdata.target.HA).sexagesimal(true),
mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
logger.logTrace(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle(tdata.HA).sexagesimal(true),
mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
cpt.X = tdata.AZ + distXY.first;
cpt.Y = tdata.ZD + distXY.second;
// cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
// cpt.Y = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
if (cpt.Y < 0.0) {
cpt.Y = 0.0;
}
if (cpt.Y > std::numbers::pi) {
cpt.Y = std::numbers::pi;
}
logger.logTrace(std::format(" target: AZ = {}, ZD = {}",
mcc::MccAngle(tdata.target.AZ).sexagesimal(),
mcc::MccAngle(tdata.target.ZD).sexagesimal()));
logger.logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
mcc::MccAngle(tdata.ZD).sexagesimal()));
}
mcc_tp2tp(tdata.time_point, cpt.time_point);
logger.logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s",
mcc::MccAngleFancyString(tdata.speedX),
mcc::MccAngleFancyString(tdata.speedY)));
in_zone_vec.clear();
auto pz_err = controls->inPZone(cpt, &in_zone, &in_zone_vec);
if (pz_err) {
*_lastError =
mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
size_t i = 0;
for (; i < in_zone_vec.size(); ++i) {
if (in_zone_vec[i]) {
break;
} }
} }
if (*_stopSlewing) { logger.logError(
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; "target point is near prohibited zone (zone index: {})! Entered target coordinates:", i);
} logger.logError(std::format(
" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal(),
mcc::MccAngle{tdata.ALT}.sexagesimal()));
logger.logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(),
mcc::MccAngle{tdata.Y}.sexagesimal()));
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
break;
// *_stopSlewing = true;
// return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
} }
{ {
std::lock_guard lock{*_currentParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) { if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT; logger.logError("slewing process timeout!");
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT;
break;
// return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT;
} }
} }
hw_err = hardware->hardwareGetState(&hw_state); logger.logTrace(std::format("get hw state ..."));
hw_err = controls->hardwareGetState(&hw_state);
if (hw_err) { if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); *_lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
} }
t_err = telemetry->targetToMountDist(&dist); logger.logTrace(std::format("hw state was updated ({}, {} state = <{}>)",
MccAngle(hw_state.X).sexagesimal(true), MccAngle(hw_state.Y).sexagesimal(),
(int)hw_state.moving_state));
if (store_path) {
fst << std::chrono::duration_cast<path_tp_t>(tdata.time_point - start_point).count() << " "
<< tdata.target.HA << " " << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP
<< " " << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
<< (int)hw_state.moving_state << "\n";
}
if (slew_and_stop && !tag_var_coord) { // just wait for mount to be stopped
if (hw_state.moving_state == CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) {
logger.logInfo("mount moving state is STOPPED - exit!");
break;
}
} else {
if (last_hw_time == tdata.time_point) {
logger.logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
continue;
}
last_hw_time = tdata.time_point;
t_err = controls->targetToMountDist(&dist);
if (t_err) {
*_lastError =
mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
}
logger.logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
if ((dist <= _currentParams.slewToleranceRadius) &&
(hw_state.moving_state ==
CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING)) { // stop slewing and exit from
// cycle
logger.logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");
if (slew_and_stop) {
controls->hardwareStop();
}
break;
}
if (*_stopSlewing) {
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
break;
// return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// resend new position since target coordinates are changed in time
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
// controls->targetToMountDiff(tdata.pair_kind, &dx, &dy);
// // hw_state.endptX = hw_state.X + std::copysign(1.0_degs, dx);
// // hw_state.endptY = hw_state.Y + std::copysign(1.0_degs, dy);
// hw_state.endptX = hw_state.X + std::copysign(10.0_degs, dx);
// hw_state.endptY = hw_state.Y + std::copysign(10.0_degs, dy);
// logger.logTrace(std::format(
// "Send to hardware: {}, {}, tag: {}, {} (X = {} degs, Y = {} degs)",
// MccAngle(hw_state.X).sexagesimal(true), MccAngle(hw_state.Y).sexagesimal(),
// MccAngle(hw_state.endptX).sexagesimal(true), MccAngle(hw_state.endptY).sexagesimal(),
// mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
logger.logTrace(std::format("Send to hardware: {}, {}, (X = {} degs, Y = {} degs)",
MccAngle(hw_state.X).sexagesimal(true),
MccAngle(hw_state.Y).sexagesimal(), mcc::MccAngle{hw_state.X}.degrees(),
mcc::MccAngle{hw_state.Y}.degrees()));
// hw_state.time_point += std::chrono::milliseconds(50);
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE;
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
// FOR DEBUG PURPOSE!!!!
// std::this_thread::sleep_for(std::chrono::milliseconds(50));
logger.logTrace(std::format("get hw state right after hardwareSetState ..."));
hw_err = controls->hardwareGetState(&hw_state);
if (hw_err) {
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE;
break;
// *_stopSlewing = true;
// return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
}
logger.logTrace(std::format("hw state was updated ({}, {})", MccAngle(hw_state.X).sexagesimal(true),
MccAngle(hw_state.Y).sexagesimal()));
}
if (*_stopSlewing) {
*_lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
break;
// return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
}
*_stopSlewing = true;
logger.logInfo("Slewing finished");
error_t err = *_lastError;
logger.logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
// get final position
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) { if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); return *_lastError =
} mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
{
std::lock_guard lock{*_currentParamsMutex};
if (adjust_mode && !_currentParams.slewAndStop) {
// do not allow mount speed fall below sideral
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
// turn on sideral rate only if the current position point catches up with the target
if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) {
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
hw_state.speedX = slewing_params_t::sideralRate;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
hw_err = hardware->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!!");
}
}
if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle
if (hw_state.moving_type ==
HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped
break;
}
}
if (dist <= _currentParams.adjustCoordDiff) { // adjust mount pointing
auto now = std::chrono::steady_clock::now();
if ((now - last_adjust_tp) < _currentParams.adjustCycleInterval) {
continue;
}
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
hw_state.speedX = _currentParams.adjustXRate;
hw_state.speedY = _currentParams.adjustYRate;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING;
hw_err = hardware->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
last_adjust_tp = now;
adjust_mode = true;
} else {
adjust_mode = false;
}
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
} }
} }
return MccSimpleSlewingModelErrorCode::ERROR_OK; t_err = controls->targetToMountDist(&dist);
if (t_err) {
return *_lastError = mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
logger.logDebug(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
if (store_path) {
fst << std::chrono::duration_cast<path_tp_t>(tdata.time_point - start_point).count() << " "
<< tdata.target.HA << " " << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP
<< " " << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
<< (int)hw_state.moving_state << "\n";
}
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal()));
}
logger.logDebug(std::format(" target-to-mount distance {}", mcc::MccAngleFancyString(dist)));
if (store_path) {
fst.close();
}
return *_lastError = MccSimpleSlewingModelErrorCode::ERROR_OK;
}; };
} }
@@ -328,16 +689,35 @@ public:
virtual ~MccSimpleSlewingModel() = default; virtual ~MccSimpleSlewingModel() = default;
error_t slewToTarget() error_t slewToTarget(bool slew_and_stop = false)
{ {
if (!(*_stopSlewing)) {
return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_SLEW;
}
*_stopSlewing = false; *_stopSlewing = false;
return _slewingFunc(); // check for target in p-zone
*_lastError = _checkTargetFunc();
if (_lastError->load()) { // return here immediately
return *_lastError;
}
// // asynchronous slewing process
// _slewFuncFuture = std::async(std::launch::async, _slewingFunc, slew_and_stop);
// return MccSimpleSlewingModelErrorCode::ERROR_OK;
return _slewingFunc(slew_and_stop);
} }
error_t stopSlewing() error_t stopSlewing()
{ {
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED;
}
*_stopSlewing = true; *_stopSlewing = true;
return MccSimpleSlewingModelErrorCode::ERROR_OK; return MccSimpleSlewingModelErrorCode::ERROR_OK;
@@ -361,12 +741,21 @@ public:
return _currentParams; return _currentParams;
} }
error_t slewingLastError() const
{
return *_lastError;
}
protected: protected:
std::function<error_t()> _slewingFunc{}; std::function<error_t(bool)> _slewingFunc{};
std::unique_ptr<std::atomic_bool> _stopSlewing; std::unique_ptr<std::atomic_bool> _stopSlewing;
std::function<error_t()> _checkTargetFunc{};
slewing_params_t _currentParams{}; slewing_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{}; std::unique_ptr<std::mutex> _currentParamsMutex{};
std::unique_ptr<std::atomic<error_t>> _lastError;
std::future<error_t> _slewFuncFuture{};
}; };

View File

@@ -135,11 +135,6 @@ public:
_loggerSPtr->log(spdlog::level::trace, fmt, std::forward<ArgTs>(args)...); _loggerSPtr->log(spdlog::level::trace, fmt, std::forward<ArgTs>(args)...);
} }
protected:
std::list<std::string> _currentLogPatternRange;
std::string _currentLogPattern;
std::shared_ptr<spdlog::logger> _loggerSPtr;
// helper methods // helper methods
@@ -180,6 +175,12 @@ protected:
{ {
addMarkToPatternIdx(std::string_view{mark}, after_idx); addMarkToPatternIdx(std::string_view{mark}, after_idx);
} }
protected:
std::list<std::string> _currentLogPatternRange;
std::string _currentLogPattern;
std::shared_ptr<spdlog::logger> _loggerSPtr;
}; };
} // namespace mcc::utils } // namespace mcc::utils

File diff suppressed because it is too large Load Diff

734
mcc/mcc_telemetry.h.bad Normal file
View File

@@ -0,0 +1,734 @@
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* IMPLEMENTATION OF TELEMETRY CLASS */
#include <condition_variable>
#include <future>
#include <mutex>
#include <thread>
#include "mcc_defaults.h"
namespace mcc
{
enum class MccTelemetryErrorCode : int {
ERROR_OK,
ERROR_NULLPTR,
ERROR_COORD_TRANSFORM,
ERROR_PCM_COMP,
ERROR_HARDWARE_GETPOS,
ERROR_UPDATE_STOPPED,
ERROR_DATA_TIMEOUT,
ERROR_UNSUPPORTED_COORD_PAIR
};
} // namespace mcc
namespace std
{
template <>
class is_error_code_enum<mcc::MccTelemetryErrorCode> : public true_type
{
};
} // namespace std
namespace mcc
{
/* error category definition */
// error category
struct MccTelemetryCategory : public std::error_category {
MccTelemetryCategory() : std::error_category() {}
const char* name() const noexcept
{
return "MCC-TELEMETRY";
}
std::string message(int ec) const
{
MccTelemetryErrorCode err = static_cast<MccTelemetryErrorCode>(ec);
switch (err) {
case MccTelemetryErrorCode::ERROR_OK:
return "OK";
case MccTelemetryErrorCode::ERROR_NULLPTR:
return "nullptr input argument";
case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM:
return "coordinate transformation error";
case MccTelemetryErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS:
return "cannot get hardware position";
case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED:
return "telemetry update was stopped";
case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT:
return "a timeout occured while waiting for new data";
case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
return "unsupported coordinate pair";
default:
return "UNKNOWN";
}
}
static const MccTelemetryCategory& get()
{
static const MccTelemetryCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccTelemetryErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccTelemetryCategory::get());
}
class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
{
protected:
static constexpr uint16_t internalUpdatingIntervalDiv = 5;
public:
static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100);
static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5;
typedef std::error_code error_t;
template <mcc_position_controls_c CONTROLS_T>
MccTelemetry(CONTROLS_T* controls)
: _isDataUpdated(new std::atomic_bool()),
_data(),
_userTarget(),
_internalUpdating(new std::atomic_bool),
_currentUpdateInterval(defaultUpdateInterval),
_currentUpdateIntervalMutex(new std::mutex),
_updateMutex(new std::mutex),
_updateCondVar(new std::condition_variable)
{
*_isDataUpdated = false;
*_internalUpdating = false;
// using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
using pcm_t = std::remove_cvref_t<decltype(*controls)>;
using hardware_t = std::remove_cvref_t<decltype(*controls)>;
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
_updateTargetFunc = [controls, this](std::stop_token stop_token) -> error_t {
MccPCMResult pcm_res;
mcc_tp2tp(_data.time_point, _data.target.time_point);
bool hw_coords = _data.target.pair_kind == MccCoordPairKind::COORDS_KIND_XY;
MccCelestialPoint hw_cp{.pair_kind = MccCoordPairKind::COORDS_KIND_XY};
mcc_tp2tp(_data.time_point, hw_cp.time_point);
if (hw_coords) { // compute corresponded apparent coordinates
hw_cp.X = _data.target.X;
hw_cp.Y = _data.target.Y;
auto pcm_err = controls->computePCM(_data.target, &pcm_res, &_data.target);
if (pcm_err) {
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
if (stop_token.stop_requested()) {
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
}
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!!!");
}
}
if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
_data.target.X = _data.target.RA_ICRS;
_data.target.Y = _data.target.DEC_ICRS;
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
_data.target.X = _data.target.AZ;
_data.target.Y = _data.target.ZD;
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
_data.target.X = _data.target.AZ;
_data.target.Y = _data.target.ALT;
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
_data.target.X = _data.target.HA;
_data.target.Y = _data.target.DEC_APP;
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
_data.target.X = _data.target.RA_APP;
_data.target.Y = _data.target.DEC_APP;
} else {
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
auto ccte_err = controls->transformCoordinates(_data.target, &_data.target);
if (ccte_err) {
if (hw_coords) { // restore coordinates pair kind
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
}
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
if (stop_token.stop_requested()) {
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
}
if (_data.target.pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
// fixed apparent coordinates (AZZD or HADEC)
// needs to compute ICRS
// (.X and .Y are already assigned above!)
// if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
// _data.target.X = _data.target.AZ;
// _data.target.Y = _data.target.ZD;
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
// _data.target.X = _data.target.AZ;
// _data.target.Y = _data.target.ALT;
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
// _data.target.X = _data.target.HA;
// _data.target.Y = _data.target.DEC_APP;
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
// _data.target.X = _data.target.RA_APP;
// _data.target.Y = _data.target.DEC_APP;
// } else {
// return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
// }
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
ccte_err = controls->transformCoordinates(_data.target, &pt);
if (ccte_err) {
if (hw_coords) { // restore coordinates pair kind
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
_data.target.X = hw_cp.X;
_data.target.Y = hw_cp.Y;
}
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
_data.target.RA_ICRS = pt.X;
_data.target.DEC_ICRS = pt.Y;
} // from ICRS to apparent calculation is already performed above
if (stop_token.stop_requested()) {
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
}
// hardware coordinates
if (!hw_coords) {
auto pcm_err = controls->computeInversePCM(_data.target, &pcm_res, &_data.target);
if (pcm_err) {
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
} else { // restore coordinates pair kind
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
_data.target.X = hw_cp.X;
_data.target.Y = hw_cp.Y;
}
return MccTelemetryErrorCode::ERROR_OK;
};
_updateFunc = [controls, this](std::stop_token stop_token) -> std::error_code {
// first, update mount quantities
typename hardware_t::hardware_state_t hw_pos;
auto hw_err = controls->hardwareGetState(&hw_pos);
if (hw_err) {
return mcc_deduce_error_code(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
}
// if (stop_token.stop_requested()) {
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
// }
double eo;
_data.time_point =
std::chrono::time_point_cast<typename decltype(_data.time_point)::duration>(hw_pos.time_point);
auto ccte_err = controls->timepointToJulday(_data.time_point, &_data.JD);
if (!ccte_err) {
if (stop_token.stop_requested()) {
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
}
ccte_err = controls->juldayToAppSideral(_data.JD, &_data.LST, true);
if (!ccte_err) {
ccte_err = controls->equationOrigins(_data.JD, &eo);
_data.EO = eo;
}
}
if (ccte_err) {
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
// if (stop_token.stop_requested()) {
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
// }
_data.X = (double)hw_pos.X;
_data.Y = (double)hw_pos.Y;
_data.speedX = (double)hw_pos.speedX;
_data.speedY = (double)hw_pos.speedY;
// fill _data.pcmX, _data.pcmY and corresponded apparent coordinates
auto pcm_err = controls->computePCM(_data, &_data, &_data);
if (pcm_err) {
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
// if (stop_token.stop_requested()) {
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
// }
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point};
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
_data.RA_APP =
MccAngle((double)_data.LST - (double)_data.HA - eo).normalize<MccAngle::NORM_KIND_0_360>();
// MccAngle((double)_data.LST - (double)_data.HA + eo).normalize<MccAngle::NORM_KIND_0_360>();
_data.X = _data.HA;
_data.Y = _data.DEC_APP;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = controls->transformCoordinates(_data, &pt);
if (!ccte_err) {
_data.AZ = pt.X;
_data.ALT = pt.Y;
_data.ZD = std::numbers::pi / 2.0 - _data.ALT;
}
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
_data.ALT = std::numbers::pi / 2.0 - _data.ZD;
_data.X = _data.AZ;
_data.Y = _data.ZD;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = controls->transformCoordinates(_data, &pt);
if (!ccte_err) {
_data.HA = pt.X;
_data.DEC_APP = pt.Y;
_data.RA_APP =
MccAngle((double)_data.LST - (double)_data.HA - eo).normalize<MccAngle::NORM_KIND_0_360>();
// MccAngle((double)_data.LST - (double)_data.HA + eo).normalize<MccAngle::NORM_KIND_0_360>();
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
if (!ccte_err) {
// if (stop_token.stop_requested()) {
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
// }
// to compute refraction coefficients
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
_data.X = _data.AZ;
_data.Y = _data.ZD;
ccte_err = controls->refractionCorrection(_data, &_data.refCorr);
if (!ccte_err) {
// restore hardware encoders coordinates
_data.X = (double)hw_pos.X;
_data.Y = (double)hw_pos.Y;
// update target (assuming target ICRS coordinates are already set)
// auto ret = _updateTargetFunc(false, stop_token);
// update target according to its .pair_kind!
auto ret = _updateTargetFunc(stop_token);
if (ret) {
return ret;
}
}
}
// restore according to the mount type
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
if (ccte_err) {
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
return MccTelemetryErrorCode::ERROR_OK;
};
}
MccTelemetry(MccTelemetry&&) = default;
MccTelemetry& operator=(MccTelemetry&&) = default;
MccTelemetry(const MccTelemetry&) = delete;
MccTelemetry& operator=(const MccTelemetry&) = delete;
virtual ~MccTelemetry()
{
stopInternalTelemetryDataUpdating();
if (_internalUpdatingFuture.valid()) {
// try to exit correctly
// auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
_internalUpdatingFuture.wait_for(std::chrono::seconds(1));
// _internalUpdatingFuture.get();
}
};
template <traits::mcc_time_duration_c DT>
DT telemetryDataUpdateInterval() const
{
std::lock_guard lock{*_currentUpdateIntervalMutex};
return std::chrono::duration_cast<DT>(_currentUpdateInterval);
}
std::chrono::milliseconds telemetryDataUpdateInterval() const
{
return telemetryDataUpdateInterval<std::chrono::milliseconds>();
}
void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval)
{
using d_t = std::remove_cvref_t<decltype(interval)>;
std::lock_guard lock{*_currentUpdateIntervalMutex};
if constexpr (std::floating_point<typename d_t::rep>) {
_currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval;
} else {
_currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval;
}
}
void setTelemetryUpdateTimeout(traits::mcc_time_duration_c auto const& timeout)
{
if (timeout.count() > 0) {
_currentUpdateTimeout = std::chrono::duration_cast<decltype(_currentUpdateTimeout)>(timeout);
}
}
auto getTelemetryUpdateTimeout() const
{
return _currentUpdateTimeout;
}
// asynchronuosly periodicaly update telemetry data (internal synchronization)
void startInternalTelemetryDataUpdating()
{
using intv_t = std::remove_cvref_t<decltype(_currentUpdateInterval)>;
_internalUpdatingStopSource = std::stop_source{}; // reset state
*_internalUpdating = true;
_internalUpdatingFuture = std::async(
std::launch::async,
[this](std::stop_token stop_token) -> error_t {
while (!stop_token.stop_requested()) {
// while (true) {
_lastUpdateError = updateTelemetryData(_currentUpdateTimeout);
if (_lastUpdateError) {
*_internalUpdating = false;
return _lastUpdateError;
}
// auto nn = std::this_thread::get_id();
std::this_thread::sleep_for(_currentUpdateInterval);
// {
// std::lock_guard lock{*_currentUpdateIntervalMutex};
// // compute it here because of possible changing _currentUpdateInterval
// auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv;
// for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) {
// if (stop_token.stop_requested()) {
// break;
// }
// std::this_thread::sleep_for(sleep_td);
// }
// if (stop_token.stop_requested()) {
// break;
// }
// if constexpr (std::floating_point<intv_t>) {
// std::this_thread::sleep_for(sleep_td);
// } else {
// auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv;
// if (rem.count()) {
// std::this_thread::sleep_for(rem);
// } else {
// std::this_thread::sleep_for(sleep_td);
// }
// }
// }
}
*_internalUpdating = false;
return MccTelemetryErrorCode::ERROR_OK;
},
_internalUpdatingStopSource.get_token());
}
void stopInternalTelemetryDataUpdating()
{
_internalUpdatingStopSource.request_stop();
*_internalUpdating = false;
}
bool isInternalTelemetryDataUpdating() const
{
return *_internalUpdating;
}
error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout)
{
{
std::lock_guard thread_lock{*_updateMutex};
std::stop_source stop_source;
*_isDataUpdated = false;
// std::future<error_t> update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token());
// // std::future<error_t> update_ft =
// // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token());
// auto status = update_ft.wait_for(timeout);
// if (status == std::future_status::ready) {
// *_isDataUpdated = true;
// _lastUpdateError = update_ft.get();
// } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get
// result
// _lastUpdateError = update_ft.get();
// if (!_lastUpdateError) {
// *_isDataUpdated = true;
// }
// } else { // timeout
// stop_source.request_stop();
// _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
// }
_lastUpdateError = _updateFunc(_internalUpdatingStopSource.get_token());
*_isDataUpdated = true;
}
// unblock waiting threads even in the case of timeout!
_updateCondVar->notify_all();
// *_isDataUpdated = false;
return _lastUpdateError;
}
// block the thread and wait for data to be ready (internal synchronization)
error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout)
{
if (tdata == nullptr) {
return MccTelemetryErrorCode::ERROR_NULLPTR;
}
std::unique_lock ulock(*_updateMutex);
auto res = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; });
if (!res) {
return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
}
// std::lock_guard thread_lock{*_updateMutex};
if (!_lastUpdateError) {
mcc_copy_telemetry_data(_data, tdata);
}
return _lastUpdateError;
}
// just get current data
error_t telemetryData(mcc_telemetry_data_c auto* tdata)
{
if (tdata == nullptr) {
return MccTelemetryErrorCode::ERROR_NULLPTR;
}
std::lock_guard thread_lock{*_updateMutex};
mcc_copy_telemetry_data(_data, tdata);
return MccTelemetryErrorCode::ERROR_OK;
}
error_t lastUpdateError() const
{
return _lastUpdateError;
}
error_t setPointingTarget(mcc_celestial_point_c auto pt)
{
/*
* If apparent coordinates are specified (e.g. AZZD),
* they are assumed to be fixed in time (i.e. pt.time_point will be ignored),
* and other coordinates will be calculated from them
*/
std::lock_guard lock{*_updateMutex};
mcc_copy_celestial_point(pt, &_userTarget);
_data.target.pair_kind = pt.pair_kind;
if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
_data.target.AZ = pt.X;
_data.target.ALT = pt.Y;
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
_data.target.AZ = pt.X;
_data.target.ZD = pt.Y;
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
_data.target.HA = pt.X;
_data.target.DEC_APP = pt.Y;
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
_data.target.RA_APP = pt.X;
_data.target.DEC_APP = pt.Y;
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
_data.target.RA_ICRS = pt.X;
_data.target.DEC_ICRS = pt.Y;
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
_data.target.X = pt.X;
_data.target.Y = pt.Y;
} else {
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
return _updateTargetFunc({});
// return _setTargetFunc(pt);
}
error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy)
{
std::lock_guard lock{*_updateMutex};
if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
*dx = (double)_data.target.AZ - (double)_data.AZ;
*dy = (double)_data.target.ALT - (double)_data.ALT;
} else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
*dx = (double)_data.target.HA - (double)_data.HA;
*dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP;
} else {
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
return MccTelemetryErrorCode::ERROR_OK;
}
error_t targetToMountDist(mcc_angle_c auto* dist)
{
if (dist == nullptr) {
return MccTelemetryErrorCode::ERROR_NULLPTR;
}
std::lock_guard lock{*_updateMutex};
double dHA = _data.HA - _data.target.HA;
double cosDHA = cos(dHA);
double cosT = cos(_data.target.DEC_APP);
double sinT = sin(_data.target.DEC_APP);
double cosM = cos(_data.DEC_APP);
double sinM = sin(_data.DEC_APP);
double term1 = cosT * sin(dHA);
double term2 = cosM * sinT - sinM * cosT * cosDHA;
*dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA)));
return MccTelemetryErrorCode::ERROR_OK;
}
protected:
std::unique_ptr<std::atomic_bool> _isDataUpdated;
MccTelemetryData _data;
MccCelestialPoint _userTarget{};
std::unique_ptr<std::atomic_bool> _internalUpdating;
std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)};
std::unique_ptr<std::mutex> _currentUpdateIntervalMutex;
std::future<error_t> _internalUpdatingFuture{};
std::stop_source _internalUpdatingStopSource{};
std::chrono::nanoseconds _currentUpdateTimeout{defaultInternalUpdateTimeout};
std ::function<error_t(std::stop_token)> _updateTargetFunc{};
// std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
std::function<error_t(std::stop_token)> _updateFunc{};
std::function<error_t()> _setTargetFunc{};
std::unique_ptr<std::mutex> _updateMutex;
std::unique_ptr<std::condition_variable> _updateCondVar;
// std::future<void> _internalUpdatingLoopFuture{};
// std::unique_ptr<std::mutex> _internalUpdatingLoopMutex{new std::mutex()};
// std::unique_ptr<std::condition_variable> _internalUpdatingLoopCondVar{new std::condition_variable()};
// std::unique_ptr<std::atomic_bool> _internalUpdatingLoopStop{new std::atomic_bool{false}};
// std::unique_ptr<std::atomic_bool> _dataUpdatingRequested{new std::atomic_bool{false}};
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
};
static_assert(mcc_telemetry_c<MccTelemetry>, "");
} // namespace mcc

View File

@@ -3,11 +3,12 @@
/* MOUNT CONTROL COMPONENTS LIBRARY */ /* MOUNT CONTROL COMPONENTS LIBRARY */
/* SIMPLE TRACKING MODEL IMPLEMENTATION */ /* SIMPLE Tracking MODEL IMPLEMENTATION */
#include <fstream>
#include "mcc_defaults.h" #include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h" #include "mcc_moving_model_common.h"
namespace mcc namespace mcc
@@ -15,14 +16,18 @@ namespace mcc
enum class MccSimpleTrackingModelErrorCode : int { enum class MccSimpleTrackingModelErrorCode : int {
ERROR_OK, ERROR_OK,
ERROR_CCTE,
ERROR_HW_GETSTATE, ERROR_HW_GETSTATE,
ERROR_HW_SETSTATE, ERROR_HW_SETSTATE,
ERROR_PCM_COMP, ERROR_PCM_COMP,
ERROR_GET_TELEMETRY, ERROR_GET_TELEMETRY,
ERROR_DIST_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP, ERROR_PZONE_CONTAINER_COMP,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE, ERROR_NEAR_PZONE,
ERROR_UNEXPECTED_AXIS_RATES ERROR_IN_PZONE,
ERROR_ALREADY_TRACK,
ERROR_ALREADY_STOPPED,
ERROR_STOPPED
}; };
} // namespace mcc } // namespace mcc
@@ -43,6 +48,65 @@ class is_error_code_enum<mcc::MccSimpleTrackingModelErrorCode> : public true_typ
namespace mcc namespace mcc
{ {
// error category
struct MccSimpleTrackingModelCategory : public std::error_category {
MccSimpleTrackingModelCategory() : std::error_category() {}
const char* name() const noexcept
{
return "SIMPLE-TRACKING-MODEL";
}
std::string message(int ec) const
{
MccSimpleTrackingModelErrorCode err = static_cast<MccSimpleTrackingModelErrorCode>(ec);
switch (err) {
case MccSimpleTrackingModelErrorCode::ERROR_OK:
return "OK";
case MccSimpleTrackingModelErrorCode::ERROR_CCTE:
return "coordinate transformation error";
case MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE:
return "cannot get hardware state";
case MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE:
return "cannot set hardware state";
case MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
case MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY:
return "cannot get telemetry";
case MccSimpleTrackingModelErrorCode::ERROR_DIST_TELEMETRY:
return "cannot get target-to-mount-position distance";
case MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP:
return "pzone container computation error";
case MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE:
return "near prohibited zone";
case MccSimpleTrackingModelErrorCode::ERROR_IN_PZONE:
return "in prohibited zone";
case MccSimpleTrackingModelErrorCode::ERROR_ALREADY_TRACK:
return "already tracking";
case MccSimpleTrackingModelErrorCode::ERROR_ALREADY_STOPPED:
return "tracking is already stopped";
default:
return "UNKNOWN";
}
}
static const MccSimpleTrackingModelCategory& get()
{
static const MccSimpleTrackingModelCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccSimpleTrackingModelErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccSimpleTrackingModelCategory::get());
}
class MccSimpleTrackingModel class MccSimpleTrackingModel
{ {
public: public:
@@ -50,227 +114,438 @@ public:
typedef MccSimpleMovingModelParams tracking_params_t; typedef MccSimpleMovingModelParams tracking_params_t;
// struct tracking_params_t {
// static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second
// double trackSpeedX{}; template <mcc_all_controls_c CONTROLS_T, mcc_logger_c LoggerT = MccNullLogger>
// double trackSpeedY{}; MccSimpleTrackingModel(CONTROLS_T* controls, LoggerT logger)
: _stopTracking(new std::atomic_bool()),
// std::chrono::seconds telemetryTimeout{3}; _currentParamsMutex(new std::mutex()),
// // minimal time to prohibited zone. if it is lesser then exit with error _lastError(MccSimpleTrackingModelErrorCode::ERROR_OK)
// std::chrono::seconds minTimeToPZone{10};
// };
template <mcc_telemetry_data_c TelemetryT,
mcc_hardware_c HardwareT,
mcc_PCM_c PcmT,
mcc_pzone_container_c PZoneContT>
MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
: _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex)
{ {
*_stopTracking = false; std::ostringstream os;
os << std::this_thread::get_id();
// set default values logger.logDebug(std::format("Create MccSimpleTrackingModel class instance (thread: {})", os.str()));
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
_currentTrackParams.trackSpeedX = tracking_params_t::sideralRate; // move along HA-axis with sideral rate
_currentTrackParams.trackSpeedY = 0.0;
_currentTrackParams.telemetryTimeout = std::chrono::seconds(3); *_stopTracking = true;
_currentTrackParams.minTimeToPZone = std::chrono::seconds(10);
}
_trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t { _trackingFunc = [logger = std::move(logger), controls, this]() mutable -> error_t {
typename HardwareT::hardware_state_t hw_state; typename CONTROLS_T::hardware_state_t hw_state;
MccEqtHrzCoords intsc_coords;
MccTelemetryData tdata; MccTelemetryData tdata;
auto t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout); MccEqtHrzCoords intsc_coords;
MccCelestialPoint target_in_future_pt;
bool store_path = false;
std::ofstream fst;
using path_tp_t = std::chrono::duration<double>; // seconds represented as double
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
// double dist, dx, dy;
logger.logInfo("Start tracking:");
logger.logInfo(" timesift: {} millisecs", _currentParams.timeShiftToTargetPoint.count());
logger.logInfo(" min time to pzone: {} secs", _currentParams.minTimeToPZone.count());
auto t_err = controls->telemetryData(&tdata);
if (t_err) { if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY); *_stopTracking = true;
return mcc_deduce_error_code(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
} }
mcc_tp2tp(tdata.time_point, intsc_coords.time_point); auto start_point = tdata.time_point; // needed for trajectory file
bool in_zone;
std::vector<std::chrono::duration<double>> pz_timeto; // in seconds auto pz_err = controls->inPZone(tdata, &in_zone);
std::chrono::duration<double> min_time{0.0};
// compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords);
if (pz_err) { if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
} }
if (in_zone) {
logger.logError("mount current coordinates are in prohibited zone:");
logger.logError(std::format(
" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal(),
mcc::MccAngle{tdata.ALT}.sexagesimal()));
logger.logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(),
mcc::MccAngle{tdata.Y}.sexagesimal()));
return MccSimpleTrackingModelErrorCode::ERROR_IN_PZONE;
}
bool no_intersects = false; bool no_intersects = false;
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { // function to update the closest prohibited zone intersect point
if (std::isfinite(intsc_coords.HA)) { auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state,
intsc_coords.X = intsc_coords.HA; this]() -> error_t {
intsc_coords.Y = intsc_coords.DEC_APP; // compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords);
if (pz_err) {
return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.HA)) {
intsc_coords.X = intsc_coords.HA;
intsc_coords.Y = intsc_coords.DEC_APP;
} else {
no_intersects = true;
// intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
// intsc_coords.Y = tdata.DEC_APP;
}
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.AZ)) {
intsc_coords.X = intsc_coords.AZ;
intsc_coords.Y = intsc_coords.ZD;
} else {
no_intersects = true;
}
} else { } else {
no_intersects = true; static_assert(false, "UNKNOWN MOUNT TYPE!");
intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
intsc_coords.Y = tdata.DEC_APP;
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
if (std::isfinite(intsc_coords.AZ)) {
intsc_coords.X = intsc_coords.AZ;
intsc_coords.Y = intsc_coords.ZD;
} else {
no_intersects = true;
}
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
// compute position in future
auto hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
}
MccPCMResult pcm_inv_res;
// endpoint of the mount moving
auto pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
}
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
{
std::lock_guard lock{*_currentTrackParamsMutex};
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
while (!*_stopTracking) {
// control prohibited zones
pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
min_time = std::chrono::duration<double>{0};
for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
if (pz_timeto[i] < _currentTrackParams.minTimeToPZone) {
return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_timeto[i] < min_time) {
min_time = pz_timeto[i];
}
}
t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
}
if (*_stopTracking) {
break;
}
if (no_intersects) {
if ((intsc_coords.HA - tdata.HA) < 10.0_mins) { // recompute target point
intsc_coords.X += 11.0_hours;
hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
}
pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err,
MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
}
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
{
std::lock_guard lock{*_currentTrackParamsMutex};
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
}
}
} }
return MccSimpleTrackingModelErrorCode::ERROR_OK; return MccSimpleTrackingModelErrorCode::ERROR_OK;
};
} else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
static_assert(false, "NOT IMPLEMENTED!"); auto target_point = [&, this](MccCelestialPoint* point) -> std::error_code {
} else { double dha =
static_assert(false, "UNKNOW MOUNT TYPE!"); std::chrono::duration<double>(_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio)
.count(); // sideral hour seconds
dha *= std::numbers::pi / 43200.0; // radians
auto target_ha = tdata.target.HA + dha;
// auto dt = std::chrono::duration<double>{tdata.HA} +
// _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
_currentParams.timeShiftToTargetPoint);
// point in +time_dist future
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP,
.X = MccAngle(target_ha).normalize<MccAngle::NORM_KIND_180_180>(),
.Y = tdata.target.DEC_APP};
mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);
point->time_point = pt.time_point;
// check for prohibited zone
if (std::isfinite(intsc_coords.HA)) {
bool through_pzone =
(intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone
through_pzone &=
(intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone
if (through_pzone) {
pt.X = intsc_coords.HA;
}
}
auto ret = controls->transformCoordinates(std::move(pt), point);
if (ret) {
return mcc_deduce_error_code(ret, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
} else {
MccPCMResult pcm_inv_res;
// endpoint of the mount moving
auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error_code(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
}
mcc_tp2tp(tdata.time_point + tp_dt, hw_state.time_point);
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
};
pz_err = update_pzones_ipoint();
if (pz_err) {
*_stopTracking = true;
return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
} }
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;
{
std::lock_guard lock{*_currentParamsMutex};
mcc_copy_eqt_hrz_coord(tdata, &tdata.target);
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
*_stopTracking = true;
return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
hw_state.speedX = _currentParams.trackSpeedX;
hw_state.speedY = _currentParams.trackSpeedY;
}
logger.logTrace("The updated target point:");
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true),
MccAngle(target_in_future_pt.Y).sexagesimal());
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(),
MccAngle(target_in_future_pt.Y).sexagesimal());
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
if (!_currentParams.trackingPathFilename.empty()) { // open tracking trajectory file
fst.open(_currentParams.trackingPathFilename);
if (fst.is_open()) {
store_path = true;
logger.logInfo(" timesift: {} millisecs", _currentParams.timeShiftToTargetPoint.count());
logger.logInfo(" min time to pzone: {} secs", _currentParams.minTimeToPZone.count());
fst << "# \n";
fst << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n";
fst << "# Config:\n";
fst << "# timeshift: " << _currentParams.timeShiftToTargetPoint.count() << " millisecs\n";
fst << "# min time to pzone: " << _currentParams.minTimeToPZone.count() << " secs\n";
fst << "# \n";
fst << "# Format (time is in seconds, coordinates are in radians): \n";
fst << "# <time-since-start> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n";
} else {
logger.logError(std::format("Cannot open tracking path file: {}! Do not save it!",
_currentParams.trackingPathFilename));
}
}
}
// move mount
logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
auto hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
*_stopTracking = true;
return mcc_deduce_error_code(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
last_corr_tp = std::chrono::steady_clock::now();
last_ipzone_update_tp = last_corr_tp;
while (!*_stopTracking) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
*_stopTracking = true;
return mcc_deduce_error_code(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (store_path) {
fst << std::chrono::duration_cast<path_tp_t>(tdata.time_point - start_point).count() << " "
<< tdata.target.HA << " " << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP
<< " " << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
<< (int)hw_state.moving_state << "\n";
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logTrace(std::format(" current target: HA = {}, DEC = {}",
mcc::MccAngle(tdata.target.HA).sexagesimal(true),
mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
logger.logTrace(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle(tdata.HA).sexagesimal(true),
mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logTrace(std::format(" target: AZ = {}, ZD = {}",
mcc::MccAngle(tdata.target.AZ).sexagesimal(),
mcc::MccAngle(tdata.target.ZD).sexagesimal()));
logger.logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
mcc::MccAngle(tdata.ZD).sexagesimal()));
}
logger.logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s",
mcc::MccAngleFancyString(tdata.speedX),
mcc::MccAngleFancyString(tdata.speedY)));
if (*_stopTracking) {
break;
}
// control prohibited zones
// if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
// logger.logError("Mount is near zone!");
// *_stopTracking = true;
// return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
// }
// if (pz_err) {
// *_stopTracking = true;
// return mcc_deduce_error_code(pz_err,
// MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
// }
if (*_stopTracking) {
break;
}
// {
// std::lock_guard lock{*_currentParamsMutex};
// auto now = std::chrono::steady_clock::now();
// if ((now - last_corr_tp) > _currentParams.trackingCycleInterval) {
// // update prohibited zones intersection point
// if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
// pz_err = update_pzones_ipoint();
// if (pz_err) {
// *_stopTracking = true;
// return mcc_deduce_error_code(
// pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
// }
// }
// // compute new target-in-future point
// auto ccte_err = target_point(&target_in_future_pt);
// if (ccte_err) {
// *_stopTracking = true;
// return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
// }
// logger.logTrace("The updated target point:");
// if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
// logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true),
// MccAngle(target_in_future_pt.Y).sexagesimal());
// } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
// logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(),
// MccAngle(target_in_future_pt.Y).sexagesimal());
// } else {
// static_assert(false, "UNKNOWN MOUNT TYPE!");
// }
// }
// }
// compute new target-in-future point
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
*_stopTracking = true;
return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
}
logger.logTrace("The updated target point:");
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true),
MccAngle(target_in_future_pt.Y).sexagesimal());
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(),
MccAngle(target_in_future_pt.Y).sexagesimal());
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
// send corrections
logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
*_stopTracking = true;
return mcc_deduce_error_code(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
// sleep here
{
std::lock_guard lock{*_currentParamsMutex};
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
}
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
}; };
} }
MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default; MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default; MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel(const MccSimpleTrackingModel&) = delete;
MccSimpleTrackingModel& operator=(const MccSimpleTrackingModel&) = delete;
error_t trackMount() error_t trackTarget()
{ {
if (!(*_stopTracking)) { // already tracking
return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_TRACK;
}
*_stopTracking = false;
return _trackingFunc(); return _trackingFunc();
} }
error_t stopTracking() error_t stopTracking()
{ {
if (*_stopTracking) { // already stopped
return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_STOPPED;
}
*_stopTracking = true; *_stopTracking = true;
return MccSimpleTrackingModelErrorCode::ERROR_OK; return MccSimpleTrackingModelErrorCode::ERROR_OK;
} }
error_t setTrackingParams(tracking_params_t params) error_t setTrackingParams(tracking_params_t params)
{ {
std::lock_guard lock{*_currentTrackParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
_currentTrackParams = std::move(params); _currentParams = std::move(params);
return MccSimpleTrackingModelErrorCode::ERROR_OK; return MccSimpleTrackingModelErrorCode::ERROR_OK;
} }
tracking_params_t getTrackingParams() const tracking_params_t getTrackingParams() const
{ {
std::lock_guard lock{*_currentTrackParamsMutex}; std::lock_guard lock{*_currentParamsMutex};
return _currentTrackParams; return _currentParams;
}
error_t trackinLastError() const
{
return _lastError;
} }
protected: protected:
std::function<error_t()> _trackingFunc{}; std::function<error_t()> _trackingFunc{};
std::unique_ptr<std::atomic_bool> _stopTracking{}; std::unique_ptr<std::atomic_bool> _stopTracking;
tracking_params_t _currentTrackParams; tracking_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentTrackParamsMutex; std::unique_ptr<std::mutex> _currentParamsMutex{};
error_t _lastError;
}; };
} // namespace mcc } // namespace mcc

View File

@@ -23,6 +23,11 @@ template <typename R>
concept mcc_char_view = std::ranges::view<R> && std::same_as<std::ranges::range_value_t<R>, char>; concept mcc_char_view = std::ranges::view<R> && std::same_as<std::ranges::range_value_t<R>, char>;
// range of char/const char
template <typename R, typename CharT = char>
concept mcc_char_range = std::ranges::range<R> && std::same_as<std::remove_cv_t<std::ranges::range_value_t<R>>, CharT>;
// input range of char/const char // input range of char/const char
template <typename R, typename CharT = char> template <typename R, typename CharT = char>
concept mcc_input_char_range = concept mcc_input_char_range =
@@ -39,6 +44,9 @@ template <typename R>
concept mcc_view_or_output_char_range = mcc_char_view<R> || mcc_output_char_range<R>; concept mcc_view_or_output_char_range = mcc_char_view<R> || mcc_output_char_range<R>;
template <typename R>
concept mcc_range_of_char_range = std::ranges::range<R> && mcc_char_range<std::ranges::range_value_t<R>>;
template <typename R> template <typename R>
concept mcc_range_of_input_char_range = concept mcc_range_of_input_char_range =
std::ranges::range<R> && traits::mcc_input_char_range<std::ranges::range_value_t<R>>; std::ranges::range<R> && traits::mcc_input_char_range<std::ranges::range_value_t<R>>;
@@ -165,6 +173,15 @@ using mcc_func_argN_t = std::conditional_t<N >= mcc_func_traits<T>::arity,
void>; void>;
// std::array
template <typename T>
concept mcc_array_c = requires(T t) {
[]<typename VT, size_t N>(std::array<VT, N>) {
}(t);
};
// non-const lvalue reference, constructible from CtorArgTs (an output argument of function) // non-const lvalue reference, constructible from CtorArgTs (an output argument of function)
template <typename T, typename... CtorArgTs> template <typename T, typename... CtorArgTs>
concept mcc_output_arg_c = !std::is_const_v<std::remove_reference_t<T>> && std::is_lvalue_reference_v<T> && concept mcc_output_arg_c = !std::is_const_v<std::remove_reference_t<T>> && std::is_lvalue_reference_v<T> &&

View File

@@ -3,10 +3,12 @@
#include <charconv> #include <charconv>
#include <cmath> #include <cmath>
#include <cstring> #include <cstring>
#include <expected>
#include <format> #include <format>
#include <numbers> #include <numbers>
#include <ranges> #include <ranges>
#include <regex> #include <regex>
#include "mcc_traits.h"
namespace mcc::utils namespace mcc::utils
{ {
@@ -14,7 +16,8 @@ namespace mcc::utils
static const std::regex decimalNumberRx{" *[-+]?([0-9]*[.])?[0-9]+([eE][-+]?[0-9]+)? *"}; static const std::regex decimalNumberRx{" *[-+]?([0-9]*[.])?[0-9]+([eE][-+]?[0-9]+)? *"};
static const std::regex sexagesimalReprRx{" *[-+]?[0-9]{1,2}:[0-9]{1,2}:([0-9]{0,2}[.])?[0-9]+ *"}; static const std::regex sexagesimalReprRx{" *[-+]?[0-9]{1,2}:[0-9]{1,2}:([0-9]{0,2}[.])?[0-9]+ *"};
static bool isEqual(std::floating_point auto const& v1, std::floating_point auto const& v2)
constexpr static bool isEqual(std::floating_point auto const& v1, std::floating_point auto const& v2)
{ {
constexpr auto eps = std::numeric_limits<std::common_type_t<decltype(v1), decltype(v2)>>::epsilon(); constexpr auto eps = std::numeric_limits<std::common_type_t<decltype(v1), decltype(v2)>>::epsilon();
@@ -24,7 +27,7 @@ static bool isEqual(std::floating_point auto const& v1, std::floating_point auto
enum class TrimType { TRIM_LEFT, TRIM_RIGHT, TRIM_BOTH }; enum class TrimType { TRIM_LEFT, TRIM_RIGHT, TRIM_BOTH };
template <std::ranges::contiguous_range R> template <std::ranges::contiguous_range R>
static std::string_view trimSpaces(R&& r, TrimType type = TrimType::TRIM_BOTH) constexpr static std::string_view trimSpaces(R&& r, TrimType type = TrimType::TRIM_BOTH)
requires std::same_as<char, std::remove_cvref_t<std::ranges::range_value_t<R>>> requires std::same_as<char, std::remove_cvref_t<std::ranges::range_value_t<R>>>
{ {
auto is_space = [](const auto& ch) { return ch == ' '; }; auto is_space = [](const auto& ch) { return ch == ' '; };
@@ -57,7 +60,7 @@ static std::string_view trimSpaces(R&& r, TrimType type = TrimType::TRIM_BOTH)
return std::string_view(f1, f2); return std::string_view(f1, f2);
} }
static std::string_view trimSpaces(const char* r, TrimType type = TrimType::TRIM_BOTH) constexpr static std::string_view trimSpaces(const char* r, TrimType type = TrimType::TRIM_BOTH)
{ {
return trimSpaces(std::string_view(r), type); return trimSpaces(std::string_view(r), type);
} }
@@ -155,7 +158,7 @@ static std::optional<double> parsAngleString(R&& r, bool hms = false)
return std::nullopt; return std::nullopt;
} }
static std::optional<double> parsAngleString(const char* s, bool hms = false) [[maybe_unused]] static std::optional<double> parsAngleString(const char* s, bool hms = false)
{ {
return parsAngleString(std::span{s, std::strlen(s)}, hms); return parsAngleString(std::span{s, std::strlen(s)}, hms);
} }
@@ -241,6 +244,10 @@ static R rad2sxg(double ang, bool hms = false, int prec = 2)
term *= 10.0; term *= 10.0;
} }
// round to given precision of arcseconds/seconds
degs = std::round(degs * 3600.0 * term) / term / 3600.0;
auto d = std::trunc(degs); auto d = std::trunc(degs);
auto s = (degs - d) * 60.0; auto s = (degs - d) * 60.0;
auto m = std::trunc(s); auto m = std::trunc(s);
@@ -258,7 +265,9 @@ static R rad2sxg(double ang, bool hms = false, int prec = 2)
} }
if (ang < 0) { if (ang < 0) {
std::ranges::copy(std::string_view("-"), std::back_inserter(res)); if (!isEqual(d, 0.0) || !isEqual(m, 0.0) || !isEqual(s, 0.0)) {
std::ranges::copy(std::string_view("-"), std::back_inserter(res));
}
} }
std::vformat_to(std::back_inserter(res), std::string_view{fmt.begin(), fmt.end()}, std::make_format_args(d, m, s)); std::vformat_to(std::back_inserter(res), std::string_view{fmt.begin(), fmt.end()}, std::make_format_args(d, m, s));
@@ -313,4 +322,429 @@ static std::string AZZD_rad2sxg(double az, double zd, std::string_view delim = "
} }
// string to pair of angles
// " 12.3453467,102.4345346"
// "12:43:23.423, 102:43:12.124"
// " 12.3453467, 102:43:12.124 "
template <mcc::traits::mcc_input_char_range R>
std::pair<double, double> parseAnglePair(R&& str, bool hms1 = false, bool hms2 = false, std::string_view delim = ",")
{
std::pair<double, double> res{std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN()};
auto found1 = std::ranges::search(std::forward<R>(str), delim);
if (found1.empty()) {
return res;
}
std::string s1, s2;
std::ranges::copy(str.begin(), found1.begin(), std::back_inserter(s1));
auto rem_len = std::distance(found1.end(), str.end());
if (!rem_len) {
return res;
}
auto found2 = std::ranges::search(found1.end(), str.end(), delim.begin(), delim.end());
if (found2.empty()) {
std::ranges::copy(found1.end(), str.end(), std::back_inserter(s2));
} else {
std::ranges::copy(found1.end(), found2.end(), std::back_inserter(s2));
}
auto vo1 = parsAngleString(s1, hms1);
if (vo1) {
auto vo2 = parsAngleString(s2, hms2);
if (vo2) {
res = {vo1.value(), vo2.value()};
}
}
return res;
}
template <traits::mcc_input_char_range R>
static constexpr size_t FNV1aHash(const R& r)
{
static_assert(sizeof(size_t) == 8 || sizeof(size_t) == 4, "ONLY FOR 32 or 64-bit size_t!!!");
size_t hash = 0, prime = 0;
if constexpr (sizeof(size_t) == 8) { // 64-bit
prime = 1099511628211UL;
hash = 14695981039346656037UL;
} else if constexpr (sizeof(size_t) == 4) { // 32-bit
prime = 16777619;
hash = 2166136261;
}
for (const char& ch : r) {
hash ^= ch;
hash *= prime;
}
return hash;
}
static constexpr size_t FNV1aHash(std::forward_iterator auto begin, std::sentinel_for<decltype(begin)> auto end)
requires std::same_as<std::remove_cv_t<std::iter_value_t<decltype(begin)>>, char>
{
static_assert(sizeof(size_t) == 8 || sizeof(size_t) == 4, "ONLY FOR 32 or 64-bit size_t!!!");
size_t hash = 0, prime = 0;
if constexpr (sizeof(size_t) == 8) { // 64-bit
prime = 1099511628211UL;
hash = 14695981039346656037UL;
} else if constexpr (sizeof(size_t) == 4) { // 32-bit
prime = 16777619;
hash = 2166136261;
}
for (auto it = begin; it != end; ++it) {
hash ^= *it;
hash *= prime;
}
return hash;
}
class MccSimpleDeserializer
{
public:
static constexpr std::string_view RANGE_DELIM_SEQ = ",";
MccSimpleDeserializer() : _rangeDelim(RANGE_DELIM_SEQ) {}
template <traits::mcc_input_char_range R>
MccSimpleDeserializer(R&& r) : MccSimpleDeserializer()
{
setRangeDelim(std::forward<R>(r));
}
template <traits::mcc_input_char_range R>
MccSimpleDeserializer& setRangeDelim(R&& r)
{
if (std::ranges::size(std::forward<R>(r))) {
_rangeDelim.clear();
std::ranges::copy(std::forward<R>(r), std::back_inserter(_rangeDelim));
}
return *this;
}
template <traits::mcc_output_char_range R>
R getRangeDelim() const
{
R r;
std::ranges::copy(_rangeDelim, std::back_inserter(r));
return r;
}
std::string getRangeDelim() const
{
return getRangeDelim<std::string>();
}
template <traits::mcc_input_char_range IR, typename VT>
std::error_code operator()(IR&& bytes, VT& value) const
{
std::error_code ret{};
if constexpr (std::is_arithmetic_v<VT>) {
auto v = mcc::utils::numFromStr<VT>(trimSpaces(bytes));
if (!v.has_value()) {
return std::make_error_code(std::errc::invalid_argument);
}
value = v.value();
} else if constexpr (mcc::traits::mcc_output_char_range<VT>) {
VT r;
if constexpr (traits::mcc_array_c<VT>) {
size_t N =
std::ranges::size(r) <= std::ranges::size(bytes) ? std::ranges::size(r) : std::ranges::size(bytes);
for (size_t i = 0; i < N; ++i) {
r[i] = bytes[i];
}
if (std::ranges::size(r) > N) {
for (size_t i = N; i < std::ranges::size(r); ++i) {
r[i] = '\0';
}
}
} else {
std::ranges::copy(bytes, std::back_inserter(r));
}
value = r;
} else if constexpr (std::ranges::range<VT>) {
using el_t = std::ranges::range_value_t<VT>;
if constexpr (std::is_reference_v<el_t> || std::is_const_v<el_t>) { // no reference or constants allowed
return std::make_error_code(std::errc::invalid_argument);
}
VT r;
el_t elem;
size_t i = 0;
if (trimSpaces(bytes).size()) {
auto els = std::views::split(bytes, _rangeDelim);
for (auto const& el : els) {
ret = (*this)(std::string_view(el), elem);
if (!ret) {
if constexpr (traits::mcc_array_c<VT>) {
if (i < std::ranges::size(r)) {
r[i] = elem;
}
++i;
} else {
std::back_inserter(r) = elem;
}
} else {
return std::make_error_code(std::errc::invalid_argument);
}
}
}
value = r;
} else if constexpr (mcc::traits::mcc_time_duration_c<VT>) {
typename VT::rep vd;
ret = (*this)(trimSpaces(bytes), vd);
if (!ret) {
value = VT{vd};
}
} else {
ret = std::make_error_code(std::errc::invalid_argument);
}
return ret;
}
protected:
std::string _rangeDelim;
};
/* key-value pair holder */
// to follow std::variant requirements (not references, not array, not void)
template <typename T>
concept variant_valid_type_c = requires { !std::is_array_v<T> && !std::is_void_v<T> && !std::is_reference_v<T>; };
template <typename T>
concept keyvalue_record_c = requires(T t) {
requires std::same_as<decltype(t.key), std::string_view>;
requires variant_valid_type_c<decltype(t.value)>;
};
template <typename T>
concept keyvalue_desc_c = requires(T t) { []<keyvalue_record_c... Ts>(std::tuple<Ts...>) {}(t); };
template <keyvalue_desc_c DESCR_T>
class KeyValueHolder
{
public:
static constexpr std::string_view COMMENT_SEQ{"#"};
static constexpr std::string_view KEY_VALUE_DELIM{"="};
static constexpr std::string_view VALUE_ARRAY_DELIM{","};
inline static auto defaultDeserializeFunc = []<typename VT>(this auto&& self, std::string_view str, VT& value) {
std::error_code ret{};
if constexpr (std::is_arithmetic_v<VT>) {
auto v = mcc::utils::numFromStr<VT>(trimSpaces(str));
if (!v.has_value()) {
return std::make_error_code(std::errc::invalid_argument);
}
value = v.value();
} else if constexpr (mcc::traits::mcc_output_char_range<VT>) {
VT r;
std::ranges::copy(str, std::back_inserter(r));
value = r;
} else if constexpr (std::ranges::range<VT>) {
using el_t = std::ranges::range_value_t<VT>;
if constexpr (std::is_reference_v<el_t> || std::is_const_v<el_t>) { // no reference or constants allowed
return std::make_error_code(std::errc::invalid_argument);
}
VT r;
el_t elem;
auto els = std::views::split(str, VALUE_ARRAY_DELIM);
for (auto const& el : els) {
ret = std::forward<decltype(self)>(self)(std::string_view(el), elem);
if (!ret) {
std::back_inserter(r) = elem;
} else {
return std::make_error_code(std::errc::invalid_argument);
}
}
value = r;
} else if constexpr (mcc::traits::mcc_time_duration_c<VT>) {
typename VT::rep vd;
ret = std::forward<decltype(self)>(self)(trimSpaces(str), vd);
if (!ret) {
value = VT{vd};
}
} else {
ret = std::make_error_code(std::errc::invalid_argument);
}
return ret;
};
KeyValueHolder(DESCR_T desc) : _keyValue(desc)
{
[this]<size_t... I>(std::index_sequence<I...>) {
((_hashes[I] = FNV1aHash(std::get<I>(_keyValue).key)), ...);
}(std::make_index_sequence<std::tuple_size_v<DESCR_T>>());
}
template <typename T>
std::expected<T, std::error_code> getValue(std::string_view key) const
{
T v;
auto err = forKey(key, [&v]<typename VT>(const VT& val) {
if constexpr (std::convertible_to<VT, T>) {
v = val;
return std::error_code();
} else {
return std::make_error_code(std::errc::invalid_argument);
}
});
if (err) {
return std::unexpected(err);
} else {
return v;
}
}
template <typename T>
std::error_code setValue(std::string_view key, const T& value)
{
return forKey(key, [value]<typename VT>(VT& val) {
if constexpr (std::convertible_to<T, VT>) {
val = value;
return std::error_code();
} else if constexpr (std::constructible_from<VT, T>) {
val = VT(value);
return std::error_code();
} else {
return std::make_error_code(std::errc::invalid_argument);
}
});
}
template <std::ranges::contiguous_range R, std::ranges::input_range RecDelimT = std::string_view>
std::error_code fromCharRange(const R& buffer, RecDelimT rec_delim = std::string_view("\n"))
{
// return fromCharRange(buffer, KeyValueHolder::defaultDeserializeFunc, std::move(rec_delim));
return fromCharRange(buffer, MccSimpleDeserializer{}.setRangeDelim(VALUE_ARRAY_DELIM), std::move(rec_delim));
}
template <std::ranges::contiguous_range R,
typename DeserFuncT,
std::ranges::input_range RecDelimT = std::string_view>
std::error_code fromCharRange(const R& buffer,
DeserFuncT&& deser_func,
RecDelimT rec_delim = std::string_view("\n"))
{
// static_assert(mcc::traits::mcc_callable_c<std::decay_t<DeserFuncT>>, "!!!!!!!");
if constexpr (std::is_array_v<std::decay_t<R>>) { // char*, const char*
if constexpr (std::is_array_v<std::decay_t<RecDelimT>>) {
return fromCharRange(std::string_view{buffer}, std::forward<DeserFuncT>(deser_func),
std::string_view(rec_delim));
} else {
return fromCharRange(std::string_view{buffer}, std::forward<DeserFuncT>(deser_func),
std::move(rec_delim));
}
} else {
if constexpr (std::is_array_v<std::decay_t<RecDelimT>>) {
return fromCharRange(buffer, std::forward<DeserFuncT>(deser_func), std::string_view(rec_delim));
}
}
std::error_code ec{};
std::string_view rec, key, value;
auto recs = std::views::split(buffer, std::move(rec_delim));
for (auto const& el : recs) {
rec = mcc::utils::trimSpaces(el, TrimType::TRIM_LEFT);
if (rec.size()) {
auto found = std::ranges::search(rec, COMMENT_SEQ);
if (found.begin() != rec.end()) { // there was the comment sequence in record
rec = std::string_view(rec.begin(), found.begin());
}
if (rec.size()) {
found = std::ranges::search(rec, KEY_VALUE_DELIM);
if (found.begin() != rec.begin()) { // ignore an empty key
key = trimSpaces(std::string_view(rec.begin(), found.begin()), TrimType::TRIM_RIGHT);
value = std::string_view(found.end(), rec.end());
ec = forKey(key, [value, &deser_func]<typename VT>(VT& v) { return deser_func(value, v); });
}
} // just comment string starting from the beginning, just skip it (no error)
} // empty record, just skip it (no error)
if (ec) {
break;
}
}
return ec;
}
protected:
DESCR_T _keyValue;
std::array<size_t, std::tuple_size_v<DESCR_T>> _hashes;
//
// NOTE: deduced this is needed here to use "forKey" method in getter and setter (const and non-const contexts)!!!
//
std::error_code forKey(this auto&& self, std::string_view key, auto&& func)
{
return std::forward<decltype(self)>(self).template forHash<0>(FNV1aHash(key),
std::forward<decltype(func)>(func));
}
template <size_t I = 0>
std::error_code forHash(this auto&& self, size_t hash, auto&& func)
{
if constexpr (I < std::tuple_size_v<DESCR_T>) {
if (hash == std::forward<decltype(self)>(self)._hashes[I]) {
return std::forward<decltype(func)>(func)(
std::get<I>(std::forward<decltype(self)>(self)._keyValue).value);
} else {
return std::forward<decltype(self)>(self).template forHash<I + 1>(hash,
std::forward<decltype(func)>(func));
}
}
return std::make_error_code(std::errc::argument_out_of_domain);
}
};
} // namespace mcc::utils } // namespace mcc::utils

View File

@@ -20,7 +20,7 @@ int main()
auto now = std::chrono::system_clock::now(); auto now = std::chrono::system_clock::now();
mcc::MccCelestialPoint cp{ mcc::MccCelestialPoint cp{
.pair_kind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS, .X = "10:20:30.44"_hms, .Y = "20:30:40.55"_dms}; .pair_kind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS, .X = "17:20:30.44"_hms, .Y = "20:30:40.55"_dms};
mcc::MccEqtHrzCoords eqhrz; mcc::MccEqtHrzCoords eqhrz;
eqhrz.time_point = now; eqhrz.time_point = now;
@@ -140,5 +140,68 @@ int main()
erfa.refractionCorrection(cp, &dZ); erfa.refractionCorrection(cp, &dZ);
std::cout << "dZ(" << mcc::MccAngle(cp.Y).degrees() << ") = " << mcc::MccAngle(dZ).arcsecs() << "\n"; std::cout << "dZ(" << mcc::MccAngle(cp.Y).degrees() << ") = " << mcc::MccAngle(dZ).arcsecs() << "\n";
std::cout << "\n\n";
// std::string sc2{" 11:34:21.21 , 104.4567892"};
std::string sc2{" 11:34:21.21 , 1.044567892E02"};
auto p2 = mcc::utils::parseAnglePair(sc2);
std::cout << "ORIG STR: '" << sc2 << "'\n";
std::cout << p2.first << "; " << p2.second << "\n";
mcc::MccAngle a1{p2.first, mcc::MccDegreeTag{}};
mcc::MccAngle a2{p2.second, mcc::MccDegreeTag{}};
std::cout << a1.sexagesimal() << "\n" << a2.sexagesimal() << "\n";
std::cout << "\n\n";
std::memset(&eqhrz, 0, sizeof(eqhrz));
eqhrz.X = "12:13:10.08"_hms;
eqhrz.Y = "0:07:39.5"_dms;
eqhrz.X = "00:00:0.0"_hms;
eqhrz.Y = "00:00:00.0"_dms;
// eqhrz.pair_kind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP;
// eqhrz.pair_kind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
eqhrz.pair_kind = mcc::MccCoordPairKind::COORDS_KIND_AZZD;
eqhrz.time_point = std::chrono::system_clock::now();
erfa.transformCoordinates(eqhrz, &eqhrz);
// std::cout << "RA_ICRS = " << mcc::MccAngle(cp.X).sexagesimal(true) << "\n";
// std::cout << "DEC_ICRS = " << mcc::MccAngle(cp.Y).sexagesimal() << "\n\n";
std::cout << "time point = " << eqhrz.time_point << "\n";
std::cout << "RA_ICRS = " << mcc::MccAngle(eqhrz.RA_ICRS).sexagesimal(true) << "\n";
std::cout << "DEC_ICRS = " << mcc::MccAngle(eqhrz.DEC_ICRS).sexagesimal() << "\n";
std::cout << "RA_APP = " << mcc::MccAngle(eqhrz.RA_APP).sexagesimal(true) << "\n";
std::cout << "DEC_APP = " << mcc::MccAngle(eqhrz.DEC_APP).sexagesimal() << "\n";
std::cout << "HA = " << mcc::MccAngle(eqhrz.HA).sexagesimal(true) << "\n";
std::cout << "AZ = " << mcc::MccAngle(eqhrz.AZ).sexagesimal() << "\n";
std::cout << "ZD = " << mcc::MccAngle(eqhrz.ZD).sexagesimal() << "\n";
std::cout << "ALT = " << mcc::MccAngle(eqhrz.ALT).sexagesimal() << "\n";
// std::cout << "\n";
// std::cout << "\n";
// state.meteo = {10.0, 0.5, 0.0}; // refraction must be 0!!!
// erfa.setStateERFA(state);
// eqhrz.X = "00:00:0.0"_hms;
// eqhrz.Y = "60:00:00.0"_dms;
// // restore useful quantities
// state.meteo = {10.0, 0.5, 1010.0};
// erfa.setStateERFA(state);
return 0; return 0;
} }

View File

@@ -0,0 +1,85 @@
#include <iostream>
#include "../mcc_ccte_erfa_new.h"
#include "../mcc_coord.h"
using namespace mcc;
typedef MccGenericSkyPoint<mcc::ccte::erfa::MccCCTE_ERFA> skypt_t;
static skypt_t::ccte_t::engine_state_t saoras{.meteo{.temperature = 0.0, .humidity = 0.5, .pressure = 1010.0},
.wavelength = 0.5,
.lat = 43.646711_degs,
.lon = 41.440732_degs,
.elev = 2100.0};
// skypt_t::cctEngine.setStateERFA(saoras);
static_assert(mcc_angle_c<double>, "!!!!!!!!!!!!");
int main()
{
skypt_t::cctEngine.setStateERFA(saoras);
skypt_t pt;
MccSkyRADEC_ICRS icrs(0.0, 70.0_degs);
pt = icrs;
MccSkyRADEC_OBS radec_obs{0.0, 0.0};
MccSkyRADEC_APP radec_app;
MccSkyAZALT azalt{0, 0};
MccSkyAZZD azzd{0, 0};
MccSkyHADEC_OBS hadec_obs;
MccAngle eo, lst;
skypt_t::cctEngine.equationOrigins(radec_obs.epoch(), &eo);
skypt_t::cctEngine.apparentSideralTime(radec_obs.epoch(), &lst, true);
std::cout << "EO = " << eo.sexagesimal(true) << "\n";
std::cout << "LST = " << lst.sexagesimal(true) << "\n\n";
pt.to(radec_obs, azalt, azzd, radec_app);
std::cout << "FROM ICRS TO OBSERVED:\n";
std::cout << "RA_ICRS = " << icrs.x().sexagesimal(true) << "\n";
std::cout << "DEC_ICRS = " << icrs.y().sexagesimal() << "\n";
std::cout << "OBS COORD EPOCH: " << radec_obs.epoch().UTC() << "\n";
std::cout << "RA_OBS = " << radec_obs.x().sexagesimal(true) << "\n";
std::cout << "DEC_OBS = " << radec_obs.y().sexagesimal() << "\n";
std::cout << "AZ = " << azalt.x().sexagesimal() << "\n";
std::cout << "ALT = " << azalt.y().sexagesimal() << "\n";
std::cout << "ZD = " << azzd.y().sexagesimal() << "\n";
std::cout << "RA_APP = " << radec_app.x().sexagesimal(true) << "\n";
std::cout << "DEC_APP = " << radec_app.y().sexagesimal() << "\n";
// radec_obs = {10.2387983_degs, "43:21:34.5465"_dms};
icrs.setX(111.0_degs), icrs.setY(111.0_degs);
azzd.setX(111.0_degs), azzd.setY(111.0_degs);
azalt.setX(111.0_degs), azalt.setY(111.0_degs);
hadec_obs.setX(111.0_degs), hadec_obs.setY(111.0_degs);
pt = radec_obs;
pt.to(icrs, azzd, hadec_obs);
std::cout << "\n\nFROM OBSERVED TO ICRS:\n";
std::cout << "OBS COORD EPOCH: " << radec_obs.epoch().UTC() << "\n";
std::cout << "RA_OBS = " << radec_obs.x().sexagesimal(true) << "\n";
std::cout << "DEC_OBS = " << radec_obs.y().sexagesimal() << "\n";
std::cout << "RA_ICRS = " << icrs.x().sexagesimal(true) << "\n";
std::cout << "DEC_ICRS = " << icrs.y().sexagesimal() << "\n";
std::cout << "\n\nFROM OBSERVED TO OBSERVED:\n";
std::cout << "OBS COORD EPOCH: " << radec_obs.epoch().UTC() << "\n";
std::cout << "RA_OBS = " << radec_obs.x().sexagesimal(true) << "\n";
std::cout << "DEC_OBS = " << radec_obs.y().sexagesimal() << "\n";
std::cout << "HA_OBS = " << hadec_obs.x().sexagesimal(true) << "\n";
std::cout << "AZ = " << azzd.x().sexagesimal() << "\n";
std::cout << "ZD = " << azzd.y().sexagesimal() << "\n";
azalt = pt;
std::cout << "ALT = " << azalt.y().sexagesimal() << "\n";
return 0;
}

130
mcc/tests/netmsg_test.cpp Normal file
View File

@@ -0,0 +1,130 @@
#include <iostream>
#include <list>
#include "../mcc_defaults.h"
#include "../mcc_netserver_proto.h"
int main()
{
// using msg1_t = mcc::network::MccNetMessage<mcc::network::MccNetMessageValidKeywords, std::string_view>;
// using msg2_t = mcc::network::MccNetMessage<mcc::network::MccNetMessageValidKeywords, std::string>;
using msg1_t = mcc::network::MccNetMessage<std::string_view>;
using msg2_t = mcc::network::MccNetMessage<std::string>;
mcc::network::MccNetMessage msg3("ACK");
std::string_view sv{"TARGET 11:22:33.212; -0.23876984; RADEC "};
std::vector<char> arr{sv.begin(), sv.end()};
msg1_t msg1;
msg1.fromCharRange(arr);
std::cout << "msg.key = <" << msg1.keyword() << ">\n";
std::cout << "msg.par[1] = <" << msg1.param(1) << ">\n";
std::vector<double> vd{1.1, 2.2, 3.3};
msg2_t msg2("ACK", 12.43298042, "EEE", std::chrono::seconds(10), vd);
// msg2_t msg2("ACK");
std::cout << "msg.bytes = <" << msg2.byteRepr() << ">\n";
std::cout << "msg.key = <" << msg2.keyword() << ">\n";
std::cout << "msg.par[1] = <" << msg2.param(1) << ">\n";
std::cout << "msg.par[2] = <" << msg2.param(2) << ">\n";
auto p2 = msg2.paramValue<std::chrono::seconds>(2);
std::cout << "msg.par[2] = <" << p2.value_or(std::chrono::seconds{}) << ">\n";
std::cout << "msg.par[3] = <";
auto p3 = msg2.paramValue<decltype(vd)>(3);
if (p3.has_value()) {
for (auto const& el : p3.value()) {
std::cout << el << " ";
}
}
std::cout << ">\n";
std::cout << "msg.par[1-2] joined = <" << msg2.params(1, 2) << ">\n";
auto vv = msg2.params<std::list<std::string_view>>(1, 3);
std::cout << "msg.par[1-3] array = <";
for (auto const& el : vv) {
std::cout << el << " ";
}
std::cout << ">\n";
mcc::MccCelestialPoint cpt{.pair_kind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP,
.time_point = std::chrono::system_clock::now(),
.X = mcc::MccAngle("10:23:56.12331", mcc::mcc_hms),
.Y = mcc::MccAngle(67.9827148715, mcc::mcc_degrees)};
// msg2.construct("ACK", "MOUNT", msg2_t::CFMT_DEGREES, msg2_t::MccNetMessageCoordPrec{2, 3}, cpt);
// msg2.construct("ACK", "MOUNT", mcc::network::MccNetMessageCoordPrec{2, 3}, cpt);
msg2.construct("ACK", "MOUNT", mcc::MccCoordinateSerializer::SerializedCoordFormat::CFMT_SGM,
mcc::MccCoordinateSerializer::SexagesimalCoordPrec{3, 2}, cpt);
std::cout << "MSG2: " << msg2.byteRepr() << "\n";
auto cpt1 = msg2.paramValue<mcc::MccCelestialPoint>(1);
if (cpt1) {
std::cout << "cpt.time_point = " << cpt1.value().time_point << "\n";
std::cout << "cpt.X = " << cpt1.value().X << "\n";
}
std::cout << "\n\n\n";
mcc::MccCelestialPointSerializer cpser;
mcc::MccCelestialPointDeserializer cpdser;
std::string s;
cpser(cpt, s);
std::cout << "Serialized: " << s << "\n";
// auto err = cpdser(s, cpt);
std::error_code err = cpdser(s, cpt);
if (err) {
std::cout << "deserialization error: " << err.message() << "\n";
} else {
std::cout << "Deserialized: tp = " << cpt.time_point << "; kind = " << mcc::MccCoordPairKindToStr(cpt.pair_kind)
<< "\n";
}
std::cout << "\n\n\n";
mcc::MccCelestialCoordEpoch cep;
std::cout << "UTC: " << cep.UTC() << "\n";
std::cout << "MJD: " << cep.MJD() << "\n";
std::cout << "Epoch: " << cep.JEpoch() << "\n";
cep.fromTimePoint(std::chrono::system_clock::now());
std::cout << "\n";
std::cout << "UTC: " << cep.UTC() << "\n";
std::cout << "MJD: " << cep.MJD() << "\n";
std::cout << "Epoch: " << cep.JEpoch() << "\n";
cep.fromCharRange("60958.90342");
std::cout << "\n";
std::cout << "UTC: " << cep.UTC() << "\n";
std::cout << "MJD: " << cep.MJD() << "\n";
std::cout << "Epoch: " << cep.JEpoch(2) << "\n";
cep -= std::chrono::days(10);
std::cout << "\n";
std::cout << "UTC: " << cep.UTC() << "\n";
std::cout << "MJD: " << cep.MJD() << "\n";
std::cout << "Epoch: " << cep.JEpoch() << "\n";
std::cout << "\n" << (cep = mcc::MccCelestialCoordEpoch::now()).UTC() << "\n";
std::cout << "MJD: " << cep.MJD() << "\n";
std::cout << "Epoch: " << cep.JEpoch() << "\n";
auto ep1 = mcc::MccCelestialCoordEpoch::now();
auto ep2 = mcc::MccCelestialCoordEpoch::now();
std::cout << "\n" << std::boolalpha << (ep1 < ep2) << "\n";
std::cout << "\n"
<< std::boolalpha << (mcc::MccCelestialCoordEpoch::now() != mcc::MccCelestialCoordEpoch::now()) << "\n";
return 0;
}