Compare commits
191 Commits
c862d160fe
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 01261272b4 | |||
| e61146db69 | |||
| 2eb7b214b2 | |||
| 27b1cecc7b | |||
| 9e84306a94 | |||
| c0c36388db | |||
| ed24d0b9e2 | |||
| bccc7a9b29 | |||
| 873f292a11 | |||
| f7cb279841 | |||
| fd96dc395b | |||
|
|
0aa0113be3 | ||
| 01d5657b1b | |||
| 09cf5f9c19 | |||
| 66c3c7e6c7 | |||
| fd2776084a | |||
|
|
bbc35b02bb | ||
|
|
8ce6ffc41c | ||
| 54d6c25171 | |||
| 2c7d563994 | |||
| bc12777f18 | |||
| 7948321cce | |||
| b12c0ec521 | |||
|
|
d417c03f59 | ||
| a8dd511366 | |||
| a30729fb37 | |||
| a70339c55e | |||
| 6fca94e571 | |||
| 255c34dbb2 | |||
| 3c0c719e37 | |||
|
|
0ddd633bc9 | ||
| 28ecf307a8 | |||
| 57467ce48f | |||
| 196ed3be1b | |||
| acd26edc9c | |||
| da9cd51e5c | |||
| d868810048 | |||
|
|
7c9612c3a2 | ||
| 54419b8e60 | |||
| 7dfb0d5e9b | |||
| bbf7314592 | |||
| 6dde28e8d9 | |||
| 9066b3f091 | |||
| c514d4adcc | |||
| cca58e8ba9 | |||
| bf55a45cf9 | |||
| a825a6935b | |||
| 43638f383f | |||
| a42f6dbc98 | |||
|
|
acced75fa2 | ||
| e548451617 | |||
| e529265a63 | |||
| b2c27a6f5c | |||
| 6214b82a6c | |||
|
|
c6b47d8ad6 | ||
| 273f239abb | |||
|
|
14e583a244 | ||
| 771619b832 | |||
|
|
e0c8d8f39b | ||
|
|
0ce4430668 | ||
|
|
e18066e4a6 | ||
| 1c774d2d69 | |||
| 9e8a7a62c9 | |||
| 1ea5fb623d | |||
| 078e3f38f2 | |||
| 94fb4c6a48 | |||
| b3a257fab6 | |||
| 08ad1e665b | |||
| 90acf1ee8c | |||
| 15cf04f164 | |||
| 6fc0b8bb4e | |||
| c0f274cec0 | |||
|
|
511956531e | ||
| 3f108fcc13 | |||
| 683da9739d | |||
| a7fbae47f0 | |||
| 8a202bd38c | |||
| d69ea51b0c | |||
| a1fa54c636 | |||
|
|
cb362c6e49 | ||
| f2be52d17c | |||
| 3682ccdda6 | |||
|
|
85259fc6ad | ||
| 620f8ba136 | |||
| 50e79aa0ae | |||
| 6a72ead855 | |||
| bc300bb3de | |||
| 78e4bb182c | |||
|
|
85dfa2e9a5 | ||
|
|
bdfc5dbc1c | ||
|
|
ec27cd981a | ||
| 47c57dca72 | |||
| e6b4604bfa | |||
| 412f038eb0 | |||
|
|
80ec2382ea | ||
| 42a4349c76 | |||
|
|
e50fbfc57e | ||
| 49a2e2f9c1 | |||
| fc64642bd6 | |||
|
|
cbe106fe95 | ||
| f618fb64cb | |||
|
|
04272b8e1d | ||
| e0e10395fb | |||
|
|
27dccfe7c0 | ||
| 8b16ac79b8 | |||
| 58d62d85b3 | |||
|
|
9c13def8be | ||
| 5fe2788cd7 | |||
| 962504ed98 | |||
|
|
4d7e830798 | ||
|
|
3d769d79eb | ||
| 0b7261a431 | |||
| c5aa3dc495 | |||
| 98c46c2b8c | |||
| d8fae31406 | |||
| 4a9ecf8639 | |||
|
|
b8383c1375 | ||
| f729799335 | |||
| b1a48d2b77 | |||
| fedc324410 | |||
|
|
1a4d998141 | ||
| 0f955b3c91 | |||
| f5039a329b | |||
| 83b7e0d924 | |||
| 1087e043a8 | |||
| 281ceacf89 | |||
| 4e3a50acba | |||
| 732cd33947 | |||
|
|
bb41710645 | ||
|
|
0b084f44f6 | ||
|
|
92b1a3cfd5 | ||
| 3ae2d41fc8 | |||
| c7dd816481 | |||
| 5f802ff57e | |||
| 8e8cb543ae | |||
| ab49f927fb | |||
| 00354d9b41 | |||
| 2478c1e8d2 | |||
|
|
460fc360c6 | ||
|
|
36ffde80f5 | ||
| fe6492e4fc | |||
|
|
de80acf315 | ||
| 3d3b57a311 | |||
| 227f501d6f | |||
|
|
218da42a1d | ||
|
|
c2627ecd89 | ||
| 4696daa2ee | |||
|
|
2e5e1918e1 | ||
| 45f655dc90 | |||
| 9fb33e5bec | |||
|
|
31cf0a45dd | ||
|
|
4bf95c1043 | ||
| 052d4e2eb4 | |||
| 7556539084 | |||
|
|
8b1873b40b | ||
| 0295d93cd3 | |||
| 60cade4d1f | |||
| dc87ce0fb9 | |||
| 06c8345fc9 | |||
|
|
33002f1711 | ||
| 99a28d87ec | |||
| c6a1caea08 | |||
|
|
da46ab3e3b | ||
| 3640882874 | |||
| ca74d1dd73 | |||
|
|
6c10c6b6ff | ||
| 61e41b1a1d | |||
|
|
e934bd3932 | ||
| 3659ddab01 | |||
| e59f9a05b2 | |||
| 22097610f9 | |||
| b99263a014 | |||
| c45d17b236 | |||
| a3b901223a | |||
| 34d22614a6 | |||
| f0ab4ae770 | |||
| 2fee216924 | |||
| 6315d5e18e | |||
| 138e4bf84d | |||
| 864e257884 | |||
| f661dfad44 | |||
| 219cec6055 | |||
| 25438960e6 | |||
| 9bfe1c3ad5 | |||
| 750d29ceb9 | |||
| 9fbd858086 | |||
| ca9dcdfa68 | |||
| c39496ff25 | |||
| a96682bc12 | |||
| 04ee999159 | |||
| 502014bee4 |
@@ -2,14 +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(asibfm700)
|
||||||
|
|||||||
218
LibSidServo/.qtcreator/libsidservo.creator.user
Normal file
218
LibSidServo/.qtcreator/libsidservo.creator.user
Normal 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<int>" 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<int>" 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>
|
||||||
218
LibSidServo/.qtcreator/libsidservo.creator.user.cf63021
Normal file
218
LibSidServo/.qtcreator/libsidservo.creator.user.cf63021
Normal 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<int>" 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<int>" 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>
|
||||||
221
LibSidServo/PID.c
Normal file
221
LibSidServo/PID.c
Normal file
@@ -0,0 +1,221 @@
|
|||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <strings.h>
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include "PID.h"
|
||||||
|
#include "serial.h"
|
||||||
|
|
||||||
|
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
|
||||||
|
if(!gain || Iarrsz < 3) return NULL;
|
||||||
|
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
|
||||||
|
pid->gain = *gain;
|
||||||
|
DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
|
||||||
|
pid->pidIarrSize = Iarrsz;
|
||||||
|
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
|
||||||
|
return pid;
|
||||||
|
}
|
||||||
|
|
||||||
|
// don't clear lastT!
|
||||||
|
void pid_clear(PIDController_t *pid){
|
||||||
|
if(!pid) return;
|
||||||
|
DBG("CLEAR PID PARAMETERS");
|
||||||
|
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||||
|
pid->integral = 0.;
|
||||||
|
pid->prev_error = 0.;
|
||||||
|
pid->curIidx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pid_delete(PIDController_t **pid){
|
||||||
|
if(!pid || !*pid) return;
|
||||||
|
if((*pid)->pidIarray) free((*pid)->pidIarray);
|
||||||
|
free(*pid);
|
||||||
|
*pid = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
double pid_calculate(PIDController_t *pid, double error, double dt){
|
||||||
|
// calculate flowing integral
|
||||||
|
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
|
||||||
|
//DBG("oldi/new: %g, %g", oldi, newi);
|
||||||
|
pid->pidIarray[pid->curIidx++] = newi;
|
||||||
|
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
|
||||||
|
pid->integral += newi - oldi;
|
||||||
|
double derivative = (error - pid->prev_error) / dt;
|
||||||
|
pid->prev_error = error;
|
||||||
|
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
|
||||||
|
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
|
||||||
|
return sum;
|
||||||
|
}
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
PIDController_t *PIDC;
|
||||||
|
PIDController_t *PIDV;
|
||||||
|
} PIDpair_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
axis_status_t state;
|
||||||
|
coordval_t position;
|
||||||
|
coordval_t speed;
|
||||||
|
} axisdata_t;
|
||||||
|
/**
|
||||||
|
* @brief process - Process PID for given axis
|
||||||
|
* @param tagpos - given coordinate of target position
|
||||||
|
* @param endpoint - endpoint for this coordinate
|
||||||
|
* @param pid - pid itself
|
||||||
|
* @return calculated new speed or -1 for max speed
|
||||||
|
*/
|
||||||
|
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
|
||||||
|
double dt = timediff(&tagpos->t, &axis->position.t);
|
||||||
|
if(dt < 0 || dt > Conf.PIDMaxDt){
|
||||||
|
DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt);
|
||||||
|
return axis->speed.val; // data is too old or wrong
|
||||||
|
}
|
||||||
|
double error = tagpos->val - axis->position.val, fe = fabs(error);
|
||||||
|
DBG("error: %g", error);
|
||||||
|
PIDController_t *pid = NULL;
|
||||||
|
switch(axis->state){
|
||||||
|
case AXIS_SLEWING:
|
||||||
|
if(fe < Conf.MaxPointingErr){
|
||||||
|
axis->state = AXIS_POINTING;
|
||||||
|
DBG("--> Pointing");
|
||||||
|
pid = pidpair->PIDC;
|
||||||
|
}else{
|
||||||
|
DBG("Slewing...");
|
||||||
|
return NAN; // max speed for given axis
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case AXIS_POINTING:
|
||||||
|
if(fe < Conf.MaxFinePointingErr){
|
||||||
|
axis->state = AXIS_GUIDING;
|
||||||
|
DBG("--> Guiding");
|
||||||
|
pid = pidpair->PIDV;
|
||||||
|
}else if(fe > Conf.MaxPointingErr){
|
||||||
|
DBG("--> Slewing");
|
||||||
|
axis->state = AXIS_SLEWING;
|
||||||
|
return NAN;
|
||||||
|
} else pid = pidpair->PIDC;
|
||||||
|
break;
|
||||||
|
case AXIS_GUIDING:
|
||||||
|
pid = pidpair->PIDV;
|
||||||
|
if(fe > Conf.MaxFinePointingErr){
|
||||||
|
DBG("--> Pointing");
|
||||||
|
axis->state = AXIS_POINTING;
|
||||||
|
pid = pidpair->PIDC;
|
||||||
|
}else if(fe < Conf.MaxGuidingErr){
|
||||||
|
DBG("At target");
|
||||||
|
// TODO: we can point somehow that we are at target or introduce new axis state
|
||||||
|
}else DBG("Current error: %g", fe);
|
||||||
|
break;
|
||||||
|
case AXIS_STOPPED: // start pointing to target; will change speed next time
|
||||||
|
DBG("AXIS STOPPED!!!! --> Slewing");
|
||||||
|
axis->state = AXIS_SLEWING;
|
||||||
|
return getspeed(tagpos, pidpair, axis);
|
||||||
|
case AXIS_ERROR:
|
||||||
|
DBG("Can't move from erroneous state");
|
||||||
|
return 0.;
|
||||||
|
}
|
||||||
|
if(!pid){
|
||||||
|
DBG("WTF? Where is a PID?");
|
||||||
|
return axis->speed.val;
|
||||||
|
}
|
||||||
|
double dtpid = timediff(&tagpos->t, &pid->prevT);
|
||||||
|
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
|
||||||
|
DBG("time diff too big: clear PID");
|
||||||
|
pid_clear(pid);
|
||||||
|
}
|
||||||
|
if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
|
||||||
|
pid->prevT = tagpos->t;
|
||||||
|
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
|
||||||
|
double tagspeed = pid_calculate(pid, error, dtpid);
|
||||||
|
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
|
||||||
|
return tagspeed; // coordinate-based
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief correct2 - recalculate PID and move telescope to new point with new speed
|
||||||
|
* @param target - target position (for error calculations)
|
||||||
|
* @param endpoint - stop point (some far enough point to stop in case of hang)
|
||||||
|
* @return error code
|
||||||
|
*/
|
||||||
|
mcc_errcodes_t correct2(const coordval_pair_t *target){
|
||||||
|
static PIDpair_t pidX = {0}, pidY = {0};
|
||||||
|
if(!pidX.PIDC){
|
||||||
|
pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||||
|
if(!pidX.PIDC) return MCC_E_FATAL;
|
||||||
|
pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||||
|
if(!pidX.PIDV) return MCC_E_FATAL;
|
||||||
|
}
|
||||||
|
if(!pidY.PIDC){
|
||||||
|
pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||||
|
if(!pidY.PIDC) return MCC_E_FATAL;
|
||||||
|
pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||||
|
if(!pidY.PIDV) return MCC_E_FATAL;
|
||||||
|
}
|
||||||
|
mountdata_t m;
|
||||||
|
coordpair_t tagspeed; // absolute value of speed
|
||||||
|
double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
|
||||||
|
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
|
||||||
|
axisdata_t axis;
|
||||||
|
DBG("state: %d/%d", m.Xstate, m.Ystate);
|
||||||
|
axis.state = m.Xstate;
|
||||||
|
axis.position = m.encXposition;
|
||||||
|
axis.speed = m.encXspeed;
|
||||||
|
tagspeed.X = getspeed(&target->X, &pidX, &axis);
|
||||||
|
if(isnan(tagspeed.X)){ // max speed
|
||||||
|
if(target->X.val < axis.position.val) Xsign = -1.;
|
||||||
|
tagspeed.X = Xlimits.max.speed;
|
||||||
|
}else{
|
||||||
|
if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
|
||||||
|
if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
|
||||||
|
}
|
||||||
|
axis_status_t xstate = axis.state;
|
||||||
|
axis.state = m.Ystate;
|
||||||
|
axis.position = m.encYposition;
|
||||||
|
axis.speed = m.encYspeed;
|
||||||
|
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
|
||||||
|
if(isnan(tagspeed.Y)){ // max speed
|
||||||
|
if(target->Y.val < axis.position.val) Ysign = -1.;
|
||||||
|
tagspeed.Y = Ylimits.max.speed;
|
||||||
|
}else{
|
||||||
|
if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
|
||||||
|
if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
|
||||||
|
}
|
||||||
|
axis_status_t ystate = axis.state;
|
||||||
|
if(m.Xstate != xstate || m.Ystate != ystate){
|
||||||
|
DBG("State changed");
|
||||||
|
setStat(xstate, ystate);
|
||||||
|
}
|
||||||
|
coordpair_t endpoint;
|
||||||
|
// allow at least PIDMaxDt moving with target speed
|
||||||
|
double dv = fabs(tagspeed.X - m.encXspeed.val);
|
||||||
|
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
|
||||||
|
+ Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
|
||||||
|
+ tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
|
||||||
|
endpoint.X = m.encXposition.val + Xsign * adder;
|
||||||
|
dv = fabs(tagspeed.Y - m.encYspeed.val);
|
||||||
|
adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
|
||||||
|
+ Conf.PIDMaxDt * tagspeed.Y
|
||||||
|
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
|
||||||
|
endpoint.Y = m.encYposition.val + Ysign * adder;
|
||||||
|
DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
|
||||||
|
return Mount.moveWspeed(&endpoint, &tagspeed);
|
||||||
|
}
|
||||||
40
LibSidServo/PID.h
Normal file
40
LibSidServo/PID.h
Normal file
@@ -0,0 +1,40 @@
|
|||||||
|
/*
|
||||||
|
* 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 <stddef.h>
|
||||||
|
|
||||||
|
#include "sidservo.h"
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
PIDpar_t gain; // PID gains
|
||||||
|
double prev_error; // Previous error
|
||||||
|
double integral; // Integral term
|
||||||
|
double *pidIarray; // array for Integral
|
||||||
|
struct timespec prevT; // time of previous correction
|
||||||
|
size_t pidIarrSize; // it's size
|
||||||
|
size_t curIidx; // and index of current element
|
||||||
|
} PIDController_t;
|
||||||
|
|
||||||
|
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
|
||||||
|
void pid_clear(PIDController_t *pid);
|
||||||
|
void pid_delete(PIDController_t **pid);
|
||||||
|
double pid_calculate(PIDController_t *pid, double error, double dt);
|
||||||
|
|
||||||
|
mcc_errcodes_t correct2(const coordval_pair_t *target);
|
||||||
57
LibSidServo/PID_test.deprecated/Makefile
Normal file
57
LibSidServo/PID_test.deprecated/Makefile
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
# run `make DEF=...` to add extra defines
|
||||||
|
PROGRAM := moving
|
||||||
|
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
|
||||||
|
LDFLAGS += -lusefull_macros -lm
|
||||||
|
SRCS := $(wildcard *.c)
|
||||||
|
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
|
||||||
|
OBJDIR := mk
|
||||||
|
CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
|
||||||
|
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
|
||||||
|
DEPS := $(OBJS:.o=.d)
|
||||||
|
TARGFILE := $(OBJDIR)/TARGET
|
||||||
|
CC = gcc
|
||||||
|
#TARGET := RELEASE
|
||||||
|
|
||||||
|
ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
|
||||||
|
TARGET := $(file < $(TARGFILE))
|
||||||
|
else
|
||||||
|
TARGET := RELEASE
|
||||||
|
endif
|
||||||
|
|
||||||
|
ifeq ($(TARGET), DEBUG)
|
||||||
|
.DEFAULT_GOAL := debug
|
||||||
|
endif
|
||||||
|
|
||||||
|
release: $(PROGRAM)
|
||||||
|
|
||||||
|
debug: CFLAGS += -DEBUG -Werror
|
||||||
|
debug: TARGET := DEBUG
|
||||||
|
debug: $(PROGRAM)
|
||||||
|
|
||||||
|
$(TARGFILE): $(OBJDIR)
|
||||||
|
@echo -e "\t\tTARGET: $(TARGET)"
|
||||||
|
@echo "$(TARGET)" > $(TARGFILE)
|
||||||
|
|
||||||
|
$(PROGRAM) : $(TARGFILE) $(OBJS)
|
||||||
|
@echo -e "\t\tLD $(PROGRAM)"
|
||||||
|
$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
|
||||||
|
|
||||||
|
$(OBJDIR):
|
||||||
|
@mkdir $(OBJDIR)
|
||||||
|
|
||||||
|
ifneq ($(MAKECMDGOALS),clean)
|
||||||
|
-include $(DEPS)
|
||||||
|
endif
|
||||||
|
|
||||||
|
$(OBJDIR)/%.o: %.c
|
||||||
|
@echo -e "\t\tCC $<"
|
||||||
|
$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@
|
||||||
|
|
||||||
|
clean:
|
||||||
|
@echo -e "\t\tCLEAN"
|
||||||
|
@rm -rf $(OBJDIR) 2>/dev/null || true
|
||||||
|
|
||||||
|
xclean: clean
|
||||||
|
@rm -f $(PROGRAM)
|
||||||
|
|
||||||
|
.PHONY: clean xclean
|
||||||
223
LibSidServo/PID_test.deprecated/Tramp.c
Normal file
223
LibSidServo/PID_test.deprecated/Tramp.c
Normal file
@@ -0,0 +1,223 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
// simplest trapezioidal ramp
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <strings.h>
|
||||||
|
#include <usefull_macros.h>
|
||||||
|
|
||||||
|
#include "Tramp.h"
|
||||||
|
|
||||||
|
#undef DBG
|
||||||
|
#define DBG(...)
|
||||||
|
|
||||||
|
static movestate_t state = ST_STOP;
|
||||||
|
static moveparam_t Min, Max; // `Min` acceleration not used!
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
|
||||||
|
STAGE_MAXSPEED, // go with target speed
|
||||||
|
STAGE_DECEL, // go from target speed to zero
|
||||||
|
STAGE_STOPPED, // stop
|
||||||
|
STAGE_AMOUNT
|
||||||
|
} movingstage_t;
|
||||||
|
|
||||||
|
static movingstage_t movingstage = STAGE_STOPPED;
|
||||||
|
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
|
||||||
|
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
|
||||||
|
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||||
|
|
||||||
|
static int initlims(limits_t *lim){
|
||||||
|
if(!lim) return FALSE;
|
||||||
|
Min = lim->min;
|
||||||
|
Max = lim->max;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void emstop(double _U_ t){
|
||||||
|
curparams.accel = 0.;
|
||||||
|
curparams.speed = 0.;
|
||||||
|
bzero(Times, sizeof(Times));
|
||||||
|
bzero(Params, sizeof(Params));
|
||||||
|
state = ST_STOP;
|
||||||
|
movingstage = STAGE_STOPPED;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void stop(double t){
|
||||||
|
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
|
||||||
|
movingstage = STAGE_DECEL;
|
||||||
|
state = ST_MOVE;
|
||||||
|
Times[STAGE_DECEL] = t;
|
||||||
|
Params[STAGE_DECEL].speed = curparams.speed;
|
||||||
|
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
|
||||||
|
else Params[STAGE_DECEL].accel = Max.accel;
|
||||||
|
Params[STAGE_DECEL].coord = curparams.coord;
|
||||||
|
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||||
|
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
|
||||||
|
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||||
|
double dt = Times[STAGE_STOPPED] - t;
|
||||||
|
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
|
||||||
|
Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief calc - moving calculation
|
||||||
|
* @param x - using max speed (>0!!!) and coordinate
|
||||||
|
* @param t - current time value
|
||||||
|
* @return FALSE if can't move with given parameters
|
||||||
|
*/
|
||||||
|
static int calc(moveparam_t *x, double t){
|
||||||
|
if(!x) return FALSE;
|
||||||
|
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
||||||
|
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
|
||||||
|
double Dx = fabs(x->coord - curparams.coord); // full distance
|
||||||
|
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
|
||||||
|
// we have two variants: with or without stage with constant speed
|
||||||
|
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
|
||||||
|
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
|
||||||
|
DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
|
||||||
|
double setspeed = x->speed; // new max speed (we can change it if need)
|
||||||
|
double dt01, dx01; // we'll fill them depending on starting conditions
|
||||||
|
Times[0] = t;
|
||||||
|
Params[0].speed = curparams.speed;
|
||||||
|
Params[0].coord = curparams.coord;
|
||||||
|
|
||||||
|
double curspeed = fabs(curparams.speed);
|
||||||
|
double dt0s = curspeed / Max.accel; // time of stopping phase
|
||||||
|
double dx0s = curspeed * dt0s / 2.; // distance
|
||||||
|
DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
|
||||||
|
if(dx0s > Dx){
|
||||||
|
WARNX("distance too short");
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
|
||||||
|
DBG("Distance good to just stop");
|
||||||
|
stop(t);
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
|
||||||
|
// after stop we will have full profile
|
||||||
|
double dxs3 = Dx - dx0s;
|
||||||
|
double newspeed = sqrt(Max.accel * dxs3);
|
||||||
|
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
|
||||||
|
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
|
||||||
|
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||||
|
Params[0].accel = sign * Max.accel;
|
||||||
|
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
|
||||||
|
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
|
||||||
|
DBG("dx01=%g, dt01=%g", dx01, dt01);
|
||||||
|
}else{ // increase or decrease speed without stopping phase
|
||||||
|
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||||
|
double a = sign * Max.accel;
|
||||||
|
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
|
||||||
|
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
|
||||||
|
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
|
||||||
|
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
|
||||||
|
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
|
||||||
|
if(dx01 + dx23 > Dx){ // calculate max speed
|
||||||
|
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
|
||||||
|
if(setspeed < curspeed){
|
||||||
|
setspeed = curparams.speed;
|
||||||
|
dt01 = 0.; dx01 = 0.;
|
||||||
|
Params[0].accel = 0.;
|
||||||
|
}else{
|
||||||
|
Params[0].accel = a;
|
||||||
|
dt01 = fabs(setspeed - curspeed) / Max.accel;
|
||||||
|
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
|
||||||
|
}
|
||||||
|
}else Params[0].accel = a;
|
||||||
|
}
|
||||||
|
if(setspeed < Min.speed){
|
||||||
|
WARNX("New speed should be too small");
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
moveparam_t *p = &Params[STAGE_MAXSPEED];
|
||||||
|
p->accel = 0.; p->speed = sign * setspeed;
|
||||||
|
p->coord = curparams.coord + dx01 * sign;
|
||||||
|
Times[STAGE_MAXSPEED] = Times[0] + dt01;
|
||||||
|
dt23 = setspeed / Max.accel;
|
||||||
|
dx23 = setspeed * dt23 / 2.;
|
||||||
|
// calculate dx12 and dt12
|
||||||
|
double dx12 = Dx - dx01 - dx23;
|
||||||
|
if(dx12 < -coord_tolerance){
|
||||||
|
WARNX("Oops, WTF dx12=%g?", dx12);
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
double dt12 = dx12 / setspeed;
|
||||||
|
p = &Params[STAGE_DECEL];
|
||||||
|
p->accel = -sign * Max.accel;
|
||||||
|
p->speed = sign * setspeed;
|
||||||
|
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
|
||||||
|
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
|
||||||
|
p = &Params[STAGE_STOPPED];
|
||||||
|
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
|
||||||
|
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
|
||||||
|
for(int i = 0; i < 4; ++i)
|
||||||
|
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
|
||||||
|
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
|
||||||
|
state = ST_MOVE;
|
||||||
|
movingstage = STAGE_ACCEL;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static movestate_t proc(moveparam_t *next, double t){
|
||||||
|
if(state == ST_STOP) goto ret;
|
||||||
|
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||||
|
if(Times[s] <= t){ // check time for current stage
|
||||||
|
movingstage = s;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(movingstage == STAGE_STOPPED){
|
||||||
|
curparams.coord = Params[STAGE_STOPPED].coord;
|
||||||
|
emstop(t);
|
||||||
|
goto ret;
|
||||||
|
}
|
||||||
|
// calculate current parameters
|
||||||
|
double dt = t - Times[movingstage];
|
||||||
|
double a = Params[movingstage].accel;
|
||||||
|
double v0 = Params[movingstage].speed;
|
||||||
|
double x0 = Params[movingstage].coord;
|
||||||
|
curparams.accel = a;
|
||||||
|
curparams.speed = v0 + a * dt;
|
||||||
|
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||||
|
ret:
|
||||||
|
if(next) *next = curparams;
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
static movestate_t getst(moveparam_t *cur){
|
||||||
|
if(cur) *cur = curparams;
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
static double gettstop(){
|
||||||
|
return Times[STAGE_STOPPED];
|
||||||
|
}
|
||||||
|
|
||||||
|
movemodel_t trapez = {
|
||||||
|
.init_limits = initlims,
|
||||||
|
.stop = stop,
|
||||||
|
.emergency_stop = emstop,
|
||||||
|
.get_state = getst,
|
||||||
|
.calculate = calc,
|
||||||
|
.proc_move = proc,
|
||||||
|
.stoppedtime = gettstop,
|
||||||
|
};
|
||||||
23
LibSidServo/PID_test.deprecated/Tramp.h
Normal file
23
LibSidServo/PID_test.deprecated/Tramp.h
Normal file
@@ -0,0 +1,23 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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 "moving_private.h"
|
||||||
|
|
||||||
|
extern movemodel_t trapez;
|
||||||
243
LibSidServo/PID_test.deprecated/main.c
Normal file
243
LibSidServo/PID_test.deprecated/main.c
Normal file
@@ -0,0 +1,243 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <strings.h>
|
||||||
|
#include <usefull_macros.h>
|
||||||
|
|
||||||
|
#include "moving.h"
|
||||||
|
|
||||||
|
// errors for states: slewing/pointing/guiding
|
||||||
|
#define MAX_POINTING_ERR (50.)
|
||||||
|
#define MAX_GUIDING_ERR (5.)
|
||||||
|
// timeout to "forget" old data from I sum array; seconds
|
||||||
|
#define PID_I_PERIOD (3.)
|
||||||
|
|
||||||
|
static movemodel_t *model = NULL;
|
||||||
|
static FILE *coordslog = NULL;
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
Slewing,
|
||||||
|
Pointing,
|
||||||
|
Guiding
|
||||||
|
} state_t;
|
||||||
|
|
||||||
|
static state_t state = Slewing;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
int help;
|
||||||
|
char *ramptype;
|
||||||
|
char *xlog;
|
||||||
|
double dTmon;
|
||||||
|
double dTcorr;
|
||||||
|
double Tend;
|
||||||
|
double minerr;
|
||||||
|
double P, I, D;
|
||||||
|
} pars;
|
||||||
|
|
||||||
|
static pars G = {
|
||||||
|
.ramptype = "t",
|
||||||
|
.dTmon = 0.01,
|
||||||
|
.dTcorr = 0.05,
|
||||||
|
.Tend = 100.,
|
||||||
|
.minerr = 0.1,
|
||||||
|
.P = 0.8,
|
||||||
|
};
|
||||||
|
|
||||||
|
static limits_t limits = {
|
||||||
|
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
||||||
|
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
|
||||||
|
.jerk = 10.
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double kp, ki, kd; // PID gains
|
||||||
|
double prev_error; // Previous error
|
||||||
|
double integral; // Integral term
|
||||||
|
double *pidIarray; // array for Integral
|
||||||
|
size_t pidIarrSize; // it's size
|
||||||
|
size_t curIidx; // and index of current element
|
||||||
|
} PIDController;
|
||||||
|
|
||||||
|
static PIDController pid;
|
||||||
|
|
||||||
|
static sl_option_t opts[] = {
|
||||||
|
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||||
|
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
|
||||||
|
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
|
||||||
|
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
|
||||||
|
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
||||||
|
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
|
||||||
|
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
|
||||||
|
{"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"},
|
||||||
|
{"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"},
|
||||||
|
{"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"},
|
||||||
|
// TODO: add parameters for limits setting
|
||||||
|
end_option
|
||||||
|
};
|
||||||
|
|
||||||
|
// calculate coordinate target for given time (starting from zero)
|
||||||
|
static double target_coord(double t){
|
||||||
|
if(t > 20. && t < 30.) return target_coord(20.);
|
||||||
|
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
|
||||||
|
return pos;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* P-only == oscillations
|
||||||
|
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||||
|
double error = targcoord - p->coord;
|
||||||
|
if(fabs(error) < G.minerr) return p->speed;
|
||||||
|
return p->speed + error / dt / 500.;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
static void pid_init(PIDController *pid, double kp, double ki, double kd) {
|
||||||
|
pid->kp = fabs(kp);
|
||||||
|
pid->ki = fabs(ki);
|
||||||
|
pid->kd = fabs(kd);
|
||||||
|
pid->prev_error = 0.;
|
||||||
|
pid->integral = 0.;
|
||||||
|
pid->curIidx = 0;
|
||||||
|
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
|
||||||
|
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
|
||||||
|
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pid_clear(PIDController *pid){
|
||||||
|
if(!pid) return;
|
||||||
|
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||||
|
pid->integral = 0.;
|
||||||
|
pid->prev_error = 0.;
|
||||||
|
pid->curIidx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||||
|
double error = targcoord - p->coord, fe = fabs(error);
|
||||||
|
switch(state){
|
||||||
|
case Slewing:
|
||||||
|
if(fe < MAX_POINTING_ERR){
|
||||||
|
pid_clear(&pid);
|
||||||
|
state = Pointing;
|
||||||
|
green("--> Pointing\n");
|
||||||
|
}else{
|
||||||
|
red("Slewing...\n");
|
||||||
|
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case Pointing:
|
||||||
|
if(fe < MAX_GUIDING_ERR){
|
||||||
|
pid_clear(&pid);
|
||||||
|
state = Guiding;
|
||||||
|
green("--> Guiding\n");
|
||||||
|
}else if(fe > MAX_POINTING_ERR){
|
||||||
|
red("--> Slewing\n");
|
||||||
|
state = Slewing;
|
||||||
|
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case Guiding:
|
||||||
|
if(fe > MAX_GUIDING_ERR){
|
||||||
|
red("--> Pointing\n");
|
||||||
|
state = Pointing;
|
||||||
|
}else if(fe < G.minerr){
|
||||||
|
green("At target\n");
|
||||||
|
//pid_clear(&pid);
|
||||||
|
//return p->speed;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
red("Calculate PID\n");
|
||||||
|
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
|
||||||
|
pid.pidIarray[pid.curIidx++] = oldi;
|
||||||
|
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
|
||||||
|
pid.integral += newi - oldi;
|
||||||
|
double derivative = (error - pid.prev_error) / dt;
|
||||||
|
pid.prev_error = error;
|
||||||
|
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
|
||||||
|
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
|
||||||
|
if(state == Pointing) add /= 3.;
|
||||||
|
else if(state == Guiding) add /= 7.;
|
||||||
|
DBG("ADD = %g; new speed = %g", add, p->speed + add);
|
||||||
|
if(state == Guiding) return p->speed + add / dt / 10.;
|
||||||
|
return add / dt;
|
||||||
|
}
|
||||||
|
// ./moving -l coords -P.5 -I.05 -D1.5
|
||||||
|
// ./moving -l coords -P1.3 -D1.6
|
||||||
|
|
||||||
|
static void start_model(double Tend){
|
||||||
|
double T = 0., Tcorr = 0.;//, Tlast = 0.;
|
||||||
|
moveparam_t target;
|
||||||
|
while(T <= Tend){
|
||||||
|
moveparam_t p;
|
||||||
|
movestate_t st = model->get_state(&p);
|
||||||
|
if(st == ST_MOVE) st = model->proc_move(&p, T);
|
||||||
|
double nextcoord = target_coord(T);
|
||||||
|
double error = nextcoord - p.coord;
|
||||||
|
if(T - Tcorr >= G.dTcorr){ // check correction
|
||||||
|
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
|
||||||
|
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
|
||||||
|
target.speed = fabs(speed);
|
||||||
|
double res_speed = limits.max.speed / 2.;
|
||||||
|
if(target.speed > limits.max.speed){
|
||||||
|
target.speed = limits.max.speed;
|
||||||
|
res_speed = limits.max.speed / 4.;
|
||||||
|
}else if(target.speed < limits.min.speed){
|
||||||
|
target.speed = limits.min.speed;
|
||||||
|
res_speed = limits.min.speed * 4.;
|
||||||
|
}
|
||||||
|
if(!move_to(&target, T)){
|
||||||
|
target.speed = res_speed;
|
||||||
|
if(!move_to(&target, T))
|
||||||
|
WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed);
|
||||||
|
}
|
||||||
|
DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error);
|
||||||
|
Tcorr = T;
|
||||||
|
}
|
||||||
|
// make log
|
||||||
|
fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
|
||||||
|
T, nextcoord, p.coord, p.speed, p.accel, error);
|
||||||
|
T += G.dTmon;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv){
|
||||||
|
sl_init();
|
||||||
|
sl_parseargs(&argc, &argv, opts);
|
||||||
|
if(G.help) sl_showhelp(-1, opts);
|
||||||
|
if(G.xlog){
|
||||||
|
coordslog = fopen(G.xlog, "w");
|
||||||
|
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||||
|
} else coordslog = stdout;
|
||||||
|
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
||||||
|
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
||||||
|
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
||||||
|
pid_init(&pid, G.P, G.I, G.D);
|
||||||
|
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
||||||
|
ramptype_t ramp = RAMP_AMOUNT;
|
||||||
|
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
|
||||||
|
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
|
||||||
|
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
|
||||||
|
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
|
||||||
|
model = init_moving(ramp, &limits);
|
||||||
|
if(!model) ERRX("Can't init moving model: check parameters");
|
||||||
|
start_model(G.Tend);
|
||||||
|
fclose(coordslog);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
105
LibSidServo/PID_test.deprecated/moving.c
Normal file
105
LibSidServo/PID_test.deprecated/moving.c
Normal file
@@ -0,0 +1,105 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <usefull_macros.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
#include "moving.h"
|
||||||
|
#include "moving_private.h"
|
||||||
|
#include "Dramp.h"
|
||||||
|
#include "Sramp.h"
|
||||||
|
#include "Tramp.h"
|
||||||
|
|
||||||
|
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||||
|
static movemodel_t *model = NULL;
|
||||||
|
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||||
|
double time_tick = TIME_TICK_DEFAULT;
|
||||||
|
|
||||||
|
// difference of time from first call, using nanoseconds
|
||||||
|
double nanot(){
|
||||||
|
static struct timespec *start = NULL;
|
||||||
|
struct timespec now;
|
||||||
|
if(!start){
|
||||||
|
start = MALLOC(struct timespec, 1);
|
||||||
|
if(!start) return -1.;
|
||||||
|
if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
|
||||||
|
}
|
||||||
|
if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
|
||||||
|
//DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
|
||||||
|
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
||||||
|
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
||||||
|
return sd + nd;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void chkminmax(double *min, double *max){
|
||||||
|
if(*min <= *max) return;
|
||||||
|
double t = *min;
|
||||||
|
*min = *max;
|
||||||
|
*max = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
movemodel_t *init_moving(ramptype_t type, limits_t *l){
|
||||||
|
if(!l) return FALSE;
|
||||||
|
switch(type){
|
||||||
|
case RAMP_DUMB:
|
||||||
|
model = &dumb;
|
||||||
|
break;
|
||||||
|
case RAMP_TRAPEZIUM:
|
||||||
|
model = &trapez;
|
||||||
|
break;
|
||||||
|
case RAMP_S:
|
||||||
|
model = &s_shaped;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
if(!model->init_limits) return NULL;
|
||||||
|
moveparam_t *max = &l->max, *min = &l->min;
|
||||||
|
if(min->speed < 0.) min->speed = -min->speed;
|
||||||
|
if(max->speed < 0.) max->speed = -max->speed;
|
||||||
|
if(min->accel < 0.) min->accel = -min->accel;
|
||||||
|
if(max->accel < 0.) max->accel = -max->accel;
|
||||||
|
chkminmax(&min->coord, &max->coord);
|
||||||
|
chkminmax(&min->speed, &max->speed);
|
||||||
|
chkminmax(&min->accel, &max->accel);
|
||||||
|
if(!model->init_limits(l)) return NULL;
|
||||||
|
return model;
|
||||||
|
}
|
||||||
|
|
||||||
|
int move_to(moveparam_t *target, double t){
|
||||||
|
if(!target || !model) return FALSE;
|
||||||
|
DBG("MOVE to %g at speed %g", target->coord, target->speed);
|
||||||
|
// only positive velocity
|
||||||
|
if(target->speed < 0.) target->speed = -target->speed;
|
||||||
|
// don't mind about acceleration - user cannot set it now
|
||||||
|
return model->calculate(target, t);
|
||||||
|
}
|
||||||
|
|
||||||
|
int init_coordtol(double tolerance){
|
||||||
|
if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
|
||||||
|
coord_tolerance = tolerance;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
int init_timetick(double tick){
|
||||||
|
if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
|
||||||
|
time_tick = tick;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
70
LibSidServo/PID_test.deprecated/moving.h
Normal file
70
LibSidServo/PID_test.deprecated/moving.h
Normal file
@@ -0,0 +1,70 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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
|
||||||
|
|
||||||
|
// tolerance, time ticks
|
||||||
|
#define COORD_TOLERANCE_DEFAULT (0.01)
|
||||||
|
#define COORD_TOLERANCE_MIN (0.0001)
|
||||||
|
#define COORD_TOLERANCE_MAX (10.)
|
||||||
|
#define TIME_TICK_DEFAULT (0.0001)
|
||||||
|
#define TIME_TICK_MIN (1e-9)
|
||||||
|
#define TIME_TICK_MAX (10.)
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
|
||||||
|
RAMP_TRAPEZIUM, // trapezium ramp
|
||||||
|
RAMP_S, // s-shaped ramp
|
||||||
|
RAMP_AMOUNT
|
||||||
|
} ramptype_t;
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
ST_STOP, // stopped
|
||||||
|
ST_MOVE, // moving
|
||||||
|
ST_AMOUNT
|
||||||
|
} movestate_t;
|
||||||
|
|
||||||
|
typedef struct{ // all values could be both as positive and negative
|
||||||
|
double coord;
|
||||||
|
double speed;
|
||||||
|
double accel;
|
||||||
|
} moveparam_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
moveparam_t min;
|
||||||
|
moveparam_t max;
|
||||||
|
double jerk;
|
||||||
|
} limits_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
int (*init_limits)(limits_t *lim); // init values of limits, jerk
|
||||||
|
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||||
|
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
|
||||||
|
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
|
||||||
|
void (*stop)(double t); // stop by ramp
|
||||||
|
void (*emergency_stop)(double t); // stop with highest acceleration
|
||||||
|
double (*stoppedtime)(); // time when moving will ends
|
||||||
|
} movemodel_t;
|
||||||
|
|
||||||
|
extern double coord_tolerance;
|
||||||
|
|
||||||
|
double nanot();
|
||||||
|
movemodel_t *init_moving(ramptype_t type, limits_t *l);
|
||||||
|
int init_coordtol(double tolerance);
|
||||||
|
int init_timetick(double tick);
|
||||||
|
int move_to(moveparam_t *target, double t);
|
||||||
1
LibSidServo/PID_test.deprecated/moving_model.cflags
Normal file
1
LibSidServo/PID_test.deprecated/moving_model.cflags
Normal file
@@ -0,0 +1 @@
|
|||||||
|
-std=c17
|
||||||
4
LibSidServo/PID_test.deprecated/moving_model.config
Normal file
4
LibSidServo/PID_test.deprecated/moving_model.config
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
// Add predefined macros for your project here. For example:
|
||||||
|
// #define THE_ANSWER 42
|
||||||
|
#define _XOPEN_SOURCE 666
|
||||||
|
#define EBUG
|
||||||
1
LibSidServo/PID_test.deprecated/moving_model.creator
Normal file
1
LibSidServo/PID_test.deprecated/moving_model.creator
Normal file
@@ -0,0 +1 @@
|
|||||||
|
[General]
|
||||||
221
LibSidServo/PID_test.deprecated/moving_model.creator.user
Normal file
221
LibSidServo/PID_test.deprecated/moving_model.creator.user
Normal file
@@ -0,0 +1,221 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<!DOCTYPE QtCreatorProject>
|
||||||
|
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
|
||||||
|
<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">true</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">false</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">false</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">1</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>
|
||||||
|
</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/C-files/mountcontrol.git/moving_model</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">Сборка</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</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">Очистка</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</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">Развёртывание</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</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="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||||
|
<value type="QList<int>" 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="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">Развёртывание</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</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="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||||
|
<value type="QList<int>" 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="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>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||||
|
<value type="int">22</value>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>Version</variable>
|
||||||
|
<value type="int">22</value>
|
||||||
|
</data>
|
||||||
|
</qtcreator>
|
||||||
@@ -0,0 +1,184 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<!DOCTYPE QtCreatorProject>
|
||||||
|
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
|
||||||
|
<qtcreator>
|
||||||
|
<data>
|
||||||
|
<variable>EnvironmentId</variable>
|
||||||
|
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</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">true</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||||
|
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</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">false</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">false</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">8</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>
|
||||||
|
</valuemap>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||||
|
<valuemap type="QVariantMap">
|
||||||
|
<value type="QString" key="DeviceType">Desktop</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">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</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">/Big/Data/00__Small_tel/moving_model</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">Default</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</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<int>" 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="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>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||||
|
<value type="int">22</value>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>Version</variable>
|
||||||
|
<value type="int">22</value>
|
||||||
|
</data>
|
||||||
|
</qtcreator>
|
||||||
1
LibSidServo/PID_test.deprecated/moving_model.cxxflags
Normal file
1
LibSidServo/PID_test.deprecated/moving_model.cxxflags
Normal file
@@ -0,0 +1 @@
|
|||||||
|
-std=c++17
|
||||||
10
LibSidServo/PID_test.deprecated/moving_model.files
Normal file
10
LibSidServo/PID_test.deprecated/moving_model.files
Normal file
@@ -0,0 +1,10 @@
|
|||||||
|
Dramp.c
|
||||||
|
Dramp.h
|
||||||
|
Sramp.c
|
||||||
|
Sramp.h
|
||||||
|
Tramp.c
|
||||||
|
Tramp.h
|
||||||
|
main.c
|
||||||
|
moving.c
|
||||||
|
moving.h
|
||||||
|
moving_private.h
|
||||||
26
LibSidServo/PID_test.deprecated/moving_private.h
Normal file
26
LibSidServo/PID_test.deprecated/moving_private.h
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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 "moving.h"
|
||||||
|
|
||||||
|
extern double coord_tolerance;
|
||||||
|
extern double time_tick;
|
||||||
|
|
||||||
|
|
||||||
4
LibSidServo/PID_test.deprecated/plot
Executable file
4
LibSidServo/PID_test.deprecated/plot
Executable file
@@ -0,0 +1,4 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||||
|
pause mouse
|
||||||
8
LibSidServo/PID_test.deprecated/plot.cont
Executable file
8
LibSidServo/PID_test.deprecated/plot.cont
Executable file
@@ -0,0 +1,8 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
#set term pdf
|
||||||
|
#set output "output.pdf"
|
||||||
|
while(1){
|
||||||
|
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||||
|
pause 1
|
||||||
|
}
|
||||||
6
LibSidServo/PID_test.deprecated/plot_jpg
Executable file
6
LibSidServo/PID_test.deprecated/plot_jpg
Executable file
@@ -0,0 +1,6 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
set terminal jpeg size 1000,500
|
||||||
|
set output "all.jpg"
|
||||||
|
|
||||||
|
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||||
5
LibSidServo/PID_test.deprecated/plot_pdf
Executable file
5
LibSidServo/PID_test.deprecated/plot_pdf
Executable file
@@ -0,0 +1,5 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
set term pdf
|
||||||
|
set output "output.pdf"
|
||||||
|
plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader
|
||||||
4
LibSidServo/PID_test.deprecated/plotacc
Executable file
4
LibSidServo/PID_test.deprecated/plotacc
Executable file
@@ -0,0 +1,4 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
plot 'coords' using 1:5 with lines title columnheader
|
||||||
|
pause mouse
|
||||||
4
LibSidServo/PID_test.deprecated/ploterr
Executable file
4
LibSidServo/PID_test.deprecated/ploterr
Executable file
@@ -0,0 +1,4 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
plot 'coords' using 1:6 with lines title columnheader
|
||||||
|
pause mouse
|
||||||
6
LibSidServo/PID_test.deprecated/ploterr.cont
Executable file
6
LibSidServo/PID_test.deprecated/ploterr.cont
Executable file
@@ -0,0 +1,6 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
while(1){
|
||||||
|
plot 'coords' using 1:6 with lines title columnheader
|
||||||
|
pause 1
|
||||||
|
}
|
||||||
5
LibSidServo/PID_test.deprecated/ploterr_jpg
Executable file
5
LibSidServo/PID_test.deprecated/ploterr_jpg
Executable file
@@ -0,0 +1,5 @@
|
|||||||
|
#!/usr/bin/gnuplot
|
||||||
|
|
||||||
|
set term jpeg size 1000,500
|
||||||
|
set output "error.jpg"
|
||||||
|
plot 'coords' using 1:6 with lines title columnheader
|
||||||
64
LibSidServo/PID_test/PID.c
Normal file
64
LibSidServo/PID_test/PID.c
Normal file
@@ -0,0 +1,64 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <strings.h>
|
||||||
|
#include <usefull_macros.h>
|
||||||
|
|
||||||
|
#include "PID.h"
|
||||||
|
|
||||||
|
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){
|
||||||
|
if(!gain || Iarrsz < 3) return NULL;
|
||||||
|
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
|
||||||
|
pid->gain = *gain;
|
||||||
|
pid->pidIarrSize = Iarrsz;
|
||||||
|
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
|
||||||
|
return pid;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pid_clear(PIDController_t *pid){
|
||||||
|
if(!pid) return;
|
||||||
|
DBG("CLEAR PID PARAMETERS");
|
||||||
|
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||||
|
pid->integral = 0.;
|
||||||
|
pid->prev_error = 0.;
|
||||||
|
pid->curIidx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pid_delete(PIDController_t **pid){
|
||||||
|
if(!pid || !*pid) return;
|
||||||
|
if((*pid)->pidIarray) free((*pid)->pidIarray);
|
||||||
|
free(*pid);
|
||||||
|
*pid = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
double pid_calculate(PIDController_t *pid, double error, double dt){
|
||||||
|
// calculate flowing integral
|
||||||
|
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
|
||||||
|
DBG("oldi/new: %g, %g", oldi, newi);
|
||||||
|
pid->pidIarray[pid->curIidx++] = newi;
|
||||||
|
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
|
||||||
|
pid->integral += newi - oldi;
|
||||||
|
double derivative = (error - pid->prev_error) / dt;
|
||||||
|
pid->prev_error = error;
|
||||||
|
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
|
||||||
|
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
|
||||||
|
return sum;
|
||||||
|
}
|
||||||
39
LibSidServo/PID_test/PID.h
Normal file
39
LibSidServo/PID_test/PID.h
Normal file
@@ -0,0 +1,39 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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 <stddef.h>
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
double P, I, D;
|
||||||
|
} PIDpar_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
PIDpar_t gain; // PID gains
|
||||||
|
double prev_error; // Previous error
|
||||||
|
double integral; // Integral term
|
||||||
|
double *pidIarray; // array for Integral
|
||||||
|
size_t pidIarrSize; // it's size
|
||||||
|
size_t curIidx; // and index of current element
|
||||||
|
} PIDController_t;
|
||||||
|
|
||||||
|
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz);
|
||||||
|
void pid_clear(PIDController_t *pid);
|
||||||
|
void pid_delete(PIDController_t **pid);
|
||||||
|
double pid_calculate(PIDController_t *pid, double error, double dt);
|
||||||
@@ -22,13 +22,18 @@
|
|||||||
#include <usefull_macros.h>
|
#include <usefull_macros.h>
|
||||||
|
|
||||||
#include "moving.h"
|
#include "moving.h"
|
||||||
|
#include "PID.h"
|
||||||
|
|
||||||
// errors for states: slewing/pointing/guiding
|
// errors for states: slewing/pointing/guiding
|
||||||
#define MAX_POINTING_ERR (50.)
|
// 10-degrees zone - Coordinate-driven PID
|
||||||
#define MAX_GUIDING_ERR (5.)
|
#define MAX_POINTING_ERR (36000.)
|
||||||
|
// 1-arcminute zone - Velocity-dtiven PID
|
||||||
|
#define MAX_GUIDING_ERR (60.)
|
||||||
// timeout to "forget" old data from I sum array; seconds
|
// timeout to "forget" old data from I sum array; seconds
|
||||||
#define PID_I_PERIOD (3.)
|
#define PID_I_PERIOD (3.)
|
||||||
|
|
||||||
|
// PID for coordinate-driven and velocity-driven parts
|
||||||
|
static PIDController_t *pidC = NULL, *pidV = NULL;
|
||||||
static movemodel_t *model = NULL;
|
static movemodel_t *model = NULL;
|
||||||
static FILE *coordslog = NULL;
|
static FILE *coordslog = NULL;
|
||||||
|
|
||||||
@@ -48,93 +53,64 @@ typedef struct{
|
|||||||
double dTcorr;
|
double dTcorr;
|
||||||
double Tend;
|
double Tend;
|
||||||
double minerr;
|
double minerr;
|
||||||
double P, I, D;
|
double startcoord;
|
||||||
|
double error;
|
||||||
|
PIDpar_t gainC, gainV;
|
||||||
} pars;
|
} pars;
|
||||||
|
|
||||||
static pars G = {
|
static pars G = {
|
||||||
.ramptype = "t",
|
|
||||||
.dTmon = 0.01,
|
.dTmon = 0.01,
|
||||||
.dTcorr = 0.05,
|
.dTcorr = 0.05,
|
||||||
.Tend = 100.,
|
.Tend = 100.,
|
||||||
.minerr = 0.1,
|
.minerr = 0.1,
|
||||||
.P = 0.8,
|
.gainC.P = 0.1,
|
||||||
|
.gainV.P = 0.1,
|
||||||
|
.startcoord = 100.,
|
||||||
};
|
};
|
||||||
|
|
||||||
static limits_t limits = {
|
static limits_t limits = {
|
||||||
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
||||||
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
|
.max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
|
||||||
.jerk = 10.
|
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
double kp, ki, kd; // PID gains
|
|
||||||
double prev_error; // Previous error
|
|
||||||
double integral; // Integral term
|
|
||||||
double *pidIarray; // array for Integral
|
|
||||||
size_t pidIarrSize; // it's size
|
|
||||||
size_t curIidx; // and index of current element
|
|
||||||
} PIDController;
|
|
||||||
|
|
||||||
static PIDController pid;
|
|
||||||
|
|
||||||
static sl_option_t opts[] = {
|
static sl_option_t opts[] = {
|
||||||
{"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"},
|
||||||
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
|
|
||||||
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
|
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
|
||||||
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
|
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
|
||||||
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
||||||
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
|
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
|
||||||
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
|
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
|
||||||
{"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"},
|
{"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"},
|
||||||
{"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"},
|
{"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"},
|
||||||
{"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"},
|
{"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"},
|
||||||
|
{"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"},
|
||||||
|
{"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"},
|
||||||
|
{"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"},
|
||||||
|
{"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"},
|
||||||
|
{"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"},
|
||||||
// TODO: add parameters for limits setting
|
// TODO: add parameters for limits setting
|
||||||
end_option
|
end_option
|
||||||
};
|
};
|
||||||
|
|
||||||
// calculate coordinate target for given time (starting from zero)
|
// calculate coordinate target for given time (starting from zero)
|
||||||
static double target_coord(double t){
|
static double target_coord(double t){
|
||||||
if(t > 20. && t < 30.) return target_coord(20.);
|
if(t > 20. && t < 30.) return 0.;
|
||||||
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
|
//double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5);
|
||||||
|
double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5);
|
||||||
return pos;
|
return pos;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* P-only == oscillations
|
|
||||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
|
||||||
double error = targcoord - p->coord;
|
|
||||||
if(fabs(error) < G.minerr) return p->speed;
|
|
||||||
return p->speed + error / dt / 500.;
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
static void pid_init(PIDController *pid, double kp, double ki, double kd) {
|
|
||||||
pid->kp = fabs(kp);
|
|
||||||
pid->ki = fabs(ki);
|
|
||||||
pid->kd = fabs(kd);
|
|
||||||
pid->prev_error = 0.;
|
|
||||||
pid->integral = 0.;
|
|
||||||
pid->curIidx = 0;
|
|
||||||
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
|
|
||||||
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
|
|
||||||
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void pid_clear(PIDController *pid){
|
|
||||||
if(!pid) return;
|
|
||||||
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
|
||||||
pid->integral = 0.;
|
|
||||||
pid->prev_error = 0.;
|
|
||||||
pid->curIidx = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||||
double error = targcoord - p->coord, fe = fabs(error);
|
double error = targcoord - p->coord, fe = fabs(error);
|
||||||
|
PIDController_t *pid = NULL;
|
||||||
switch(state){
|
switch(state){
|
||||||
case Slewing:
|
case Slewing:
|
||||||
if(fe < MAX_POINTING_ERR){
|
if(fe < MAX_POINTING_ERR){
|
||||||
pid_clear(&pid);
|
pid_clear(pidC);
|
||||||
state = Pointing;
|
state = Pointing;
|
||||||
green("--> Pointing\n");
|
green("--> Pointing\n");
|
||||||
|
pid = pidC;
|
||||||
}else{
|
}else{
|
||||||
red("Slewing...\n");
|
red("Slewing...\n");
|
||||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||||
@@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
|||||||
break;
|
break;
|
||||||
case Pointing:
|
case Pointing:
|
||||||
if(fe < MAX_GUIDING_ERR){
|
if(fe < MAX_GUIDING_ERR){
|
||||||
pid_clear(&pid);
|
pid_clear(pidV);
|
||||||
state = Guiding;
|
state = Guiding;
|
||||||
green("--> Guiding\n");
|
green("--> Guiding\n");
|
||||||
|
pid = pidV;
|
||||||
}else if(fe > MAX_POINTING_ERR){
|
}else if(fe > MAX_POINTING_ERR){
|
||||||
red("--> Slewing\n");
|
red("--> Slewing\n");
|
||||||
state = Slewing;
|
state = Slewing;
|
||||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||||
}
|
} else pid = pidC;
|
||||||
break;
|
break;
|
||||||
case Guiding:
|
case Guiding:
|
||||||
|
pid= pidV;
|
||||||
if(fe > MAX_GUIDING_ERR){
|
if(fe > MAX_GUIDING_ERR){
|
||||||
red("--> Pointing\n");
|
red("--> Pointing\n");
|
||||||
state = Pointing;
|
state = Pointing;
|
||||||
|
pid_clear(pidC);
|
||||||
|
pid = pidC;
|
||||||
}else if(fe < G.minerr){
|
}else if(fe < G.minerr){
|
||||||
green("At target\n");
|
green("At target\n");
|
||||||
//pid_clear(&pid);
|
}else printf("Current error: %g\n", fe);
|
||||||
//return p->speed;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
if(!pid){
|
||||||
red("Calculate PID\n");
|
WARNX("where is PID?"); return p->speed;
|
||||||
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
|
}
|
||||||
pid.pidIarray[pid.curIidx++] = oldi;
|
double tagspeed = pid_calculate(pid, error, dt);
|
||||||
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
|
if(state == Guiding) return p->speed + tagspeed;
|
||||||
pid.integral += newi - oldi;
|
return tagspeed;
|
||||||
double derivative = (error - pid.prev_error) / dt;
|
|
||||||
pid.prev_error = error;
|
|
||||||
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
|
|
||||||
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
|
|
||||||
if(state == Pointing) add /= 3.;
|
|
||||||
else if(state == Guiding) add /= 7.;
|
|
||||||
DBG("ADD = %g; new speed = %g", add, p->speed + add);
|
|
||||||
if(state == Guiding) return p->speed + add / dt / 10.;
|
|
||||||
return add / dt;
|
|
||||||
}
|
}
|
||||||
// ./moving -l coords -P.5 -I.05 -D1.5
|
// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
|
||||||
// ./moving -l coords -P1.3 -D1.6
|
// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
|
||||||
|
|
||||||
static void start_model(double Tend){
|
static void start_model(double Tend){
|
||||||
double T = 0., Tcorr = 0.;//, Tlast = 0.;
|
double T = 0., Tcorr = 0.;
|
||||||
moveparam_t target;
|
moveparam_t target;
|
||||||
|
uint64_t N = 0;
|
||||||
|
double errmax = 0., errsum = 0., errsum2 = 0.;
|
||||||
while(T <= Tend){
|
while(T <= Tend){
|
||||||
moveparam_t p;
|
moveparam_t p;
|
||||||
movestate_t st = model->get_state(&p);
|
movestate_t st = model->get_state(&p);
|
||||||
if(st == ST_MOVE) st = model->proc_move(&p, T);
|
if(st == ST_MOVE) st = model->proc_move(&p, T);
|
||||||
double nextcoord = target_coord(T);
|
double nextcoord = target_coord(T);
|
||||||
double error = nextcoord - p.coord;
|
double error = nextcoord - p.coord;
|
||||||
|
if(state == Guiding){
|
||||||
|
double ae = fabs(error);
|
||||||
|
if(ae > errmax) errmax = ae;
|
||||||
|
errsum += error; errsum2 += error * error;
|
||||||
|
++N;
|
||||||
|
}
|
||||||
if(T - Tcorr >= G.dTcorr){ // check correction
|
if(T - Tcorr >= G.dTcorr){ // check correction
|
||||||
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
|
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
|
||||||
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
|
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
|
||||||
@@ -215,6 +192,9 @@ static void start_model(double Tend){
|
|||||||
T, nextcoord, p.coord, p.speed, p.accel, error);
|
T, nextcoord, p.coord, p.speed, p.accel, error);
|
||||||
T += G.dTmon;
|
T += G.dTmon;
|
||||||
}
|
}
|
||||||
|
printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n");
|
||||||
|
double mean = errsum / (double)N;
|
||||||
|
printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean));
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
int main(int argc, char **argv){
|
||||||
@@ -228,16 +208,15 @@ int main(int argc, char **argv){
|
|||||||
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
||||||
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
||||||
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
||||||
pid_init(&pid, G.P, G.I, G.D);
|
pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
|
||||||
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
|
||||||
ramptype_t ramp = RAMP_AMOUNT;
|
if(!pidC || !pidV) ERRX("Can't init PID regulators");
|
||||||
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
|
model = init_moving(&limits);
|
||||||
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
|
|
||||||
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
|
|
||||||
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
|
|
||||||
model = init_moving(ramp, &limits);
|
|
||||||
if(!model) ERRX("Can't init moving model: check parameters");
|
if(!model) ERRX("Can't init moving model: check parameters");
|
||||||
|
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
||||||
start_model(G.Tend);
|
start_model(G.Tend);
|
||||||
|
pid_delete(&pidC);
|
||||||
|
pid_delete(&pidV);
|
||||||
fclose(coordslog);
|
fclose(coordslog);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -24,12 +24,9 @@
|
|||||||
|
|
||||||
#include "moving.h"
|
#include "moving.h"
|
||||||
#include "moving_private.h"
|
#include "moving_private.h"
|
||||||
#include "Dramp.h"
|
|
||||||
#include "Sramp.h"
|
|
||||||
#include "Tramp.h"
|
#include "Tramp.h"
|
||||||
|
|
||||||
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
static movemodel_t *model = &trapez;
|
||||||
static movemodel_t *model = NULL;
|
|
||||||
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||||
double time_tick = TIME_TICK_DEFAULT;
|
double time_tick = TIME_TICK_DEFAULT;
|
||||||
|
|
||||||
@@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){
|
|||||||
*max = t;
|
*max = t;
|
||||||
}
|
}
|
||||||
|
|
||||||
movemodel_t *init_moving(ramptype_t type, limits_t *l){
|
movemodel_t *init_moving(limits_t *l){
|
||||||
if(!l) return FALSE;
|
if(!l) return FALSE;
|
||||||
switch(type){
|
|
||||||
case RAMP_DUMB:
|
|
||||||
model = &dumb;
|
|
||||||
break;
|
|
||||||
case RAMP_TRAPEZIUM:
|
|
||||||
model = &trapez;
|
|
||||||
break;
|
|
||||||
case RAMP_S:
|
|
||||||
model = &s_shaped;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
return FALSE;
|
|
||||||
}
|
|
||||||
if(!model->init_limits) return NULL;
|
if(!model->init_limits) return NULL;
|
||||||
moveparam_t *max = &l->max, *min = &l->min;
|
moveparam_t *max = &l->max, *min = &l->min;
|
||||||
if(min->speed < 0.) min->speed = -min->speed;
|
if(min->speed < 0.) min->speed = -min->speed;
|
||||||
|
|||||||
@@ -26,13 +26,6 @@
|
|||||||
#define TIME_TICK_MIN (1e-9)
|
#define TIME_TICK_MIN (1e-9)
|
||||||
#define TIME_TICK_MAX (10.)
|
#define TIME_TICK_MAX (10.)
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
|
|
||||||
RAMP_TRAPEZIUM, // trapezium ramp
|
|
||||||
RAMP_S, // s-shaped ramp
|
|
||||||
RAMP_AMOUNT
|
|
||||||
} ramptype_t;
|
|
||||||
|
|
||||||
typedef enum{
|
typedef enum{
|
||||||
ST_STOP, // stopped
|
ST_STOP, // stopped
|
||||||
ST_MOVE, // moving
|
ST_MOVE, // moving
|
||||||
@@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative
|
|||||||
typedef struct{
|
typedef struct{
|
||||||
moveparam_t min;
|
moveparam_t min;
|
||||||
moveparam_t max;
|
moveparam_t max;
|
||||||
double jerk;
|
|
||||||
} limits_t;
|
} limits_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
@@ -64,7 +56,7 @@ typedef struct{
|
|||||||
extern double coord_tolerance;
|
extern double coord_tolerance;
|
||||||
|
|
||||||
double nanot();
|
double nanot();
|
||||||
movemodel_t *init_moving(ramptype_t type, limits_t *l);
|
movemodel_t *init_moving(limits_t *l);
|
||||||
int init_coordtol(double tolerance);
|
int init_coordtol(double tolerance);
|
||||||
int init_timetick(double tick);
|
int init_timetick(double tick);
|
||||||
int move_to(moveparam_t *target, double t);
|
int move_to(moveparam_t *target, double t);
|
||||||
|
|||||||
@@ -1,5 +1,7 @@
|
|||||||
Dramp.c
|
Dramp.c
|
||||||
Dramp.h
|
Dramp.h
|
||||||
|
PID.c
|
||||||
|
PID.h
|
||||||
Sramp.c
|
Sramp.c
|
||||||
Sramp.h
|
Sramp.h
|
||||||
Tramp.c
|
Tramp.c
|
||||||
|
|||||||
@@ -1,3 +1,3 @@
|
|||||||
1. PID: slew2
|
fix encoders opening for several tries
|
||||||
2. add model & config "model ON"
|
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
|
||||||
|
|||||||
@@ -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"},
|
||||||
@@ -51,10 +53,10 @@ static sl_option_t confopts[] = {
|
|||||||
end_option
|
end_option
|
||||||
};
|
};
|
||||||
|
|
||||||
static void dumpaxe(char axe, axe_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", axe, #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", axe, #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);
|
||||||
DUMPD(errlimit);
|
DUMPD(errlimit);
|
||||||
@@ -64,6 +66,8 @@ static void dumpaxe(char axe, axe_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
|
||||||
}
|
}
|
||||||
@@ -99,14 +103,15 @@ static void dumpHWconf(){
|
|||||||
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
||||||
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
|
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
|
||||||
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
|
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
|
||||||
green("X axe configuration:\n");
|
green("X axis configuration:\n");
|
||||||
dumpaxe('X', &HW.Xconf);
|
dumpaxis('X', &HW.Xconf);
|
||||||
green("X bits:\n");
|
green("X bits:\n");
|
||||||
dumpxbits(&HW.xbits);
|
dumpxbits(&HW.xbits);
|
||||||
green("Y axe configuration:\n");
|
green("Y axis configuration:\n");
|
||||||
dumpaxe('Y', &HW.Yconf);
|
dumpaxis('Y', &HW.Yconf);
|
||||||
green("Y bits:\n");
|
green("Y bits:\n");
|
||||||
dumpybits(&HW.ybits);
|
dumpybits(&HW.ybits);
|
||||||
|
green("Other:\n");
|
||||||
printf("address=%d\n", HW.address);
|
printf("address=%d\n", HW.address);
|
||||||
DUMP(eqrate);
|
DUMP(eqrate);
|
||||||
DUMP(eqadj);
|
DUMP(eqadj);
|
||||||
@@ -142,10 +147,12 @@ int main(int argc, char** argv){
|
|||||||
}
|
}
|
||||||
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
|
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
|
||||||
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
|
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
|
||||||
|
/*
|
||||||
char *c = sl_print_opts(confopts, TRUE);
|
char *c = sl_print_opts(confopts, TRUE);
|
||||||
green("Got configuration:\n");
|
green("Got configuration:\n");
|
||||||
printf("%s\n", c);
|
printf("%s\n", c);
|
||||||
FREE(c);
|
FREE(c);
|
||||||
|
*/
|
||||||
dumpHWconf();
|
dumpHWconf();
|
||||||
/*
|
/*
|
||||||
if(G.hwconffile && G.writeconf){
|
if(G.hwconffile && G.writeconf){
|
||||||
|
|||||||
@@ -24,13 +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,
|
||||||
|
.EncodersDisagreement = 1e-5, // 2''
|
||||||
|
.PIDMaxDt = 1.,
|
||||||
|
.PIDRefreshDt = 0.1,
|
||||||
|
.PIDCycleDt = 5.,
|
||||||
|
.XPIDC.P = 0.5,
|
||||||
|
.XPIDC.I = 0.1,
|
||||||
|
.XPIDC.D = 0.2,
|
||||||
|
.XPIDV.P = 0.09,
|
||||||
|
.XPIDV.I = 0.0,
|
||||||
|
.XPIDV.D = 0.05,
|
||||||
|
.YPIDC.P = 0.5,
|
||||||
|
.YPIDC.I = 0.1,
|
||||||
|
.YPIDC.D = 0.2,
|
||||||
|
.YPIDV.P = 0.09,
|
||||||
|
.YPIDV.I = 0.0,
|
||||||
|
.YPIDV.D = 0.05,
|
||||||
|
.MaxPointingErr = 0.13962634,
|
||||||
|
.MaxFinePointingErr = 0.026179939,
|
||||||
|
.MaxGuidingErr = 4.8481368e-7,
|
||||||
};
|
};
|
||||||
|
|
||||||
static sl_option_t opts[] = {
|
static sl_option_t opts[] = {
|
||||||
@@ -38,12 +57,35 @@ 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"},
|
||||||
|
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
|
||||||
|
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
|
||||||
|
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
|
||||||
|
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
|
||||||
|
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
|
||||||
|
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
|
||||||
|
{"YPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.P), "P of Y PID (coordinate driven)"},
|
||||||
|
{"YPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.I), "I of Y PID (coordinate driven)"},
|
||||||
|
{"YPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.D), "D of Y PID (coordinate driven)"},
|
||||||
|
{"XPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.P), "P of X PID (velocity driven)"},
|
||||||
|
{"XPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.I), "I of X PID (velocity driven)"},
|
||||||
|
{"XPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.D), "D of X PID (velocity driven)"},
|
||||||
|
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
|
||||||
|
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
|
||||||
|
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
|
||||||
|
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
|
||||||
|
{"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
|
||||||
|
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
|
||||||
|
{"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"},
|
||||||
|
{"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"},
|
||||||
|
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
|
||||||
end_option
|
end_option
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -68,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];
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -67,17 +76,13 @@ static double filter(double val, int idx){
|
|||||||
void logmnt(FILE *fcoords, mountdata_t *m){
|
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");
|
||||||
static double t0 = -1.;
|
|
||||||
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;
|
||||||
double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
|
|
||||||
if(t0 < 0.) t0 = tnow;
|
|
||||||
double t = tnow - t0;
|
|
||||||
// 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",
|
||||||
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);
|
||||||
@@ -103,19 +108,20 @@ 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", ctr);
|
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
|
||||||
mdmillis = mdata.millis;
|
mdmillis = mdata.millis;
|
||||||
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
||||||
xlast = mdata.motXposition.val;
|
xlast = mdata.motXposition.val;
|
||||||
@@ -123,27 +129,26 @@ 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.timeFromStart() - t0);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief waitmoving - wait until moving by both axes stops at least for N cycles
|
* @brief waitmoving - wait until moving by both axiss stops at least for N cycles
|
||||||
* @param N - amount of stopped cycles
|
* @param N - amount of stopped cycles
|
||||||
*/
|
*/
|
||||||
void waitmoving(int N){
|
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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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){
|
||||||
|
|||||||
@@ -120,16 +120,13 @@ static coordpair_t lastTag = {0}, lastSpeed = {0};
|
|||||||
|
|
||||||
// slew to given position and start tracking
|
// slew to given position and start tracking
|
||||||
// pos/speed in deg and deg/s
|
// pos/speed in deg and deg/s
|
||||||
static mcc_errcodes_t gotos(coordpair_t *target, coordpair_t *speed){
|
static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){
|
||||||
short_command_t cmd = {0};
|
short_command_t cmd = {0};
|
||||||
DBG("Try to move to (%g, %g) with speed (%g, %g)",
|
DBG("Try to move to (%g, %g) with speed (%g, %g)",
|
||||||
target->X, target->Y, speed->X, speed->Y);
|
target->X, target->Y, speed->X, speed->Y);
|
||||||
target->X = DEG2RAD(target->X);
|
cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
|
||||||
target->Y = DEG2RAD(target->Y);
|
cmd.Xspeed = DEG2RAD(speed->X);
|
||||||
speed->X = DEG2RAD(speed->X);
|
cmd.Yspeed = DEG2RAD(speed->Y);
|
||||||
speed->Y = DEG2RAD(speed->Y);
|
|
||||||
cmd.Xmot = target->X; cmd.Ymot = target->Y;
|
|
||||||
cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y;
|
|
||||||
lastTag = *target;
|
lastTag = *target;
|
||||||
lastSpeed = *speed;
|
lastSpeed = *speed;
|
||||||
/*cmd.xychange = 1;
|
/*cmd.xychange = 1;
|
||||||
@@ -142,7 +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 = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.);
|
coordpair_t maxspd;
|
||||||
|
if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
|
||||||
|
cmd.Xspeed = maxspd.X;
|
||||||
|
cmd.Yspeed = maxspd.Y;
|
||||||
/*cmd.xychange = 1;
|
/*cmd.xychange = 1;
|
||||||
cmd.XBits = 100;
|
cmd.XBits = 100;
|
||||||
cmd.YBits = 20;*/
|
cmd.YBits = 20;*/
|
||||||
@@ -151,11 +151,11 @@ static mcc_errcodes_t return2zero(){
|
|||||||
|
|
||||||
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
|
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
|
||||||
long_command_t cmd = {0};
|
long_command_t cmd = {0};
|
||||||
cmd.Xspeed = lastSpeed.X;
|
cmd.Xspeed = DEG2RAD(lastSpeed.X);
|
||||||
cmd.Yspeed = lastSpeed.Y;
|
cmd.Yspeed = DEG2RAD(lastSpeed.Y);
|
||||||
cmd.Xmot = lastTag.X;
|
cmd.Xmot = DEG2RAD(lastTag.X);
|
||||||
cmd.Ymot = lastTag.Y;
|
cmd.Ymot = DEG2RAD(lastTag.Y);
|
||||||
cmd.Xadder = adder->X; cmd.Yadder = adder->Y;
|
cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
|
||||||
cmd.Xatime = time->X; cmd.Yatime = time->Y;
|
cmd.Xatime = time->X; cmd.Yatime = time->Y;
|
||||||
return Mount.longCmd(&cmd);
|
return Mount.longCmd(&cmd);
|
||||||
}
|
}
|
||||||
@@ -218,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");
|
||||||
return2zero();
|
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
|
||||||
|
|||||||
@@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = {
|
|||||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||||
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
|
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
|
||||||
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
|
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
|
||||||
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
|
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
|
||||||
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
|
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
|
||||||
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
|
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
|
||||||
end_option
|
end_option
|
||||||
@@ -83,18 +83,18 @@ 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;
|
||||||
millis = mdata.millis;
|
millis = mdata.millis;
|
||||||
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
||||||
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
//DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
||||||
xlast = mdata.motXposition.val;
|
xlast = mdata.motXposition.val;
|
||||||
ylast = mdata.motYposition.val;
|
ylast = mdata.motYposition.val;
|
||||||
ctr = 0;
|
ctr = 0;
|
||||||
}else{
|
}else{
|
||||||
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
//DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
||||||
++ctr;
|
++ctr;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -110,15 +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){
|
||||||
if(G.amplitude < 0.01 || G.amplitude > 45.)
|
WARNX("Ncycles should be >2");
|
||||||
ERRX("Amplitude should be from 0.01 to 45 degrees");
|
return 1;
|
||||||
if(G.period < 0.1 || G.period > 900.)
|
}
|
||||||
ERRX("Period should be from 0.1 to 900s");
|
double absamp = fabs(G.amplitude);
|
||||||
if(G.Nswings < 1) ERRX("Nswings should be more than 0");
|
if(absamp < 0.01 || absamp > 45.){
|
||||||
|
WARNX("Amplitude should be from 0.01 to 45 degrees");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
if(G.period < 0.1 || G.period > 900.){
|
||||||
|
WARNX("Period should be from 0.1 to 900s");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
if(G.Nswings < 1){
|
||||||
|
WARNX("Nswings should be more than 0");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
conf_t *Config = readServoConf(G.conffile);
|
conf_t *Config = readServoConf(G.conffile);
|
||||||
if(!Config){
|
if(!Config){
|
||||||
dumpConf();
|
dumpConf();
|
||||||
@@ -145,26 +158,30 @@ 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);
|
||||||
}
|
}
|
||||||
tag = (coordpair_t){.X = 0., .Y = 0.};
|
green("Move to zero @ %g\n", Mount.timeFromStart());
|
||||||
|
tag = (coordpair_t){0};
|
||||||
// be sure to move @ 0,0
|
// be sure to move @ 0,0
|
||||||
|
if(MCC_E_OK != Mount.moveTo(&tag)){
|
||||||
|
Mount.emergStop();
|
||||||
Mount.moveTo(&tag);
|
Mount.moveTo(&tag);
|
||||||
|
}
|
||||||
// wait moving ends
|
// wait moving ends
|
||||||
pthread_join(dthr, NULL);
|
pthread_join(dthr, NULL);
|
||||||
#undef SCMD
|
#undef SCMD
|
||||||
|
|||||||
@@ -61,12 +61,13 @@ static FILE* fcoords = NULL;
|
|||||||
static pthread_t dthr;
|
static pthread_t dthr;
|
||||||
|
|
||||||
void signals(int sig){
|
void signals(int sig){
|
||||||
pthread_cancel(dthr);
|
|
||||||
if(sig){
|
if(sig){
|
||||||
signal(sig, SIG_IGN);
|
signal(sig, SIG_IGN);
|
||||||
DBG("Get signal %d, quit.\n", sig);
|
DBG("Get signal %d, quit.\n", sig);
|
||||||
}
|
}
|
||||||
|
DBG("Quit");
|
||||||
Mount.quit();
|
Mount.quit();
|
||||||
|
DBG("close");
|
||||||
if(fcoords) fclose(fcoords);
|
if(fcoords) fclose(fcoords);
|
||||||
exit(sig);
|
exit(sig);
|
||||||
}
|
}
|
||||||
@@ -90,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);
|
||||||
@@ -120,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);
|
||||||
@@ -132,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;
|
||||||
|
|||||||
@@ -30,6 +30,7 @@
|
|||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
int help;
|
int help;
|
||||||
|
int dumpconf;
|
||||||
int Ncycles; // n cycles to wait stop
|
int Ncycles; // n cycles to wait stop
|
||||||
double reqint; // requests interval (seconds)
|
double reqint; // requests interval (seconds)
|
||||||
double Xmax; // maximal X to stop
|
double Xmax; // maximal X to stop
|
||||||
@@ -38,11 +39,13 @@ typedef struct{
|
|||||||
double X0; // starting point of traectory (-30..30 degr)
|
double X0; // starting point of traectory (-30..30 degr)
|
||||||
double Y0; // -//-
|
double Y0; // -//-
|
||||||
char *coordsoutput; // dump file
|
char *coordsoutput; // dump file
|
||||||
|
char *errlog; // log with position errors
|
||||||
char *tfn; // traectory function name
|
char *tfn; // traectory function name
|
||||||
char *conffile;
|
char *conffile;
|
||||||
} parameters;
|
} parameters;
|
||||||
|
|
||||||
static FILE *fcoords = NULL;
|
static conf_t *Config = NULL;
|
||||||
|
static FILE *fcoords = NULL, *errlog = NULL;
|
||||||
static pthread_t dthr;
|
static pthread_t dthr;
|
||||||
static parameters G = {
|
static parameters G = {
|
||||||
.Ncycles = 40,
|
.Ncycles = 40,
|
||||||
@@ -61,21 +64,24 @@ static sl_option_t cmdlnopts[] = {
|
|||||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||||
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
|
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
|
||||||
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
|
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
|
||||||
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal X coordinate for traectory (default: 45 degrees)"},
|
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
|
||||||
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X coordinate for traectory (default: 45 degrees)"},
|
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y coordinate for traectory (default: 45 degrees)"},
|
||||||
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
|
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
|
||||||
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
|
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
|
||||||
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
|
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
|
||||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||||
|
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
|
||||||
|
{"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"},
|
||||||
end_option
|
end_option
|
||||||
};
|
};
|
||||||
|
|
||||||
void signals(int sig){
|
void signals(int sig){
|
||||||
pthread_cancel(dthr);
|
|
||||||
if(sig){
|
if(sig){
|
||||||
signal(sig, SIG_IGN);
|
signal(sig, SIG_IGN);
|
||||||
DBG("Get signal %d, quit.\n", sig);
|
DBG("Get signal %d, quit.\n", sig);
|
||||||
}
|
}
|
||||||
|
Mount.stop();
|
||||||
|
sleep(1);
|
||||||
Mount.quit();
|
Mount.quit();
|
||||||
if(fcoords) fclose(fcoords);
|
if(fcoords) fclose(fcoords);
|
||||||
exit(sig);
|
exit(sig);
|
||||||
@@ -90,22 +96,39 @@ static void *dumping(void _U_ *u){
|
|||||||
static void runtraectory(traectory_fn tfn){
|
static void runtraectory(traectory_fn tfn){
|
||||||
if(!tfn) return;
|
if(!tfn) return;
|
||||||
coordval_pair_t telXY;
|
coordval_pair_t telXY;
|
||||||
|
coordval_pair_t target;
|
||||||
coordpair_t traectXY;
|
coordpair_t traectXY;
|
||||||
double t0 = Mount.currentT();
|
double tlast = 0., tstart = Mount.timeFromStart();
|
||||||
double tlast = 0.;
|
long tlastXnsec = 0, tlastYnsec = 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 == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
|
if(!Mount.currentT(&tcur)) continue;
|
||||||
tlast = (telXY.X.t + telXY.Y.t) / 2.;
|
if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
|
||||||
double t = Mount.currentT();
|
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
|
||||||
if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
|
tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
|
||||||
|
double t = Mount.timeFromStart();
|
||||||
|
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
|
||||||
if(!traectory_point(&traectXY, t)) break;
|
if(!traectory_point(&traectXY, t)) break;
|
||||||
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
|
||||||
|
target.X.t = target.Y.t = tcur;
|
||||||
|
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
|
||||||
|
else{
|
||||||
|
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
||||||
|
DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||||
|
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
|
||||||
|
if(errlog)
|
||||||
|
fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||||
}
|
}
|
||||||
WARNX("No next traectory point");
|
if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
|
||||||
|
while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
|
||||||
|
tlast = t;
|
||||||
|
}
|
||||||
|
WARNX("No next traectory point or emulation ends");
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
int main(int argc, char **argv){
|
||||||
@@ -118,12 +141,18 @@ int main(int argc, char **argv){
|
|||||||
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
|
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
|
||||||
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
|
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
|
||||||
ERRX("X0 and Y0 should be -30..30 degrees");
|
ERRX("X0 and Y0 should be -30..30 degrees");
|
||||||
|
if(G.errlog){
|
||||||
|
if(!(errlog = fopen(G.errlog, "w")))
|
||||||
|
ERRX("Can't open error log %s", G.errlog);
|
||||||
|
else
|
||||||
|
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
|
||||||
|
}
|
||||||
if(G.coordsoutput){
|
if(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);
|
||||||
}else fcoords = stdout;
|
}else fcoords = stdout;
|
||||||
conf_t *Config = readServoConf(G.conffile);
|
Config = readServoConf(G.conffile);
|
||||||
if(!Config){
|
if(!Config || G.dumpconf){
|
||||||
dumpConf();
|
dumpConf();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,9 +1,25 @@
|
|||||||
|
# Current configuration
|
||||||
MountDevPath=/dev/ttyUSB0
|
MountDevPath=/dev/ttyUSB0
|
||||||
MountDevSpeed=19200
|
MountDevSpeed=19200
|
||||||
|
EncoderDevPath=(null)
|
||||||
|
EncoderDevSpeed=1000000
|
||||||
|
MountReqInterval=0.1
|
||||||
|
EncoderReqInterval=0.001
|
||||||
|
SepEncoder=2
|
||||||
EncoderXDevPath=/dev/encoder_X0
|
EncoderXDevPath=/dev/encoder_X0
|
||||||
EncoderYDevPath=/dev/encoder_Y0
|
EncoderYDevPath=/dev/encoder_Y0
|
||||||
MountReqInterval=0.05
|
|
||||||
SepEncoder=2
|
|
||||||
EncoderReqInterval=0.001
|
|
||||||
EncoderDevSpeed=1000000
|
|
||||||
EncoderSpeedInterval=0.05
|
EncoderSpeedInterval=0.05
|
||||||
|
RunModel=1
|
||||||
|
XPIDCP=0.8
|
||||||
|
XPIDCI=0.0
|
||||||
|
XPIDCD=0.0
|
||||||
|
YPIDCP=0.5
|
||||||
|
YPIDCI=0.0
|
||||||
|
YPIDCD=0.0
|
||||||
|
XPIDVP=0.2
|
||||||
|
XPIDVI=0.1
|
||||||
|
XPIDVD=0.0
|
||||||
|
YPIDVP=0.2
|
||||||
|
YPIDVI=0.1
|
||||||
|
YPIDVD=0.0
|
||||||
|
|
||||||
|
|||||||
@@ -20,10 +20,10 @@
|
|||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#define DEG2RAD(d) (d/180.*M_PI)
|
#define DEG2RAD(d) ((d)/180.*M_PI)
|
||||||
#define ASEC2RAD(d) (d/180.*M_PI/3600.)
|
#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
|
||||||
#define AMIN2RAD(d) (d/180.*M_PI/60.)
|
#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
|
||||||
#define RAD2DEG(r) (r/M_PI*180.)
|
#define RAD2DEG(r) ((r)/M_PI*180.)
|
||||||
#define RAD2ASEC(r) (r/M_PI*180.*3600.)
|
#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
|
||||||
#define RAD2AMIN(r) (r/M_PI*180.*60.)
|
#define RAD2AMIN(r) ((r)/M_PI*180.*60.)
|
||||||
|
|
||||||
|
|||||||
@@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL;
|
|||||||
// starting point of traectory
|
// starting point of traectory
|
||||||
static coordpair_t XYstart = {0};
|
static coordpair_t XYstart = {0};
|
||||||
static double tstart = 0.;
|
static double tstart = 0.;
|
||||||
// convert Xe/Ye to approximate motor coordinates:
|
|
||||||
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
|
|
||||||
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
|
|
||||||
static coordval_pair_t XYcor = {0};
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
|
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
|
||||||
@@ -45,16 +41,13 @@ 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){
|
||||||
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
||||||
}
|
}
|
||||||
if(ntries == 10) return FALSE;
|
if(ntries == 10) return FALSE;
|
||||||
XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
|
|
||||||
XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
|
|
||||||
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
|
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -83,8 +76,9 @@ int telpos(coordval_pair_t *curpos){
|
|||||||
}
|
}
|
||||||
if(ntries == 10) return FALSE;
|
if(ntries == 10) return FALSE;
|
||||||
coordval_pair_t pt;
|
coordval_pair_t pt;
|
||||||
pt.X.val = XYcor.X.val + mdata.encXposition.val;
|
//DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
|
||||||
pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
|
pt.X.val = mdata.encXposition.val;
|
||||||
|
pt.Y.val = mdata.encYposition.val;
|
||||||
pt.X.t = mdata.encXposition.t;
|
pt.X.t = mdata.encXposition.t;
|
||||||
pt.Y.t = mdata.encYposition.t;
|
pt.Y.t = mdata.encYposition.t;
|
||||||
if(curpos) *curpos = pt;
|
if(curpos) *curpos = pt;
|
||||||
@@ -94,7 +88,7 @@ int telpos(coordval_pair_t *curpos){
|
|||||||
// X=X0+1'/s, Y=Y0+15''/s
|
// X=X0+1'/s, Y=Y0+15''/s
|
||||||
int Linear(coordpair_t *nextpt, double t){
|
int Linear(coordpair_t *nextpt, double t){
|
||||||
coordpair_t pt;
|
coordpair_t pt;
|
||||||
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
|
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
|
||||||
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
|
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
|
||||||
if(nextpt) *nextpt = pt;
|
if(nextpt) *nextpt = pt;
|
||||||
return TRUE;
|
return TRUE;
|
||||||
@@ -103,8 +97,8 @@ int Linear(coordpair_t *nextpt, double t){
|
|||||||
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
|
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
|
||||||
int SinCos(coordpair_t *nextpt, double t){
|
int SinCos(coordpair_t *nextpt, double t){
|
||||||
coordpair_t pt;
|
coordpair_t pt;
|
||||||
pt.X = XYstart.X + AMIN2RAD(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;
|
||||||
}
|
}
|
||||||
@@ -116,8 +110,8 @@ typedef struct{
|
|||||||
} tr_names;
|
} tr_names;
|
||||||
|
|
||||||
static tr_names names[] = {
|
static tr_names names[] = {
|
||||||
{Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
|
{Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
|
||||||
{SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
|
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
|
||||||
{NULL, NULL, NULL}
|
{NULL, NULL, NULL}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
// Add predefined macros for your project here. For example:
|
// Add predefined macros for your project here. For example:
|
||||||
// #define THE_ANSWER 42
|
// #define THE_ANSWER 42
|
||||||
#define EBUG
|
#define EBUG
|
||||||
#define _POSIX_C_SOURCE 111
|
#define _POSIX_C_SOURCE 11111111
|
||||||
#define PACKAGE_VERSION "0.0.1"
|
#define PACKAGE_VERSION "0.0.1"
|
||||||
|
#define _XOPEN_SOURCE 666
|
||||||
|
#define _DEFAULT_SOURCE
|
||||||
|
|||||||
@@ -1,10 +1,10 @@
|
|||||||
<?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-29T22:07:00. -->
|
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
|
||||||
<qtcreator>
|
<qtcreator>
|
||||||
<data>
|
<data>
|
||||||
<variable>EnvironmentId</variable>
|
<variable>EnvironmentId</variable>
|
||||||
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
|
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||||
</data>
|
</data>
|
||||||
<data>
|
<data>
|
||||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||||
@@ -40,9 +40,9 @@
|
|||||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||||
@@ -51,10 +51,10 @@
|
|||||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||||
</valuemap>
|
</valuemap>
|
||||||
@@ -75,19 +75,18 @@
|
|||||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||||
<valuelist type="QVariantList" key="ClangCodeModel.CustomCommandLineKey"/>
|
|
||||||
<value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
|
|
||||||
<valuemap type="QVariantMap" key="ClangTools">
|
<valuemap type="QVariantMap" key="ClangTools">
|
||||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||||
<value type="int" key="ClangTools.ParallelJobs">8</value>
|
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||||
<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>
|
||||||
@@ -97,12 +96,12 @@
|
|||||||
<value type="bool" key="HasPerBcDcs">true</value>
|
<value type="bool" key="HasPerBcDcs">true</value>
|
||||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</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.DisplayName">Desktop</value>
|
||||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</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.ActiveBuildConfiguration">0</value>
|
||||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/C-sources/erfa</value>
|
<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.BuildConfiguration.BuildStepList.0">
|
||||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||||
@@ -154,7 +153,9 @@
|
|||||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||||
<value type="bool" key="Analyzer.QmlProfiler.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="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||||
|
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||||
@@ -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>
|
||||||
@@ -186,7 +188,9 @@
|
|||||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||||
<value type="bool" key="Analyzer.QmlProfiler.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="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||||
|
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||||
@@ -196,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>
|
||||||
@@ -206,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>
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
CMakeLists.txt
|
CMakeLists.txt
|
||||||
dbg.h
|
PID.c
|
||||||
|
PID.h
|
||||||
examples/SSIIconf.c
|
examples/SSIIconf.c
|
||||||
examples/conf.c
|
examples/conf.c
|
||||||
examples/conf.h
|
examples/conf.h
|
||||||
@@ -18,6 +19,12 @@ serial.c
|
|||||||
examples/CMakeLists.txt
|
examples/CMakeLists.txt
|
||||||
examples/traectories.c
|
examples/traectories.c
|
||||||
examples/traectories.h
|
examples/traectories.h
|
||||||
|
main.h
|
||||||
|
movingmodel.c
|
||||||
|
movingmodel.h
|
||||||
|
polltest/main.c
|
||||||
|
ramp.c
|
||||||
|
ramp.h
|
||||||
serial.h
|
serial.h
|
||||||
ssii.c
|
ssii.c
|
||||||
ssii.h
|
ssii.h
|
||||||
|
|||||||
@@ -16,28 +16,171 @@
|
|||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* main functions to fill struct `mount_t`
|
||||||
|
*/
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
#include <strings.h>
|
||||||
|
#include <time.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "dbg.h"
|
#include "main.h"
|
||||||
|
#include "movingmodel.h"
|
||||||
#include "serial.h"
|
#include "serial.h"
|
||||||
#include "ssii.h"
|
#include "ssii.h"
|
||||||
|
#include "PID.h"
|
||||||
|
|
||||||
|
// adder for monotonic time by realtime: inited any call of init()
|
||||||
|
static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC
|
||||||
|
t0 = {0}, // curtime() for initstarttime() call
|
||||||
|
starttime = {0}; // starting time by monotonic (for timefromstart())
|
||||||
|
|
||||||
conf_t Conf = {0};
|
conf_t Conf = {0};
|
||||||
|
// parameters for model
|
||||||
|
static movemodel_t *Xmodel, *Ymodel;
|
||||||
|
// limits for model and/or real mount (in latter case data should be read from mount on init)
|
||||||
|
// radians, rad/sec, rad/sec^2
|
||||||
|
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||||
|
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||||
|
limits_t
|
||||||
|
Xlimits = {
|
||||||
|
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||||
|
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
|
||||||
|
Ylimits = {
|
||||||
|
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||||
|
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
|
||||||
|
;
|
||||||
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
||||||
|
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief curtime - monotonic time from first run
|
||||||
|
* @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
|
||||||
|
* @return TRUE if all OK
|
||||||
|
* FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
|
||||||
|
*/
|
||||||
|
int curtime(struct timespec *t){
|
||||||
|
struct timespec now;
|
||||||
|
if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
|
||||||
|
now.tv_sec += timeadder.tv_sec;
|
||||||
|
now.tv_nsec += timeadder.tv_nsec;
|
||||||
|
if(now.tv_nsec > 999999999L){
|
||||||
|
++now.tv_sec;
|
||||||
|
now.tv_nsec -= 1000000000L;
|
||||||
|
}
|
||||||
|
if(t) *t = now;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// init starttime; @return TRUE if all OK
|
||||||
|
static int initstarttime(){
|
||||||
|
struct timespec start;
|
||||||
|
if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
|
||||||
|
if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
|
||||||
|
timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
|
||||||
|
timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
|
||||||
|
if(timeadder.tv_nsec < 0){
|
||||||
|
--timeadder.tv_sec;
|
||||||
|
timeadder.tv_nsec += 1000000000L;
|
||||||
|
}
|
||||||
|
curtime(&t0);
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return difference (in seconds) between time1 and time0
|
||||||
|
double timediff(const struct timespec *time1, const struct timespec *time0){
|
||||||
|
if(!time1 || !time0) return -1.;
|
||||||
|
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
|
||||||
|
}
|
||||||
|
// difference between given time and last initstarttime() call
|
||||||
|
double timediff0(const struct timespec *time1){
|
||||||
|
return timediff(time1, &t0);
|
||||||
|
}
|
||||||
|
// time from last initstarttime() call
|
||||||
|
double timefromstart(){
|
||||||
|
struct timespec now;
|
||||||
|
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
|
||||||
|
return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief quit - close all opened and return to default state
|
* @brief quit - close all opened and return to default state
|
||||||
|
* TODO: close serial devices even in "model" mode
|
||||||
*/
|
*/
|
||||||
static void quit(){
|
static void quit(){
|
||||||
DBG("Close serial devices");
|
if(Conf.RunModel) return;
|
||||||
for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
|
for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
|
||||||
|
DBG("Close all serial devices");
|
||||||
closeSerial();
|
closeSerial();
|
||||||
DBG("Exit");
|
DBG("Exit");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
|
||||||
|
if(!c || !Xmodel || !Ymodel) return;
|
||||||
|
double tnow = timefromstart();
|
||||||
|
moveparam_t Xp, Yp;
|
||||||
|
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
|
||||||
|
//DBG("Xstate = %d", Xst);
|
||||||
|
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
|
||||||
|
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
|
||||||
|
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
|
||||||
|
c->X = Xp.coord;
|
||||||
|
c->Y = Yp.coord;
|
||||||
|
if(xst) *xst = Xst;
|
||||||
|
if(yst) *yst = Yst;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* less square calculations of speed
|
||||||
|
*/
|
||||||
|
less_square_t *LS_init(size_t Ndata){
|
||||||
|
if(Ndata < 5){
|
||||||
|
DBG("Ndata=%zd - TOO SMALL", Ndata);
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
DBG("Init less squares: %zd", Ndata);
|
||||||
|
less_square_t *l = calloc(1, sizeof(less_square_t));
|
||||||
|
l->x = calloc(Ndata, sizeof(double));
|
||||||
|
l->t2 = calloc(Ndata, sizeof(double));
|
||||||
|
l->t = calloc(Ndata, sizeof(double));
|
||||||
|
l->xt = calloc(Ndata, sizeof(double));
|
||||||
|
l->arraysz = Ndata;
|
||||||
|
return l;
|
||||||
|
}
|
||||||
|
void LS_delete(less_square_t **l){
|
||||||
|
if(!l || !*l) return;
|
||||||
|
free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
|
||||||
|
free(*l);
|
||||||
|
*l = NULL;
|
||||||
|
}
|
||||||
|
// add next data portion and calculate current slope
|
||||||
|
double LS_calc_slope(less_square_t *l, double x, double t){
|
||||||
|
if(!l) return 0.;
|
||||||
|
size_t idx = l->idx;
|
||||||
|
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
|
||||||
|
/*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;
|
||||||
|
l->x[idx] = x; l->t2[idx] = t2;
|
||||||
|
l->t[idx] = t; l->xt[idx] = xt;
|
||||||
|
++idx;
|
||||||
|
l->idx = (idx >= l->arraysz) ? 0 : idx;
|
||||||
|
l->xsum += x - oldx;
|
||||||
|
l->t2sum += t2 - oldt2;
|
||||||
|
l->tsum += t - oldt;
|
||||||
|
l->xtsum += xt - oldxt;
|
||||||
|
double n = (double)l->arraysz;
|
||||||
|
double denominator = n * l->t2sum - l->tsum * l->tsum;
|
||||||
|
if(fabs(denominator) < 1e-7) return 0.;
|
||||||
|
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;
|
||||||
|
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
|
||||||
@@ -46,9 +189,20 @@ static void quit(){
|
|||||||
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;
|
||||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
|
Xmodel = model_init(&Xlimits);
|
||||||
|
Ymodel = model_init(&Ylimits);
|
||||||
|
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||||
|
DBG("Bad value of MountReqInterval");
|
||||||
|
ret = MCC_E_BADFORMAT;
|
||||||
|
}
|
||||||
|
if(Conf.RunModel){
|
||||||
|
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
|
||||||
|
return MCC_E_OK;
|
||||||
|
}
|
||||||
|
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
|
||||||
DBG("Define mount device path and speed");
|
DBG("Define mount device path and speed");
|
||||||
ret = MCC_E_BADFORMAT;
|
ret = MCC_E_BADFORMAT;
|
||||||
}else if(!openMount()){
|
}else if(!openMount()){
|
||||||
@@ -64,52 +218,62 @@ 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];
|
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
|
||||||
data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
|
|
||||||
// read input data as there may be some trash on start
|
|
||||||
if(!SSrawcmd(CMD_EXITACM, &d)) 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
// set SLEWING state if axis was stopped
|
||||||
(void)target;
|
static void setslewingstate(){
|
||||||
(void)flags;
|
//FNAME();
|
||||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
mountdata_t d;
|
||||||
//...
|
if(MCC_E_OK == getMD(&d)){
|
||||||
setStat(MNT_SLEWING, MNT_SLEWING);
|
axis_status_t newx = d.Xstate, newy = d.Ystate;
|
||||||
//...
|
//DBG("old state: %d/%d", d.Xstate, d.Ystate);
|
||||||
return MCC_E_FAILED;
|
if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
|
||||||
|
if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
|
||||||
|
if(newx != d.Xstate || newy != d.Ystate){
|
||||||
|
DBG("Started moving -> slew");
|
||||||
|
setStat(newx, newy);
|
||||||
|
}
|
||||||
|
}else DBG("CAN't GET MOUNT DATA!");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief move2 - simple move to given point and stop
|
* @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
|
||||||
@@ -124,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;
|
||||||
setStat(MNT_SLEWING, MNT_SLEWING);
|
setslewingstate();
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;*/
|
||||||
|
return shortcmd(&cmd);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -140,6 +305,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
|
|||||||
*/
|
*/
|
||||||
static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
|
static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
|
||||||
if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
|
if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
|
||||||
|
if(Conf.RunModel) return MCC_E_FAILED;
|
||||||
int32_t spd = X_RS2MOTSPD(tagspeed->X);
|
int32_t spd = X_RS2MOTSPD(tagspeed->X);
|
||||||
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
|
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
|
||||||
spd = Y_RS2MOTSPD(tagspeed->Y);
|
spd = Y_RS2MOTSPD(tagspeed->Y);
|
||||||
@@ -157,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;
|
||||||
@@ -165,7 +332,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
|||||||
cmd.Yspeed = speed->Y;
|
cmd.Yspeed = speed->Y;
|
||||||
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;
|
||||||
setStat(MNT_SLEWING, MNT_SLEWING);
|
setslewingstate();
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -174,11 +341,25 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
|||||||
* @return errcode
|
* @return errcode
|
||||||
*/
|
*/
|
||||||
static mcc_errcodes_t emstop(){
|
static mcc_errcodes_t emstop(){
|
||||||
|
FNAME();
|
||||||
|
if(Conf.RunModel){
|
||||||
|
double curt = timefromstart();
|
||||||
|
Xmodel->emergency_stop(Xmodel, curt);
|
||||||
|
Ymodel->emergency_stop(Ymodel, curt);
|
||||||
|
return MCC_E_OK;
|
||||||
|
}
|
||||||
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
// normal stop
|
// normal stop
|
||||||
static mcc_errcodes_t stop(){
|
static mcc_errcodes_t stop(){
|
||||||
|
FNAME();
|
||||||
|
if(Conf.RunModel){
|
||||||
|
double curt = timefromstart();
|
||||||
|
Xmodel->stop(Xmodel, curt);
|
||||||
|
Ymodel->stop(Ymodel,curt);
|
||||||
|
return MCC_E_OK;
|
||||||
|
}
|
||||||
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
@@ -190,6 +371,16 @@ 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){
|
||||||
|
double curt = timefromstart();
|
||||||
|
moveparam_t param = {0};
|
||||||
|
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||||
|
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||||
|
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
|
||||||
|
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
||||||
|
setslewingstate();
|
||||||
|
return MCC_E_OK;
|
||||||
|
}
|
||||||
SSscmd s = {0};
|
SSscmd s = {0};
|
||||||
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
|
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
|
||||||
s.Xmot = X_RAD2MOT(cmd->Xmot);
|
s.Xmot = X_RAD2MOT(cmd->Xmot);
|
||||||
@@ -201,16 +392,27 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
|
|||||||
s.YBits = cmd->YBits;
|
s.YBits = cmd->YBits;
|
||||||
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
|
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
|
||||||
if(!cmdS(&s)) return MCC_E_FAILED;
|
if(!cmdS(&s)) return MCC_E_FAILED;
|
||||||
|
setslewingstate();
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief shortcmd - send and receive long binary command
|
* @brief longcmd - send and receive long binary command
|
||||||
* @param cmd (io) - command
|
* @param cmd (io) - command
|
||||||
* @return errcode
|
* @return errcode
|
||||||
*/
|
*/
|
||||||
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){
|
||||||
|
double curt = timefromstart();
|
||||||
|
moveparam_t param = {0};
|
||||||
|
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||||
|
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||||
|
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
|
||||||
|
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
||||||
|
setslewingstate();
|
||||||
|
return MCC_E_OK;
|
||||||
|
}
|
||||||
SSlcmd l = {0};
|
SSlcmd l = {0};
|
||||||
l.Xmot = X_RAD2MOT(cmd->Xmot);
|
l.Xmot = X_RAD2MOT(cmd->Xmot);
|
||||||
l.Ymot = Y_RAD2MOT(cmd->Ymot);
|
l.Ymot = Y_RAD2MOT(cmd->Ymot);
|
||||||
@@ -221,12 +423,15 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
|
|||||||
l.Xatime = S2ADDER(cmd->Xatime);
|
l.Xatime = S2ADDER(cmd->Xatime);
|
||||||
l.Yatime = S2ADDER(cmd->Yatime);
|
l.Yatime = S2ADDER(cmd->Yatime);
|
||||||
if(!cmdL(&l)) return MCC_E_FAILED;
|
if(!cmdL(&l)) return MCC_E_FAILED;
|
||||||
|
setslewingstate();
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
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;
|
||||||
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);
|
||||||
@@ -266,7 +471,7 @@ 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);
|
hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
|
||||||
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
|
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);
|
||||||
@@ -285,11 +490,36 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
||||||
SSconfig config;
|
SSconfig config;
|
||||||
|
if(Conf.RunModel) return MCC_E_FAILED;
|
||||||
// Convert acceleration (rad/s^2 to ticks per loop^2)
|
// Convert acceleration (rad/s^2 to ticks per loop^2)
|
||||||
config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
|
config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
|
||||||
config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
|
config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
|
||||||
@@ -339,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,
|
||||||
@@ -359,5 +609,13 @@ mount_t Mount = {
|
|||||||
.longCmd = longcmd,
|
.longCmd = longcmd,
|
||||||
.getHWconfig = get_hwconf,
|
.getHWconfig = get_hwconf,
|
||||||
.saveHWconfig = write_hwconf,
|
.saveHWconfig = write_hwconf,
|
||||||
.currentT = dtime,
|
.currentT = curtime,
|
||||||
|
.timeFromStart = timefromstart,
|
||||||
|
.timeDiff = timediff,
|
||||||
|
.timeDiff0 = timediff0,
|
||||||
|
.correctTo = correct2,
|
||||||
|
.getMaxSpeed = maxspeed,
|
||||||
|
.getMinSpeed = minspeed,
|
||||||
|
.getAcceleration = acceleration,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -16,20 +16,37 @@
|
|||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Almost all here used for debug purposes
|
||||||
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "movingmodel.h"
|
||||||
#include "sidservo.h"
|
#include "sidservo.h"
|
||||||
|
|
||||||
extern conf_t Conf;
|
extern conf_t Conf;
|
||||||
|
extern limits_t Xlimits, Ylimits;
|
||||||
|
int curtime(struct timespec *t);
|
||||||
|
double timediff(const struct timespec *time1, const struct timespec *time0);
|
||||||
|
double timediff0(const struct timespec *time1);
|
||||||
|
double timefromstart();
|
||||||
|
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst);
|
||||||
|
typedef struct{
|
||||||
|
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
|
||||||
|
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
|
||||||
|
size_t idx; // index of current data in array
|
||||||
|
size_t arraysz; // size of arrays
|
||||||
|
} less_square_t;
|
||||||
|
|
||||||
|
less_square_t *LS_init(size_t Ndata);
|
||||||
|
void LS_delete(less_square_t **ls);
|
||||||
|
double LS_calc_slope(less_square_t *l, double x, double t);
|
||||||
|
|
||||||
// unused arguments of functions
|
// 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))
|
||||||
|
|
||||||
@@ -53,7 +70,7 @@ extern conf_t Conf;
|
|||||||
#define COLOR_OLD "\033[0;0;0m"
|
#define COLOR_OLD "\033[0;0;0m"
|
||||||
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
||||||
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
||||||
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
|
#define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
|
||||||
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
|
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
|
||||||
fprintf(stderr, __VA_ARGS__); \
|
fprintf(stderr, __VA_ARGS__); \
|
||||||
fprintf(stderr, "\n");} while(0)
|
fprintf(stderr, "\n");} while(0)
|
||||||
@@ -1,7 +0,0 @@
|
|||||||
# configuration file for SSII driven equatorial mount
|
|
||||||
verbose = 5
|
|
||||||
mountpath = /dev/ttyS1
|
|
||||||
mountspeed = 19200
|
|
||||||
encoderpath = /dev/ttyUSB0
|
|
||||||
encoderspeed = 153000
|
|
||||||
|
|
||||||
73
LibSidServo/movingmodel.c
Normal file
73
LibSidServo/movingmodel.c
Normal file
@@ -0,0 +1,73 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include "movingmodel.h"
|
||||||
|
#include "ramp.h"
|
||||||
|
|
||||||
|
extern movemodel_t trapez;
|
||||||
|
|
||||||
|
static void chkminmax(double *min, double *max){
|
||||||
|
if(*min <= *max) return;
|
||||||
|
double t = *min;
|
||||||
|
*min = *max;
|
||||||
|
*max = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
movemodel_t *model_init(limits_t *l){
|
||||||
|
if(!l) return FALSE;
|
||||||
|
movemodel_t *m = calloc(1, sizeof(movemodel_t));
|
||||||
|
// we can't use memcpy or assign as Times/Params would be common for all
|
||||||
|
*m = trapez;
|
||||||
|
m->Times = calloc(STAGE_AMOUNT, sizeof(double));
|
||||||
|
m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
|
||||||
|
moveparam_t *max = &l->max, *min = &l->min;
|
||||||
|
if(min->speed < 0.) min->speed = -min->speed;
|
||||||
|
if(max->speed < 0.) max->speed = -max->speed;
|
||||||
|
if(min->accel < 0.) min->accel = -min->accel;
|
||||||
|
if(max->accel < 0.) max->accel = -max->accel;
|
||||||
|
chkminmax(&min->coord, &max->coord);
|
||||||
|
chkminmax(&min->speed, &max->speed);
|
||||||
|
chkminmax(&min->accel, &max->accel);
|
||||||
|
m->Min = l->min;
|
||||||
|
m->Max = l->max;
|
||||||
|
m->movingstage = STAGE_STOPPED;
|
||||||
|
m->state = ST_STOP;
|
||||||
|
pthread_mutex_init(&m->mutex, NULL);
|
||||||
|
DBG("model inited");
|
||||||
|
return m;
|
||||||
|
}
|
||||||
|
|
||||||
|
int model_move2(movemodel_t *model, moveparam_t *target, double t){
|
||||||
|
if(!target || !model) return FALSE;
|
||||||
|
DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
|
||||||
|
// only positive velocity
|
||||||
|
if(target->speed < 0.) target->speed = -target->speed;
|
||||||
|
if(fabs(target->speed) < model->Min.speed){
|
||||||
|
DBG("STOP");
|
||||||
|
model->stop(model, t);
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
// don't mind about acceleration - user cannot set it now
|
||||||
|
return model->calculate(model, target, t);
|
||||||
|
}
|
||||||
76
LibSidServo/movingmodel.h
Normal file
76
LibSidServo/movingmodel.h
Normal file
@@ -0,0 +1,76 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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 <pthread.h>
|
||||||
|
|
||||||
|
#include "sidservo.h"
|
||||||
|
|
||||||
|
// tolerance, time ticks
|
||||||
|
#define COORD_TOLERANCE_DEFAULT (1e-8)
|
||||||
|
#define COORD_TOLERANCE_MIN (1e-12)
|
||||||
|
#define COORD_TOLERANCE_MAX (10.)
|
||||||
|
#define TIME_TICK_DEFAULT (0.0001)
|
||||||
|
#define TIME_TICK_MIN (1e-9)
|
||||||
|
#define TIME_TICK_MAX (10.)
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
ST_STOP, // stopped
|
||||||
|
ST_MOVE, // moving
|
||||||
|
ST_AMOUNT
|
||||||
|
} movestate_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
double coord;
|
||||||
|
double speed;
|
||||||
|
double accel;
|
||||||
|
} moveparam_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
moveparam_t min;
|
||||||
|
moveparam_t max;
|
||||||
|
//double acceleration;
|
||||||
|
} limits_t;
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed
|
||||||
|
STAGE_MAXSPEED, // go with target speed
|
||||||
|
STAGE_DECEL, // go from target speed to zero
|
||||||
|
STAGE_STOPPED, // stop
|
||||||
|
STAGE_AMOUNT
|
||||||
|
} movingstage_t;
|
||||||
|
|
||||||
|
typedef struct movemodel{
|
||||||
|
moveparam_t Min;
|
||||||
|
moveparam_t Max;
|
||||||
|
movingstage_t movingstage;
|
||||||
|
movestate_t state;
|
||||||
|
double *Times;
|
||||||
|
moveparam_t *Params;
|
||||||
|
moveparam_t curparams; // init values of limits, jerk
|
||||||
|
int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||||
|
movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t
|
||||||
|
movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state
|
||||||
|
void (*stop)(struct movemodel *m, double t); // stop by ramp
|
||||||
|
void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration
|
||||||
|
double (*stoppedtime)(struct movemodel *m); // time when moving will ends
|
||||||
|
pthread_mutex_t mutex;
|
||||||
|
} movemodel_t;
|
||||||
|
|
||||||
|
movemodel_t *model_init(limits_t *l);
|
||||||
|
int model_move2(movemodel_t *model, moveparam_t *target, double t);
|
||||||
197
LibSidServo/polltest/main.c
Normal file
197
LibSidServo/polltest/main.c
Normal 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;
|
||||||
|
}
|
||||||
259
LibSidServo/ramp.c
Normal file
259
LibSidServo/ramp.c
Normal file
@@ -0,0 +1,259 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
// simplest trapezioidal ramp
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <strings.h>
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include "ramp.h"
|
||||||
|
|
||||||
|
#ifdef EBUG
|
||||||
|
#undef DBG
|
||||||
|
#define DBG(...)
|
||||||
|
#undef FNAME
|
||||||
|
#define FNAME()
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||||
|
|
||||||
|
static void emstop(movemodel_t *m, double _U_ t){
|
||||||
|
FNAME();
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
m->curparams.accel = 0.;
|
||||||
|
m->curparams.speed = 0.;
|
||||||
|
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
|
||||||
|
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
|
||||||
|
m->state = ST_STOP;
|
||||||
|
m->movingstage = STAGE_STOPPED;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void stop(movemodel_t *m, double t){
|
||||||
|
FNAME();
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
|
||||||
|
m->movingstage = STAGE_DECEL;
|
||||||
|
m->state = ST_MOVE;
|
||||||
|
m->Times[STAGE_DECEL] = t;
|
||||||
|
m->Params[STAGE_DECEL].speed = m->curparams.speed;
|
||||||
|
if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
|
||||||
|
else m->Params[STAGE_DECEL].accel = m->Max.accel;
|
||||||
|
m->Params[STAGE_DECEL].coord = m->curparams.coord;
|
||||||
|
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||||
|
m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
|
||||||
|
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||||
|
double dt = m->Times[STAGE_STOPPED] - t;
|
||||||
|
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
|
||||||
|
m->Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||||
|
ret:
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
}
|
||||||
|
|
||||||
|
// inner part of `calc`, could be called recoursively for hard case
|
||||||
|
static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
|
||||||
|
// signs
|
||||||
|
double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage
|
||||||
|
// times
|
||||||
|
double dt01 = 0., dt12 = 0., dt23 = 0.;
|
||||||
|
// absolute speed at stage 23 (or in that point); absolute max acceleration
|
||||||
|
double abs_vset = x->speed, abs_a = m->Max.accel;
|
||||||
|
// absolute target movement
|
||||||
|
double abs_Dx = fabs(x->coord - m->curparams.coord);
|
||||||
|
if(m->state == ST_STOP && abs_Dx < coord_tolerance){
|
||||||
|
DBG("Movement too small -> stay at place");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// signs of Dx and current speed
|
||||||
|
double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.;
|
||||||
|
double v0 = m->curparams.speed;
|
||||||
|
double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0);
|
||||||
|
if(v0 == 0.) sign_v0 = 0.;
|
||||||
|
// preliminary calculations (vset and dependent values could be changed)
|
||||||
|
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
|
||||||
|
double abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
|
||||||
|
dt23 = abs_vset / abs_a;
|
||||||
|
double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a;
|
||||||
|
if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction
|
||||||
|
if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop
|
||||||
|
//DBG("Simplest case: stop");
|
||||||
|
dt01 = dt12 = 0.;
|
||||||
|
sign_a23 = -sign_v0;
|
||||||
|
dt23 = abs_v0 / abs_a;
|
||||||
|
}else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case
|
||||||
|
//DBG("Move with smaller speed");
|
||||||
|
sign_a01 = sign_a23 = -sign_v0;
|
||||||
|
sign_vset = sign_v0;
|
||||||
|
double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.;
|
||||||
|
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
|
||||||
|
dt12 = abs_dx12 / abs_vset;
|
||||||
|
}else{// move with larget speed
|
||||||
|
//DBG("Move with larger speed");
|
||||||
|
double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
|
||||||
|
if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other
|
||||||
|
abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.);
|
||||||
|
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
|
||||||
|
abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
|
||||||
|
dt23 = abs_vset / abs_a;
|
||||||
|
abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
|
||||||
|
DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset);
|
||||||
|
}
|
||||||
|
sign_a01 = sign_Dx; // sign_v0 could be ZERO!!!
|
||||||
|
sign_a23 = -sign_Dx;
|
||||||
|
sign_vset = sign_Dx;
|
||||||
|
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
|
||||||
|
dt12 = abs_dx12 / abs_vset;
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
// if we are here, we have the worst case: change speed direction
|
||||||
|
// DBG("Hardest case: change speed direction");
|
||||||
|
// now we should calculate coordinate at which model stops and biuld new trapezium from that point
|
||||||
|
double x0 = m->curparams.coord, v0 = m->curparams.speed;
|
||||||
|
double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
|
||||||
|
m->state = ST_STOP;
|
||||||
|
m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.;
|
||||||
|
unlockedcalc(m, x, tstop); // calculate new ramp
|
||||||
|
// and change started conditions
|
||||||
|
m->curparams.coord = x0; m->curparams.speed = v0;
|
||||||
|
m->Times[STAGE_ACCEL] = t;
|
||||||
|
m->Params[STAGE_ACCEL].coord = x0;
|
||||||
|
m->Params[STAGE_ACCEL].speed = v0;
|
||||||
|
// DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
m->state = ST_MOVE;
|
||||||
|
m->movingstage = STAGE_ACCEL;
|
||||||
|
// some knot parameters
|
||||||
|
double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a;
|
||||||
|
double v1, v2, x0, x1, x2;
|
||||||
|
v2 = v1 = sign_vset * abs_vset;
|
||||||
|
x0 = m->curparams.coord;
|
||||||
|
x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.;
|
||||||
|
x2 = x1 + v1 * dt12;
|
||||||
|
// fill knot parameters
|
||||||
|
moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed
|
||||||
|
p->accel = a01;
|
||||||
|
p->speed = m->curparams.speed;
|
||||||
|
p->coord = x0;
|
||||||
|
m->Times[STAGE_ACCEL] = t;
|
||||||
|
p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed
|
||||||
|
p->accel = 0.;
|
||||||
|
p->speed = v1;
|
||||||
|
p->coord = x1;
|
||||||
|
m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01;
|
||||||
|
p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed
|
||||||
|
p->accel = a23;
|
||||||
|
p->speed = v2;
|
||||||
|
p->coord = x2;
|
||||||
|
m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
|
||||||
|
p = &m->Params[STAGE_STOPPED]; // 3 - stop at target
|
||||||
|
p->accel = p->speed = 0.;
|
||||||
|
p->coord = x->coord;
|
||||||
|
m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief calc - moving calculation
|
||||||
|
* @param x - using max speed (>0!!!) and coordinate
|
||||||
|
* @param t - current time value
|
||||||
|
* @return FALSE if can't move with given parameters
|
||||||
|
*/
|
||||||
|
static int calc(movemodel_t *m, moveparam_t *x, double t) {
|
||||||
|
//DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
|
||||||
|
if (!x || !m) return FALSE;
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
int ret = FALSE;
|
||||||
|
// Validate input parameters
|
||||||
|
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
|
||||||
|
DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
|
||||||
|
goto ret;
|
||||||
|
}
|
||||||
|
if(x->speed < m->Min.speed || x->speed > m->Max.speed){
|
||||||
|
DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
|
||||||
|
goto ret;
|
||||||
|
}
|
||||||
|
ret = TRUE; // now there's no chanses to make error
|
||||||
|
unlockedcalc(m, x, t);
|
||||||
|
// Debug output
|
||||||
|
/*for(int i = 0; i < STAGE_AMOUNT; i++){
|
||||||
|
DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
|
||||||
|
i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
|
||||||
|
}*/
|
||||||
|
ret:
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
if(m->state == ST_STOP) goto ret;
|
||||||
|
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||||
|
if(m->Times[s] <= t){ // check time for current stage
|
||||||
|
m->movingstage = s;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(m->movingstage == STAGE_STOPPED){
|
||||||
|
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
/* DBG("REACHED STOPping stage @ t=%g", t);
|
||||||
|
for(int s = STAGE_STOPPED; s >= 0; --s){
|
||||||
|
DBG("T[%d]=%g, ", s, m->Times[s]);
|
||||||
|
}*/
|
||||||
|
emstop(m, t);
|
||||||
|
goto ret;
|
||||||
|
}
|
||||||
|
// calculate current parameters
|
||||||
|
double dt = t - m->Times[m->movingstage];
|
||||||
|
double a = m->Params[m->movingstage].accel;
|
||||||
|
double v0 = m->Params[m->movingstage].speed;
|
||||||
|
double x0 = m->Params[m->movingstage].coord;
|
||||||
|
m->curparams.accel = a;
|
||||||
|
m->curparams.speed = v0 + a * dt;
|
||||||
|
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||||
|
ret:
|
||||||
|
if(next) *next = m->curparams;
|
||||||
|
movestate_t st = m->state;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return st;
|
||||||
|
}
|
||||||
|
|
||||||
|
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
if(cur) *cur = m->curparams;
|
||||||
|
movestate_t st = m->state;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return st;
|
||||||
|
}
|
||||||
|
|
||||||
|
static double gettstop(movemodel_t *m){
|
||||||
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
double r = m->Times[STAGE_STOPPED];
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return r;
|
||||||
|
}
|
||||||
|
|
||||||
|
movemodel_t trapez = {
|
||||||
|
.stop = stop,
|
||||||
|
.emergency_stop = emstop,
|
||||||
|
.get_state = getst,
|
||||||
|
.calculate = calc,
|
||||||
|
.proc_move = proc,
|
||||||
|
.stoppedtime = gettstop,
|
||||||
|
};
|
||||||
23
LibSidServo/ramp.h
Normal file
23
LibSidServo/ramp.h
Normal file
@@ -0,0 +1,23 @@
|
|||||||
|
/*
|
||||||
|
* This file is part of the moving_model 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 "movingmodel.h"
|
||||||
|
|
||||||
|
extern movemodel_t trapez;
|
||||||
@@ -20,25 +20,28 @@
|
|||||||
#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 <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
#include <sys/stat.h>
|
#include <sys/stat.h>
|
||||||
#include <time.h>
|
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "dbg.h"
|
#include "main.h"
|
||||||
|
#include "movingmodel.h"
|
||||||
#include "serial.h"
|
#include "serial.h"
|
||||||
|
#include "ssii.h"
|
||||||
|
|
||||||
// serial devices FD
|
// serial devices FD
|
||||||
static int encfd[2] = {-1, -1}, mntfd = -1;
|
static int encfd[2] = {-1, -1}, mntfd = -1;
|
||||||
// main mount data
|
// main mount data
|
||||||
static mountdata_t mountdata = {0};
|
static mountdata_t mountdata = {0};
|
||||||
// last encoders time and last encoders data - for speed measurement
|
// last encoders time and last encoders data - for speed measurement
|
||||||
static coordval_t lastXenc = {0}, lastYenc = {0};
|
//static coordval_t lastXenc = {0}, lastYenc = {0};
|
||||||
|
|
||||||
// mutexes for RW operations with mount device and data
|
// mutexes for RW operations with mount device and data
|
||||||
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
||||||
@@ -46,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 = {0}, mntRtmout = {0};
|
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;
|
||||||
@@ -55,57 +58,32 @@ typedef struct __attribute__((packed)){
|
|||||||
uint8_t CRC[4];
|
uint8_t CRC[4];
|
||||||
} enc_t;
|
} enc_t;
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief dtime - monotonic time from first run
|
|
||||||
* @return
|
|
||||||
*/
|
|
||||||
double dtime(){
|
|
||||||
struct timespec start_time = {0}, cur_time;
|
|
||||||
if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
|
|
||||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
|
||||||
}
|
|
||||||
clock_gettime(CLOCK_MONOTONIC, &cur_time);
|
|
||||||
return ((double)(cur_time.tv_sec - start_time.tv_sec) +
|
|
||||||
(cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
|
|
||||||
}
|
|
||||||
#if 0
|
|
||||||
double dtime(){
|
|
||||||
double t;
|
|
||||||
struct timeval tv;
|
|
||||||
gettimeofday(&tv, NULL);
|
|
||||||
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
|
||||||
return t;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#if 0
|
|
||||||
double tv2d(struct timeval *tv){
|
|
||||||
if(!tv) return 0.;
|
|
||||||
double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6;
|
|
||||||
return t;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#if 0
|
|
||||||
// init start time
|
|
||||||
static void gttime(){
|
|
||||||
struct timeval tv;
|
|
||||||
gettimeofday(&tv, NULL);
|
|
||||||
tv_sec_got = tv.tv_sec;
|
|
||||||
tv_usec_got = tv.tv_usec;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// calculate current X/Y speeds
|
// calculate current X/Y speeds
|
||||||
static void getXspeed(double t){
|
void getXspeed(){
|
||||||
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
|
static less_square_t *ls = NULL;
|
||||||
mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
|
if(!ls){
|
||||||
lastXenc.val = mountdata.encXposition.val;
|
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||||
lastXenc.t = t;
|
if(!ls) return;
|
||||||
|
}
|
||||||
|
double dt = timediff0(&mountdata.encXposition.t);
|
||||||
|
double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
|
||||||
|
if(fabs(speed) < 1.5 * Xlimits.max.speed){
|
||||||
|
mountdata.encXspeed.val = speed;
|
||||||
|
mountdata.encXspeed.t = mountdata.encXposition.t;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
static void getYspeed(double t){
|
void getYspeed(){
|
||||||
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
|
static less_square_t *ls = NULL;
|
||||||
mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
|
if(!ls){
|
||||||
lastYenc.val = mountdata.encYposition.val;
|
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||||
lastYenc.t = t;
|
if(!ls) return;
|
||||||
|
}
|
||||||
|
double dt = timediff0(&mountdata.encYposition.t);
|
||||||
|
double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
|
||||||
|
if(fabs(speed) < 1.5 * Ylimits.max.speed){
|
||||||
|
mountdata.encYspeed.val = speed;
|
||||||
|
mountdata.encYspeed.t = mountdata.encYposition.t;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -113,7 +91,8 @@ static void getYspeed(double t){
|
|||||||
* @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
|
||||||
@@ -148,17 +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(t);
|
getXspeed(); getYspeed();
|
||||||
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
|
|
||||||
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
|
||||||
@@ -166,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 = dtime();
|
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 && dtime() - 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;
|
||||||
@@ -204,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;
|
||||||
@@ -254,19 +255,43 @@ static int getmntbyte(){
|
|||||||
if(FD_ISSET(mntfd, &rfds)){
|
if(FD_ISSET(mntfd, &rfds)){
|
||||||
ssize_t l = read(mntfd, &byte, 1);
|
ssize_t l = read(mntfd, &byte, 1);
|
||||||
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
||||||
if(l != 1) return -2; // disconnected ??
|
if(l != 1){
|
||||||
|
DBG("Mount disconnected?");
|
||||||
|
return -2; // disconnected ??
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
} else return -1;
|
} else return -1;
|
||||||
}while(1);
|
}while(1);
|
||||||
return (int)byte;
|
return (int)byte;
|
||||||
}
|
}
|
||||||
|
// clear data from input buffer
|
||||||
|
static void clrmntbuf(){
|
||||||
|
if(mntfd < 0) return;
|
||||||
|
uint8_t byte;
|
||||||
|
fd_set rfds;
|
||||||
|
do{
|
||||||
|
FD_ZERO(&rfds);
|
||||||
|
FD_SET(mntfd, &rfds);
|
||||||
|
struct timeval tv = {.tv_sec=0, .tv_usec=10};
|
||||||
|
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
|
||||||
|
if(retval < 0){
|
||||||
|
if(errno == EINTR) continue;
|
||||||
|
DBG("Error in select()");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if(FD_ISSET(mntfd, &rfds)){
|
||||||
|
ssize_t l = read(mntfd, &byte, 1);
|
||||||
|
if(l != 1) break;
|
||||||
|
} else break;
|
||||||
|
}while(1);
|
||||||
|
}
|
||||||
|
|
||||||
// main encoder thread (for separate encoder): read next data and make parsing
|
// main encoder thread (for separate encoder): read next data and make parsing
|
||||||
static void *encoderthread1(void _U_ *u){
|
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;
|
||||||
@@ -277,15 +302,16 @@ 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 = dtime();
|
|
||||||
}
|
}
|
||||||
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)){
|
||||||
|
parse_encbuf(databuf, &tcur);
|
||||||
wridx = 0;
|
wridx = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
if(encfd[0] > -1){
|
if(encfd[0] > -1){
|
||||||
close(encfd[0]);
|
close(encfd[0]);
|
||||||
encfd[0] = -1;
|
encfd[0] = -1;
|
||||||
@@ -293,47 +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 = dtime();
|
do{ // main cycle
|
||||||
const char *req = "next\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(5 != write(encfd[0], req, 5)) { ++errctr; continue; }
|
|
||||||
else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; }
|
|
||||||
}
|
}
|
||||||
double v, t;
|
int got = 0;
|
||||||
if(getencval(encfd[0], &v, &t)){
|
for(int i = 0; i < 2; ++i){
|
||||||
mountdata.encXposition.val = X_ENC2RAD(v);
|
if(pfds[i].revents && POLLIN){
|
||||||
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
|
if(!readstrings(&strbuf[i], encfd[i])){
|
||||||
mountdata.encXposition.t = t;
|
++errctr;
|
||||||
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
|
break;
|
||||||
if(getencval(encfd[1], &v, &t)){
|
|
||||||
mountdata.encYposition.val = Y_ENC2RAD(v);
|
|
||||||
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
|
|
||||||
mountdata.encYposition.t = t;
|
|
||||||
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
|
|
||||||
errctr = 0;
|
|
||||||
need2ask = 0;
|
|
||||||
} else {
|
|
||||||
if(need2ask) ++errctr;
|
|
||||||
else need2ask = 1;
|
|
||||||
continue;
|
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
if(need2ask) ++errctr;
|
|
||||||
else need2ask = 1;
|
|
||||||
continue;
|
|
||||||
}
|
}
|
||||||
while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
|
double curt = timefromstart();
|
||||||
//DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
|
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
|
||||||
t0 = dtime();
|
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
|
||||||
|
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
|
||||||
|
pthread_mutex_lock(&datamutex);
|
||||||
|
if(i == 0){
|
||||||
|
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
|
||||||
|
curtime(&mountdata.encXposition.t);
|
||||||
|
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
|
||||||
|
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
|
||||||
|
X_ENC_ZERO, X_ENC_STEPSPERREV);*/
|
||||||
|
getXspeed();
|
||||||
|
}else{
|
||||||
|
mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
|
||||||
|
curtime(&mountdata.encYposition.t);
|
||||||
|
getYspeed();
|
||||||
}
|
}
|
||||||
DBG("ERRCTR=%d", errctr);
|
pthread_mutex_unlock(&datamutex);
|
||||||
|
}
|
||||||
|
if(!asknext(encfd[i])){
|
||||||
|
++errctr;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
|
||||||
|
++got;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(got == 2) errctr = 0;
|
||||||
|
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
|
||||||
|
DBG("\n\nEXIT: 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]);
|
||||||
@@ -359,25 +476,78 @@ 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));
|
||||||
|
double t0 = timefromstart(), tstart = t0, tcur = t0;
|
||||||
|
double oldmt = -100.; // old `millis measurement` time
|
||||||
|
static uint32_t oldmillis = 0;
|
||||||
|
if(Conf.RunModel){
|
||||||
|
double Xprev = NAN, Yprev = NAN; // previous coordinates
|
||||||
|
int xcnt = 0, ycnt = 0;
|
||||||
|
while(1){
|
||||||
|
coordpair_t c;
|
||||||
|
movestate_t xst, yst;
|
||||||
|
// now change data
|
||||||
|
getModData(&c, &xst, &yst);
|
||||||
|
struct timespec tnow;
|
||||||
|
if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
|
||||||
|
pthread_mutex_lock(&datamutex);
|
||||||
|
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
|
||||||
|
mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
|
||||||
|
mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
|
||||||
|
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
|
||||||
|
if(tcur - oldmt > Conf.MountReqInterval){
|
||||||
|
oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
|
||||||
|
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
|
||||||
|
if(xst == ST_MOVE)
|
||||||
|
mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
|
||||||
|
//else
|
||||||
|
// mountdata.motXposition.val = c.X;
|
||||||
|
if(yst == ST_MOVE)
|
||||||
|
mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
|
||||||
|
//else
|
||||||
|
// mountdata.motYposition.val = c.Y;
|
||||||
|
oldmt = tcur;
|
||||||
|
}else mountdata.millis = oldmillis;
|
||||||
|
chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
|
||||||
|
chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
|
||||||
|
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)};
|
||||||
// cmd to send
|
// cmd to send
|
||||||
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
|
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
|
||||||
if(!cmd_getstat) goto failed;
|
if(!cmd_getstat) goto failed;
|
||||||
double t0 = dtime();
|
|
||||||
/*
|
|
||||||
#ifdef EBUG
|
|
||||||
double t00 = t0;
|
|
||||||
#endif
|
|
||||||
*/
|
|
||||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||||
// read data to status
|
// read data to status
|
||||||
double tgot = dtime();
|
struct timespec tcur;
|
||||||
|
if(!curtime(&tcur)) continue;
|
||||||
|
// 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
|
||||||
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
|
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
|
||||||
@@ -393,13 +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, tgot);
|
SSconvstat(status, &mountdata, &tcur);
|
||||||
pthread_mutex_unlock(&datamutex);
|
pthread_mutex_unlock(&datamutex);
|
||||||
// allow writing & getters
|
// allow writing & getters
|
||||||
//DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
|
do{
|
||||||
if(dtime() - t0 >= Conf.MountReqInterval) usleep(50);
|
usleep(500);
|
||||||
while(dtime() - t0 < Conf.MountReqInterval);
|
}while(timefromstart() - t0 < Conf.MountReqInterval);
|
||||||
t0 = dtime();
|
t0 = timefromstart();
|
||||||
}
|
}
|
||||||
data_free(&cmd_getstat);
|
data_free(&cmd_getstat);
|
||||||
failed:
|
failed:
|
||||||
@@ -415,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
|
||||||
@@ -425,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
|
||||||
@@ -435,6 +616,7 @@ static int ttyopen(const char *path, speed_t speed){
|
|||||||
|
|
||||||
// return FALSE if failed
|
// return FALSE if failed
|
||||||
int openEncoder(){
|
int openEncoder(){
|
||||||
|
if(Conf.RunModel) return TRUE;
|
||||||
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
|
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
|
||||||
if(Conf.SepEncoder == 1){ // only one device
|
if(Conf.SepEncoder == 1){ // only one device
|
||||||
DBG("One device");
|
DBG("One device");
|
||||||
@@ -442,7 +624,7 @@ int openEncoder(){
|
|||||||
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
|
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
|
||||||
if(encfd[0] < 0) return FALSE;
|
if(encfd[0] < 0) return FALSE;
|
||||||
encRtmout.tv_sec = 0;
|
encRtmout.tv_sec = 0;
|
||||||
encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
|
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
|
||||||
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
|
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
|
||||||
close(encfd[0]);
|
close(encfd[0]);
|
||||||
encfd[0] = -1;
|
encfd[0] = -1;
|
||||||
@@ -457,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]);
|
||||||
@@ -472,6 +654,7 @@ int openEncoder(){
|
|||||||
|
|
||||||
// return FALSE if failed
|
// return FALSE if failed
|
||||||
int openMount(){
|
int openMount(){
|
||||||
|
if(Conf.RunModel) goto create_thread;
|
||||||
if(mntfd > -1) close(mntfd);
|
if(mntfd > -1) close(mntfd);
|
||||||
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
|
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
|
||||||
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
|
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
|
||||||
@@ -487,11 +670,14 @@ int openMount(){
|
|||||||
DBG("got %zd", l);
|
DBG("got %zd", l);
|
||||||
}while(1);*/
|
}while(1);*/
|
||||||
mntRtmout.tv_sec = 0;
|
mntRtmout.tv_sec = 0;
|
||||||
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
|
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
|
||||||
|
create_thread:
|
||||||
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
|
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
|
||||||
DBG("Can't create thread");
|
DBG("Can't create mount thread");
|
||||||
|
if(!Conf.RunModel){
|
||||||
close(mntfd);
|
close(mntfd);
|
||||||
mntfd = -1;
|
mntfd = -1;
|
||||||
|
}
|
||||||
return FALSE;
|
return FALSE;
|
||||||
}
|
}
|
||||||
DBG("Mount opened, thread started");
|
DBG("Mount opened, thread started");
|
||||||
@@ -500,24 +686,26 @@ int openMount(){
|
|||||||
|
|
||||||
// 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(mntfd > -1){
|
if(mntfd > -1){
|
||||||
DBG("Kill mount thread");
|
DBG("Cancel mount thread");
|
||||||
pthread_cancel(mntthread);
|
pthread_cancel(mntthread);
|
||||||
DBG("join mount thread");
|
DBG("join mount thread");
|
||||||
pthread_join(mntthread, NULL);
|
pthread_join(mntthread, NULL);
|
||||||
DBG("close fd");
|
DBG("close mount fd");
|
||||||
close(mntfd);
|
close(mntfd);
|
||||||
mntfd = -1;
|
mntfd = -1;
|
||||||
}
|
}
|
||||||
if(encfd[0] > -1){
|
if(encfd[0] > -1){
|
||||||
DBG("Kill encoder thread");
|
DBG("Cancel encoder thread");
|
||||||
pthread_cancel(encthread);
|
pthread_cancel(encthread);
|
||||||
DBG("join encoder thread");
|
DBG("join encoder thread");
|
||||||
pthread_join(encthread, NULL);
|
pthread_join(encthread, NULL);
|
||||||
DBG("close fd");
|
DBG("close encoder's fd");
|
||||||
close(encfd[0]);
|
close(encfd[0]);
|
||||||
encfd[0] = -1;
|
encfd[0] = -1;
|
||||||
if(Conf.SepEncoder == 2){
|
if(Conf.SepEncoder == 2 && encfd[1] > -1){
|
||||||
close(encfd[1]);
|
close(encfd[1]);
|
||||||
encfd[1] = -1;
|
encfd[1] = -1;
|
||||||
}
|
}
|
||||||
@@ -530,40 +718,44 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
|
void setStat(axis_status_t Xstate, axis_status_t Ystate){
|
||||||
|
DBG("set x/y state to %d/%d", Xstate, Ystate);
|
||||||
pthread_mutex_lock(&datamutex);
|
pthread_mutex_lock(&datamutex);
|
||||||
mountdata.Xstatus = Xstatus;
|
mountdata.Xstate = Xstate;
|
||||||
mountdata.Ystatus = Ystatus;
|
mountdata.Ystate = Ystate;
|
||||||
pthread_mutex_unlock(&datamutex);
|
pthread_mutex_unlock(&datamutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write-read without locking mutex (to be used inside other functions)
|
// write-read without locking mutex (to be used inside other functions)
|
||||||
static int wr(const data_t *out, data_t *in, int needeol){
|
static int wr(const data_t *out, data_t *in, int needeol){
|
||||||
if((!out && !in) || mntfd < 0) return FALSE;
|
if((!out && !in) || mntfd < 0){
|
||||||
|
DBG("Wrong arguments or no mount fd");
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
clrmntbuf();
|
||||||
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
|
||||||
}
|
}
|
||||||
uint8_t buf[256];
|
if(!in) return TRUE;
|
||||||
data_t dumb = {.buf = buf, .maxlen = 256};
|
|
||||||
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("Clear trashing input");
|
|
||||||
while(getmntbyte() > -1);
|
while(getmntbyte() > -1);
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
@@ -575,6 +767,7 @@ static int wr(const data_t *out, data_t *in, int needeol){
|
|||||||
* @return FALSE if failed
|
* @return FALSE if failed
|
||||||
*/
|
*/
|
||||||
int MountWriteRead(const data_t *out, data_t *in){
|
int MountWriteRead(const data_t *out, data_t *in){
|
||||||
|
if(Conf.RunModel) return -1;
|
||||||
pthread_mutex_lock(&mntmutex);
|
pthread_mutex_lock(&mntmutex);
|
||||||
int ret = wr(out, in, 1);
|
int ret = wr(out, in, 1);
|
||||||
pthread_mutex_unlock(&mntmutex);
|
pthread_mutex_unlock(&mntmutex);
|
||||||
@@ -582,6 +775,7 @@ int MountWriteRead(const data_t *out, data_t *in){
|
|||||||
}
|
}
|
||||||
// send binary data - without EOL
|
// send binary data - without EOL
|
||||||
int MountWriteReadRaw(const data_t *out, data_t *in){
|
int MountWriteReadRaw(const data_t *out, data_t *in){
|
||||||
|
if(Conf.RunModel) return -1;
|
||||||
pthread_mutex_lock(&mntmutex);
|
pthread_mutex_lock(&mntmutex);
|
||||||
int ret = wr(out, in, 0);
|
int ret = wr(out, in, 0);
|
||||||
pthread_mutex_unlock(&mntmutex);
|
pthread_mutex_unlock(&mntmutex);
|
||||||
@@ -605,32 +799,32 @@ static void loglcmd(SSlcmd *c){
|
|||||||
|
|
||||||
// send short/long binary command; return FALSE if failed
|
// send short/long binary command; return FALSE if failed
|
||||||
static int bincmd(uint8_t *cmd, int len){
|
static int bincmd(uint8_t *cmd, int len){
|
||||||
|
if(Conf.RunModel) return FALSE;
|
||||||
static data_t *dscmd = NULL, *dlcmd = NULL;
|
static data_t *dscmd = NULL, *dlcmd = NULL;
|
||||||
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
|
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
|
||||||
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
|
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
|
||||||
int ret = FALSE;
|
int ret = FALSE;
|
||||||
pthread_mutex_lock(&mntmutex);
|
pthread_mutex_lock(&mntmutex);
|
||||||
// dummy buffer to clear trash in input
|
// dummy buffer to clear trash in input
|
||||||
char ans[300];
|
//char ans[300];
|
||||||
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||||
if(len == sizeof(SSscmd)){
|
if(len == sizeof(SSscmd)){
|
||||||
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||||
DBG("Short command");
|
DBG("Short command");
|
||||||
#ifdef EBUG
|
#ifdef EBUG
|
||||||
logscmd((SSscmd*)cmd);
|
logscmd((SSscmd*)cmd);
|
||||||
#endif
|
#endif
|
||||||
if(!wr(dscmd, &a, 1)) goto rtn;
|
if(!wr(dscmd, NULL, 1)) goto rtn;
|
||||||
}else if(len == sizeof(SSlcmd)){
|
}else if(len == sizeof(SSlcmd)){
|
||||||
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||||
DBG("Long command");
|
DBG("Long command");
|
||||||
#ifdef EBUG
|
#ifdef EBUG
|
||||||
loglcmd((SSlcmd*)cmd);
|
loglcmd((SSlcmd*)cmd);
|
||||||
#endif
|
#endif
|
||||||
if(!wr(dlcmd, &a, 1)) goto rtn;
|
if(!wr(dlcmd, NULL, 1)) goto rtn;
|
||||||
}else{
|
}else{
|
||||||
goto rtn;
|
goto rtn;
|
||||||
}
|
}
|
||||||
DBG("Write %d bytes and wait for ans", len);
|
|
||||||
data_t d;
|
data_t d;
|
||||||
d.buf = cmd;
|
d.buf = cmd;
|
||||||
d.len = d.maxlen = len;
|
d.len = d.maxlen = len;
|
||||||
@@ -641,6 +835,7 @@ rtn:
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// short, long and config text-binary commands
|
||||||
// return TRUE if OK
|
// return TRUE if OK
|
||||||
int cmdS(SSscmd *cmd){
|
int cmdS(SSscmd *cmd){
|
||||||
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
|
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
|
||||||
@@ -650,6 +845,7 @@ int cmdL(SSlcmd *cmd){
|
|||||||
}
|
}
|
||||||
// rw == 1 to write, 0 to read
|
// rw == 1 to write, 0 to read
|
||||||
int cmdC(SSconfig *conf, int rw){
|
int cmdC(SSconfig *conf, int rw){
|
||||||
|
if(Conf.RunModel) return FALSE;
|
||||||
static data_t *wcmd = NULL, *rcmd = NULL;
|
static data_t *wcmd = NULL, *rcmd = NULL;
|
||||||
int ret = FALSE;
|
int ret = FALSE;
|
||||||
// dummy buffer to clear trash in input
|
// dummy buffer to clear trash in input
|
||||||
@@ -663,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 = 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);
|
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:
|
||||||
|
|||||||
@@ -28,16 +28,17 @@
|
|||||||
// max error counter (when read() returns -1)
|
// max error counter (when read() returns -1)
|
||||||
#define MAX_ERR_CTR (100)
|
#define MAX_ERR_CTR (100)
|
||||||
|
|
||||||
double dtime();
|
|
||||||
data_t *cmd2dat(const char *cmd);
|
data_t *cmd2dat(const char *cmd);
|
||||||
void data_free(data_t **x);
|
void data_free(data_t **x);
|
||||||
int openEncoder();
|
int openEncoder();
|
||||||
int openMount();
|
int openMount();
|
||||||
void closeSerial();
|
void closeSerial();
|
||||||
mcc_errcodes_t getMD(mountdata_t *d);
|
mcc_errcodes_t getMD(mountdata_t *d);
|
||||||
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
|
void setStat(axis_status_t Xstate, axis_status_t Ystate);
|
||||||
int MountWriteRead(const data_t *out, data_t *in);
|
int MountWriteRead(const data_t *out, data_t *in);
|
||||||
int MountWriteReadRaw(const data_t *out, data_t *in);
|
int MountWriteReadRaw(const data_t *out, data_t *in);
|
||||||
int cmdS(SSscmd *cmd);
|
int cmdS(SSscmd *cmd);
|
||||||
int cmdL(SSlcmd *cmd);
|
int cmdL(SSlcmd *cmd);
|
||||||
int cmdC(SSconfig *conf, int rw);
|
int cmdC(SSconfig *conf, int rw);
|
||||||
|
void getXspeed();
|
||||||
|
void getYspeed();
|
||||||
|
|||||||
@@ -16,6 +16,11 @@
|
|||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file contains all need for external usage
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
@@ -27,15 +32,14 @@ extern "C"
|
|||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <sys/time.h>
|
#include <sys/time.h>
|
||||||
|
|
||||||
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
// minimal serial speed of mount device
|
||||||
#define MCC_MAX_X_SPEED (0.174533)
|
#define MOUNT_BAUDRATE_MIN (1200)
|
||||||
#define MCC_MAX_Y_SPEED (0.139626)
|
|
||||||
|
|
||||||
// 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.)
|
||||||
|
|
||||||
|
|
||||||
// error codes
|
// error codes
|
||||||
typedef enum{
|
typedef enum{
|
||||||
MCC_E_OK = 0, // all OK
|
MCC_E_OK = 0, // all OK
|
||||||
@@ -44,8 +48,13 @@ 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{
|
||||||
|
double P, I, D;
|
||||||
|
} PIDpar_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
char* MountDevPath; // path to mount device
|
char* MountDevPath; // path to mount device
|
||||||
int MountDevSpeed; // serial speed
|
int MountDevSpeed; // serial speed
|
||||||
@@ -54,9 +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
|
||||||
|
double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared)
|
||||||
|
double PIDRefreshDt; // normal PID refresh interval
|
||||||
|
double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps)
|
||||||
|
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
|
||||||
|
PIDpar_t XPIDV;
|
||||||
|
PIDpar_t YPIDC;
|
||||||
|
PIDpar_t YPIDV;
|
||||||
|
double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
|
||||||
|
double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
|
||||||
|
double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
|
||||||
|
int XEncZero; // encoders' zero position
|
||||||
|
int YEncZero;
|
||||||
} conf_t;
|
} conf_t;
|
||||||
|
|
||||||
// coordinates/speeds in degrees or d/s: X, Y
|
// coordinates/speeds in degrees or d/s: X, Y
|
||||||
@@ -67,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{
|
||||||
@@ -113,16 +136,16 @@ typedef struct{
|
|||||||
} extradata_t;
|
} extradata_t;
|
||||||
|
|
||||||
typedef enum{
|
typedef enum{
|
||||||
MNT_STOPPED,
|
AXIS_STOPPED,
|
||||||
MNT_SLEWING,
|
AXIS_SLEWING,
|
||||||
MNT_POINTING,
|
AXIS_POINTING,
|
||||||
MNT_GUIDING,
|
AXIS_GUIDING,
|
||||||
MNT_ERROR,
|
AXIS_ERROR,
|
||||||
} mnt_status_t;
|
} axis_status_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
mnt_status_t Xstatus;
|
axis_status_t Xstate;
|
||||||
mnt_status_t Ystatus;
|
axis_status_t Ystate;
|
||||||
coordval_t motXposition;
|
coordval_t motXposition;
|
||||||
coordval_t motYposition;
|
coordval_t motYposition;
|
||||||
coordval_t encXposition;
|
coordval_t encXposition;
|
||||||
@@ -157,7 +180,7 @@ typedef struct{
|
|||||||
double Yatime; // 28
|
double Yatime; // 28
|
||||||
} long_command_t; // long command
|
} long_command_t; // long command
|
||||||
|
|
||||||
// hardware axe configuration
|
// hardware axis configuration
|
||||||
typedef struct{
|
typedef struct{
|
||||||
double accel; // Default Acceleration, rad/s^2
|
double accel; // Default Acceleration, rad/s^2
|
||||||
double backlash; // Backlash (???)
|
double backlash; // Backlash (???)
|
||||||
@@ -168,13 +191,16 @@ 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 (???)
|
||||||
} __attribute__((packed)) axe_config_t;
|
// these params are taken from mount by text commands (don't save negative values - better save these marks in xybits
|
||||||
|
double motor_stepsperrev;// encoder's steps per revolution: motor and axis
|
||||||
|
double axis_stepsperrev; // negative sign of these values means reverse direction
|
||||||
|
} __attribute__((packed)) axis_config_t;
|
||||||
|
|
||||||
// hardware configuration
|
// hardware configuration
|
||||||
typedef struct{
|
typedef struct{
|
||||||
axe_config_t Xconf;
|
axis_config_t Xconf;
|
||||||
xbits_t xbits;
|
xbits_t xbits;
|
||||||
axe_config_t Yconf;
|
axis_config_t Yconf;
|
||||||
ybits_t ybits;
|
ybits_t ybits;
|
||||||
uint8_t address;
|
uint8_t address;
|
||||||
double eqrate; // Equatorial Rate (???)
|
double eqrate; // Equatorial Rate (???)
|
||||||
@@ -197,19 +223,19 @@ typedef struct{
|
|||||||
double backlspd; // Backlash speed (rad/s)
|
double backlspd; // Backlash speed (rad/s)
|
||||||
} hardware_configuration_t;
|
} hardware_configuration_t;
|
||||||
|
|
||||||
// flags for slew function
|
/* flags for slew function
|
||||||
typedef struct{
|
typedef struct{
|
||||||
uint32_t slewNguide : 1; // ==1 to guide after slewing
|
uint32_t slewNguide : 1; // ==1 to guide after slewing
|
||||||
} slewflags_t;
|
} slewflags_t;
|
||||||
|
*/
|
||||||
// mount class
|
// mount class
|
||||||
typedef struct{
|
typedef struct{
|
||||||
// TODO: on init/quit clear all XY-bits to default`
|
// TODO: on init/quit clear all XY-bits to default`
|
||||||
mcc_errcodes_t (*init)(conf_t *c); // init device
|
mcc_errcodes_t (*init)(conf_t *c); // init device
|
||||||
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)(coordval_pair_t *target);
|
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
|
||||||
@@ -219,12 +245,17 @@ 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;
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -18,12 +18,21 @@
|
|||||||
|
|
||||||
#include <ctype.h>
|
#include <ctype.h>
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
#include <stdio.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "dbg.h"
|
#include "main.h"
|
||||||
#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++){
|
||||||
@@ -35,16 +44,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
|
|||||||
return checksum;
|
return checksum;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
|
||||||
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
|
static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
|
||||||
if(*prev == INT32_MAX){
|
if(*prev == INT32_MAX){
|
||||||
*stat = MNT_STOPPED;
|
*stat = AXIS_STOPPED;
|
||||||
}else if(*stat != MNT_STOPPED){
|
DBG("START");
|
||||||
if(*prev == cur){
|
}else if(*stat != AXIS_STOPPED){
|
||||||
if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
|
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||||
|
*stat = AXIS_STOPPED;
|
||||||
|
DBG("AXIS stopped");
|
||||||
}
|
}
|
||||||
}else if(*prev != cur){
|
}else if(*prev != cur){
|
||||||
//*stat = MNT_SLEWING;
|
DBG("AXIS moving");
|
||||||
*nstopped = 0;
|
*nstopped = 0;
|
||||||
}
|
}
|
||||||
*prev = cur;
|
*prev = cur;
|
||||||
@@ -53,8 +64,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta
|
|||||||
static void ChkStopped(const SSstat *s, mountdata_t *m){
|
static void ChkStopped(const SSstat *s, mountdata_t *m){
|
||||||
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
|
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
|
||||||
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
|
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
|
||||||
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
|
chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
|
||||||
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
|
chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -63,28 +74,20 @@ 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;
|
||||||
/*
|
|
||||||
#ifdef EBUG
|
|
||||||
static double t0 = -1.;
|
|
||||||
if(t0 < 0.) t0 = dtime();
|
|
||||||
#endif
|
|
||||||
DBG("Convert, t=%g", dtime()-t0);
|
|
||||||
*/
|
|
||||||
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();
|
||||||
}
|
}
|
||||||
//m->lastmotposition.X = X_MOT2RAD(s->XLast);
|
|
||||||
//m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
|
|
||||||
//m->lastmotposition.msrtime = *tdat;
|
|
||||||
m->keypad = s->keypad;
|
m->keypad = s->keypad;
|
||||||
m->extradata.ExtraBits = s->ExtraBits;
|
m->extradata.ExtraBits = s->ExtraBits;
|
||||||
m->extradata.ain0 = s->ain0;
|
m->extradata.ain0 = s->ain0;
|
||||||
@@ -110,7 +113,7 @@ int SStextcmd(const char *cmd, data_t *answer){
|
|||||||
data_t d;
|
data_t d;
|
||||||
d.buf = (uint8_t*) cmd;
|
d.buf = (uint8_t*) cmd;
|
||||||
d.len = d.maxlen = strlen(cmd);
|
d.len = d.maxlen = strlen(cmd);
|
||||||
DBG("send %zd bytes: %s", d.len, d.buf);
|
//DBG("send %zd bytes: %s", d.len, d.buf);
|
||||||
return MountWriteRead(&d, answer);
|
return MountWriteRead(&d, answer);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -123,7 +126,7 @@ int SSrawcmd(const char *cmd, data_t *answer){
|
|||||||
data_t d;
|
data_t d;
|
||||||
d.buf = (uint8_t*) cmd;
|
d.buf = (uint8_t*) cmd;
|
||||||
d.len = d.maxlen = strlen(cmd);
|
d.len = d.maxlen = strlen(cmd);
|
||||||
DBG("send %zd bytes: %s", d.len, d.buf);
|
//DBG("send %zd bytes: %s", d.len, d.buf);
|
||||||
return MountWriteReadRaw(&d, answer);
|
return MountWriteReadRaw(&d, answer);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -180,22 +183,44 @@ int SSstop(int emerg){
|
|||||||
// update motors' positions due to encoders'
|
// update motors' positions due to encoders'
|
||||||
mcc_errcodes_t updateMotorPos(){
|
mcc_errcodes_t updateMotorPos(){
|
||||||
mountdata_t md = {0};
|
mountdata_t md = {0};
|
||||||
double t0 = dtime(), t = 0.;
|
if(Conf.RunModel) return MCC_E_OK;
|
||||||
|
double t0 = timefromstart(), t = 0.;
|
||||||
|
struct timespec curt;
|
||||||
DBG("start @ %g", t0);
|
DBG("start @ %g", t0);
|
||||||
do{
|
do{
|
||||||
t = dtime();
|
t = timefromstart();
|
||||||
if(MCC_E_OK == getMD(&md)){
|
if(!curtime(&curt)){
|
||||||
DBG("got");
|
usleep(10000);
|
||||||
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
|
continue;
|
||||||
DBG("FIX motors position to encoders");
|
|
||||||
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
|
|
||||||
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
|
|
||||||
DBG("All OK");
|
|
||||||
return MCC_E_OK;
|
|
||||||
}
|
}
|
||||||
}else{
|
//DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
|
||||||
DBG("on position");
|
if(MCC_E_OK == getMD(&md)){
|
||||||
return MCC_E_OK;
|
if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
|
||||||
|
DBG("Just started? t-t0 = %g!", t - t0);
|
||||||
|
usleep(10000);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
|
||||||
|
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
|
||||||
|
mcc_errcodes_t OK = MCC_E_OK;
|
||||||
|
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
|
||||||
|
DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
|
||||||
|
DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
|
||||||
|
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
|
||||||
|
DBG("Xpos sync failed!");
|
||||||
|
OK = MCC_E_FAILED;
|
||||||
|
}else DBG("Xpos sync OK, Dt=%g", t - t0);
|
||||||
|
}
|
||||||
|
if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
|
||||||
|
DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
|
||||||
|
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
|
||||||
|
DBG("Ypos sync failed!");
|
||||||
|
OK = MCC_E_FAILED;
|
||||||
|
}else DBG("Ypos sync OK, Dt=%g", t - t0);
|
||||||
|
}
|
||||||
|
if(MCC_E_OK == OK){
|
||||||
|
DBG("Encoders synced");
|
||||||
|
return OK;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
DBG("NO DATA; dt = %g", t - t0);
|
DBG("NO DATA; dt = %g", t - t0);
|
||||||
|
|||||||
@@ -16,6 +16,10 @@
|
|||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file contains stuff for sidereal-servo specific protocol
|
||||||
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
@@ -169,65 +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 (20)
|
#define MOTOR_STOPPED_CNT (19)
|
||||||
|
|
||||||
|
// replace macros with global variables inited when config read
|
||||||
|
extern int X_ENC_ZERO, Y_ENC_ZERO;
|
||||||
|
extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
|
||||||
|
|
||||||
|
// TODO: take it from settings?
|
||||||
// steps per revolution (SSI - x4 - for SSI)
|
// steps per revolution (SSI - x4 - for SSI)
|
||||||
|
// -> hwconf.Xconf.mot/enc_stepsperrev
|
||||||
|
//#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||||
// 13312000 / 4 = 3328000
|
// 13312000 / 4 = 3328000
|
||||||
#define X_MOT_STEPSPERREV_SSI (13312000.)
|
//#define X_MOT_STEPSPERREV (3328000.)
|
||||||
//#define X_MOT_STEPSPERREV (3325952.)
|
//#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
||||||
#define X_MOT_STEPSPERREV (3328000.)
|
|
||||||
// 17578668 / 4 = 4394667
|
// 17578668 / 4 = 4394667
|
||||||
#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
//#define Y_MOT_STEPSPERREV (4394667.)
|
||||||
//#define Y_MOT_STEPSPERREV (4394960.)
|
|
||||||
#define Y_MOT_STEPSPERREV (4394667.)
|
// encoder per revolution
|
||||||
|
//#define X_ENC_STEPSPERREV (67108864.)
|
||||||
|
//#define Y_ENC_STEPSPERREV (67108864.)
|
||||||
|
// encoder zero position
|
||||||
|
// -> conf.XEncZero/YEncZero
|
||||||
|
//#define X_ENC_ZERO (61245239)
|
||||||
|
//#define Y_ENC_ZERO (36999830)
|
||||||
|
// encoder reversed (no: +1) -> sign of ...stepsperrev
|
||||||
|
//#define X_ENC_SIGN (-1.)
|
||||||
|
//#define Y_ENC_SIGN (-1.)
|
||||||
|
|
||||||
|
|
||||||
|
// encoder position to radians and back
|
||||||
|
#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
|
||||||
|
#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
|
||||||
|
#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
|
||||||
|
#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
|
||||||
|
|
||||||
// convert angle in radians to +-pi
|
// 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 per revolution
|
|
||||||
#define X_ENC_STEPSPERREV (67108864.)
|
|
||||||
#define Y_ENC_STEPSPERREV (67108864.)
|
|
||||||
// encoder zero position
|
|
||||||
#define X_ENC_ZERO (61245239)
|
|
||||||
#define Y_ENC_ZERO (36999830)
|
|
||||||
// encoder reversed (no: +1)
|
|
||||||
#define X_ENC_SIGN (-1.)
|
|
||||||
#define Y_ENC_SIGN (-1.)
|
|
||||||
// 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 Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 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 Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
|
|
||||||
|
|
||||||
// encoder's tolerance (ticks)
|
// encoder's tolerance (ticks)
|
||||||
#define YencTOL (25.)
|
#define YencTOL (25.)
|
||||||
@@ -328,11 +341,10 @@ 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);
|
||||||
int SSsetterI(const char *cmd, int32_t ival);
|
int SSsetterI(const char *cmd, int32_t ival);
|
||||||
int SSstop(int emerg);
|
int SSstop(int emerg);
|
||||||
int SSshortCmd(SSscmd *cmd);
|
|
||||||
mcc_errcodes_t updateMotorPos();
|
mcc_errcodes_t updateMotorPos();
|
||||||
|
|||||||
88
asibfm700/CMakeLists.txt
Normal file
88
asibfm700/CMakeLists.txt
Normal 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()
|
||||||
30
asibfm700/asibfm700_common.h
Normal file
30
asibfm700/asibfm700_common.h
Normal 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
|
||||||
860
asibfm700/asibfm700_configfile.h
Normal file
860
asibfm700/asibfm700_configfile.h
Normal 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
|
||||||
355
asibfm700/asibfm700_mount.cpp
Normal file
355
asibfm700/asibfm700_mount.cpp
Normal 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
189
asibfm700/asibfm700_mount.h
Normal 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
|
||||||
59
asibfm700/asibfm700_netserver.cpp
Normal file
59
asibfm700/asibfm700_netserver.cpp
Normal 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
|
||||||
146
asibfm700/asibfm700_netserver.h
Normal file
146
asibfm700/asibfm700_netserver.h
Normal 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
|
||||||
507
asibfm700/asibfm700_netserver_endpoint.h
Normal file
507
asibfm700/asibfm700_netserver_endpoint.h
Normal 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
|
||||||
172
asibfm700/asibfm700_netserver_main.cpp
Normal file
172
asibfm700/asibfm700_netserver_main.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
292
asibfm700/asibfm700_servocontroller.cpp
Normal file
292
asibfm700/asibfm700_servocontroller.cpp
Normal 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
|
||||||
144
asibfm700/asibfm700_servocontroller.h
Normal file
144
asibfm700/asibfm700_servocontroller.h
Normal 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
|
||||||
69
asibfm700/tests/cfg_test.cpp
Normal file
69
asibfm700/tests/cfg_test.cpp
Normal 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;
|
||||||
|
}
|
||||||
@@ -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)
|
||||||
|
|||||||
@@ -103,6 +103,8 @@ 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})
|
include_directories(${ERFA_INCLUDE_DIR})
|
||||||
|
|
||||||
|
message(STATUS "ERFA: " ${ERFA_INCLUDE_DIR})
|
||||||
|
|
||||||
option(WITH_TESTS "Build tests" ON)
|
option(WITH_TESTS "Build tests" ON)
|
||||||
|
|
||||||
|
|
||||||
@@ -123,21 +125,24 @@ set(CNTR_PROTO_LIB comm_proto)
|
|||||||
add_library(${CNTR_PROTO_LIB} STATIC ${CNTR_PROTO_LIB_SRC})
|
add_library(${CNTR_PROTO_LIB} STATIC ${CNTR_PROTO_LIB_SRC})
|
||||||
|
|
||||||
|
|
||||||
set(MCC_LIBRARY_SRC mcc_mount_concepts.h mcc_fsm_mount.h mcc_mount_coord.h mcc_mount_events_states.h mcc_finite_state_machine.h
|
include_directories(${FITPACK_INCLUDE_DIR})
|
||||||
|
|
||||||
|
|
||||||
|
set(MCC_LIBRARY_SRC mcc_mount_concepts.h mcc_fsm_mount.h mcc_generic_mount.h mcc_mount_coord.h mcc_mount_events_states.h mcc_finite_state_machine.h
|
||||||
mcc_mount_pec.h mcc_mount_pz.h mcc_traits.h mcc_mount_telemetry_astrom.h mcc_mount_telemetry.h mcc_mount_config.h mcc_mount_astro_erfa.h
|
mcc_mount_pec.h mcc_mount_pz.h mcc_traits.h mcc_mount_telemetry_astrom.h mcc_mount_telemetry.h mcc_mount_config.h mcc_mount_astro_erfa.h
|
||||||
mcc_astrom_iers.h mcc_astrom_iers_default.h mcc_slew_model.h mcc_guiding_model.h mcc_utils.h mcc_spdlog.h)
|
mcc_astrom_iers.h mcc_astrom_iers_default.h mcc_slew_model.h mcc_guiding_model.h mcc_slew_guiding_model_common.h mcc_utils.h mcc_spdlog.h)
|
||||||
set(MCC_LIBRARY mcc)
|
set(MCC_LIBRARY mcc)
|
||||||
add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC})
|
add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC})
|
||||||
target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23)
|
target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23)
|
||||||
target_include_directories(${MCC_LIBRARY} INTERFACE ${FITPACK_INCLUDE_DIR})
|
target_include_directories(${MCC_LIBRARY} INTERFACE ${FITPACK_INCLUDE_DIR} ${ERFA_INCLUDE_DIR})
|
||||||
|
|
||||||
|
|
||||||
set(ASIBFM700_LIB_SRC asibfm700_hardware.h asibfm700_hardware.cpp)
|
set(ASIBFM700_LIB_SRC asibfm700_common.h asibfm700_hardware.h asibfm700_hardware.cpp asibfm700_mount.h asibfm700_mount.cpp
|
||||||
|
asibfm700_config.h)
|
||||||
set(ASIBFM700_LIB asibfm700)
|
set(ASIBFM700_LIB asibfm700)
|
||||||
add_library(${ASIBFM700_LIB} STATIC ${ASIBFM700_LIB_SRC}
|
add_library(${ASIBFM700_LIB} STATIC ${ASIBFM700_LIB_SRC})
|
||||||
asibfm700_slew_model.h asibfm700_slew_model.cpp
|
# target_include_directories(${ASIBFM700_LIB} PRIVATE ${FITPACK_INCLUDE_DIR} ${ERFA_INCLUDE_DIR})
|
||||||
asibfm700_common.h)
|
target_include_directories(${ASIBFM700_LIB} PUBLIC ${FITPACK_INCLUDE_DIR} ${ERFA_INCLUDE_DIR})
|
||||||
target_include_directories(${ASIBFM700_LIB} PRIVATE ${FITPACK_INCLUDE_DIR})
|
|
||||||
|
|
||||||
# set(MOUNT_SERVER_APP_SRC mount.h mount_state.h mount_server.cpp comm_server.h comm_server_endpoint.h comm_server_configfile.h mount_astrom.h
|
# set(MOUNT_SERVER_APP_SRC mount.h mount_state.h mount_server.cpp comm_server.h comm_server_endpoint.h comm_server_configfile.h mount_astrom.h
|
||||||
# mount_astrom_default.h mcc_coord.h mount_pz.h mcc_fsm.h mcc_fsm_utils.h mcc_finite_state_machine.h mcc_mount_events_states.h)
|
# mount_astrom_default.h mcc_coord.h mount_pz.h mcc_fsm.h mcc_fsm_utils.h mcc_finite_state_machine.h mcc_mount_events_states.h)
|
||||||
@@ -158,6 +163,7 @@ if (WITH_TESTS)
|
|||||||
|
|
||||||
set(ASTROM_TEST_APP astrom_test)
|
set(ASTROM_TEST_APP astrom_test)
|
||||||
add_executable(${ASTROM_TEST_APP} tests/astrom_test.cpp)
|
add_executable(${ASTROM_TEST_APP} tests/astrom_test.cpp)
|
||||||
|
target_include_directories(${ASTROM_TEST_APP} PRIVATE ${FITPACK_INCLUDE_DIR} ${ERFA_INCLUDE_DIR})
|
||||||
target_link_libraries(${ASTROM_TEST_APP} ERFA_LIB)
|
target_link_libraries(${ASTROM_TEST_APP} ERFA_LIB)
|
||||||
|
|
||||||
set(FITPACK_TEST_APP fitpack_test)
|
set(FITPACK_TEST_APP fitpack_test)
|
||||||
|
|||||||
@@ -5,26 +5,24 @@
|
|||||||
/* COMMON DEFINITIONS */
|
/* COMMON DEFINITIONS */
|
||||||
|
|
||||||
|
|
||||||
|
#include "asibfm700_hardware.h"
|
||||||
|
// #include "mcc_fsm_mount.h"
|
||||||
|
#include "mcc_guiding_model.h"
|
||||||
#include "mcc_mount_astro_erfa.h"
|
#include "mcc_mount_astro_erfa.h"
|
||||||
#include "mcc_mount_pec.h"
|
#include "mcc_mount_pec.h"
|
||||||
|
#include "mcc_mount_pz.h"
|
||||||
#include "mcc_mount_telemetry.h"
|
#include "mcc_mount_telemetry.h"
|
||||||
|
#include "mcc_slew_model.h"
|
||||||
#include "asibfm700_hardware.h"
|
// #include "mcc_spdlog.h"
|
||||||
|
|
||||||
namespace asibfm700
|
namespace asibfm700
|
||||||
{
|
{
|
||||||
|
|
||||||
typedef mcc::astrom::erfa::MccMountAstromEngineERFA<mcc::MccAngle> AsibFM700AstromEngine;
|
typedef mcc::astrom::erfa::MccMountAstromEngineERFA AsibFM700AstromEngine;
|
||||||
|
|
||||||
typedef mcc::MccMountDefaultPEC<mcc::MccMountType::FORK_TYPE> AsibFM700PointingErrorCorrection;
|
typedef mcc::MccMountDefaultPEC<mcc::MccMountType::FORK_TYPE> AsibFM700PointingErrorCorrection;
|
||||||
|
|
||||||
struct AsibFM700TelemetryData : mcc::MccMountTelemetryData<AsibFM700AstromEngine, AsibFM700PointingErrorCorrection> {
|
typedef mcc::MccMountTelemetryData<AsibFM700AstromEngine> AsibFM700TelemetryData;
|
||||||
// apparent target (user-input) current coordinates
|
|
||||||
coord_t tagRA, tagDEC;
|
|
||||||
coord_t tagHA;
|
|
||||||
coord_t tagAZ, tagALT;
|
|
||||||
coord_t tagPA;
|
|
||||||
};
|
|
||||||
|
|
||||||
typedef mcc::MccMountTelemetry<AsibFM700AstromEngine,
|
typedef mcc::MccMountTelemetry<AsibFM700AstromEngine,
|
||||||
AsibFM700PointingErrorCorrection,
|
AsibFM700PointingErrorCorrection,
|
||||||
@@ -32,14 +30,52 @@ typedef mcc::MccMountTelemetry<AsibFM700AstromEngine,
|
|||||||
AsibFM700TelemetryData>
|
AsibFM700TelemetryData>
|
||||||
AsibFM700Telemetry;
|
AsibFM700Telemetry;
|
||||||
|
|
||||||
|
static_assert(std::movable<AsibFM700Telemetry>);
|
||||||
|
static_assert(std::movable<AsibFM700AstromEngine>);
|
||||||
|
static_assert(std::movable<mcc::MccAltLimitPZ<>>);
|
||||||
|
|
||||||
|
// typedef mcc::MccSimpleSlewModel<mcc::utils::MccSpdlogLogger> AsibFM700SlewModel;
|
||||||
|
// typedef mcc::MccSimpleGuidingModel<mcc::utils::MccSpdlogLogger> AsibFM700GuidingModel;
|
||||||
|
|
||||||
|
|
||||||
|
template <mcc::traits::mcc_logger_c LoggerT>
|
||||||
|
using AsibFM700SlewModel = mcc::MccSimpleSlewModel<LoggerT>;
|
||||||
|
|
||||||
|
template <mcc::traits::mcc_logger_c LoggerT>
|
||||||
|
using AsibFM700GuidingModel = mcc::MccSimpleGuidingModel<LoggerT>;
|
||||||
|
|
||||||
|
/*
|
||||||
|
|
||||||
|
template <mcc::traits::mcc_logger_c LoggerT>
|
||||||
|
struct AsibFM700MountControls {
|
||||||
|
AsibFM700MountControls(AsibFM700MountControls&&) = default;
|
||||||
|
AsibFM700MountControls& operator=(AsibFM700MountControls&&) = default;
|
||||||
|
|
||||||
|
AsibFM700AstromEngine astrometryEngine;
|
||||||
|
AsibFM700PointingErrorCorrection PEC;
|
||||||
|
AsibFM700Hardware hardware;
|
||||||
|
AsibFM700Telemetry telemetry{astrometryEngine, PEC, hardware};
|
||||||
|
|
||||||
|
std::tuple<mcc::MccAltLimitPZ<mcc::MccAltLimitKind::MIN_ALT_LIMIT>> prohibitedZones;
|
||||||
|
|
||||||
|
AsibFM700SlewModel<LoggerT> slewModel{PEC, hardware, prohibitedZones};
|
||||||
|
AsibFM700GuidingModel<LoggerT> guidingModel{PEC, hardware, prohibitedZones};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
static_assert(mcc::traits::mcc_mount_controls_c<AsibFM700MountControls<mcc::utils::MccSpdlogLogger>>);
|
||||||
|
|
||||||
// global mount configuration
|
// global mount configuration
|
||||||
|
|
||||||
struct AsibFM700Config {
|
|
||||||
std::chrono::milliseconds hardwareAskingPeriod{100}; // main cycle period
|
|
||||||
|
|
||||||
// mount hardware config
|
|
||||||
AsibFM700Hardware::hardware_config_t hardwareConfig;
|
class AsibFM700Mount : public mcc::MccMount<AsibFM700MountControls<mcc::utils::MccSpdlogLogger>>
|
||||||
|
{
|
||||||
|
class InitState : public AsibFM700Mount::MccMountEventBase
|
||||||
|
{
|
||||||
|
};
|
||||||
|
|
||||||
|
public:
|
||||||
};
|
};
|
||||||
|
*/
|
||||||
} // namespace asibfm700
|
} // namespace asibfm700
|
||||||
|
|||||||
34
cxx/asibfm700_config.h
Normal file
34
cxx/asibfm700_config.h
Normal file
@@ -0,0 +1,34 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
|
||||||
|
/* AstroSIB-FM700 FORK MOUNT CONTROL LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
|
/* MOUNT CONFIGURATION DEFINITION */
|
||||||
|
|
||||||
|
#include "asibfm700_common.h"
|
||||||
|
|
||||||
|
namespace asibfm700
|
||||||
|
{
|
||||||
|
|
||||||
|
struct AsibFM700Config {
|
||||||
|
std::chrono::milliseconds hardwarePollingPeriod{100}; // main cycle period
|
||||||
|
|
||||||
|
// mount hardware config
|
||||||
|
AsibFM700Hardware::hardware_config_t hardwareConfig;
|
||||||
|
|
||||||
|
// astrometry engine
|
||||||
|
AsibFM700AstromEngine::engine_state_t astromEngineState;
|
||||||
|
std::string leapSecondFilename;
|
||||||
|
std::string bulletinAFilename;
|
||||||
|
|
||||||
|
// PEC
|
||||||
|
AsibFM700PointingErrorCorrection::pec_data_t pecData;
|
||||||
|
|
||||||
|
// slew and guiding
|
||||||
|
mcc::MccSlewAndGuidingPoint::slew_guiding_params_t slewGuidingParams;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace asibfm700
|
||||||
@@ -69,33 +69,6 @@ AsibFM700Hardware::AsibFM700Hardware(const hardware_config_t& conf)
|
|||||||
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
||||||
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
||||||
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
||||||
|
|
||||||
_sideralRate2 *= _sideralRate2;
|
|
||||||
_sideralRateEps2 = 0.01; // 1%
|
|
||||||
|
|
||||||
// start state polling
|
|
||||||
|
|
||||||
_statePollingThread = std::jthread([this](std::stop_token stoken) {
|
|
||||||
mountdata_t data;
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
if (stoken.stop_requested()) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
error_t err = static_cast<AsibFM700HardwareErrorCode>(Mount.getMountData(&data));
|
|
||||||
|
|
||||||
|
|
||||||
if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
|
|
||||||
// are both motors stopped?
|
|
||||||
bool stop_motors =
|
|
||||||
(data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT);
|
|
||||||
if (stop_motors) {
|
|
||||||
_state = hw_state_t::HW_STATE_STOP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// AsibFM700Hardware::AsibFM700Hardware(AsibFM700Hardware&& other)
|
// AsibFM700Hardware::AsibFM700Hardware(AsibFM700Hardware&& other)
|
||||||
@@ -118,52 +91,35 @@ std::string_view AsibFM700Hardware::id() const
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
AsibFM700Hardware::error_t AsibFM700Hardware::getState(AsibFM700Hardware::hw_state_t& state) const
|
|
||||||
{
|
|
||||||
mountdata_t data;
|
|
||||||
error_t err = static_cast<AsibFM700HardwareErrorCode>(Mount.getMountData(&data));
|
|
||||||
|
|
||||||
if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
|
|
||||||
// are both motors stopped?
|
|
||||||
bool stop_motors = (data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT);
|
|
||||||
if (stop_motors) {
|
|
||||||
state = hw_state_t::HW_STATE_STOP;
|
|
||||||
return AsibFM700HardwareErrorCode::ERROR_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute current speed
|
|
||||||
auto rate2 = data.encXspeed.val * data.encXspeed.val + data.encYspeed.val * data.encYspeed.val;
|
|
||||||
auto ratio2 = rate2 / _sideralRate2;
|
|
||||||
if (ratio2 <= _sideralRateEps2) { // tracking
|
|
||||||
state = hw_state_t::HW_STATE_TRACK;
|
|
||||||
} else {
|
|
||||||
state = hw_state_t::HW_STATE_SLEW;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
AsibFM700Hardware::error_t AsibFM700Hardware::setPos(AsibFM700Hardware::axes_pos_t pos)
|
AsibFM700Hardware::error_t AsibFM700Hardware::setPos(AsibFM700Hardware::axes_pos_t pos)
|
||||||
{
|
{
|
||||||
error_t err;
|
error_t err;
|
||||||
|
|
||||||
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
||||||
coordpair_t hw_pos{.X = pos.y, .Y = pos.x};
|
coordpair_t hw_pos{.X = pos.y, .Y = pos.x};
|
||||||
|
double tp = std::chrono::duration<double>(pos.time_point.time_since_epoch()).count();
|
||||||
|
coordval_pair_t hw_posval{.X{.val = pos.x, .t = tp}, .Y{.val = pos.y, .t = tp}};
|
||||||
|
|
||||||
if (!pos.flags.slewNguide) {
|
//
|
||||||
return static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
|
// WARNING: The LibSidservo API was chagned! hw_pos is endpoint where mount must
|
||||||
|
// go "after" slewing
|
||||||
|
//
|
||||||
|
|
||||||
|
switch (pos.moving_type) {
|
||||||
|
case hw_moving_type_t::HW_MOVE_SLEWING: // slew mount
|
||||||
|
if (pos.moveAndStop) {
|
||||||
|
// err = static_cast<AsibFM700HardwareErrorCode>(Mount.moveTo(&hw_pos));
|
||||||
|
err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
|
||||||
|
} else {
|
||||||
|
// err = static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
|
||||||
|
err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (pos.state) {
|
|
||||||
case hw_state_t::HW_STATE_SLEW: // slew mount
|
|
||||||
err = static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
|
|
||||||
break;
|
break;
|
||||||
case hw_state_t::HW_STATE_TRACK: // interpretate as guiding correction
|
case hw_moving_type_t::HW_MOVE_ADJUSTING: // corrections at the end of slewing
|
||||||
err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctBy(&hw_pos));
|
err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
|
||||||
break;
|
break;
|
||||||
case hw_state_t::HW_STATE_STOP:
|
case hw_moving_type_t::HW_MOVE_GUIDING: // interpretate as guiding correction
|
||||||
|
err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
@@ -194,9 +150,6 @@ AsibFM700Hardware::error_t AsibFM700Hardware::getPos(AsibFM700Hardware::axes_pos
|
|||||||
|
|
||||||
pos.xrate = data.encYspeed.val;
|
pos.xrate = data.encYspeed.val;
|
||||||
pos.yrate = data.encXspeed.val;
|
pos.yrate = data.encXspeed.val;
|
||||||
|
|
||||||
// mount state
|
|
||||||
err = getState(pos.state);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return err;
|
return err;
|
||||||
|
|||||||
@@ -71,15 +71,16 @@ public:
|
|||||||
typedef mcc::MccAngle coord_t;
|
typedef mcc::MccAngle coord_t;
|
||||||
typedef std::chrono::system_clock::time_point time_point_t; // UTC time
|
typedef std::chrono::system_clock::time_point time_point_t; // UTC time
|
||||||
|
|
||||||
enum class hw_state_t : int { HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK };
|
enum class hw_moving_type_t : int { HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING };
|
||||||
|
|
||||||
struct axes_pos_t {
|
struct axes_pos_t {
|
||||||
time_point_t time_point;
|
time_point_t time_point;
|
||||||
coord_t x, y;
|
coord_t x, y;
|
||||||
coord_t xrate, yrate;
|
coord_t xrate, yrate;
|
||||||
|
|
||||||
hw_state_t state;
|
hw_moving_type_t moving_type{hw_moving_type_t::HW_MOVE_TRACKING};
|
||||||
slewflags_t flags;
|
// slewflags_t flags;
|
||||||
|
bool moveAndStop{false};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -108,7 +109,6 @@ public:
|
|||||||
|
|
||||||
std::string_view id() const;
|
std::string_view id() const;
|
||||||
|
|
||||||
error_t getState(hw_state_t&) const;
|
|
||||||
error_t setPos(axes_pos_t);
|
error_t setPos(axes_pos_t);
|
||||||
error_t getPos(axes_pos_t&);
|
error_t getPos(axes_pos_t&);
|
||||||
|
|
||||||
@@ -118,9 +118,6 @@ public:
|
|||||||
private:
|
private:
|
||||||
hardware_config_t _hardwareConfig;
|
hardware_config_t _hardwareConfig;
|
||||||
|
|
||||||
std::jthread _statePollingThread;
|
|
||||||
hw_state_t _state;
|
|
||||||
|
|
||||||
double _sideralRate2; // square of sideral rate
|
double _sideralRate2; // square of sideral rate
|
||||||
double _sideralRateEps2;
|
double _sideralRateEps2;
|
||||||
};
|
};
|
||||||
|
|||||||
20
cxx/asibfm700_mount.cpp
Normal file
20
cxx/asibfm700_mount.cpp
Normal file
@@ -0,0 +1,20 @@
|
|||||||
|
#include "asibfm700_mount.h"
|
||||||
|
|
||||||
|
namespace asibfm700
|
||||||
|
{
|
||||||
|
|
||||||
|
template <mcc::traits::mcc_range_of_input_char_range R>
|
||||||
|
AsibFM700Mount::AsibFM700Mount(AsibFM700Config config, std::shared_ptr<spdlog::logger> logger, const R& pattern_range)
|
||||||
|
: mcc::utils::MccSpdlogLogger(std::move(logger), pattern_range),
|
||||||
|
_currentConfig(std::move(config)),
|
||||||
|
base_gm_t({_currentConfig.astromEngineState},
|
||||||
|
{_currentConfig.hardwareConfig},
|
||||||
|
{_currentConfig.pecData},
|
||||||
|
AsibFM700Telemetry{&_astromEngine, &_pec, &_hardware},
|
||||||
|
slew_model_t{&_telemetry, &_hardware, &_pzFuncs},
|
||||||
|
guiding_model_t{&_telemetry, &_hardware, &_pzFuncs})
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace asibfm700
|
||||||
48
cxx/asibfm700_mount.h
Normal file
48
cxx/asibfm700_mount.h
Normal file
@@ -0,0 +1,48 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
/* AstroSIB-FM700 FORK MOUNT CONTROL LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
|
#include "asibfm700_common.h"
|
||||||
|
#include "asibfm700_config.h"
|
||||||
|
|
||||||
|
#include "mcc_generic_mount.h"
|
||||||
|
#include "mcc_spdlog.h"
|
||||||
|
|
||||||
|
namespace asibfm700
|
||||||
|
{
|
||||||
|
|
||||||
|
|
||||||
|
class AsibFM700Mount : public mcc::utils::MccSpdlogLogger,
|
||||||
|
public mcc::MccGenericMount<AsibFM700AstromEngine,
|
||||||
|
AsibFM700Hardware,
|
||||||
|
AsibFM700PointingErrorCorrection,
|
||||||
|
AsibFM700Telemetry,
|
||||||
|
AsibFM700SlewModel<mcc::utils::MccSpdlogLogger>,
|
||||||
|
AsibFM700GuidingModel<mcc::utils::MccSpdlogLogger>>
|
||||||
|
{
|
||||||
|
using base_gm_t = mcc::MccGenericMount<AsibFM700AstromEngine,
|
||||||
|
AsibFM700Hardware,
|
||||||
|
AsibFM700PointingErrorCorrection,
|
||||||
|
AsibFM700Telemetry,
|
||||||
|
AsibFM700SlewModel<mcc::utils::MccSpdlogLogger>,
|
||||||
|
AsibFM700GuidingModel<mcc::utils::MccSpdlogLogger>>;
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef AsibFM700SlewModel<mcc::utils::MccSpdlogLogger> slew_model_t;
|
||||||
|
typedef AsibFM700GuidingModel<mcc::utils::MccSpdlogLogger> guiding_model_t;
|
||||||
|
|
||||||
|
template <mcc::traits::mcc_range_of_input_char_range R = decltype(LOGGER_DEFAULT_FORMAT)>
|
||||||
|
AsibFM700Mount(AsibFM700Config config,
|
||||||
|
std::shared_ptr<spdlog::logger> logger,
|
||||||
|
const R& pattern_range = LOGGER_DEFAULT_FORMAT);
|
||||||
|
|
||||||
|
~AsibFM700Mount();
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
AsibFM700Config _currentConfig;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace asibfm700
|
||||||
@@ -1,110 +0,0 @@
|
|||||||
#include "asibfm700_slew_model.h"
|
|
||||||
|
|
||||||
|
|
||||||
namespace asibfm700
|
|
||||||
{
|
|
||||||
|
|
||||||
/* error category implementation */
|
|
||||||
|
|
||||||
const char* AsibFM700SlewModelErrorCategory::name() const noexcept
|
|
||||||
{
|
|
||||||
return "ASTROSIB FM700 MOUNT SLEW MODEL ERROR CATEGORY";
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string AsibFM700SlewModelErrorCategory::message(int ec) const
|
|
||||||
{
|
|
||||||
AsibFM700SlewModelErrorCode code = static_cast<AsibFM700SlewModelErrorCode>(ec);
|
|
||||||
|
|
||||||
std::string msg;
|
|
||||||
|
|
||||||
switch (code) {
|
|
||||||
case AsibFM700SlewModelErrorCode::ERROR_OK:
|
|
||||||
msg = "OK";
|
|
||||||
default:
|
|
||||||
msg = "UNKNOWN ERROR";
|
|
||||||
}
|
|
||||||
|
|
||||||
return msg;
|
|
||||||
}
|
|
||||||
|
|
||||||
const AsibFM700SlewModelErrorCategory& AsibFM700SlewModelErrorCategory::get()
|
|
||||||
{
|
|
||||||
static const AsibFM700SlewModelErrorCategory constInst;
|
|
||||||
return constInst;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
AsibFM700SlewModel::AsibFM700SlewModel(AsibFM700Hardware& hw_control, AsibFM700AstromEngine& astrom_engine)
|
|
||||||
: _hwControl(hw_control), _astromEngine(astrom_engine)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
AsibFM700SlewModel::error_t AsibFM700SlewModel::slew(const slew_params_t& slew_pars, AsibFM700Telemetry& telemetry)
|
|
||||||
{
|
|
||||||
using astrom_t = std::remove_reference_t<decltype(_astromEngine)>;
|
|
||||||
using coord_t = typename astrom_t::coord_t;
|
|
||||||
using jd_t = typename astrom_t::juldate_t;
|
|
||||||
|
|
||||||
AsibFM700SlewModel::error_t res_err = AsibFM700SlewModelErrorCode::ERROR_OK;
|
|
||||||
|
|
||||||
AsibFM700Hardware::axes_pos_t ax_pos;
|
|
||||||
|
|
||||||
if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
|
|
||||||
// trivial case (the pair is interpretated as raw encoder coordinates)
|
|
||||||
ax_pos.x = slew_pars.x;
|
|
||||||
ax_pos.y = slew_pars.y;
|
|
||||||
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // catalog coordinates
|
|
||||||
jd_t jd;
|
|
||||||
coord_t ra_app, dec_app, ha, az, alt;
|
|
||||||
typename astrom_t::eo_t eo;
|
|
||||||
|
|
||||||
auto err = _astromEngine.greg2jul(std::chrono::system_clock::now(), jd);
|
|
||||||
|
|
||||||
if (!err) {
|
|
||||||
err = _astromEngine.icrs2obs(slew_pars.x, slew_pars.y, jd, ra_app, dec_app, ha, az, alt, eo);
|
|
||||||
|
|
||||||
if (!err) {
|
|
||||||
res_err = slew({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
|
|
||||||
.x = ha,
|
|
||||||
.y = dec_app,
|
|
||||||
.stop = slew_pars.stop},
|
|
||||||
telemetry);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) { // apparent
|
|
||||||
jd_t jd;
|
|
||||||
typename astrom_t::eo_t eo;
|
|
||||||
|
|
||||||
auto err = _astromEngine.greg2jul(std::chrono::system_clock::now(), jd);
|
|
||||||
if (!err) {
|
|
||||||
typename astrom_t::sideral_time_t lst;
|
|
||||||
err = _astromEngine.apparentSiderTime(jd, lst, true);
|
|
||||||
|
|
||||||
if (!err) {
|
|
||||||
err = _astromEngine.eqOrigins(jd, eo);
|
|
||||||
if (!err) {
|
|
||||||
res_err = slew({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
|
|
||||||
.x = lst - slew_pars.x + eo, // HA = LST - RA_APP + EO
|
|
||||||
.y = slew_pars.y,
|
|
||||||
.stop = slew_pars.stop},
|
|
||||||
telemetry);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) { // apparent
|
|
||||||
// compute encoders coordinates
|
|
||||||
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
|
|
||||||
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
|
|
||||||
}
|
|
||||||
|
|
||||||
auto err = _hwControl.setPos(std::move(ax_pos));
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
}
|
|
||||||
|
|
||||||
return res_err;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace asibfm700
|
|
||||||
@@ -1,75 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
/* AstroSIB-FM700 FORK MOUNT CONTROL LIBRARY */
|
|
||||||
|
|
||||||
/* SLEW MODEL IMPLEMENTATION */
|
|
||||||
|
|
||||||
|
|
||||||
#include "asibfm700_common.h"
|
|
||||||
|
|
||||||
namespace asibfm700
|
|
||||||
{
|
|
||||||
|
|
||||||
enum class AsibFM700SlewModelErrorCode : int { ERROR_OK };
|
|
||||||
|
|
||||||
// error category
|
|
||||||
struct AsibFM700SlewModelErrorCategory : public std::error_category {
|
|
||||||
const char* name() const noexcept;
|
|
||||||
std::string message(int ec) const;
|
|
||||||
|
|
||||||
static const AsibFM700SlewModelErrorCategory& get();
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
inline std::error_code make_error_code(AsibFM700SlewModelErrorCode ec)
|
|
||||||
{
|
|
||||||
return std::error_code(static_cast<int>(ec), AsibFM700SlewModelErrorCategory::get());
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace asibfm700
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
namespace std
|
|
||||||
{
|
|
||||||
|
|
||||||
template <>
|
|
||||||
class is_error_code_enum<asibfm700::AsibFM700SlewModelErrorCode> : public true_type
|
|
||||||
{
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace std
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
namespace asibfm700
|
|
||||||
{
|
|
||||||
|
|
||||||
class AsibFM700SlewModel final
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
typedef std::error_code error_t;
|
|
||||||
|
|
||||||
struct slew_params_t {
|
|
||||||
typedef mcc::MccAngle coord_t;
|
|
||||||
|
|
||||||
mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP};
|
|
||||||
|
|
||||||
coord_t x{0.0};
|
|
||||||
coord_t y{0.0};
|
|
||||||
|
|
||||||
bool stop{false};
|
|
||||||
};
|
|
||||||
|
|
||||||
AsibFM700SlewModel(AsibFM700Hardware& hw_control, AsibFM700AstromEngine& astrom_engine);
|
|
||||||
|
|
||||||
~AsibFM700SlewModel();
|
|
||||||
|
|
||||||
error_t slew(const slew_params_t&, AsibFM700Telemetry&);
|
|
||||||
|
|
||||||
private:
|
|
||||||
AsibFM700Hardware& _hwControl;
|
|
||||||
AsibFM700AstromEngine& _astromEngine;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace asibfm700
|
|
||||||
@@ -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 69 issued in January 2025
|
# Updated through IERS Bulletin 70 issued in July 2025
|
||||||
#
|
#
|
||||||
#
|
#
|
||||||
# File expires on 28 December 2025
|
# File expires on 28 June 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 *
|
||||||
**********************************************************************
|
**********************************************************************
|
||||||
3 July 2025 Vol. XXXVIII No. 027
|
14 August 2025 Vol. XXXVIII No. 033
|
||||||
______________________________________________________________________
|
______________________________________________________________________
|
||||||
GENERAL INFORMATION:
|
GENERAL INFORMATION:
|
||||||
MJD = Julian Date - 2 400 000.5 days
|
MJD = Julian Date - 2 400 000.5 days
|
||||||
@@ -76,13 +76,15 @@ static std::string MCC_DEFAULT_IERS_BULLETIN_A_FILE = R"--(
|
|||||||
where pi = 3.14159265... and T is the date in Besselian years.
|
where pi = 3.14159265... and T is the date in Besselian years.
|
||||||
TT = TAI + 32.184 seconds
|
TT = TAI + 32.184 seconds
|
||||||
DUT1= (UT1-UTC) transmitted with time signals
|
DUT1= (UT1-UTC) transmitted with time signals
|
||||||
= 0.0 seconds beginning 26 December 2024 at 0000 UTC
|
|
||||||
= +0.1 seconds beginning 10 July 2025 at 0000 UTC
|
= +0.1 seconds beginning 10 July 2025 at 0000 UTC
|
||||||
Beginning 1 January 2017:
|
Beginning 1 January 2017:
|
||||||
TAI-UTC = 37.000 000 seconds
|
TAI-UTC = 37.000 000 seconds
|
||||||
***********************************************************************
|
***********************************************************************
|
||||||
* ANNOUNCEMENTS: *
|
* ANNOUNCEMENTS: *
|
||||||
* *
|
* *
|
||||||
|
* There will NOT be a leap second introduced in UTC *
|
||||||
|
* at the end of December 2025. *
|
||||||
|
* *
|
||||||
* 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: *
|
||||||
* https://maia.usno.navy.mil *
|
* https://maia.usno.navy.mil *
|
||||||
@@ -114,48 +116,13 @@ 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 6 27 60853 0.15472 .00009 0.44036 .00009 0.040354 0.000022
|
25 8 8 60895 0.21521 .00009 0.41904 .00009 0.069826 0.000016
|
||||||
25 6 28 60854 0.15644 .00009 0.44013 .00009 0.041389 0.000023
|
25 8 9 60896 0.21558 .00009 0.41771 .00009 0.070764 0.000016
|
||||||
25 6 29 60855 0.15844 .00009 0.44006 .00009 0.042204 0.000024
|
25 8 10 60897 0.21616 .00009 0.41640 .00009 0.071442 0.000017
|
||||||
25 6 30 60856 0.16029 .00009 0.44003 .00009 0.042854 0.000016
|
25 8 11 60898 0.21726 .00009 0.41513 .00009 0.071836 0.000016
|
||||||
25 7 1 60857 0.16204 .00009 0.43988 .00009 0.043427 0.000013
|
25 8 12 60899 0.21832 .00009 0.41407 .00009 0.071996 0.000017
|
||||||
25 7 2 60858 0.16365 .00009 0.43965 .00009 0.043890 0.000012
|
25 8 13 60900 0.21871 .00009 0.41314 .00009 0.072040 0.000013
|
||||||
25 7 3 60859 0.16516 .00009 0.43931 .00009 0.044366 0.000009
|
25 8 14 60901 0.21892 .00009 0.41223 .00009 0.072161 0.000011
|
||||||
|
|
||||||
IERS Final Values
|
|
||||||
MJD x y UT1-UTC
|
|
||||||
" " s
|
|
||||||
25 5 2 60797 0.0865 0.4180 0.03007
|
|
||||||
25 5 3 60798 0.0873 0.4191 0.03031
|
|
||||||
25 5 4 60799 0.0882 0.4202 0.03039
|
|
||||||
25 5 5 60800 0.0889 0.4212 0.03031
|
|
||||||
25 5 6 60801 0.0898 0.4221 0.03006
|
|
||||||
25 5 7 60802 0.0907 0.4232 0.02970
|
|
||||||
25 5 8 60803 0.0915 0.4238 0.02932
|
|
||||||
25 5 9 60804 0.0925 0.4244 0.02898
|
|
||||||
25 5 10 60805 0.0939 0.4247 0.02874
|
|
||||||
25 5 11 60806 0.0955 0.4251 0.02855
|
|
||||||
25 5 12 60807 0.0970 0.4259 0.02849
|
|
||||||
25 5 13 60808 0.0980 0.4268 0.02857
|
|
||||||
25 5 14 60809 0.0990 0.4277 0.02880
|
|
||||||
25 5 15 60810 0.1001 0.4288 0.02915
|
|
||||||
25 5 16 60811 0.1008 0.4299 0.02958
|
|
||||||
25 5 17 60812 0.1012 0.4309 0.02999
|
|
||||||
25 5 18 60813 0.1016 0.4316 0.03034
|
|
||||||
25 5 19 60814 0.1027 0.4318 0.03056
|
|
||||||
25 5 20 60815 0.1040 0.4321 0.03061
|
|
||||||
25 5 21 60816 0.1052 0.4328 0.03042
|
|
||||||
25 5 22 60817 0.1063 0.4338 0.02998
|
|
||||||
25 5 23 60818 0.1073 0.4346 0.02931
|
|
||||||
25 5 24 60819 0.1081 0.4356 0.02856
|
|
||||||
25 5 25 60820 0.1086 0.4365 0.02791
|
|
||||||
25 5 26 60821 0.1096 0.4368 0.02748
|
|
||||||
25 5 27 60822 0.1106 0.4373 0.02744
|
|
||||||
25 5 28 60823 0.1114 0.4375 0.02772
|
|
||||||
25 5 29 60824 0.1116 0.4380 0.02813
|
|
||||||
25 5 30 60825 0.1120 0.4375 0.02851
|
|
||||||
25 5 31 60826 0.1127 0.4374 0.02881
|
|
||||||
25 6 1 60827 0.1132 0.4377 0.02899
|
|
||||||
|
|
||||||
_______________________________________________________________________
|
_______________________________________________________________________
|
||||||
|
|
||||||
@@ -163,487 +130,387 @@ 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.1314 + 0.0400 cos A + 0.1547 sin A - 0.0075 cos C - 0.0886 sin C
|
x = 0.1410 + 0.1141 cos A + 0.0905 sin A - 0.0377 cos C - 0.0594 sin C
|
||||||
y = 0.3810 + 0.1454 cos A - 0.0330 sin A - 0.0886 cos C + 0.0075 sin C
|
y = 0.3821 + 0.0927 cos A - 0.1005 sin A - 0.0594 cos C + 0.0377 sin C
|
||||||
UT1-UTC = 0.0606 + 0.00011 (MJD - 60867) - (UT2-UT1)
|
UT1-UTC = 0.0478 + 0.00010 (MJD - 60909) - (UT2-UT1)
|
||||||
|
|
||||||
where A = 2*pi*(MJD-60859)/365.25 and C = 2*pi*(MJD-60859)/435.
|
where A = 2*pi*(MJD-60901)/365.25 and C = 2*pi*(MJD-60901)/435.
|
||||||
|
|
||||||
TAI-UTC(MJD 60860) = 37.0
|
TAI-UTC(MJD 60902) = 37.0
|
||||||
The accuracy may be estimated from the expressions:
|
The accuracy may be estimated from the expressions:
|
||||||
S x,y = 0.00068 (MJD-60859)**0.80 S t = 0.00025 (MJD-60859)**0.75
|
S x,y = 0.00068 (MJD-60901)**0.80 S t = 0.00025 (MJD-60901)**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 7 4 60860 0.1666 0.4389 0.04491
|
2025 8 15 60902 0.2191 0.4112 0.07241
|
||||||
2025 7 5 60861 0.1680 0.4385 0.04561
|
2025 8 16 60903 0.2193 0.4101 0.07289
|
||||||
2025 7 6 60862 0.1694 0.4380 0.04651
|
2025 8 17 60904 0.2196 0.4090 0.07364
|
||||||
2025 7 7 60863 0.1708 0.4375 0.04762
|
2025 8 18 60905 0.2200 0.4078 0.07462
|
||||||
2025 7 8 60864 0.1721 0.4369 0.04889
|
2025 8 19 60906 0.2205 0.4066 0.07573
|
||||||
2025 7 9 60865 0.1734 0.4364 0.05025
|
2025 8 20 60907 0.2210 0.4054 0.07685
|
||||||
2025 7 10 60866 0.1747 0.4359 0.05163
|
2025 8 21 60908 0.2215 0.4042 0.07788
|
||||||
2025 7 11 60867 0.1760 0.4353 0.05295
|
2025 8 22 60909 0.2220 0.4029 0.07870
|
||||||
2025 7 12 60868 0.1772 0.4347 0.05411
|
2025 8 23 60910 0.2225 0.4018 0.07929
|
||||||
2025 7 13 60869 0.1785 0.4341 0.05505
|
2025 8 24 60911 0.2229 0.4006 0.07964
|
||||||
2025 7 14 60870 0.1797 0.4335 0.05573
|
2025 8 25 60912 0.2233 0.3994 0.07982
|
||||||
2025 7 15 60871 0.1809 0.4329 0.05615
|
2025 8 26 60913 0.2237 0.3982 0.07994
|
||||||
2025 7 16 60872 0.1821 0.4322 0.05641
|
2025 8 27 60914 0.2241 0.3970 0.08012
|
||||||
2025 7 17 60873 0.1832 0.4315 0.05666
|
2025 8 28 60915 0.2244 0.3957 0.08043
|
||||||
2025 7 18 60874 0.1844 0.4308 0.05701
|
2025 8 29 60916 0.2247 0.3945 0.08090
|
||||||
2025 7 19 60875 0.1855 0.4301 0.05759
|
2025 8 30 60917 0.2250 0.3932 0.08154
|
||||||
2025 7 20 60876 0.1867 0.4294 0.05843
|
2025 8 31 60918 0.2253 0.3919 0.08233
|
||||||
2025 7 21 60877 0.1878 0.4286 0.05953
|
2025 9 1 60919 0.2256 0.3907 0.08321
|
||||||
2025 7 22 60878 0.1889 0.4278 0.06082
|
2025 9 2 60920 0.2258 0.3894 0.08411
|
||||||
2025 7 23 60879 0.1899 0.4271 0.06216
|
2025 9 3 60921 0.2260 0.3881 0.08494
|
||||||
2025 7 24 60880 0.1910 0.4263 0.06342
|
2025 9 4 60922 0.2262 0.3868 0.08561
|
||||||
2025 7 25 60881 0.1920 0.4254 0.06451
|
2025 9 5 60923 0.2263 0.3855 0.08601
|
||||||
2025 7 26 60882 0.1931 0.4246 0.06536
|
2025 9 6 60924 0.2265 0.3842 0.08608
|
||||||
2025 7 27 60883 0.1941 0.4237 0.06600
|
2025 9 7 60925 0.2266 0.3829 0.08580
|
||||||
2025 7 28 60884 0.1951 0.4229 0.06647
|
2025 9 8 60926 0.2267 0.3816 0.08522
|
||||||
2025 7 29 60885 0.1960 0.4220 0.06686
|
2025 9 9 60927 0.2267 0.3803 0.08448
|
||||||
2025 7 30 60886 0.1970 0.4211 0.06726
|
2025 9 10 60928 0.2267 0.3790 0.08373
|
||||||
2025 7 31 60887 0.1979 0.4201 0.06774
|
2025 9 11 60929 0.2267 0.3777 0.08314
|
||||||
2025 8 1 60888 0.1988 0.4192 0.06835
|
2025 9 12 60930 0.2267 0.3764 0.08283
|
||||||
2025 8 2 60889 0.1997 0.4182 0.06912
|
2025 9 13 60931 0.2267 0.3751 0.08283
|
||||||
2025 8 3 60890 0.2006 0.4173 0.07005
|
2025 9 14 60932 0.2266 0.3738 0.08308
|
||||||
2025 8 4 60891 0.2015 0.4163 0.07113
|
2025 9 15 60933 0.2265 0.3724 0.08349
|
||||||
2025 8 5 60892 0.2023 0.4153 0.07233
|
2025 9 16 60934 0.2263 0.3711 0.08390
|
||||||
2025 8 6 60893 0.2031 0.4143 0.07357
|
2025 9 17 60935 0.2262 0.3698 0.08421
|
||||||
2025 8 7 60894 0.2039 0.4132 0.07478
|
2025 9 18 60936 0.2260 0.3685 0.08434
|
||||||
2025 8 8 60895 0.2047 0.4122 0.07584
|
2025 9 19 60937 0.2258 0.3672 0.08426
|
||||||
2025 8 9 60896 0.2054 0.4111 0.07666
|
2025 9 20 60938 0.2256 0.3659 0.08400
|
||||||
2025 8 10 60897 0.2062 0.4101 0.07718
|
2025 9 21 60939 0.2253 0.3646 0.08360
|
||||||
2025 8 11 60898 0.2069 0.4090 0.07742
|
2025 9 22 60940 0.2250 0.3633 0.08315
|
||||||
2025 8 12 60899 0.2075 0.4079 0.07746
|
2025 9 23 60941 0.2247 0.3620 0.08272
|
||||||
2025 8 13 60900 0.2082 0.4068 0.07741
|
2025 9 24 60942 0.2244 0.3607 0.08239
|
||||||
2025 8 14 60901 0.2089 0.4057 0.07742
|
2025 9 25 60943 0.2240 0.3595 0.08220
|
||||||
2025 8 15 60902 0.2095 0.4045 0.07762
|
2025 9 26 60944 0.2236 0.3582 0.08219
|
||||||
2025 8 16 60903 0.2101 0.4034 0.07809
|
2025 9 27 60945 0.2232 0.3569 0.08235
|
||||||
2025 8 17 60904 0.2106 0.4022 0.07884
|
2025 9 28 60946 0.2228 0.3556 0.08264
|
||||||
2025 8 18 60905 0.2112 0.4011 0.07981
|
2025 9 29 60947 0.2223 0.3544 0.08302
|
||||||
2025 8 19 60906 0.2117 0.3999 0.08088
|
2025 9 30 60948 0.2218 0.3531 0.08339
|
||||||
2025 8 20 60907 0.2122 0.3987 0.08193
|
2025 10 1 60949 0.2213 0.3519 0.08369
|
||||||
2025 8 21 60908 0.2127 0.3975 0.08285
|
2025 10 2 60950 0.2208 0.3506 0.08381
|
||||||
2025 8 22 60909 0.2132 0.3963 0.08355
|
2025 10 3 60951 0.2202 0.3494 0.08367
|
||||||
2025 8 23 60910 0.2136 0.3951 0.08403
|
2025 10 4 60952 0.2196 0.3482 0.08322
|
||||||
2025 8 24 60911 0.2140 0.3939 0.08432
|
2025 10 5 60953 0.2190 0.3470 0.08244
|
||||||
2025 8 25 60912 0.2144 0.3927 0.08451
|
2025 10 6 60954 0.2184 0.3458 0.08143
|
||||||
2025 8 26 60913 0.2147 0.3915 0.08467
|
2025 10 7 60955 0.2177 0.3446 0.08031
|
||||||
2025 8 27 60914 0.2151 0.3902 0.08488
|
2025 10 8 60956 0.2171 0.3434 0.07929
|
||||||
2025 8 28 60915 0.2154 0.3890 0.08522
|
2025 10 9 60957 0.2164 0.3422 0.07851
|
||||||
2025 8 29 60916 0.2157 0.3877 0.08573
|
2025 10 10 60958 0.2156 0.3410 0.07804
|
||||||
2025 8 30 60917 0.2159 0.3865 0.08641
|
2025 10 11 60959 0.2149 0.3399 0.07788
|
||||||
2025 8 31 60918 0.2162 0.3852 0.08727
|
2025 10 12 60960 0.2141 0.3387 0.07790
|
||||||
2025 9 1 60919 0.2164 0.3840 0.08825
|
2025 10 13 60961 0.2133 0.3376 0.07799
|
||||||
2025 9 2 60920 0.2165 0.3827 0.08931
|
2025 10 14 60962 0.2125 0.3365 0.07800
|
||||||
2025 9 3 60921 0.2167 0.3814 0.09034
|
2025 10 15 60963 0.2117 0.3354 0.07785
|
||||||
2025 9 4 60922 0.2168 0.3802 0.09125
|
2025 10 16 60964 0.2108 0.3343 0.07750
|
||||||
2025 9 5 60923 0.2169 0.3789 0.09195
|
2025 10 17 60965 0.2100 0.3332 0.07696
|
||||||
2025 9 6 60924 0.2170 0.3776 0.09236
|
2025 10 18 60966 0.2091 0.3321 0.07629
|
||||||
2025 9 7 60925 0.2171 0.3763 0.09245
|
2025 10 19 60967 0.2082 0.3311 0.07556
|
||||||
2025 9 8 60926 0.2171 0.3750 0.09226
|
2025 10 20 60968 0.2072 0.3300 0.07484
|
||||||
2025 9 9 60927 0.2171 0.3738 0.09190
|
2025 10 21 60969 0.2063 0.3290 0.07422
|
||||||
2025 9 10 60928 0.2171 0.3725 0.09154
|
2025 10 22 60970 0.2053 0.3280 0.07375
|
||||||
2025 9 11 60929 0.2171 0.3712 0.09134
|
2025 10 23 60971 0.2043 0.3270 0.07347
|
||||||
2025 9 12 60930 0.2170 0.3699 0.09141
|
2025 10 24 60972 0.2033 0.3260 0.07337
|
||||||
2025 9 13 60931 0.2169 0.3686 0.09178
|
2025 10 25 60973 0.2022 0.3250 0.07345
|
||||||
2025 9 14 60932 0.2168 0.3673 0.09239
|
2025 10 26 60974 0.2012 0.3241 0.07364
|
||||||
2025 9 15 60933 0.2167 0.3661 0.09314
|
2025 10 27 60975 0.2001 0.3231 0.07388
|
||||||
2025 9 16 60934 0.2165 0.3648 0.09389
|
2025 10 28 60976 0.1990 0.3222 0.07410
|
||||||
2025 9 17 60935 0.2163 0.3635 0.09452
|
2025 10 29 60977 0.1979 0.3213 0.07419
|
||||||
2025 9 18 60936 0.2161 0.3622 0.09495
|
2025 10 30 60978 0.1968 0.3204 0.07409
|
||||||
2025 9 19 60937 0.2158 0.3609 0.09516
|
2025 10 31 60979 0.1957 0.3196 0.07374
|
||||||
2025 9 20 60938 0.2156 0.3597 0.09517
|
2025 11 1 60980 0.1945 0.3187 0.07309
|
||||||
2025 9 21 60939 0.2153 0.3584 0.09503
|
2025 11 2 60981 0.1933 0.3179 0.07220
|
||||||
2025 9 22 60940 0.2150 0.3571 0.09484
|
2025 11 3 60982 0.1922 0.3171 0.07115
|
||||||
2025 9 23 60941 0.2146 0.3559 0.09466
|
2025 11 4 60983 0.1910 0.3163 0.07011
|
||||||
2025 9 24 60942 0.2142 0.3546 0.09458
|
2025 11 5 60984 0.1898 0.3155 0.06924
|
||||||
2025 9 25 60943 0.2139 0.3534 0.09465
|
2025 11 6 60985 0.1885 0.3147 0.06866
|
||||||
2025 9 26 60944 0.2134 0.3521 0.09489
|
2025 11 7 60986 0.1873 0.3140 0.06841
|
||||||
2025 9 27 60945 0.2130 0.3509 0.09530
|
2025 11 8 60987 0.1860 0.3133 0.06842
|
||||||
2025 9 28 60946 0.2125 0.3497 0.09585
|
2025 11 9 60988 0.1848 0.3126 0.06856
|
||||||
2025 9 29 60947 0.2120 0.3484 0.09649
|
2025 11 10 60989 0.1835 0.3119 0.06868
|
||||||
2025 9 30 60948 0.2115 0.3472 0.09713
|
2025 11 11 60990 0.1822 0.3112 0.06866
|
||||||
2025 10 1 60949 0.2110 0.3460 0.09770
|
2025 11 12 60991 0.1809 0.3106 0.06845
|
||||||
2025 10 2 60950 0.2104 0.3448 0.09809
|
2025 11 13 60992 0.1796 0.3100 0.06806
|
||||||
2025 10 3 60951 0.2099 0.3436 0.09823
|
2025 11 14 60993 0.1782 0.3094 0.06755
|
||||||
2025 10 4 60952 0.2093 0.3424 0.09807
|
2025 11 15 60994 0.1769 0.3088 0.06698
|
||||||
2025 10 5 60953 0.2086 0.3412 0.09759
|
2025 11 16 60995 0.1755 0.3082 0.06643
|
||||||
2025 10 6 60954 0.2080 0.3401 0.09687
|
2025 11 17 60996 0.1742 0.3077 0.06596
|
||||||
2025 10 7 60955 0.2073 0.3389 0.09605
|
2025 11 18 60997 0.1728 0.3072 0.06564
|
||||||
2025 10 8 60956 0.2066 0.3378 0.09532
|
2025 11 19 60998 0.1714 0.3067 0.06550
|
||||||
2025 10 9 60957 0.2059 0.3366 0.09483
|
2025 11 20 60999 0.1701 0.3062 0.06555
|
||||||
2025 10 10 60958 0.2052 0.3355 0.09464
|
2025 11 21 61000 0.1687 0.3058 0.06578
|
||||||
2025 10 11 60959 0.2044 0.3344 0.09475
|
2025 11 22 61001 0.1673 0.3053 0.06615
|
||||||
2025 10 12 60960 0.2036 0.3333 0.09504
|
2025 11 23 61002 0.1659 0.3049 0.06660
|
||||||
2025 10 13 60961 0.2028 0.3322 0.09538
|
2025 11 24 61003 0.1644 0.3045 0.06704
|
||||||
2025 10 14 60962 0.2020 0.3311 0.09563
|
2025 11 25 61004 0.1630 0.3042 0.06739
|
||||||
2025 10 15 60963 0.2012 0.3300 0.09571
|
2025 11 26 61005 0.1616 0.3038 0.06759
|
||||||
2025 10 16 60964 0.2003 0.3290 0.09557
|
2025 11 27 61006 0.1602 0.3035 0.06756
|
||||||
2025 10 17 60965 0.1994 0.3280 0.09524
|
2025 11 28 61007 0.1587 0.3032 0.06728
|
||||||
2025 10 18 60966 0.1985 0.3269 0.09475
|
2025 11 29 61008 0.1573 0.3029 0.06677
|
||||||
2025 10 19 60967 0.1976 0.3259 0.09419
|
2025 11 30 61009 0.1558 0.3027 0.06607
|
||||||
2025 10 20 60968 0.1967 0.3249 0.09361
|
2025 12 1 61010 0.1544 0.3024 0.06523
|
||||||
2025 10 21 60969 0.1957 0.3239 0.09312
|
2025 12 2 61011 0.1529 0.3022 0.06448
|
||||||
2025 10 22 60970 0.1947 0.3230 0.09276
|
2025 12 3 61012 0.1515 0.3020 0.06395
|
||||||
2025 10 23 60971 0.1937 0.3220 0.09259
|
2025 12 4 61013 0.1500 0.3019 0.06373
|
||||||
2025 10 24 60972 0.1927 0.3211 0.09259
|
2025 12 5 61014 0.1486 0.3017 0.06379
|
||||||
2025 10 25 60973 0.1917 0.3202 0.09276
|
2025 12 6 61015 0.1471 0.3016 0.06403
|
||||||
2025 10 26 60974 0.1906 0.3193 0.09305
|
2025 12 7 61016 0.1456 0.3015 0.06430
|
||||||
2025 10 27 60975 0.1896 0.3184 0.09337
|
2025 12 8 61017 0.1442 0.3015 0.06446
|
||||||
2025 10 28 60976 0.1885 0.3175 0.09366
|
2025 12 9 61018 0.1427 0.3014 0.06442
|
||||||
2025 10 29 60977 0.1874 0.3166 0.09383
|
2025 12 10 61019 0.1413 0.3014 0.06419
|
||||||
2025 10 30 60978 0.1863 0.3158 0.09380
|
2025 12 11 61020 0.1398 0.3014 0.06380
|
||||||
2025 10 31 60979 0.1851 0.3150 0.09350
|
2025 12 12 61021 0.1383 0.3014 0.06335
|
||||||
2025 11 1 60980 0.1840 0.3142 0.09291
|
2025 12 13 61022 0.1369 0.3015 0.06291
|
||||||
2025 11 2 60981 0.1828 0.3134 0.09206
|
2025 12 14 61023 0.1354 0.3015 0.06256
|
||||||
2025 11 3 60982 0.1817 0.3126 0.09105
|
2025 12 15 61024 0.1340 0.3016 0.06234
|
||||||
2025 11 4 60983 0.1805 0.3119 0.09004
|
2025 12 16 61025 0.1325 0.3017 0.06230
|
||||||
2025 11 5 60984 0.1793 0.3112 0.08919
|
2025 12 17 61026 0.1311 0.3019 0.06246
|
||||||
2025 11 6 60985 0.1781 0.3105 0.08864
|
2025 12 18 61027 0.1297 0.3020 0.06279
|
||||||
2025 11 7 60986 0.1768 0.3098 0.08841
|
2025 12 19 61028 0.1282 0.3022 0.06328
|
||||||
2025 11 8 60987 0.1756 0.3091 0.08843
|
2025 12 20 61029 0.1268 0.3024 0.06386
|
||||||
2025 11 9 60988 0.1743 0.3085 0.08858
|
2025 12 21 61030 0.1254 0.3027 0.06444
|
||||||
2025 11 10 60989 0.1731 0.3078 0.08870
|
2025 12 22 61031 0.1239 0.3029 0.06496
|
||||||
2025 11 11 60990 0.1718 0.3072 0.08868
|
2025 12 23 61032 0.1225 0.3032 0.06533
|
||||||
2025 11 12 60991 0.1705 0.3066 0.08847
|
2025 12 24 61033 0.1211 0.3035 0.06550
|
||||||
2025 11 13 60992 0.1692 0.3061 0.08807
|
2025 12 25 61034 0.1197 0.3038 0.06544
|
||||||
2025 11 14 60993 0.1679 0.3055 0.08754
|
2025 12 26 61035 0.1183 0.3041 0.06515
|
||||||
2025 11 15 60994 0.1666 0.3050 0.08696
|
2025 12 27 61036 0.1170 0.3045 0.06469
|
||||||
2025 11 16 60995 0.1652 0.3045 0.08638
|
2025 12 28 61037 0.1156 0.3049 0.06414
|
||||||
2025 11 17 60996 0.1639 0.3040 0.08589
|
2025 12 29 61038 0.1142 0.3053 0.06364
|
||||||
2025 11 18 60997 0.1626 0.3035 0.08554
|
2025 12 30 61039 0.1129 0.3057 0.06331
|
||||||
2025 11 19 60998 0.1612 0.3031 0.08537
|
2025 12 31 61040 0.1115 0.3062 0.06323
|
||||||
2025 11 20 60999 0.1598 0.3027 0.08539
|
2026 1 1 61041 0.1102 0.3066 0.06342
|
||||||
2025 11 21 61000 0.1585 0.3023 0.08559
|
2026 1 2 61042 0.1089 0.3071 0.06382
|
||||||
2025 11 22 61001 0.1571 0.3019 0.08593
|
2026 1 3 61043 0.1076 0.3077 0.06429
|
||||||
2025 11 23 61002 0.1557 0.3015 0.08634
|
2026 1 4 61044 0.1063 0.3082 0.06469
|
||||||
2025 11 24 61003 0.1543 0.3012 0.08675
|
2026 1 5 61045 0.1050 0.3087 0.06489
|
||||||
2025 11 25 61004 0.1529 0.3009 0.08707
|
2026 1 6 61046 0.1037 0.3093 0.06486
|
||||||
2025 11 26 61005 0.1515 0.3006 0.08724
|
2026 1 7 61047 0.1024 0.3099 0.06463
|
||||||
2025 11 27 61006 0.1501 0.3004 0.08718
|
2026 1 8 61048 0.1012 0.3105 0.06428
|
||||||
2025 11 28 61007 0.1487 0.3001 0.08688
|
2026 1 9 61049 0.0999 0.3112 0.06391
|
||||||
2025 11 29 61008 0.1473 0.2999 0.08634
|
2026 1 10 61050 0.0987 0.3118 0.06361
|
||||||
2025 11 30 61009 0.1459 0.2997 0.08562
|
2026 1 11 61051 0.0975 0.3125 0.06344
|
||||||
2025 12 1 61010 0.1445 0.2995 0.08484
|
2026 1 12 61052 0.0963 0.3132 0.06344
|
||||||
2025 12 2 61011 0.1431 0.2994 0.08414
|
2026 1 13 61053 0.0951 0.3139 0.06364
|
||||||
2025 12 3 61012 0.1417 0.2993 0.08368
|
2026 1 14 61054 0.0940 0.3147 0.06401
|
||||||
2025 12 4 61013 0.1402 0.2991 0.08352
|
2026 1 15 61055 0.0928 0.3154 0.06454
|
||||||
2025 12 5 61014 0.1388 0.2991 0.08365
|
2026 1 16 61056 0.0917 0.3162 0.06517
|
||||||
2025 12 6 61015 0.1374 0.2990 0.08395
|
2026 1 17 61057 0.0906 0.3170 0.06582
|
||||||
2025 12 7 61016 0.1360 0.2990 0.08429
|
2026 1 18 61058 0.0895 0.3178 0.06640
|
||||||
2025 12 8 61017 0.1345 0.2990 0.08452
|
2026 1 19 61059 0.0884 0.3186 0.06684
|
||||||
2025 12 9 61018 0.1331 0.2990 0.08456
|
2026 1 20 61060 0.0874 0.3195 0.06707
|
||||||
2025 12 10 61019 0.1317 0.2990 0.08439
|
2026 1 21 61061 0.0863 0.3203 0.06706
|
||||||
2025 12 11 61020 0.1303 0.2991 0.08408
|
2026 1 22 61062 0.0853 0.3212 0.06680
|
||||||
2025 12 12 61021 0.1288 0.2991 0.08369
|
2026 1 23 61063 0.0843 0.3221 0.06636
|
||||||
2025 12 13 61022 0.1274 0.2993 0.08332
|
2026 1 24 61064 0.0833 0.3230 0.06582
|
||||||
2025 12 14 61023 0.1260 0.2994 0.08304
|
2026 1 25 61065 0.0823 0.3239 0.06531
|
||||||
2025 12 15 61024 0.1246 0.2995 0.08289
|
2026 1 26 61066 0.0814 0.3249 0.06494
|
||||||
2025 12 16 61025 0.1232 0.2997 0.08293
|
2026 1 27 61067 0.0804 0.3259 0.06479
|
||||||
2025 12 17 61026 0.1218 0.2999 0.08316
|
2026 1 28 61068 0.0795 0.3268 0.06489
|
||||||
2025 12 18 61027 0.1204 0.3001 0.08357
|
2026 1 29 61069 0.0786 0.3278 0.06520
|
||||||
2025 12 19 61028 0.1190 0.3004 0.08414
|
2026 1 30 61070 0.0778 0.3288 0.06562
|
||||||
2025 12 20 61029 0.1176 0.3006 0.08479
|
2026 1 31 61071 0.0769 0.3298 0.06599
|
||||||
2025 12 21 61030 0.1162 0.3009 0.08546
|
2026 2 1 61072 0.0761 0.3309 0.06620
|
||||||
2025 12 22 61031 0.1149 0.3012 0.08606
|
2026 2 2 61073 0.0753 0.3319 0.06617
|
||||||
2025 12 23 61032 0.1135 0.3016 0.08651
|
2026 2 3 61074 0.0745 0.3330 0.06589
|
||||||
2025 12 24 61033 0.1121 0.3019 0.08676
|
2026 2 4 61075 0.0737 0.3341 0.06543
|
||||||
2025 12 25 61034 0.1108 0.3023 0.08678
|
2026 2 5 61076 0.0730 0.3351 0.06490
|
||||||
2025 12 26 61035 0.1094 0.3027 0.08657
|
2026 2 6 61077 0.0723 0.3362 0.06439
|
||||||
2025 12 27 61036 0.1081 0.3031 0.08620
|
2026 2 7 61078 0.0716 0.3373 0.06401
|
||||||
2025 12 28 61037 0.1068 0.3035 0.08575
|
2026 2 8 61079 0.0709 0.3385 0.06379
|
||||||
2025 12 29 61038 0.1055 0.3040 0.08535
|
2026 2 9 61080 0.0703 0.3396 0.06378
|
||||||
2025 12 30 61039 0.1042 0.3045 0.08513
|
2026 2 10 61081 0.0696 0.3407 0.06396
|
||||||
2025 12 31 61040 0.1029 0.3050 0.08516
|
2026 2 11 61082 0.0690 0.3419 0.06432
|
||||||
2026 1 1 61041 0.1016 0.3055 0.08548
|
2026 2 12 61083 0.0685 0.3431 0.06481
|
||||||
2026 1 2 61042 0.1003 0.3061 0.08601
|
2026 2 13 61084 0.0679 0.3442 0.06537
|
||||||
2026 1 3 61043 0.0990 0.3067 0.08663
|
2026 2 14 61085 0.0674 0.3454 0.06590
|
||||||
2026 1 4 61044 0.0978 0.3072 0.08720
|
2026 2 15 61086 0.0669 0.3466 0.06634
|
||||||
2026 1 5 61045 0.0966 0.3079 0.08759
|
2026 2 16 61087 0.0664 0.3478 0.06660
|
||||||
2026 1 6 61046 0.0953 0.3085 0.08776
|
2026 2 17 61088 0.0659 0.3490 0.06664
|
||||||
2026 1 7 61047 0.0941 0.3091 0.08775
|
2026 2 18 61089 0.0655 0.3503 0.06646
|
||||||
2026 1 8 61048 0.0929 0.3098 0.08763
|
2026 2 19 61090 0.0651 0.3515 0.06608
|
||||||
2026 1 9 61049 0.0917 0.3105 0.08749
|
2026 2 20 61091 0.0647 0.3527 0.06560
|
||||||
2026 1 10 61050 0.0906 0.3112 0.08742
|
2026 2 21 61092 0.0643 0.3540 0.06514
|
||||||
2026 1 11 61051 0.0894 0.3120 0.08746
|
2026 2 22 61093 0.0640 0.3552 0.06482
|
||||||
2026 1 12 61052 0.0883 0.3127 0.08766
|
2026 2 23 61094 0.0637 0.3564 0.06470
|
||||||
2026 1 13 61053 0.0871 0.3135 0.08801
|
2026 2 24 61095 0.0634 0.3577 0.06483
|
||||||
2026 1 14 61054 0.0860 0.3143 0.08852
|
2026 2 25 61096 0.0632 0.3590 0.06515
|
||||||
2026 1 15 61055 0.0849 0.3151 0.08915
|
2026 2 26 61097 0.0629 0.3602 0.06557
|
||||||
2026 1 16 61056 0.0838 0.3159 0.08984
|
2026 2 27 61098 0.0627 0.3615 0.06596
|
||||||
2026 1 17 61057 0.0828 0.3168 0.09052
|
2026 2 28 61099 0.0625 0.3628 0.06620
|
||||||
2026 1 18 61058 0.0817 0.3176 0.09108
|
2026 3 1 61100 0.0624 0.3641 0.06619
|
||||||
2026 1 19 61059 0.0807 0.3185 0.09145
|
2026 3 2 61101 0.0623 0.3654 0.06591
|
||||||
2026 1 20 61060 0.0797 0.3194 0.09156
|
2026 3 3 61102 0.0622 0.3666 0.06538
|
||||||
2026 1 21 61061 0.0787 0.3203 0.09138
|
2026 3 4 61103 0.0621 0.3679 0.06470
|
||||||
2026 1 22 61062 0.0777 0.3212 0.09092
|
2026 3 5 61104 0.0620 0.3692 0.06398
|
||||||
2026 1 23 61063 0.0768 0.3222 0.09023
|
2026 3 6 61105 0.0620 0.3705 0.06332
|
||||||
2026 1 24 61064 0.0758 0.3232 0.08943
|
2026 3 7 61106 0.0620 0.3718 0.06280
|
||||||
2026 1 25 61065 0.0749 0.3241 0.08864
|
2026 3 8 61107 0.0620 0.3731 0.06247
|
||||||
2026 1 26 61066 0.0740 0.3251 0.08797
|
2026 3 9 61108 0.0621 0.3744 0.06235
|
||||||
2026 1 27 61067 0.0731 0.3261 0.08752
|
2026 3 10 61109 0.0621 0.3757 0.06241
|
||||||
2026 1 28 61068 0.0723 0.3272 0.08730
|
2026 3 11 61110 0.0622 0.3770 0.06263
|
||||||
2026 1 29 61069 0.0715 0.3282 0.08730
|
2026 3 12 61111 0.0624 0.3783 0.06293
|
||||||
2026 1 30 61070 0.0706 0.3293 0.08740
|
2026 3 13 61112 0.0625 0.3796 0.06322
|
||||||
2026 1 31 61071 0.0698 0.3303 0.08748
|
2026 3 14 61113 0.0627 0.3809 0.06341
|
||||||
2026 2 1 61072 0.0691 0.3314 0.08742
|
2026 3 15 61114 0.0629 0.3821 0.06342
|
||||||
2026 2 2 61073 0.0683 0.3325 0.08715
|
2026 3 16 61115 0.0631 0.3834 0.06320
|
||||||
2026 2 3 61074 0.0676 0.3336 0.08668
|
2026 3 17 61116 0.0634 0.3847 0.06271
|
||||||
2026 2 4 61075 0.0669 0.3347 0.08606
|
2026 3 18 61117 0.0637 0.3860 0.06199
|
||||||
2026 2 5 61076 0.0662 0.3359 0.08538
|
2026 3 19 61118 0.0640 0.3873 0.06113
|
||||||
2026 2 6 61077 0.0655 0.3370 0.08476
|
2026 3 20 61119 0.0643 0.3885 0.06024
|
||||||
2026 2 7 61078 0.0649 0.3381 0.08430
|
2026 3 21 61120 0.0647 0.3898 0.05943
|
||||||
2026 2 8 61079 0.0643 0.3393 0.08405
|
2026 3 22 61121 0.0651 0.3911 0.05880
|
||||||
2026 2 9 61080 0.0637 0.3405 0.08401
|
2026 3 23 61122 0.0655 0.3923 0.05842
|
||||||
2026 2 10 61081 0.0631 0.3417 0.08423
|
2026 3 24 61123 0.0659 0.3936 0.05830
|
||||||
2026 2 11 61082 0.0625 0.3429 0.08462
|
2026 3 25 61124 0.0664 0.3948 0.05832
|
||||||
2026 2 12 61083 0.0620 0.3441 0.08513
|
2026 3 26 61125 0.0668 0.3961 0.05841
|
||||||
2026 2 13 61084 0.0615 0.3453 0.08570
|
2026 3 27 61126 0.0673 0.3973 0.05840
|
||||||
2026 2 14 61085 0.0610 0.3465 0.08618
|
2026 3 28 61127 0.0679 0.3985 0.05821
|
||||||
2026 2 15 61086 0.0606 0.3478 0.08646
|
2026 3 29 61128 0.0684 0.3997 0.05777
|
||||||
2026 2 16 61087 0.0601 0.3490 0.08651
|
2026 3 30 61129 0.0690 0.4010 0.05709
|
||||||
2026 2 17 61088 0.0597 0.3502 0.08629
|
2026 3 31 61130 0.0696 0.4022 0.05624
|
||||||
2026 2 18 61089 0.0594 0.3515 0.08575
|
2026 4 1 61131 0.0702 0.4034 0.05523
|
||||||
2026 2 19 61090 0.0590 0.3528 0.08502
|
2026 4 2 61132 0.0709 0.4045 0.05420
|
||||||
2026 2 20 61091 0.0587 0.3540 0.08419
|
2026 4 3 61133 0.0715 0.4057 0.05330
|
||||||
2026 2 21 61092 0.0584 0.3553 0.08335
|
2026 4 4 61134 0.0722 0.4069 0.05257
|
||||||
2026 2 22 61093 0.0581 0.3566 0.08261
|
2026 4 5 61135 0.0729 0.4080 0.05202
|
||||||
2026 2 23 61094 0.0578 0.3579 0.08206
|
2026 4 6 61136 0.0737 0.4092 0.05164
|
||||||
2026 2 24 61095 0.0576 0.3592 0.08182
|
2026 4 7 61137 0.0744 0.4103 0.05137
|
||||||
2026 2 25 61096 0.0574 0.3605 0.08174
|
2026 4 8 61138 0.0752 0.4114 0.05123
|
||||||
2026 2 26 61097 0.0572 0.3618 0.08178
|
2026 4 9 61139 0.0760 0.4125 0.05118
|
||||||
2026 2 27 61098 0.0570 0.3631 0.08189
|
2026 4 10 61140 0.0768 0.4136 0.05105
|
||||||
2026 2 28 61099 0.0569 0.3644 0.08189
|
2026 4 11 61141 0.0777 0.4147 0.05087
|
||||||
2026 3 1 61100 0.0568 0.3657 0.08172
|
2026 4 12 61142 0.0786 0.4158 0.05044
|
||||||
2026 3 2 61101 0.0567 0.3670 0.08136
|
2026 4 13 61143 0.0794 0.4169 0.04970
|
||||||
2026 3 3 61102 0.0567 0.3683 0.08079
|
2026 4 14 61144 0.0803 0.4179 0.04867
|
||||||
2026 3 4 61103 0.0566 0.3697 0.08008
|
2026 4 15 61145 0.0813 0.4189 0.04739
|
||||||
2026 3 5 61104 0.0566 0.3710 0.07935
|
2026 4 16 61146 0.0822 0.4200 0.04601
|
||||||
2026 3 6 61105 0.0567 0.3723 0.07875
|
2026 4 17 61147 0.0832 0.4210 0.04462
|
||||||
2026 3 7 61106 0.0567 0.3736 0.07836
|
2026 4 18 61148 0.0842 0.4219 0.04343
|
||||||
2026 3 8 61107 0.0568 0.3749 0.07813
|
2026 4 19 61149 0.0852 0.4229 0.04254
|
||||||
2026 3 9 61108 0.0569 0.3763 0.07808
|
2026 4 20 61150 0.0862 0.4239 0.04195
|
||||||
2026 3 10 61109 0.0570 0.3776 0.07827
|
2026 4 21 61151 0.0872 0.4248 0.04156
|
||||||
2026 3 11 61110 0.0571 0.3789 0.07855
|
2026 4 22 61152 0.0883 0.4258 0.04129
|
||||||
2026 3 12 61111 0.0573 0.3802 0.07886
|
2026 4 23 61153 0.0894 0.4267 0.04098
|
||||||
2026 3 13 61112 0.0575 0.3815 0.07917
|
2026 4 24 61154 0.0905 0.4276 0.04050
|
||||||
2026 3 14 61113 0.0577 0.3828 0.07941
|
2026 4 25 61155 0.0916 0.4284 0.03988
|
||||||
2026 3 15 61114 0.0580 0.3841 0.07945
|
2026 4 26 61156 0.0927 0.4293 0.03914
|
||||||
2026 3 16 61115 0.0583 0.3855 0.07926
|
2026 4 27 61157 0.0939 0.4302 0.03829
|
||||||
2026 3 17 61116 0.0586 0.3868 0.07879
|
2026 4 28 61158 0.0950 0.4310 0.03744
|
||||||
2026 3 18 61117 0.0589 0.3881 0.07805
|
2026 4 29 61159 0.0962 0.4318 0.03659
|
||||||
2026 3 19 61118 0.0592 0.3893 0.07714
|
2026 4 30 61160 0.0974 0.4326 0.03591
|
||||||
2026 3 20 61119 0.0596 0.3906 0.07617
|
2026 5 1 61161 0.0986 0.4334 0.03540
|
||||||
2026 3 21 61120 0.0600 0.3919 0.07528
|
2026 5 2 61162 0.0998 0.4341 0.03522
|
||||||
2026 3 22 61121 0.0604 0.3932 0.07457
|
2026 5 3 61163 0.1010 0.4348 0.03525
|
||||||
2026 3 23 61122 0.0609 0.3945 0.07407
|
2026 5 4 61164 0.1023 0.4356 0.03547
|
||||||
2026 3 24 61123 0.0614 0.3957 0.07389
|
2026 5 5 61165 0.1035 0.4363 0.03594
|
||||||
2026 3 25 61124 0.0619 0.3970 0.07393
|
2026 5 6 61166 0.1048 0.4369 0.03644
|
||||||
2026 3 26 61125 0.0624 0.3982 0.07404
|
2026 5 7 61167 0.1061 0.4376 0.03697
|
||||||
2026 3 27 61126 0.0629 0.3995 0.07410
|
2026 5 8 61168 0.1074 0.4382 0.03741
|
||||||
2026 3 28 61127 0.0635 0.4007 0.07400
|
2026 5 9 61169 0.1087 0.4389 0.03765
|
||||||
2026 3 29 61128 0.0641 0.4020 0.07359
|
2026 5 10 61170 0.1100 0.4395 0.03762
|
||||||
2026 3 30 61129 0.0647 0.4032 0.07292
|
2026 5 11 61171 0.1114 0.4400 0.03733
|
||||||
2026 3 31 61130 0.0653 0.4044 0.07215
|
2026 5 12 61172 0.1127 0.4406 0.03682
|
||||||
2026 4 1 61131 0.0660 0.4056 0.07125
|
2026 5 13 61173 0.1140 0.4411 0.03611
|
||||||
2026 4 2 61132 0.0667 0.4068 0.07037
|
2026 5 14 61174 0.1154 0.4416 0.03540
|
||||||
2026 4 3 61133 0.0674 0.4080 0.06956
|
2026 5 15 61175 0.1168 0.4421 0.03481
|
||||||
2026 4 4 61134 0.0681 0.4091 0.06893
|
2026 5 16 61176 0.1182 0.4426 0.03447
|
||||||
2026 4 5 61135 0.0689 0.4103 0.06842
|
2026 5 17 61177 0.1195 0.4431 0.03440
|
||||||
2026 4 6 61136 0.0697 0.4114 0.06812
|
2026 5 18 61178 0.1209 0.4435 0.03458
|
||||||
2026 4 7 61137 0.0705 0.4126 0.06802
|
2026 5 19 61179 0.1223 0.4439 0.03496
|
||||||
2026 4 8 61138 0.0713 0.4137 0.06814
|
2026 5 20 61180 0.1237 0.4443 0.03531
|
||||||
2026 4 9 61139 0.0721 0.4148 0.06828
|
2026 5 21 61181 0.1252 0.4447 0.03553
|
||||||
2026 4 10 61140 0.0730 0.4159 0.06842
|
2026 5 22 61182 0.1266 0.4450 0.03563
|
||||||
2026 4 11 61141 0.0739 0.4170 0.06844
|
2026 5 23 61183 0.1280 0.4453 0.03555
|
||||||
2026 4 12 61142 0.0748 0.4180 0.06820
|
2026 5 24 61184 0.1294 0.4456 0.03538
|
||||||
2026 4 13 61143 0.0757 0.4191 0.06770
|
2026 5 25 61185 0.1309 0.4459 0.03522
|
||||||
2026 4 14 61144 0.0766 0.4201 0.06699
|
2026 5 26 61186 0.1323 0.4462 0.03510
|
||||||
2026 4 15 61145 0.0776 0.4212 0.06611
|
2026 5 27 61187 0.1337 0.4464 0.03509
|
||||||
2026 4 16 61146 0.0786 0.4222 0.06511
|
2026 5 28 61188 0.1352 0.4466 0.03526
|
||||||
2026 4 17 61147 0.0796 0.4232 0.06423
|
2026 5 29 61189 0.1366 0.4468 0.03571
|
||||||
2026 4 18 61148 0.0806 0.4242 0.06358
|
2026 5 30 61190 0.1381 0.4469 0.03643
|
||||||
2026 4 19 61149 0.0816 0.4251 0.06321
|
2026 5 31 61191 0.1395 0.4471 0.03730
|
||||||
2026 4 20 61150 0.0827 0.4261 0.06322
|
2026 6 1 61192 0.1410 0.4472 0.03832
|
||||||
2026 4 21 61151 0.0837 0.4270 0.06337
|
2026 6 2 61193 0.1425 0.4473 0.03948
|
||||||
2026 4 22 61152 0.0848 0.4279 0.06356
|
2026 6 3 61194 0.1439 0.4474 0.04058
|
||||||
2026 4 23 61153 0.0859 0.4288 0.06364
|
2026 6 4 61195 0.1454 0.4474 0.04160
|
||||||
2026 4 24 61154 0.0871 0.4297 0.06349
|
2026 6 5 61196 0.1468 0.4474 0.04249
|
||||||
2026 4 25 61155 0.0882 0.4306 0.06314
|
2026 6 6 61197 0.1483 0.4475 0.04322
|
||||||
2026 4 26 61156 0.0894 0.4314 0.06257
|
2026 6 7 61198 0.1497 0.4474 0.04372
|
||||||
2026 4 27 61157 0.0905 0.4322 0.06185
|
2026 6 8 61199 0.1512 0.4474 0.04404
|
||||||
2026 4 28 61158 0.0917 0.4331 0.06111
|
2026 6 9 61200 0.1526 0.4473 0.04420
|
||||||
2026 4 29 61159 0.0929 0.4339 0.06040
|
2026 6 10 61201 0.1541 0.4472 0.04427
|
||||||
2026 4 30 61160 0.0941 0.4346 0.05980
|
2026 6 11 61202 0.1555 0.4471 0.04441
|
||||||
2026 5 1 61161 0.0954 0.4354 0.05946
|
2026 6 12 61203 0.1570 0.4470 0.04471
|
||||||
2026 5 2 61162 0.0966 0.4361 0.05933
|
2026 6 13 61204 0.1584 0.4468 0.04527
|
||||||
2026 5 3 61163 0.0979 0.4368 0.05942
|
2026 6 14 61205 0.1598 0.4467 0.04607
|
||||||
2026 5 4 61164 0.0991 0.4375 0.05971
|
2026 6 15 61206 0.1613 0.4465 0.04699
|
||||||
2026 5 5 61165 0.1004 0.4382 0.06017
|
2026 6 16 61207 0.1627 0.4462 0.04805
|
||||||
2026 5 6 61166 0.1017 0.4388 0.06065
|
2026 6 17 61208 0.1641 0.4460 0.04907
|
||||||
2026 5 7 61167 0.1030 0.4395 0.06110
|
2026 6 18 61209 0.1655 0.4457 0.04991
|
||||||
2026 5 8 61168 0.1043 0.4401 0.06146
|
2026 6 19 61210 0.1669 0.4454 0.05056
|
||||||
2026 5 9 61169 0.1057 0.4407 0.06171
|
2026 6 20 61211 0.1683 0.4451 0.05105
|
||||||
2026 5 10 61170 0.1070 0.4413 0.06171
|
2026 6 21 61212 0.1697 0.4448 0.05136
|
||||||
2026 5 11 61171 0.1084 0.4418 0.06155
|
2026 6 22 61213 0.1711 0.4444 0.05165
|
||||||
2026 5 12 61172 0.1097 0.4423 0.06119
|
2026 6 23 61214 0.1725 0.4440 0.05210
|
||||||
2026 5 13 61173 0.1111 0.4428 0.06070
|
2026 6 24 61215 0.1738 0.4436 0.05266
|
||||||
2026 5 14 61174 0.1125 0.4433 0.06014
|
2026 6 25 61216 0.1752 0.4432 0.05340
|
||||||
2026 5 15 61175 0.1139 0.4438 0.05964
|
2026 6 26 61217 0.1765 0.4428 0.05430
|
||||||
2026 5 16 61176 0.1153 0.4442 0.05948
|
2026 6 27 61218 0.1779 0.4423 0.05539
|
||||||
2026 5 17 61177 0.1167 0.4446 0.05963
|
2026 6 28 61219 0.1792 0.4418 0.05654
|
||||||
2026 5 18 61178 0.1181 0.4450 0.06010
|
2026 6 29 61220 0.1805 0.4413 0.05779
|
||||||
2026 5 19 61179 0.1195 0.4454 0.06068
|
2026 6 30 61221 0.1818 0.4408 0.05910
|
||||||
2026 5 20 61180 0.1209 0.4458 0.06123
|
2026 7 1 61222 0.1831 0.4402 0.06048
|
||||||
2026 5 21 61181 0.1224 0.4461 0.06169
|
2026 7 2 61223 0.1844 0.4397 0.06171
|
||||||
2026 5 22 61182 0.1238 0.4464 0.06196
|
2026 7 3 61224 0.1856 0.4391 0.06280
|
||||||
2026 5 23 61183 0.1252 0.4467 0.06208
|
2026 7 4 61225 0.1869 0.4385 0.06369
|
||||||
2026 5 24 61184 0.1267 0.4469 0.06202
|
2026 7 5 61226 0.1881 0.4378 0.06434
|
||||||
2026 5 25 61185 0.1281 0.4472 0.06187
|
2026 7 6 61227 0.1893 0.4372 0.06480
|
||||||
2026 5 26 61186 0.1296 0.4474 0.06184
|
2026 7 7 61228 0.1905 0.4365 0.06523
|
||||||
2026 5 27 61187 0.1311 0.4476 0.06187
|
2026 7 8 61229 0.1917 0.4358 0.06572
|
||||||
2026 5 28 61188 0.1325 0.4477 0.06209
|
2026 7 9 61230 0.1929 0.4351 0.06633
|
||||||
2026 5 29 61189 0.1340 0.4479 0.06251
|
2026 7 10 61231 0.1941 0.4344 0.06723
|
||||||
2026 5 30 61190 0.1355 0.4480 0.06313
|
2026 7 11 61232 0.1952 0.4336 0.06843
|
||||||
2026 5 31 61191 0.1369 0.4481 0.06394
|
2026 7 12 61233 0.1964 0.4329 0.06985
|
||||||
2026 6 1 61192 0.1384 0.4482 0.06487
|
2026 7 13 61234 0.1975 0.4321 0.07146
|
||||||
2026 6 2 61193 0.1399 0.4482 0.06591
|
2026 7 14 61235 0.1986 0.4313 0.07292
|
||||||
2026 6 3 61194 0.1413 0.4482 0.06694
|
2026 7 15 61236 0.1996 0.4304 0.07413
|
||||||
2026 6 4 61195 0.1428 0.4482 0.06790
|
2026 7 16 61237 0.2007 0.4296 0.07502
|
||||||
2026 6 5 61196 0.1443 0.4482 0.06877
|
2026 7 17 61238 0.2018 0.4288 0.07557
|
||||||
2026 6 6 61197 0.1457 0.4482 0.06950
|
2026 7 18 61239 0.2028 0.4279 0.07596
|
||||||
2026 6 7 61198 0.1472 0.4481 0.07002
|
2026 7 19 61240 0.2038 0.4270 0.07629
|
||||||
2026 6 8 61199 0.1487 0.4480 0.07030
|
2026 7 20 61241 0.2048 0.4261 0.07668
|
||||||
2026 6 9 61200 0.1501 0.4479 0.07051
|
2026 7 21 61242 0.2058 0.4252 0.07728
|
||||||
2026 6 10 61201 0.1516 0.4477 0.07062
|
2026 7 22 61243 0.2067 0.4242 0.07807
|
||||||
2026 6 11 61202 0.1530 0.4476 0.07078
|
2026 7 23 61244 0.2076 0.4233 0.07908
|
||||||
2026 6 12 61203 0.1545 0.4474 0.07103
|
2026 7 24 61245 0.2085 0.4223 0.08036
|
||||||
2026 6 13 61204 0.1559 0.4472 0.07158
|
2026 7 25 61246 0.2094 0.4213 0.08180
|
||||||
2026 6 14 61205 0.1574 0.4469 0.07239
|
2026 7 26 61247 0.2103 0.4203 0.08338
|
||||||
2026 6 15 61206 0.1588 0.4467 0.07343
|
2026 7 27 61248 0.2112 0.4193 0.08491
|
||||||
2026 6 16 61207 0.1602 0.4464 0.07452
|
2026 7 28 61249 0.2120 0.4183 0.08647
|
||||||
2026 6 17 61208 0.1616 0.4461 0.07551
|
2026 7 29 61250 0.2128 0.4173 0.08786
|
||||||
2026 6 18 61209 0.1631 0.4458 0.07621
|
2026 7 30 61251 0.2136 0.4162 0.08904
|
||||||
2026 6 19 61210 0.1645 0.4454 0.07668
|
2026 7 31 61252 0.2143 0.4151 0.08998
|
||||||
2026 6 20 61211 0.1659 0.4450 0.07691
|
2026 8 1 61253 0.2151 0.4141 0.09076
|
||||||
2026 6 21 61212 0.1673 0.4446 0.07694
|
2026 8 2 61254 0.2158 0.4130 0.09130
|
||||||
2026 6 22 61213 0.1686 0.4442 0.07699
|
2026 8 3 61255 0.2165 0.4119 0.09180
|
||||||
2026 6 23 61214 0.1700 0.4438 0.07723
|
2026 8 4 61256 0.2172 0.4108 0.09232
|
||||||
2026 6 24 61215 0.1714 0.4433 0.07764
|
2026 8 5 61257 0.2178 0.4096 0.09294
|
||||||
2026 6 25 61216 0.1727 0.4428 0.07823
|
2026 8 6 61258 0.2184 0.4085 0.09372
|
||||||
2026 6 26 61217 0.1741 0.4423 0.07906
|
2026 8 7 61259 0.2190 0.4074 0.09466
|
||||||
2026 6 27 61218 0.1754 0.4418 0.08011
|
2026 8 8 61260 0.2196 0.4062 0.09591
|
||||||
2026 6 28 61219 0.1768 0.4412 0.08129
|
2026 8 9 61261 0.2202 0.4050 0.09728
|
||||||
2026 6 29 61220 0.1781 0.4406 0.08253
|
2026 8 10 61262 0.2207 0.4039 0.09868
|
||||||
2026 6 30 61221 0.1794 0.4400 0.08379
|
2026 8 11 61263 0.2212 0.4027 0.09988
|
||||||
2026 7 1 61222 0.1807 0.4394 0.08503
|
2026 8 12 61264 0.2217 0.4015 0.10077
|
||||||
2026 7 2 61223 0.1819 0.4388 0.08607
|
2026 8 13 61265 0.2221 0.4003 0.10141
|
||||||
2026 7 3 61224 0.1832 0.4381 0.08690
|
2026 8 14 61266 0.2226 0.3991 0.10182
|
||||||
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)
|
|
||||||
60837 -112.47 0.89 -11.70 0.14
|
|
||||||
60838 -112.54 0.89 -11.60 0.14
|
|
||||||
60839 -112.51 0.89 -11.35 0.14
|
|
||||||
60840 -112.45 0.32 -11.16 0.16
|
|
||||||
60841 -112.45 1.48 -11.11 0.30
|
|
||||||
60842 -112.49 1.48 -11.13 0.30
|
|
||||||
60843 -112.49 1.48 -11.15 0.30
|
|
||||||
60844 -112.49 1.20 -11.12 0.28
|
|
||||||
60845 -112.62 1.20 -11.03 0.28
|
|
||||||
60846 -112.97 1.20 -10.87 0.28
|
|
||||||
|
|
||||||
IERS Celestial Pole Offset Final Series
|
|
||||||
MJD dpsi deps
|
|
||||||
(msec. of arc)
|
|
||||||
60797 -109.8 -11.9
|
|
||||||
60798 -109.1 -12.1
|
|
||||||
60799 -108.8 -12.0
|
|
||||||
60800 -108.9 -11.6
|
|
||||||
60801 -109.3 -11.1
|
|
||||||
60802 -109.7 -10.8
|
|
||||||
60803 -109.8 -10.7
|
|
||||||
60804 -109.8 -10.6
|
|
||||||
60805 -109.7 -10.6
|
|
||||||
60806 -109.6 -10.8
|
|
||||||
60807 -109.5 -11.3
|
|
||||||
60808 -109.5 -11.7
|
|
||||||
60809 -109.6 -11.9
|
|
||||||
60810 -109.7 -11.8
|
|
||||||
60811 -109.7 -11.6
|
|
||||||
60812 -109.6 -11.6
|
|
||||||
60813 -109.5 -11.6
|
|
||||||
60814 -109.3 -11.6
|
|
||||||
60815 -109.2 -11.6
|
|
||||||
60816 -109.3 -11.6
|
|
||||||
60817 -109.4 -11.6
|
|
||||||
60818 -109.7 -11.4
|
|
||||||
60819 -110.2 -11.0
|
|
||||||
60820 -111.0 -10.7
|
|
||||||
60821 -111.6 -10.8
|
|
||||||
60822 -111.9 -11.1
|
|
||||||
60823 -111.8 -11.3
|
|
||||||
60824 -111.5 -11.6
|
|
||||||
60825 -110.9 -11.7
|
|
||||||
60826 -110.5 -11.8
|
|
||||||
60827 -110.5 -11.7
|
|
||||||
|
|
||||||
IAU2000A Celestial Pole Offset Series
|
|
||||||
MJD dX error dY error
|
|
||||||
(msec. of arc)
|
|
||||||
60837 0.432 0.352 -0.216 0.145
|
|
||||||
60838 0.439 0.352 -0.218 0.145
|
|
||||||
60839 0.448 0.352 -0.222 0.145
|
|
||||||
60840 0.457 0.128 -0.227 0.160
|
|
||||||
60841 0.465 0.588 -0.235 0.303
|
|
||||||
60842 0.472 0.588 -0.243 0.303
|
|
||||||
60843 0.476 0.588 -0.251 0.303
|
|
||||||
60844 0.476 0.476 -0.260 0.282
|
|
||||||
60845 0.472 0.476 -0.267 0.282
|
|
||||||
60846 0.467 0.476 -0.275 0.282
|
|
||||||
|
|
||||||
|
|
||||||
IAU2000A Celestial Pole Offset Final Series
|
|
||||||
MJD dX dY
|
|
||||||
(msec. of arc)
|
|
||||||
60797 0.44 -0.19
|
|
||||||
60798 0.51 -0.19
|
|
||||||
60799 0.53 -0.19
|
|
||||||
60800 0.52 -0.18
|
|
||||||
60801 0.49 -0.17
|
|
||||||
60802 0.41 -0.16
|
|
||||||
60803 0.33 -0.14
|
|
||||||
60804 0.31 -0.16
|
|
||||||
60805 0.34 -0.21
|
|
||||||
60806 0.39 -0.26
|
|
||||||
60807 0.44 -0.30
|
|
||||||
60808 0.49 -0.33
|
|
||||||
60809 0.48 -0.26
|
|
||||||
60810 0.45 -0.17
|
|
||||||
60811 0.42 -0.09
|
|
||||||
60812 0.42 -0.07
|
|
||||||
60813 0.43 -0.10
|
|
||||||
60814 0.44 -0.14
|
|
||||||
60815 0.45 -0.18
|
|
||||||
60816 0.43 -0.21
|
|
||||||
60817 0.40 -0.22
|
|
||||||
60818 0.37 -0.22
|
|
||||||
60819 0.37 -0.21
|
|
||||||
60820 0.38 -0.19
|
|
||||||
60821 0.39 -0.16
|
|
||||||
60822 0.40 -0.14
|
|
||||||
60823 0.41 -0.12
|
|
||||||
60824 0.41 -0.11
|
|
||||||
60825 0.41 -0.11
|
|
||||||
60826 0.39 -0.14
|
|
||||||
60827 0.34 -0.18
|
|
||||||
|
|
||||||
)--";
|
)--";
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -23,12 +23,33 @@
|
|||||||
namespace mcc
|
namespace mcc
|
||||||
{
|
{
|
||||||
|
|
||||||
|
// adaptor class for prohibited zones
|
||||||
|
template <traits::mcc_mount_telemetry_data_c TelemetryDataT>
|
||||||
|
struct MccPZoneWrapper {
|
||||||
|
// a type to which the result of calling prohibited zone class methods 'timeTo' and 'timeFrom' will be converted
|
||||||
|
typedef std::chrono::duration<double> pz_duration_t; // seconds as floating-point number
|
||||||
|
|
||||||
|
static constexpr pz_duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
||||||
|
static constexpr pz_duration_t zeroDuration{0.0};
|
||||||
|
|
||||||
|
typedef std::function<bool(const TelemetryDataT&)> pz_inzone_func_t;
|
||||||
|
typedef std::function<pz_duration_t(const TelemetryDataT&)> pz_timeto_func_t;
|
||||||
|
typedef std::function<pz_duration_t(const TelemetryDataT&)> pz_timefrom_func_t;
|
||||||
|
|
||||||
|
MccCoordPairKind coordPairKind;
|
||||||
|
const std::function<std::string()> name;
|
||||||
|
pz_inzone_func_t inZone;
|
||||||
|
pz_timeto_func_t timeTo;
|
||||||
|
pz_timefrom_func_t timeFrom;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
template <traits::mcc_mount_controls_c MOUNT_CONTROLS>
|
template <traits::mcc_mount_controls_c MOUNT_CONTROLS>
|
||||||
class MccMount : public fsm::MccFiniteStateMachine, public utils::MccSpdlogLogger, protected MOUNT_CONTROLS
|
class MccMount : public fsm::MccFiniteStateMachine, public utils::MccSpdlogLogger, protected MOUNT_CONTROLS
|
||||||
{
|
{
|
||||||
// declare these classes as friends to allow them access protected members 'slewModel' and 'guidingModel'
|
// declare these classes as friends to allow them access protected members 'slewModel' and 'guidingModel'
|
||||||
friend class MccMountStateSlew<MccMount>;
|
// friend class MccMountStateSlew<MccMount<MOUNT_CONTROLS>>;
|
||||||
friend class MccMountStateGuiding<MccMount>;
|
// friend class MccMountStateGuiding<MccMount<MOUNT_CONTROLS>>;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef MOUNT_CONTROLS mount_controls_t;
|
typedef MOUNT_CONTROLS mount_controls_t;
|
||||||
@@ -41,8 +62,49 @@ public:
|
|||||||
typedef decltype(mount_controls_t::slewModel) slew_model_t;
|
typedef decltype(mount_controls_t::slewModel) slew_model_t;
|
||||||
typedef decltype(mount_controls_t::guidingModel) guiding_model_t;
|
typedef decltype(mount_controls_t::guidingModel) guiding_model_t;
|
||||||
|
|
||||||
typedef typename slew_model_t::slew_params_t slew_params_t;
|
// typedef typename slew_model_t::slew_params_t slew_params_t;
|
||||||
|
|
||||||
|
/* base classes for event and state */
|
||||||
|
|
||||||
|
class MccMountEventBase
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
MccMount& _mount;
|
||||||
|
|
||||||
|
public:
|
||||||
|
MccMountEventBase(MccMount& mount) : _mount(mount) {}
|
||||||
|
virtual ~MccMountEventBase() = default;
|
||||||
|
|
||||||
|
MccMount& mount() { return _mount; }
|
||||||
|
};
|
||||||
|
|
||||||
|
// The implementation of FSM (see mcc_finite_state_machine.h) requires
|
||||||
|
// state to be a default constructible class, so one needs to hold
|
||||||
|
// a pointer to mount class to guarantee access to its private or protected
|
||||||
|
// members and methods
|
||||||
|
class MccMountStateBase
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
// a pointer to mount class to allow access to private/protected
|
||||||
|
// members/methods of MccMount class from descendent state-classes.
|
||||||
|
// the memory address can be obtained from a call of
|
||||||
|
// MccMountEventBase::mount method (see above)
|
||||||
|
MccMount* _mount;
|
||||||
|
|
||||||
|
public:
|
||||||
|
MccMountStateBase() = default;
|
||||||
|
|
||||||
|
MccMountStateBase(const MccMountStateBase&) = delete;
|
||||||
|
MccMountStateBase& operator=(const MccMountStateBase&) = delete;
|
||||||
|
|
||||||
|
// just as an example (ugly but I still cannot find a solution)
|
||||||
|
// template <std::derived_from<MccMountEventBase> EvT>
|
||||||
|
// void enter(EvT& event)
|
||||||
|
// {
|
||||||
|
// _mount = &event.mount();
|
||||||
|
// _mount->forEachPZone(...);
|
||||||
|
// }
|
||||||
|
};
|
||||||
|
|
||||||
/* constructors and destructor */
|
/* constructors and destructor */
|
||||||
|
|
||||||
@@ -64,25 +126,21 @@ public:
|
|||||||
logDebug("{}", std::string(r.begin(), r.end()));
|
logDebug("{}", std::string(r.begin(), r.end()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MccMount(const MccMount&) = delete;
|
||||||
|
MccMount& operator=(const MccMount&) = delete;
|
||||||
|
|
||||||
virtual ~MccMount()
|
virtual ~MccMount() { logDebug("Delete MccMount class instance: thread = {}", getThreadId()); }
|
||||||
{
|
|
||||||
logDebug("Delete MccMount class instance: thread = {}", getThreadId());
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* public methods */
|
/* public methods */
|
||||||
|
|
||||||
void initMount()
|
void initMount() { this->logInfo("STATE: {}", this->currentStateID()); }
|
||||||
{
|
|
||||||
this->logInfo("STATE: {}", this->currentStateID());
|
|
||||||
}
|
|
||||||
|
|
||||||
void stopMount() {}
|
void stopMount() {}
|
||||||
|
|
||||||
void shutdownMount() {}
|
void shutdownMount() {}
|
||||||
|
|
||||||
void slewMount(slew_params_t params) {}
|
// void slewMount(slew_params_t params) {}
|
||||||
|
|
||||||
void startGuiding() {}
|
void startGuiding() {}
|
||||||
|
|
||||||
@@ -109,15 +167,34 @@ public:
|
|||||||
{
|
{
|
||||||
auto zone_ptr = std::make_shared<ZT>(std::move(zone));
|
auto zone_ptr = std::make_shared<ZT>(std::move(zone));
|
||||||
|
|
||||||
|
using d_t = typename MccPZoneWrapper<mount_telemetry_data_t>::pz_duration_t;
|
||||||
|
|
||||||
_pzFuncs.emplace_back(
|
_pzFuncs.emplace_back(
|
||||||
{.coordPairKind = ZT::zoneCoordPairKind,
|
{.coordPairKind = ZT::zoneCoordPairKind,
|
||||||
.name = std::format("{}", zone_ptr->name()),
|
.name = [zone_ptr]() { return std::format("{}", zone_ptr->name()); },
|
||||||
.inZoneFunc = [zone_ptr,
|
.inZone = [zone_ptr](const mount_telemetry_data_t& tmry_data) { return zone_ptr->inZone(tmry_data); },
|
||||||
this](const mount_telemetry_data_t& tmry_data) { return zone_ptr->inZone(tmry_data); },
|
.timeTo =
|
||||||
.timeToFunc = [zone_ptr,
|
[zone_ptr](const mount_telemetry_data_t& tmry_data) {
|
||||||
this](const mount_telemetry_data_t& tmry_data) { return zone_ptr->timeTo(tmry_data); },
|
auto d = zone_ptr->timeTo(tmry_data);
|
||||||
.timeFromFunc =
|
|
||||||
[zone_ptr, this](const mount_telemetry_data_t& tmry_data) { return zone_ptr->timeFrom(tmry_data); }});
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper<mount_telemetry_data_t>::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper<mount_telemetry_data_t>::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
},
|
||||||
|
.timeFrom =
|
||||||
|
[zone_ptr](const mount_telemetry_data_t& tmry_data) {
|
||||||
|
auto d = zone_ptr->timeFrom(tmry_data);
|
||||||
|
|
||||||
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper<mount_telemetry_data_t>::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper<mount_telemetry_data_t>::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
}});
|
||||||
|
|
||||||
|
|
||||||
if constexpr (sizeof...(ZTs)) {
|
if constexpr (sizeof...(ZTs)) {
|
||||||
@@ -150,8 +227,8 @@ protected:
|
|||||||
pz_timefrom_func_t timeFromFunc;
|
pz_timefrom_func_t timeFromFunc;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::vector<pz_funcs_t> _pzFuncs{};
|
// std::vector<pz_funcs_t> _pzFuncs{};
|
||||||
|
std::vector<MccPZoneWrapper<mount_telemetry_data_t>> _pzFuncs{};
|
||||||
|
|
||||||
template <typename FuncT, traits::mcc_prohibited_zone_c<mount_telemetry_data_t>... ZTs>
|
template <typename FuncT, traits::mcc_prohibited_zone_c<mount_telemetry_data_t>... ZTs>
|
||||||
auto forEachPZone(FuncT&& func, std::tuple<ZTs...>& zones)
|
auto forEachPZone(FuncT&& func, std::tuple<ZTs...>& zones)
|
||||||
|
|||||||
282
cxx/mcc_generic_mount.h
Normal file
282
cxx/mcc_generic_mount.h
Normal file
@@ -0,0 +1,282 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
|
||||||
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* A GENERIC MOUNT CLASS IMPLEMENTATION */
|
||||||
|
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include "mcc_mount_concepts.h"
|
||||||
|
|
||||||
|
namespace mcc
|
||||||
|
{
|
||||||
|
|
||||||
|
// adaptor class for prohibited zones
|
||||||
|
template <traits::mcc_mount_telemetry_data_c TelemetryDataT>
|
||||||
|
struct MccPZoneWrapper {
|
||||||
|
// a type to which the result of calling prohibited zone class methods 'timeTo' and 'timeFrom' will be converted
|
||||||
|
typedef std::chrono::duration<double> duration_t; // seconds as floating-point number
|
||||||
|
|
||||||
|
static constexpr duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
||||||
|
static constexpr duration_t zeroDuration{0.0};
|
||||||
|
|
||||||
|
typedef std::function<bool(const TelemetryDataT&)> pz_inzone_func_t;
|
||||||
|
typedef std::function<duration_t(const TelemetryDataT&)> pz_timeto_func_t;
|
||||||
|
typedef std::function<duration_t(const TelemetryDataT&)> pz_timefrom_func_t;
|
||||||
|
|
||||||
|
MccCoordPairKind coordPairKind;
|
||||||
|
const std::function<std::string()> name;
|
||||||
|
pz_inzone_func_t inZone;
|
||||||
|
pz_timeto_func_t timeTo;
|
||||||
|
pz_timefrom_func_t timeFrom;
|
||||||
|
|
||||||
|
MccPZoneWrapper(MccPZoneWrapper&& other)
|
||||||
|
: inZone(std::move(other.inZone)), timeTo(std::move(other.timeTo)), timeFrom(std::move(other.timeFrom)) {};
|
||||||
|
|
||||||
|
MccPZoneWrapper& operator=(MccPZoneWrapper&& other)
|
||||||
|
{
|
||||||
|
if (this != &other) {
|
||||||
|
inZone = std::move(other.inZone);
|
||||||
|
timeTo = std::move(other.timeTo);
|
||||||
|
timeFrom = std::move(other.timeFrom);
|
||||||
|
}
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
};
|
||||||
|
|
||||||
|
MccPZoneWrapper() = default;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T,
|
||||||
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
||||||
|
traits::mcc_mount_pec_c PEC_T,
|
||||||
|
traits::mcc_mount_telemetry_c TELEMETRY_T,
|
||||||
|
traits::mcc_slew_model_c SLEWMODEL_T,
|
||||||
|
traits::mcc_guiding_model_c GUIDINGMODEL_T>
|
||||||
|
class MccGenericMount
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
typedef ASTROM_ENGINE_T astrom_engine_t;
|
||||||
|
typedef HARDWARE_T hardware_t;
|
||||||
|
typedef PEC_T pec_t;
|
||||||
|
typedef TELEMETRY_T telemetry_t;
|
||||||
|
typedef typename TELEMETRY_T::mount_telemetry_data_t telemetry_data_t;
|
||||||
|
typedef SLEWMODEL_T slew_model_t;
|
||||||
|
typedef GUIDINGMODEL_T guiding_model_t;
|
||||||
|
|
||||||
|
typedef typename MccPZoneWrapper<telemetry_data_t>::duration_t pz_duration_t;
|
||||||
|
|
||||||
|
static constexpr MccMountType mountType = pec_t::mountType;
|
||||||
|
|
||||||
|
MccGenericMount(ASTROM_ENGINE_T aengine,
|
||||||
|
HARDWARE_T hw,
|
||||||
|
PEC_T pec,
|
||||||
|
TELEMETRY_T tmtry,
|
||||||
|
SLEWMODEL_T smodel,
|
||||||
|
GUIDINGMODEL_T gmodel)
|
||||||
|
: _astromEngine(std::move(aengine)),
|
||||||
|
_hardware(std::move(hw)),
|
||||||
|
_pec(std::move(pec)),
|
||||||
|
_telemetry(std::move(tmtry)),
|
||||||
|
_slewModel(std::move(smodel)),
|
||||||
|
_guidingModel(std::move(gmodel))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~MccGenericMount() = default;
|
||||||
|
|
||||||
|
// get telemetry data
|
||||||
|
auto mountTelemetryData(telemetry_data_t& data) { return _telemetry.data(data); }
|
||||||
|
|
||||||
|
|
||||||
|
/* prohibited zone related public methods */
|
||||||
|
|
||||||
|
// add zones to mount control system
|
||||||
|
template <traits::mcc_prohibited_zone_c<telemetry_data_t> ZT,
|
||||||
|
traits::mcc_prohibited_zone_c<telemetry_data_t>... ZTs>
|
||||||
|
size_t pzAddZone(ZT zone, ZTs... zones)
|
||||||
|
{
|
||||||
|
auto zone_ptr = std::make_shared<ZT>(std::move(zone));
|
||||||
|
|
||||||
|
using d_t = typename MccPZoneWrapper<telemetry_data_t>::duration_t;
|
||||||
|
|
||||||
|
_pzFuncs.emplace_back(
|
||||||
|
{.coordPairKind = ZT::zoneCoordPairKind,
|
||||||
|
.name = [zone_ptr]() { return std::format("{}", zone_ptr->name()); },
|
||||||
|
.inZone = [zone_ptr](const telemetry_data_t& tmry_data) { return zone_ptr->inZone(tmry_data); },
|
||||||
|
.timeTo =
|
||||||
|
[zone_ptr](const telemetry_data_t& tmry_data) {
|
||||||
|
auto d = zone_ptr->timeTo(tmry_data);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<typename ZT::duration_t, d_t>) {
|
||||||
|
return d;
|
||||||
|
} else {
|
||||||
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper<telemetry_data_t>::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper<telemetry_data_t>::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
}
|
||||||
|
},
|
||||||
|
.timeFrom =
|
||||||
|
[zone_ptr](const telemetry_data_t& tmry_data) {
|
||||||
|
auto d = zone_ptr->timeFrom(tmry_data);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<typename ZT::duration_t, d_t>) {
|
||||||
|
return d;
|
||||||
|
} else {
|
||||||
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper<telemetry_data_t>::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper<telemetry_data_t>::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
}
|
||||||
|
}});
|
||||||
|
|
||||||
|
|
||||||
|
if constexpr (sizeof...(ZTs)) {
|
||||||
|
pzAddZone(std::move(zones)...);
|
||||||
|
}
|
||||||
|
|
||||||
|
return _pzFuncs.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
// delete all zones from mount control system
|
||||||
|
void pzClearZone()
|
||||||
|
{
|
||||||
|
// stop mount here?!!
|
||||||
|
_pzFuncs.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
// prohibited zones timeTo from given time point
|
||||||
|
template <std::ranges::output_range<pz_duration_t> RT, traits::mcc_celestial_point_c CT, traits::mcc_systime_c TPT>
|
||||||
|
RT pzTimeTo(CT coord, const TPT& time_point)
|
||||||
|
requires std::convertible_to<TPT, typename astrom_engine_t::time_point_t>
|
||||||
|
{
|
||||||
|
RT res;
|
||||||
|
|
||||||
|
telemetry_data_t tdata;
|
||||||
|
typename astrom_engine_t::juldate_t jd;
|
||||||
|
|
||||||
|
_astromEngine.greg2jul(time_point, jd);
|
||||||
|
|
||||||
|
if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's
|
||||||
|
typename pec_t::pec_result_t pec_res;
|
||||||
|
auto pec_err = _pec.compute(coord.x, coord.y, pec_res);
|
||||||
|
if (!pec_err) {
|
||||||
|
if constexpr (mccIsEquatorialMount(mountType)) {
|
||||||
|
tdata.mntHA = coord.x + pec_res.dx;
|
||||||
|
tdata.mntDEC = coord.y + pec_res.dy;
|
||||||
|
_astromEngine.hadec2azalt(tdata.mntHA, tdata.mntDEC, tdata.mntAZ, tdata.mntALT);
|
||||||
|
} else if constexpr (mccIsAltAzMount(mountType)) {
|
||||||
|
tdata.mntAZ = coord.x + pec_res.dx;
|
||||||
|
tdata.mntALT = coord.y + pec_res.dy;
|
||||||
|
_astromEngine.azalt2hadec(tdata.mntAZ, tdata.mntALT, tdata.mntHA, tdata.mntDEC);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { // from app RA-DEC
|
||||||
|
typename astrom_engine_t::eo_t eo;
|
||||||
|
typename astrom_engine_t::sideral_time_t lst;
|
||||||
|
|
||||||
|
_astromEngine.apparentSiderTime(jd, lst, true);
|
||||||
|
_astromEngine.eqOrigins(jd, eo);
|
||||||
|
|
||||||
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { // from app HA-DEC
|
||||||
|
tdata.mntHA = coord.x;
|
||||||
|
tdata.mntDEC = coord.y;
|
||||||
|
_astromEngine.hadec2azalt(tdata.mntHA, tdata.mntDEC, tdata.mntAZ, tdata.mntALT);
|
||||||
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT
|
||||||
|
tdata.mntAZ = coord.x;
|
||||||
|
tdata.mntALT = coord.y;
|
||||||
|
_astromEngine.azalt2hadec(tdata.mntAZ, tdata.mntALT, tdata.mntHA, tdata.mntDEC);
|
||||||
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZZD) { // from app AZ-ZD
|
||||||
|
tdata.mntAZ = coord.x;
|
||||||
|
tdata.mntALT = std::numbers::pi / 2.0 - coord.y;
|
||||||
|
_astromEngine.azalt2hadec(tdata.mntAZ, tdata.mntALT, tdata.mntHA, tdata.mntDEC);
|
||||||
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // from ICRS RA-DEC
|
||||||
|
typename astrom_engine_t::coord_t az, alt;
|
||||||
|
typename astrom_engine_t::eo_t eo;
|
||||||
|
typename astrom_engine_t::juldate_t jd;
|
||||||
|
|
||||||
|
_astromEngine.greg2jul(time_point, jd);
|
||||||
|
_astromEngine.icrs2obs(coord.x, coord.y, jd, tdata.mntRA, tdata.mntDEC, tdata.mntHA, tdata.mntAZ,
|
||||||
|
tdata.mntALT, eo);
|
||||||
|
} else {
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <traits::mcc_celestial_point_c CT, traits::mcc_systime_c TPT>
|
||||||
|
auto pzTimeTo(CT point, const TPT& time_point)
|
||||||
|
{
|
||||||
|
return pzTimeTo<std::vector<pz_duration_t>>(std::move(point), time_point);
|
||||||
|
}
|
||||||
|
|
||||||
|
// prohibited zones timeTo from current mount position
|
||||||
|
template <std::ranges::output_range<pz_duration_t> RT, traits::mcc_celestial_point_c CT>
|
||||||
|
RT pzTimeTo(CT point)
|
||||||
|
{
|
||||||
|
RT result;
|
||||||
|
|
||||||
|
telemetry_data_t data;
|
||||||
|
auto err = _telemetry.data(data);
|
||||||
|
if (err) {
|
||||||
|
// log...
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::ranges::for_each(_pzFuncs,
|
||||||
|
[&result, &data](auto& funcs) { std::back_inserter(result) = funcs.timeTo(data); });
|
||||||
|
}
|
||||||
|
|
||||||
|
template <traits::mcc_celestial_point_c CT>
|
||||||
|
auto pzTimeTo(CT point)
|
||||||
|
{
|
||||||
|
return pzTimeTo<std::vector<pz_duration_t>>(std::move(point));
|
||||||
|
}
|
||||||
|
|
||||||
|
// prohibited zone timeFrom
|
||||||
|
|
||||||
|
protected:
|
||||||
|
ASTROM_ENGINE_T _astromEngine;
|
||||||
|
HARDWARE_T _hardware;
|
||||||
|
PEC_T _pec;
|
||||||
|
TELEMETRY_T _telemetry;
|
||||||
|
SLEWMODEL_T _slewModel;
|
||||||
|
GUIDINGMODEL_T _guidingModel;
|
||||||
|
|
||||||
|
std::vector<MccPZoneWrapper<telemetry_data_t>> _pzFuncs{};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
namespace traits
|
||||||
|
{
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_generic_mount_c = requires(T t) {
|
||||||
|
// derived from MccGenericMount
|
||||||
|
[]<typename... Ts>(MccGenericMount<Ts...>*) {}(&t);
|
||||||
|
|
||||||
|
{ t.slewMount(std::declval<typename T::slew_model_t::slew_point_t>()) };
|
||||||
|
{ t.startGuiding(std::declval<typename T::guiding_model_t::guiding_point_t>()) };
|
||||||
|
|
||||||
|
{ t.stop() };
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_generic_fsm_mount_c = mcc_generic_mount_c<T> && std::derived_from<T, fsm::MccFiniteStateMachine>;
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace traits
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace mcc
|
||||||
@@ -7,9 +7,9 @@
|
|||||||
|
|
||||||
|
|
||||||
#include "mcc_mount_concepts.h"
|
#include "mcc_mount_concepts.h"
|
||||||
|
#include "mcc_mount_telemetry.h"
|
||||||
#include "mcc_slew_guiding_model_common.h"
|
#include "mcc_slew_guiding_model_common.h"
|
||||||
|
|
||||||
|
|
||||||
namespace mcc
|
namespace mcc
|
||||||
{
|
{
|
||||||
|
|
||||||
@@ -24,6 +24,7 @@ enum class MccSimpleGuidingModelErrorCode : int {
|
|||||||
ERROR_INVALID_CONTEXT_PARAM,
|
ERROR_INVALID_CONTEXT_PARAM,
|
||||||
ERROR_INVALID_THRESH,
|
ERROR_INVALID_THRESH,
|
||||||
ERROR_INVALID_CORR_RANGE,
|
ERROR_INVALID_CORR_RANGE,
|
||||||
|
ERROR_GUIDING_STOPPED
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
@@ -76,6 +77,8 @@ struct MccSimpleGuidingModelCategory : public std::error_category {
|
|||||||
return "guiding model: invalid guiding residual threshold";
|
return "guiding model: invalid guiding residual threshold";
|
||||||
case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CORR_RANGE:
|
case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CORR_RANGE:
|
||||||
return "guiding model: invalid guiding correction range";
|
return "guiding model: invalid guiding correction range";
|
||||||
|
case MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED:
|
||||||
|
return "guiding model: stopped";
|
||||||
default:
|
default:
|
||||||
return "UNKNOWN";
|
return "UNKNOWN";
|
||||||
}
|
}
|
||||||
@@ -96,31 +99,6 @@ inline std::error_code make_error_code(MccSimpleGuidingModelErrorCode ec)
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* */
|
|
||||||
|
|
||||||
class MccCelestialPointTrack final
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_time_duration_c DT>
|
|
||||||
MccCelestialPointTrack(ASTROM_ENGINE_T& astrom_engine,
|
|
||||||
typename ASTROM_ENGINE_T::juldate_t start,
|
|
||||||
DT step,
|
|
||||||
size_t Npoints)
|
|
||||||
{
|
|
||||||
const auto p_astrom_engine = &astrom_engine;
|
|
||||||
|
|
||||||
_compFunc = []() {
|
|
||||||
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
std::function<size_t()> _compFunc;
|
|
||||||
};
|
|
||||||
|
|
||||||
/* */
|
|
||||||
|
|
||||||
|
|
||||||
template <traits::mcc_logger_c LoggerT = MccNullLogger>
|
template <traits::mcc_logger_c LoggerT = MccNullLogger>
|
||||||
class MccSimpleGuidingModel : public LoggerT
|
class MccSimpleGuidingModel : public LoggerT
|
||||||
{
|
{
|
||||||
@@ -136,34 +114,56 @@ public:
|
|||||||
|
|
||||||
typedef std::error_code error_t;
|
typedef std::error_code error_t;
|
||||||
|
|
||||||
struct guiding_context_t {
|
|
||||||
double corrThresh{MccAngle("00:00:00.2"_dms)}; // correction threshold
|
|
||||||
double correctionRange[2]{MccAngle("00:00:00.5"_dms), MccAngle("00:00:05"_dms)};
|
|
||||||
|
|
||||||
std::chrono::duration<double> predictedTrackDuration{10.0}; // 10 seconds
|
using guiding_point_t = MccSlewAndGuidingPoint;
|
||||||
std::chrono::duration<double> predictedTrackResolution{0.1}; // 0.1 seconds
|
|
||||||
};
|
|
||||||
|
|
||||||
struct guiding_point_t : MccCelestialPoint {
|
template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
||||||
coord_t corrThresh{(double)MccAngle("00:00:00.2"_dms)}; // correction threshold
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
||||||
coord_t correctionRange[2]{(double)MccAngle(0.5_arcsecs), (double)MccAngle(5.0_arcsecs)};
|
traits::mcc_irange_of_pzones_c<typename TELEMETRY_T::mount_telemetry_data_t> PZ_T,
|
||||||
};
|
// traits::mcc_tuple_c PZ_T,
|
||||||
|
typename... LoggerCtorArgTs>
|
||||||
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T, typename... LoggerCtorArgTs>
|
MccSimpleGuidingModel(TELEMETRY_T* telemetry,
|
||||||
MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context, LoggerCtorArgTs&&... ctor_args)
|
HARDWARE_T* hardware,
|
||||||
|
PZ_T* prohibited_zone,
|
||||||
|
LoggerCtorArgTs&&... ctor_args)
|
||||||
requires(!std::same_as<LoggerT, MccNullLogger>)
|
requires(!std::same_as<LoggerT, MccNullLogger>)
|
||||||
: LoggerT(std::forward<LoggerCtorArgTs>(ctor_args)...)
|
: LoggerT(std::forward<LoggerCtorArgTs>(ctor_args)...)
|
||||||
{
|
{
|
||||||
logDebug(std::format("Create 'MccSimpleGuidingModel' class instance ({})", (void*)this));
|
logDebug(std::format("Create 'MccSimpleGuidingModel' class instance ({})", (void*)this));
|
||||||
|
|
||||||
init(mount_controls, std::move(context));
|
init(telemetry, hardware, prohibited_zone);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
|
// template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
||||||
MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context)
|
// traits::mcc_mount_hardware_c HARDWARE_T,
|
||||||
requires(std::same_as<LoggerT, MccNullLogger>)
|
// traits::mcc_irange_of_pzones_c<typename TELEMETRY_T::mount_telemetry_data_t> PZ_T
|
||||||
|
// // traits::mcc_tuple_c PZ_T
|
||||||
|
// >
|
||||||
|
// MccSimpleGuidingModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
|
||||||
|
// requires(std::same_as<LoggerT, MccNullLogger>)
|
||||||
|
// {
|
||||||
|
// init(telemetry, hardware, prohibited_zone);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
MccSimpleGuidingModel(MccSimpleGuidingModel&& other)
|
||||||
|
: _guidingFunc(std::move(other._guidingFunc)),
|
||||||
|
_doCorrection(other._doCorrection.load()),
|
||||||
|
_stopRequested(other._stopRequested.load())
|
||||||
{
|
{
|
||||||
init(mount_controls, std::move(context));
|
}
|
||||||
|
|
||||||
|
MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&& other)
|
||||||
|
{
|
||||||
|
if (this == &other) {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
_guidingFunc = std::move(other._guidingFunc);
|
||||||
|
_doCorrection = other._doCorrection.load();
|
||||||
|
_stopRequested = other._stopRequested.load();
|
||||||
|
|
||||||
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual ~MccSimpleGuidingModel()
|
virtual ~MccSimpleGuidingModel()
|
||||||
@@ -180,6 +180,8 @@ public:
|
|||||||
error_t stopGuiding(bool off)
|
error_t stopGuiding(bool off)
|
||||||
{
|
{
|
||||||
_doCorrection = off;
|
_doCorrection = off;
|
||||||
|
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool inGuiding()
|
bool inGuiding()
|
||||||
@@ -187,257 +189,66 @@ public:
|
|||||||
return _doCorrection;
|
return _doCorrection;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
error_t stop()
|
||||||
|
{
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::function<error_t(guiding_point_t)> _guidingFunc{};
|
std::function<error_t(guiding_point_t)> _guidingFunc{};
|
||||||
std::atomic_bool _doCorrection{true};
|
std::atomic_bool _doCorrection{true};
|
||||||
|
std::atomic_bool _stopRequested{false};
|
||||||
|
|
||||||
|
|
||||||
error_t init(auto& mount_controls, guiding_context_t context)
|
|
||||||
|
void init(auto* p_telemetry, auto* p_hardware, auto* p_prohibited_zones)
|
||||||
{
|
{
|
||||||
// deduce controls types
|
// deduce controls types
|
||||||
using astrom_engine_t = decltype(mount_controls.astrometryEngine);
|
// deduce controls types
|
||||||
using hardware_t = decltype(mount_controls.hardware);
|
using hardware_t = decltype(*p_hardware);
|
||||||
using pec_t = decltype(mount_controls.PEC);
|
using telemetry_t = decltype(*p_telemetry);
|
||||||
using telemetry_t = decltype(mount_controls.telemetry);
|
static_assert(traits::mcc_mount_default_telemetry_c<telemetry_t>,
|
||||||
using tpl_pz_t = decltype(mount_controls.prohibitedZones);
|
"TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry'!");
|
||||||
|
|
||||||
static constexpr size_t Nzones = std::tuple_size_v<tpl_pz_t>;
|
using astrom_engine_t = typename telemetry_t::astrom_engine_t;
|
||||||
|
|
||||||
size_t predicted_Npoints = context.predictedTrackDuration / context.predictedTrackResolution;
|
static constexpr size_t Nzones = std::tuple_size_v<decltype(*p_prohibited_zones)>;
|
||||||
if (predicted_Npoints == 0) {
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_CONTEXT_PARAM;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto resi_thresh2 = context.corrThresh * context.corrThresh;
|
// const auto p_telemetry = &telemetry;
|
||||||
|
// const auto p_hardware = &hardware;
|
||||||
|
// const auto p_prohibited_zones = &prohibited_zones;
|
||||||
|
|
||||||
if (utils::isEqual(resi_thresh2, 0.0)) {
|
_guidingFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](guiding_point_t guiding_point) {
|
||||||
|
_stopRequested = false;
|
||||||
|
|
||||||
|
if (guiding_point.correctionRange[0] >= guiding_point.correctionRange[1]) {
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
||||||
}
|
}
|
||||||
|
|
||||||
const auto p_mount_controls = &mount_controls;
|
auto low_corr_limit = guiding_point.correctionRange[0] * guiding_point.correctionRange[0];
|
||||||
|
auto high_corr_limit = guiding_point.correctionRange[1] * guiding_point.correctionRange[1];
|
||||||
|
|
||||||
auto check_zones = [p_mount_controls, this]() {
|
// using coord_t = typename astrom_engine_t::coord_t;
|
||||||
return [this]<size_t... Is>(std::index_sequence<Is...>) {
|
// using jd_t = typename astrom_engine_t::juldate_t;
|
||||||
error_t ret;
|
// jd_t jd;
|
||||||
|
|
||||||
(
|
|
||||||
[&ret]() {
|
|
||||||
if constexpr (Is > 0) {
|
|
||||||
if (ret) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
typename telemetry_t::mount_telemetry_data_t tdata;
|
|
||||||
|
|
||||||
auto tel_err = p_mount_controls->telemetry.data(tdata);
|
|
||||||
if (tel_err) {
|
|
||||||
if constexpr (std::same_as<decltype(tel_err), error_t>) {
|
|
||||||
ret = tel_err;
|
|
||||||
} else {
|
|
||||||
ret = MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
ret = std::get<Is>(p_mount_controls->prohibitedZones).inZone(tdata)
|
|
||||||
? MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE
|
|
||||||
: MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
||||||
if (ret) {
|
|
||||||
auto log_str = std::format("given coordinates are in prohibited zone '{}'",
|
|
||||||
std::get<Is>(p_mount_controls->prohibitedZones).name());
|
|
||||||
logError(log_str);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}(),
|
|
||||||
...);
|
|
||||||
|
|
||||||
|
|
||||||
return ret;
|
|
||||||
}(std::make_index_sequence<Nzones>{});
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
_guidingFunc = [p_mount_controls, context = std::move(context), predicted_Npoints, this](
|
|
||||||
this auto&& self, guiding_point_t guiding_point) {
|
|
||||||
if (context.correctionRange[0] >= context.correctionRange[1]) {
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto low_corr_limit = context.correctionRange[0] * context.correctionRange[0];
|
|
||||||
auto high_corr_limit = context.correctionRange[1] * context.correctionRange[1];
|
|
||||||
|
|
||||||
auto& astrom_engine = p_mount_controls->astrometryEngine;
|
|
||||||
auto& hardware = p_mount_controls->hardware;
|
|
||||||
auto& pec = p_mount_controls->PEC;
|
|
||||||
auto& telemetry = p_mount_controls->telemetry;
|
|
||||||
|
|
||||||
using coord_t = typename astrom_engine_t::coord_t;
|
|
||||||
using jd_t = typename astrom_engine_t::juldate_t;
|
|
||||||
jd_t jd;
|
|
||||||
|
|
||||||
|
|
||||||
error_t res_err;
|
error_t res_err;
|
||||||
typename astrom_engine_t::error_t ast_err;
|
typename astrom_engine_t::error_t ast_err;
|
||||||
typename pec_t::error_t pec_err;
|
// typename pec_t::error_t pec_err;
|
||||||
|
|
||||||
typename telemetry_t::error_t t_err;
|
typename telemetry_t::error_t t_err;
|
||||||
typename telemetry_t::mount_telemetry_data_t t_data;
|
typename telemetry_t::mount_telemetry_data_t t_data;
|
||||||
|
|
||||||
|
|
||||||
// first, compute ICRS coordinates of given guiding point
|
|
||||||
coord_t ra_icrs, dec_icrs;
|
|
||||||
|
|
||||||
const auto p_astrom_engine = &astrom_engine;
|
|
||||||
const auto p_pec = &pec;
|
|
||||||
|
|
||||||
auto predictedPos = [p_astrom_engine, predicted_Npoints, &context, &ra_icrs, &dec_icrs](
|
|
||||||
jd_t start, std::vector<guiding_point_t>& track) {
|
|
||||||
if (track.size() < predicted_Npoints) {
|
|
||||||
track.resize(predicted_Npoints);
|
|
||||||
}
|
|
||||||
|
|
||||||
coord_t ha, ra_app, dec_app, az, alt, eo;
|
|
||||||
typename astrom_engine_t::error_t ast_err;
|
|
||||||
typename pec_t::error_t pec_err;
|
|
||||||
typename pec_t::pec_result_t pec_res;
|
|
||||||
|
|
||||||
for (auto& g_point : track) {
|
|
||||||
ast_err = p_astrom_engine->icrs2obs(ra_icrs, dec_icrs, start, ra_app, dec_app, ha, az, alt, eo);
|
|
||||||
|
|
||||||
if (ast_err) {
|
|
||||||
if constexpr (std::same_as<decltype(ast_err), error_t>) {
|
|
||||||
logError(
|
|
||||||
std::format("An error occured while performing astrometry computations: code = {} ({})",
|
|
||||||
ast_err.value(), ast_err.message()));
|
|
||||||
return ast_err;
|
|
||||||
} else {
|
|
||||||
if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
|
|
||||||
logError(std::format(
|
|
||||||
"An error occured while performing astrometry computations: code = {}", ast_err));
|
|
||||||
}
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) { // use of HA and DEC
|
|
||||||
g_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
||||||
g_point.x = ha;
|
|
||||||
g_point.y = dec_app;
|
|
||||||
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) { // use of Az and Alt
|
|
||||||
g_point.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
||||||
g_point.x = az;
|
|
||||||
g_point.y = alt;
|
|
||||||
} else {
|
|
||||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
||||||
}
|
|
||||||
|
|
||||||
start.mjd += context.predictedTrackResolution.count() / 86400.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
||||||
}; // end of predictedPos lambda
|
|
||||||
|
|
||||||
if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
|
|
||||||
typename pec_t::pec_result_t pec_res;
|
|
||||||
|
|
||||||
pec_err = pec.compute(guiding_point.x, guiding_point.y, pec_res);
|
|
||||||
|
|
||||||
if (pec_err) {
|
|
||||||
if constexpr (std::same_as<decltype(pec_err), error_t>) {
|
|
||||||
logError(
|
|
||||||
std::format("An PEC error occured: code = {} ({})", pec_err.value(), pec_err.message()));
|
|
||||||
return pec_err;
|
|
||||||
} else {
|
|
||||||
if constexpr (traits::mcc_formattable<decltype(pec_err)>) {
|
|
||||||
logError(std::format("An PEC error occured: code = {}", pec_err));
|
|
||||||
}
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_PEC_COMP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) { // use of HA and DEC
|
|
||||||
guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
||||||
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) { // use of Az and Alt
|
|
||||||
guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
||||||
} else {
|
|
||||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
||||||
}
|
|
||||||
|
|
||||||
guiding_point.x += pec_res.dx; // app HA/Az
|
|
||||||
guiding_point.y += pec_res.dy; // app DEC/Alt
|
|
||||||
|
|
||||||
res_err = self(std::move(guiding_point));
|
|
||||||
if (res_err) {
|
|
||||||
return res_err;
|
|
||||||
}
|
|
||||||
} else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
|
||||||
} else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
|
||||||
} else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
|
|
||||||
} else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
|
|
||||||
} else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
|
||||||
ra_icrs = guiding_point.x;
|
|
||||||
dec_icrs = guiding_point.y;
|
|
||||||
} else {
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (guiding_point.coordPairKind != mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
|
||||||
ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd);
|
|
||||||
if (!ast_err) {
|
|
||||||
ast_err = astrom_engine.obs2icrs(guiding_point.coordPairKind, guiding_point.x, guiding_point.y, jd,
|
|
||||||
ra_icrs, dec_icrs);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ast_err) {
|
|
||||||
if constexpr (std::same_as<decltype(ast_err), error_t>) {
|
|
||||||
logError(
|
|
||||||
std::format("An error occured while performing astrometry computations: code = {} ({})",
|
|
||||||
ast_err.value(), ast_err.message()));
|
|
||||||
return ast_err;
|
|
||||||
} else {
|
|
||||||
if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
|
|
||||||
logError(std::format("An error occured while performing astrometry computations: code = {}",
|
|
||||||
ast_err));
|
|
||||||
}
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
coord_t ha, ra_app, dec_app, az, alt, eo;
|
|
||||||
coord_t xr, yr, coord_diff;
|
|
||||||
typename hardware_t::axes_pos_t ax_pos;
|
typename hardware_t::axes_pos_t ax_pos;
|
||||||
|
|
||||||
while (true) {
|
ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_GUIDING;
|
||||||
// check prohibited zones ...
|
|
||||||
if ((res_err = check_zones())) {
|
|
||||||
return res_err;
|
|
||||||
}
|
|
||||||
|
|
||||||
ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd);
|
t_err = p_telemetry->setTarget(guiding_point);
|
||||||
if (!ast_err) {
|
|
||||||
ast_err = astrom_engine.icrs2obs(ra_icrs, dec_icrs, jd, ra_app, dec_app, ha, az, alt, eo);
|
|
||||||
}
|
|
||||||
if (ast_err) {
|
|
||||||
if constexpr (std::same_as<decltype(ast_err), error_t>) {
|
|
||||||
logError(
|
|
||||||
std::format("An error occured while performing astrometry computations: code = {} ({})",
|
|
||||||
ast_err.value(), ast_err.message()));
|
|
||||||
return ast_err;
|
|
||||||
} else {
|
|
||||||
if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
|
|
||||||
logError(std::format("An error occured while performing astrometry computations: code = {}",
|
|
||||||
ast_err));
|
|
||||||
}
|
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
t_err = telemetry.data(t_data);
|
|
||||||
if (t_err) {
|
if (t_err) {
|
||||||
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||||
logError(
|
logError(std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
||||||
std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
|
||||||
return t_err;
|
return t_err;
|
||||||
} else {
|
} else {
|
||||||
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||||
@@ -447,42 +258,77 @@ protected:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare t_data with computed coordinates ...
|
std::array<bool, Nzones> in_zone_flag;
|
||||||
if (_doCorrection) {
|
|
||||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
while (true) {
|
||||||
xr = ha - t_data.mntHA;
|
if (_stopRequested) {
|
||||||
yr = dec_app - t_data.mntDEC;
|
// return MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED;
|
||||||
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
// interpetate stoping as 'no error' exit
|
||||||
xr = az - t_data.mntAZ;
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
yr = alt - t_data.mntALT;
|
|
||||||
} else {
|
|
||||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
coord_diff = xr * xr + yr * yr;
|
|
||||||
|
|
||||||
if (coord_diff < low_corr_limit) {
|
// suspend the thread here until telemetry data is updated
|
||||||
|
t_err = p_telemetry->waitForUpdatedData(t_data, guiding_point.telemetryUpdateTimeout);
|
||||||
|
|
||||||
|
ax_pos.x = t_data.mntPosX;
|
||||||
|
ax_pos.y = t_data.mntPosY;
|
||||||
|
|
||||||
|
// check prohibited zones ...
|
||||||
|
if (mccCheckInZonePZTuple(t_data, *p_prohibited_zones, in_zone_flag)) {
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
if (t_err) {
|
||||||
|
std::string err_str = "An error occured while waiting for updated telemetry";
|
||||||
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||||
|
std::format_to(std::back_inserter(err_str), ": code = {} ({})", t_err.value(), t_err.message());
|
||||||
|
logError(err_str);
|
||||||
|
return t_err;
|
||||||
|
} else {
|
||||||
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||||
|
std::format_to(std::back_inserter(err_str), ": code = {}", t_err.value());
|
||||||
|
}
|
||||||
|
logError(err_str);
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_stopRequested) {
|
||||||
|
// interpetate stoping as 'no error' exit
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
// compare t_data with computed coordinates ...
|
||||||
|
if (_doCorrection) {
|
||||||
|
auto coord_diff = p_telemetry->targetToMountDiff();
|
||||||
|
|
||||||
|
if (coord_diff.r2 < low_corr_limit) {
|
||||||
|
logWarn(
|
||||||
|
std::format("guiding model: the 'mount-target' square of difference is less than the limit "
|
||||||
|
"(diff = {}; lim = {}). Do not perfrom the corrections!",
|
||||||
|
(double)coord_diff.r2, (double)high_corr_limit));
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (coord_diff > high_corr_limit) {
|
if (coord_diff.r2 > high_corr_limit) {
|
||||||
logWarn(std::format(
|
logWarn(
|
||||||
"guiding model: the 'mount-target' difference exceeds the limit (diff = {}; lim = {})",
|
std::format("guiding model: the 'mount-target' square of difference exceeds the limit "
|
||||||
(double)coord_diff, (double)high_corr_limit));
|
"(diff = {}; lim = {})",
|
||||||
|
(double)coord_diff.r2, (double)high_corr_limit));
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// do correction
|
// do correction
|
||||||
ax_pos.state = hardware_t::hw_state_t::HW_STATE_TRACK; // indicates to hardware level
|
|
||||||
ax_pos.x = xr;
|
|
||||||
ax_pos.y = yr;
|
|
||||||
// ax_pos.x = t_data.mntPosX;
|
|
||||||
// ax_pos.y = t_data.mntPosY;
|
|
||||||
ax_pos.time_point = t_data.time_point;
|
ax_pos.time_point = t_data.time_point;
|
||||||
|
ax_pos.x += coord_diff.xdiff;
|
||||||
|
if (guiding_point.dualAxisGuiding) {
|
||||||
|
ax_pos.y += coord_diff.ydiff;
|
||||||
|
}
|
||||||
|
|
||||||
// asynchronous operation!
|
// asynchronous operation!
|
||||||
auto err = hardware.setPos(std::move(ax_pos));
|
auto err = p_hardware->setPos(ax_pos);
|
||||||
if (err) {
|
if (err) {
|
||||||
if constexpr (std::same_as<decltype(err), error_t>) {
|
if constexpr (std::same_as<decltype(err), error_t>) {
|
||||||
logError(
|
logError(
|
||||||
@@ -503,4 +349,6 @@ protected:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// static_assert(std::movable<MccSimpleGuidingModel<>>);
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -110,11 +110,6 @@ inline std::error_code make_error_code(MccMountAstromEngineERFAErrorCode ec)
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* A concept for ERFA-library compatible type to represent anglular quantities */
|
|
||||||
template <typename T>
|
|
||||||
concept mcc_erfa_angle_t = std::constructible_from<T, double> && std::convertible_to<T, double>;
|
|
||||||
|
|
||||||
template <mcc_erfa_angle_t AngleT = MccAngle>
|
|
||||||
class MccMountAstromEngineERFA
|
class MccMountAstromEngineERFA
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@@ -122,6 +117,9 @@ public:
|
|||||||
|
|
||||||
typedef std::error_code error_t;
|
typedef std::error_code error_t;
|
||||||
|
|
||||||
|
/* use of the same type for representation of celestial and geodetic coordinates, celestial angles (e.g. P.A.),
|
||||||
|
* and sideral time */
|
||||||
|
typedef double coord_t;
|
||||||
|
|
||||||
// meteo parameters (to compute refraction)
|
// meteo parameters (to compute refraction)
|
||||||
struct meteo_t {
|
struct meteo_t {
|
||||||
@@ -139,8 +137,8 @@ public:
|
|||||||
|
|
||||||
double wavelength = DEFAULT_WAVELENGTH; // observed wavelength in mkm
|
double wavelength = DEFAULT_WAVELENGTH; // observed wavelength in mkm
|
||||||
|
|
||||||
AngleT lat = "00:00:00"_dms; // site latitude
|
coord_t lat = "00:00:00"_dms; // site latitude
|
||||||
AngleT lon = "00:00:00"_dms; // site longitude
|
coord_t lon = "00:00:00"_dms; // site longitude
|
||||||
double elev = 0.0; // site elevation (in meters)
|
double elev = 0.0; // site elevation (in meters)
|
||||||
|
|
||||||
mcc::astrom::iers::MccLeapSeconds _leapSeconds{};
|
mcc::astrom::iers::MccLeapSeconds _leapSeconds{};
|
||||||
@@ -154,20 +152,11 @@ public:
|
|||||||
double mjd{51544.5}; // J2000.0
|
double mjd{51544.5}; // J2000.0
|
||||||
};
|
};
|
||||||
|
|
||||||
// typedef MccAngle coord_t;
|
|
||||||
|
|
||||||
// typedef MccAngle sideral_time_t;
|
|
||||||
// typedef MccAngle pa_t;
|
|
||||||
// typedef MccAngle eo_t;
|
|
||||||
|
|
||||||
|
|
||||||
/* use of the same type fro representation of celestial and geodetic coordinates, celestial angles (e.g. P.A.),
|
typedef coord_t sideral_time_t;
|
||||||
* sideral time */
|
typedef coord_t pa_t;
|
||||||
typedef AngleT coord_t;
|
typedef coord_t eo_t;
|
||||||
|
|
||||||
typedef AngleT sideral_time_t;
|
|
||||||
typedef AngleT pa_t;
|
|
||||||
typedef AngleT eo_t;
|
|
||||||
|
|
||||||
struct refract_result_t {
|
struct refract_result_t {
|
||||||
double refa, refb;
|
double refa, refb;
|
||||||
@@ -176,34 +165,51 @@ public:
|
|||||||
|
|
||||||
MccMountAstromEngineERFA() = default;
|
MccMountAstromEngineERFA() = default;
|
||||||
|
|
||||||
MccMountAstromEngineERFA(engine_state_t state) : _currentState(std::move(state)) {}
|
MccMountAstromEngineERFA(engine_state_t state) : _currentState(std::move(state)), _stateMutex(new std::mutex) {}
|
||||||
|
|
||||||
|
MccMountAstromEngineERFA(const MccMountAstromEngineERFA&) = delete;
|
||||||
|
MccMountAstromEngineERFA& operator=(const MccMountAstromEngineERFA&) = delete;
|
||||||
|
|
||||||
|
MccMountAstromEngineERFA(MccMountAstromEngineERFA&& other)
|
||||||
|
: _currentState(std::move(other._currentState)), _stateMutex(std::move(other._stateMutex)) {};
|
||||||
|
|
||||||
|
MccMountAstromEngineERFA& operator=(MccMountAstromEngineERFA&& other)
|
||||||
|
{
|
||||||
|
if (this != &other) {
|
||||||
|
_currentState = std::move(other._currentState);
|
||||||
|
_stateMutex = std::move(other._stateMutex);
|
||||||
|
}
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
virtual ~MccMountAstromEngineERFA() = default;
|
virtual ~MccMountAstromEngineERFA() = default;
|
||||||
|
|
||||||
void setState(engine_state_t state)
|
void setState(engine_state_t state)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
_currentState = std::move(state);
|
_currentState = std::move(state);
|
||||||
}
|
}
|
||||||
|
|
||||||
engine_state_t getState() const
|
engine_state_t getState() const
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
return _currentState;
|
return _currentState;
|
||||||
}
|
}
|
||||||
|
|
||||||
void updateMeteo(meteo_t meteo)
|
void updateMeteo(meteo_t meteo)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
_currentState.meteo = std::move(meteo);
|
_currentState.meteo = std::move(meteo);
|
||||||
}
|
}
|
||||||
|
|
||||||
error_t updateLeapSeconds(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '#')
|
error_t updateLeapSeconds(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '#')
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
if (!_currentState._leapSeconds.load(stream, comment_sym)) {
|
if (!_currentState._leapSeconds.load(stream, comment_sym)) {
|
||||||
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
|
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
|
||||||
@@ -215,7 +221,7 @@ public:
|
|||||||
|
|
||||||
error_t updateLeapSeconds(traits::mcc_input_char_range auto const& filename, char comment_sym = '#')
|
error_t updateLeapSeconds(traits::mcc_input_char_range auto const& filename, char comment_sym = '#')
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
if (!_currentState._leapSeconds.load(filename, comment_sym)) {
|
if (!_currentState._leapSeconds.load(filename, comment_sym)) {
|
||||||
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
|
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_LEAPSECONDS;
|
||||||
@@ -227,7 +233,7 @@ public:
|
|||||||
|
|
||||||
error_t updateBulletinA(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '*')
|
error_t updateBulletinA(std::derived_from<std::basic_istream<char>> auto& stream, char comment_sym = '*')
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
if (!_currentState._bulletinA.load(stream, comment_sym)) {
|
if (!_currentState._bulletinA.load(stream, comment_sym)) {
|
||||||
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_BULLETINA;
|
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_BULLETINA;
|
||||||
@@ -239,7 +245,7 @@ public:
|
|||||||
|
|
||||||
error_t updateBulletinA(traits::mcc_input_char_range auto const& filename, char comment_sym = '*')
|
error_t updateBulletinA(traits::mcc_input_char_range auto const& filename, char comment_sym = '*')
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
if (!_currentState._bulletinA.load(filename, comment_sym)) {
|
if (!_currentState._bulletinA.load(filename, comment_sym)) {
|
||||||
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_BULLETINA;
|
return MccMountAstromEngineERFAErrorCode::ERROR_UPDATE_BULLETINA;
|
||||||
@@ -299,7 +305,7 @@ public:
|
|||||||
|
|
||||||
error_t terrestrialTime(juldate_t juldate, juldate_t& tt)
|
error_t terrestrialTime(juldate_t juldate, juldate_t& tt)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
|
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
|
||||||
|
|
||||||
@@ -320,7 +326,7 @@ public:
|
|||||||
|
|
||||||
error_t apparentSiderTime(juldate_t juldate, sideral_time_t& gst, bool islocal = false)
|
error_t apparentSiderTime(juldate_t juldate, sideral_time_t& gst, bool islocal = false)
|
||||||
{
|
{
|
||||||
// std::lock_guard lock{_stateMutex};
|
// std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
|
using real_days_t = std::chrono::duration<double, std::ratio<86400>>;
|
||||||
|
|
||||||
@@ -328,7 +334,7 @@ public:
|
|||||||
// double tt = juldate.mjd;
|
// double tt = juldate.mjd;
|
||||||
|
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
||||||
|
|
||||||
@@ -384,7 +390,7 @@ public:
|
|||||||
|
|
||||||
error_t refraction(refract_result_t& refr)
|
error_t refraction(refract_result_t& refr)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
eraRefco(_currentState.meteo.pressure, _currentState.meteo.temperature, _currentState.meteo.humidity,
|
eraRefco(_currentState.meteo.pressure, _currentState.meteo.temperature, _currentState.meteo.humidity,
|
||||||
_currentState.wavelength, &refr.refa, &refr.refb);
|
_currentState.wavelength, &refr.refa, &refr.refb);
|
||||||
@@ -417,7 +423,7 @@ public:
|
|||||||
coord_t& alt,
|
coord_t& alt,
|
||||||
eo_t& eo)
|
eo_t& eo)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
||||||
|
|
||||||
@@ -459,7 +465,7 @@ public:
|
|||||||
|
|
||||||
error_t obs2icrs(MccCoordPairKind coord_kind, coord_t x, coord_t y, juldate_t juldate, coord_t ra, coord_t dec)
|
error_t obs2icrs(MccCoordPairKind coord_kind, coord_t x, coord_t y, juldate_t juldate, coord_t ra, coord_t dec)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
auto dut1 = _currentState._bulletinA.DUT1(juldate.mjd);
|
||||||
|
|
||||||
@@ -516,7 +522,7 @@ public:
|
|||||||
|
|
||||||
error_t hadec2azalt(coord_t ha, coord_t dec, coord_t& az, coord_t& alt)
|
error_t hadec2azalt(coord_t ha, coord_t dec, coord_t& az, coord_t& alt)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
double r_az, r_alt;
|
double r_az, r_alt;
|
||||||
eraHd2ae(ha, dec, _currentState.lat, &r_az, &r_alt);
|
eraHd2ae(ha, dec, _currentState.lat, &r_az, &r_alt);
|
||||||
@@ -529,7 +535,7 @@ public:
|
|||||||
|
|
||||||
error_t azalt2hadec(coord_t az, coord_t alt, coord_t& ha, coord_t& dec)
|
error_t azalt2hadec(coord_t az, coord_t alt, coord_t& ha, coord_t& dec)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
double r_ha, r_dec;
|
double r_ha, r_dec;
|
||||||
|
|
||||||
@@ -544,13 +550,177 @@ public:
|
|||||||
|
|
||||||
error_t hadec2pa(coord_t ha, coord_t dec, pa_t& pa)
|
error_t hadec2pa(coord_t ha, coord_t dec, pa_t& pa)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_stateMutex};
|
std::lock_guard lock{*_stateMutex};
|
||||||
|
|
||||||
pa = eraHd2pa(ha, dec, _currentState.lat);
|
pa = eraHd2pa(ha, dec, _currentState.lat);
|
||||||
|
|
||||||
return MccMountAstromEngineERFAErrorCode::ERROR_OK;
|
return MccMountAstromEngineERFAErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
error_t coord2coord(MccCoordPairKind coord_from_kind,
|
||||||
|
coord_t x_from,
|
||||||
|
coord_t y_from,
|
||||||
|
time_point_t time_point_from,
|
||||||
|
MccCoordPairKind coord_to_kind,
|
||||||
|
coord_t& x_to,
|
||||||
|
coord_t& y_to,
|
||||||
|
time_point_t time_point_to)
|
||||||
|
{
|
||||||
|
error_t ret = MccMountAstromEngineERFAErrorCode::ERROR_OK;
|
||||||
|
|
||||||
|
// no convertion
|
||||||
|
if (time_point_from == time_point_to && coord_from_kind == coord_to_kind) {
|
||||||
|
x_to = x_from;
|
||||||
|
y_to = y_from;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
juldate_t jd;
|
||||||
|
eo_t eo;
|
||||||
|
coord_t ra, dec, ha, az, alt;
|
||||||
|
sideral_time_t lst;
|
||||||
|
|
||||||
|
auto lst_eo = [&]() {
|
||||||
|
ret = greg2jul(time_point_from, jd);
|
||||||
|
if (!ret) {
|
||||||
|
ret = apparentSiderTime(jd, lst, true);
|
||||||
|
if (!ret) {
|
||||||
|
ret = eqOrigins(jd, eo);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// special case: to ICRS from apparent
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||||
|
if (coord_from_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
|
||||||
|
coord_from_kind == MccCoordPairKind::COORDS_KIND_AZZD ||
|
||||||
|
coord_from_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
|
||||||
|
coord_from_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
//
|
||||||
|
ret = greg2jul(time_point_from, jd);
|
||||||
|
if (!ret) {
|
||||||
|
ret = obs2icrs(coord_from_kind, x_from, y_from, jd, x_to, y_to);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
// special case: from ICRS to apparent
|
||||||
|
if (coord_from_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
|
||||||
|
coord_to_kind == MccCoordPairKind::COORDS_KIND_AZZD ||
|
||||||
|
coord_to_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
|
||||||
|
coord_to_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
//
|
||||||
|
ret = greg2jul(time_point_to, jd);
|
||||||
|
if (!ret) {
|
||||||
|
ret = icrs2obs(x_from, y_from, jd, ra, dec, ha, az, alt, eo);
|
||||||
|
|
||||||
|
if (!ret) {
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||||
|
x_to = az;
|
||||||
|
y_to = alt;
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||||
|
x_to = az;
|
||||||
|
y_to = std::numbers::pi / 2.0 - alt;
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||||
|
x_to = ra;
|
||||||
|
y_to = dec;
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
x_to = ha;
|
||||||
|
y_to = dec;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
// from apparent to apparent
|
||||||
|
if (time_point_from != time_point_to) { // first, convert 'from' coordinates to ICRS
|
||||||
|
ret = greg2jul(time_point_from, jd);
|
||||||
|
if (!ret) {
|
||||||
|
ret = obs2icrs(coord_from_kind, x_from, y_from, jd, ra, dec);
|
||||||
|
if (!ret) { // from ICRS to required
|
||||||
|
return coord2coord(MccCoordPairKind::COORDS_KIND_RADEC_ICRS, ra, dec, time_point_from,
|
||||||
|
coord_to_kind, x_to, y_to, time_point_to);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // same time points
|
||||||
|
if (coord_from_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||||
|
// first, compute HA from CIO-based RA!!!
|
||||||
|
lst_eo();
|
||||||
|
if (!ret) {
|
||||||
|
ha = lst - x_from + eo;
|
||||||
|
} else {
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||||
|
ret = hadec2azalt(ha, y_from, x_to, y_to);
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||||
|
ret = hadec2azalt(ha, y_from, x_to, y_to);
|
||||||
|
if (!ret) {
|
||||||
|
y_to = std::numbers::pi / 2.0 - y_to;
|
||||||
|
}
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
x_to = ha;
|
||||||
|
y_to = y_from;
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
} else if (coord_from_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||||
|
ret = hadec2azalt(x_from, y_from, x_to, y_to);
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||||
|
ret = hadec2azalt(x_from, y_from, x_to, y_to);
|
||||||
|
if (!ret) {
|
||||||
|
y_to = std::numbers::pi / 2.0 - y_to;
|
||||||
|
}
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||||
|
lst_eo();
|
||||||
|
if (!ret) {
|
||||||
|
x_to = lst - x_from + eo;
|
||||||
|
y_to = y_from;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
} else if (coord_from_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||||
|
if (coord_to_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||||
|
ret = azalt2hadec(x_from, y_from, x_to, y_to);
|
||||||
|
if (!ret) {
|
||||||
|
lst_eo();
|
||||||
|
if (!ret) {
|
||||||
|
x_to = lst - x_to + eo;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||||
|
x_to = x_from;
|
||||||
|
y_to = std::numbers::pi / 2.0 - y_from;
|
||||||
|
} else if (coord_to_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||||
|
ret = azalt2hadec(x_from, y_from, x_to, y_to);
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
} else if (coord_from_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||||
|
return coord2coord(MccCoordPairKind::COORDS_KIND_AZALT, std::move(x_from),
|
||||||
|
std::numbers::pi / 2.0 - y_to, time_point_from, coord_to_kind, x_to, y_to,
|
||||||
|
time_point_to);
|
||||||
|
} else {
|
||||||
|
ret = MccMountAstromEngineERFAErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
/* helper mathods */
|
/* helper mathods */
|
||||||
|
|
||||||
@@ -578,10 +748,10 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
engine_state_t _currentState{};
|
engine_state_t _currentState{};
|
||||||
|
|
||||||
mutable std::mutex _stateMutex;
|
std::unique_ptr<std::mutex> _stateMutex;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mcc::astrom::erfa
|
} // namespace mcc::astrom::erfa
|
||||||
|
|
||||||
|
|
||||||
static_assert(mcc::traits::mcc_astrom_engine_c<mcc::astrom::erfa::MccMountAstromEngineERFA<>>, "");
|
static_assert(mcc::traits::mcc_astrom_engine_c<mcc::astrom::erfa::MccMountAstromEngineERFA>, "");
|
||||||
|
|||||||
@@ -101,6 +101,43 @@ concept mcc_logger_c = requires(T t, const T t_const) {
|
|||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/* A CONCEPT FOR COORDINATE REPRESENTATION */
|
||||||
|
|
||||||
|
// it is a fundametal floating-point type or
|
||||||
|
// a class that can be constructed from or converted to the double fundametal type
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_coord_t = std::floating_point<T> || (std::convertible_to<T, double> && std::constructible_from<T, double>);
|
||||||
|
|
||||||
|
|
||||||
|
/* A CONCEPT FOR UTC TIME POINT REPRESENTATION */
|
||||||
|
|
||||||
|
// it is a std::chrono::sys_time<Duration> or
|
||||||
|
// fundamental arithmetic type that represent number of seconds after the 00:00:00 of 1 January 1970 (UNIX time)
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_utc_time_point_c = mcc_systime_c<T> || std::is_arithmetic_v<T>;
|
||||||
|
|
||||||
|
/* A CONCEPT FOR CLASS TO REPRESENT CELESTIAL POINT */
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_celestial_point_c = requires(T t) {
|
||||||
|
// a type to represent UTC time point of coordinates
|
||||||
|
// it's clear that this makes sense for apparent coordinates
|
||||||
|
typename T::time_point_t;
|
||||||
|
|
||||||
|
// coordinates pair type (e.g. IRCS RA,DEC, Az,Alt and so on)
|
||||||
|
requires std::same_as<decltype(t.coordPairKind), MccCoordPairKind>;
|
||||||
|
|
||||||
|
typename T::coord_t;
|
||||||
|
|
||||||
|
// co-longitude (e.g. RA or Az)
|
||||||
|
requires std::same_as<decltype(t.x), typename T::coord_t>;
|
||||||
|
|
||||||
|
// co-latitude (e.g. DEC or ZD)
|
||||||
|
requires std::same_as<decltype(t.y), typename T::coord_t>;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
/* ASTROMETRY-RELATED COMPUTATION ENGINE */
|
/* ASTROMETRY-RELATED COMPUTATION ENGINE */
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
@@ -131,10 +168,10 @@ concept mcc_astrom_engine_c = requires(T t, const T t_const) {
|
|||||||
} -> std::same_as<typename T::error_t>;
|
} -> std::same_as<typename T::error_t>;
|
||||||
|
|
||||||
// observed place to ICRS RA and DEC: obs2icrs(type, x, y, jd, ra_icrs, dec_icrs)
|
// observed place to ICRS RA and DEC: obs2icrs(type, x, y, jd, ra_icrs, dec_icrs)
|
||||||
// (x,y) = (AZ, ZD) if type = MccCoordPairKind::COORDS_KIND_AZZD
|
// (x, y) = (AZ, ZD) if type == MccCoordPairKind::COORDS_KIND_AZZD
|
||||||
// (x,y) = (AZ, ALT) if type = MccCoordPairKind::COORDS_KIND_AZALT
|
// (x, y) = (AZ, ALT) if type == MccCoordPairKind::COORDS_KIND_AZALT
|
||||||
// (x,y) = (HA, DEC) if type = MccCoordPairKind::COORDS_KIND_HADEC_APP
|
// (x, y) = (HA, DEC) if type == MccCoordPairKind::COORDS_KIND_HADEC_APP
|
||||||
// (x,y) = (RA, DEC) if type = MccCoordPairKind::COORDS_KIND_RADEC_APP
|
// (x, y) = (RA, DEC) if type == MccCoordPairKind::COORDS_KIND_RADEC_APP
|
||||||
{
|
{
|
||||||
t.obs2icrs(std::declval<MccCoordPairKind>(), std::declval<typename T::coord_t>(),
|
t.obs2icrs(std::declval<MccCoordPairKind>(), std::declval<typename T::coord_t>(),
|
||||||
std::declval<typename T::coord_t>(), std::declval<typename T::juldate_t>(),
|
std::declval<typename T::coord_t>(), std::declval<typename T::juldate_t>(),
|
||||||
@@ -160,6 +197,26 @@ concept mcc_astrom_engine_c = requires(T t, const T t_const) {
|
|||||||
std::declval<typename T::pa_t&>())
|
std::declval<typename T::pa_t&>())
|
||||||
} -> std::same_as<typename T::error_t>;
|
} -> std::same_as<typename T::error_t>;
|
||||||
|
|
||||||
|
|
||||||
|
// transform coordinates according to its pair types and time points (a high-level wrapper):
|
||||||
|
//
|
||||||
|
// coord2coord(coord_pair_kind_from, x_from, y_from, time_point_from, coord_pair_kind_to, x_to, y_to, time_point_to)
|
||||||
|
//
|
||||||
|
// (x_*, y_*) = (AZ, ZD) if coord_pair_kind_* == MccCoordPairKind::COORDS_KIND_AZZD
|
||||||
|
// (x_*, y_*) = (AZ, ALT) if coord_pair_kind_* == MccCoordPairKind::COORDS_KIND_AZALT
|
||||||
|
// (x_*, y_*) = (HA, DEC) if coord_pair_kind_* == MccCoordPairKind::COORDS_KIND_HADEC_APP (apparent)
|
||||||
|
// (x_*, y_*) = (RA, DEC) if coord_pair_kind_* == MccCoordPairKind::COORDS_KIND_RADEC_APP (apparent)
|
||||||
|
// (x_*, y_*) = (RA, DEC) if coord_pair_kind_* == MccCoordPairKind::COORDS_KIND_RADEC_ICRS (ICRS)
|
||||||
|
//
|
||||||
|
// if coord_pair_kind_* and time_point_* are equal then x_to = x_from, y_to = y_from
|
||||||
|
{
|
||||||
|
t.coord2coord(std::declval<MccCoordPairKind>(), std::declval<typename T::coord_t>(),
|
||||||
|
std::declval<typename T::coord_t>(), std::declval<typename T::time_point_t>(),
|
||||||
|
std::declval<MccCoordPairKind>(), std::declval<typename T::coord_t&>(),
|
||||||
|
std::declval<typename T::coord_t&>(), std::declval<typename T::time_point_t>())
|
||||||
|
} -> std::same_as<typename T::error_t>;
|
||||||
|
|
||||||
|
|
||||||
// compute equation of origins
|
// compute equation of origins
|
||||||
{
|
{
|
||||||
t.eqOrigins(std::declval<typename T::juldate_t>(), std::declval<typename T::eo_t&>())
|
t.eqOrigins(std::declval<typename T::juldate_t>(), std::declval<typename T::eo_t&>())
|
||||||
@@ -208,30 +265,38 @@ concept mcc_mount_hardware_c = !std::copyable<T> && std::movable<T> && requires(
|
|||||||
|
|
||||||
{ t_const.id() } -> mcc_formattable;
|
{ t_const.id() } -> mcc_formattable;
|
||||||
|
|
||||||
// a type that defines at least HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK
|
// a type that defines at least HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING
|
||||||
// compile-time constants
|
// and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a
|
||||||
// e.g. an implementations can be as follows:
|
// possible tunning of hardware setPos-related commands
|
||||||
// enum class HW_STATE: int {HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK}
|
|
||||||
//
|
//
|
||||||
// struct HW_STATE {
|
// e.g. an implementations can be as follows:
|
||||||
// uint8_t HW_STATE_STOP = 100;
|
// enum class hw_moving_type_t: int {HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING}
|
||||||
// uint8_t HW_STATE_SLEW = 200;
|
//
|
||||||
// uint8_t HW_STATE_TRACK = 300;
|
// struct hw_moving_type_t {
|
||||||
|
// uint16_t HW_MOVE_SLEWING = 111;
|
||||||
|
// uint16_t HW_MOVE_ADJUSTING = 222;
|
||||||
|
// uint16_t HW_MOVE_TRACKING = 333;
|
||||||
|
// uint16_t HW_MOVE_GUIDING = 444;
|
||||||
// }
|
// }
|
||||||
requires requires(typename T::hw_state_t state) {
|
requires requires(typename T::hw_moving_type_t state) {
|
||||||
[]() {
|
[]() {
|
||||||
// hardware is in stop state (no any moving)
|
// hardware was asked for slewing (move to given celestial point)
|
||||||
static constexpr auto v1 = T::hw_state_t::HW_STATE_STOP;
|
static constexpr auto v1 = T::hw_moving_type_t::HW_MOVE_SLEWING;
|
||||||
|
|
||||||
// hardware is in slew state (move to given celestial point)
|
// hardware was asked for adjusting after slewing ("seeking" given celestial point at the end of slewing
|
||||||
static constexpr auto v2 = T::hw_state_t::HW_STATE_SLEW;
|
// process)
|
||||||
|
static constexpr auto v2 = T::hw_moving_type_t::HW_MOVE_ADJUSTING;
|
||||||
|
|
||||||
// hardware is in track state (track given celestial point)
|
// hardware was asked for tracking (track given celestial point)
|
||||||
static constexpr auto v3 = T::hw_state_t::HW_STATE_TRACK;
|
static constexpr auto v3 = T::hw_moving_type_t::HW_MOVE_TRACKING;
|
||||||
|
|
||||||
|
// hardware was asked for guiding (small corrections to track given celestial point)
|
||||||
|
static constexpr auto v4 = T::hw_moving_type_t::HW_MOVE_GUIDING;
|
||||||
}();
|
}();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// a class that contains at least time of measurement, coordinates for x,y axes and its moving rates
|
// a class that contains at least time of measurement, coordinates for x,y axes and its moving rates
|
||||||
requires requires(typename T::axes_pos_t pos) {
|
requires requires(typename T::axes_pos_t pos) {
|
||||||
requires std::same_as<decltype(pos.time_point), typename T::time_point_t>; // time point
|
requires std::same_as<decltype(pos.time_point), typename T::time_point_t>; // time point
|
||||||
@@ -242,13 +307,17 @@ concept mcc_mount_hardware_c = !std::copyable<T> && std::movable<T> && requires(
|
|||||||
requires std::same_as<decltype(pos.xrate), typename T::coord_t>;
|
requires std::same_as<decltype(pos.xrate), typename T::coord_t>;
|
||||||
requires std::same_as<decltype(pos.yrate), typename T::coord_t>;
|
requires std::same_as<decltype(pos.yrate), typename T::coord_t>;
|
||||||
|
|
||||||
requires std::same_as<decltype(pos.state), typename T::hw_state_t>; // hardware state
|
requires std::same_as<decltype(pos.moving_type), typename T::hw_moving_type_t>; // a 'hint' to hardware
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// set positions (angles) of mount axes with given speeds
|
||||||
|
// NOTE: exact interpretation (or even ignoring) of the given moving speeds is subject of a hardware-class
|
||||||
|
// implementation.
|
||||||
|
// e.g. it can be maximal speeds at slewing ramp
|
||||||
{ t.setPos(std::declval<typename T::axes_pos_t>()) } -> std::same_as<typename T::error_t>;
|
{ t.setPos(std::declval<typename T::axes_pos_t>()) } -> std::same_as<typename T::error_t>;
|
||||||
{ t.getPos(std::declval<typename T::axes_pos_t&>()) } -> std::same_as<typename T::error_t>;
|
|
||||||
|
|
||||||
{ t_const.getState(std::declval<typename T::hw_state_t&>()) } -> std::same_as<typename T::error_t>;
|
// get current positions and speeds (angles) of mount axes
|
||||||
|
{ t.getPos(std::declval<typename T::axes_pos_t&>()) } -> std::same_as<typename T::error_t>;
|
||||||
|
|
||||||
{ t.stop() } -> std::same_as<typename T::error_t>; // stop any moving
|
{ t.stop() } -> std::same_as<typename T::error_t>; // stop any moving
|
||||||
{ t.init() } -> std::same_as<typename T::error_t>; // initialize hardware
|
{ t.init() } -> std::same_as<typename T::error_t>; // initialize hardware
|
||||||
@@ -258,7 +327,7 @@ concept mcc_mount_hardware_c = !std::copyable<T> && std::movable<T> && requires(
|
|||||||
/* POINTING-ERROR CORRECTION */
|
/* POINTING-ERROR CORRECTION */
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
concept mcc_mount_pec_c = requires(T t, const T t_const) {
|
concept mcc_mount_pec_c = requires(T t) {
|
||||||
requires mcc_error_c<typename T::error_t>;
|
requires mcc_error_c<typename T::error_t>;
|
||||||
typename T::coord_t;
|
typename T::coord_t;
|
||||||
|
|
||||||
@@ -288,13 +357,20 @@ concept mcc_mount_pec_c = requires(T t, const T t_const) {
|
|||||||
|
|
||||||
// a class that contains at least celestial (equatorial and horizontal) and harware coordinates
|
// a class that contains at least celestial (equatorial and horizontal) and harware coordinates
|
||||||
template <typename T>
|
template <typename T>
|
||||||
concept mcc_mount_telemetry_data_c = requires(T telemetry) {
|
concept mcc_mount_telemetry_data_c = std::movable<T> && requires(T telemetry) {
|
||||||
typename T::coord_t;
|
typename T::coord_t;
|
||||||
typename T::time_point_t;
|
typename T::time_point_t;
|
||||||
|
|
||||||
// time point
|
// time point
|
||||||
requires std::same_as<decltype(telemetry.time_point), typename T::time_point_t>;
|
requires std::same_as<decltype(telemetry.time_point), typename T::time_point_t>;
|
||||||
|
|
||||||
|
// target sky point ICRS and current coordinates
|
||||||
|
requires std::same_as<decltype(telemetry.tagRA), typename T::coord_t>; // apparent RA
|
||||||
|
requires std::same_as<decltype(telemetry.tagDEC), typename T::coord_t>; // apparent DEC
|
||||||
|
requires std::same_as<decltype(telemetry.tagHA), typename T::coord_t>; // hour angle
|
||||||
|
requires std::same_as<decltype(telemetry.tagAZ), typename T::coord_t>; // azimuth
|
||||||
|
requires std::same_as<decltype(telemetry.tagALT), typename T::coord_t>; // altitude
|
||||||
|
|
||||||
// mount current coordinates
|
// mount current coordinates
|
||||||
requires std::same_as<decltype(telemetry.mntRA), typename T::coord_t>; // apparent RA
|
requires std::same_as<decltype(telemetry.mntRA), typename T::coord_t>; // apparent RA
|
||||||
requires std::same_as<decltype(telemetry.mntDEC), typename T::coord_t>; // apparent DEC
|
requires std::same_as<decltype(telemetry.mntDEC), typename T::coord_t>; // apparent DEC
|
||||||
@@ -306,6 +382,11 @@ concept mcc_mount_telemetry_data_c = requires(T telemetry) {
|
|||||||
requires std::same_as<decltype(telemetry.mntPosY), typename T::coord_t>; // hardware encoder Y-axis position
|
requires std::same_as<decltype(telemetry.mntPosY), typename T::coord_t>; // hardware encoder Y-axis position
|
||||||
requires std::same_as<decltype(telemetry.mntRateX), typename T::coord_t>; // hardware encoder X-axis rate
|
requires std::same_as<decltype(telemetry.mntRateX), typename T::coord_t>; // hardware encoder X-axis rate
|
||||||
requires std::same_as<decltype(telemetry.mntRateY), typename T::coord_t>; // hardware encoder Y-axis rate
|
requires std::same_as<decltype(telemetry.mntRateY), typename T::coord_t>; // hardware encoder Y-axis rate
|
||||||
|
|
||||||
|
// corrections to transform mount hardware coordinates to apparent
|
||||||
|
// (pointing error corrections)
|
||||||
|
requires std::same_as<decltype(telemetry.pecX), typename T::coord_t>;
|
||||||
|
requires std::same_as<decltype(telemetry.pecY), typename T::coord_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -328,31 +409,12 @@ concept mcc_mount_telemetry_c = requires(T t, const T t_const) {
|
|||||||
|
|
||||||
requires mcc_mount_telemetry_data_c<typename T::mount_telemetry_data_t>;
|
requires mcc_mount_telemetry_data_c<typename T::mount_telemetry_data_t>;
|
||||||
|
|
||||||
{ t_const.errorString(std::declval<typename T::error_t>()) } -> mcc_formattable;
|
|
||||||
|
|
||||||
{ t.update() } -> std::same_as<typename T::error_t>;
|
{ t.update() } -> std::same_as<typename T::error_t>;
|
||||||
|
|
||||||
{ t.data(std::declval<typename T::mount_telemetry_data_t&>()) } -> std::same_as<typename T::error_t>;
|
{ t.data(std::declval<typename T::mount_telemetry_data_t&>()) } -> std::same_as<typename T::error_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/* A CONCEPT FOR CLASS TO REPRESENT CELESTIAL POINT */
|
|
||||||
|
|
||||||
|
|
||||||
template <typename T>
|
|
||||||
concept mcc_celestial_point_c = requires(T t) {
|
|
||||||
// input coordinates pair type (e.g. IRCS RA,DEC, Az,Alt and so on)
|
|
||||||
requires std::same_as<decltype(t.coordPairKind), MccCoordPairKind>;
|
|
||||||
|
|
||||||
typename T::coord_t;
|
|
||||||
|
|
||||||
// co-longitude (e.g. RA or Az)
|
|
||||||
requires std::same_as<decltype(t.x), typename T::coord_t>;
|
|
||||||
|
|
||||||
// co-latitude (e.g. DEC or ZD)
|
|
||||||
requires std::same_as<decltype(t.y), typename T::coord_t>;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
// /* SLEW PARAMETERS */
|
// /* SLEW PARAMETERS */
|
||||||
|
|
||||||
@@ -384,6 +446,7 @@ concept mcc_slew_model_c = requires(T t) {
|
|||||||
|
|
||||||
// { t.slew(std::declval<typename T::slew_params_t>()) } -> std::same_as<typename T::error_t>;
|
// { t.slew(std::declval<typename T::slew_params_t>()) } -> std::same_as<typename T::error_t>;
|
||||||
{ t.slew(std::declval<typename T::slew_point_t>()) } -> std::same_as<typename T::error_t>;
|
{ t.slew(std::declval<typename T::slew_point_t>()) } -> std::same_as<typename T::error_t>;
|
||||||
|
{ t.stop() } -> std::same_as<typename T::error_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -394,68 +457,114 @@ concept mcc_guiding_model_c = requires(T t) {
|
|||||||
|
|
||||||
// start process of guiding
|
// start process of guiding
|
||||||
{ t.guiding(std::declval<typename T::guiding_point_t>()) } -> std::same_as<typename T::error_t>;
|
{ t.guiding(std::declval<typename T::guiding_point_t>()) } -> std::same_as<typename T::error_t>;
|
||||||
|
{ t.stop() } -> std::same_as<typename T::error_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/* MOUNT PROHIBITED ZONE */
|
/* MOUNT PROHIBITED ZONE */
|
||||||
|
|
||||||
|
|
||||||
|
struct MccPzoneAbstractInterface {
|
||||||
|
bool inZone(this auto&& self, mcc_mount_telemetry_data_c auto const& telemetry_data)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::inZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).inZone(telemetry_data);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool inZone(this auto&& self, mcc_celestial_point_c auto const& sky_point)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::inZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).inZone(sky_point);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// returns a time to reach the zone
|
||||||
|
auto timeTo(this auto&& self, mcc_mount_telemetry_data_c auto const& telemetry_data)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::timeTo method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).timeTo(telemetry_data);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
auto timeTo(this auto&& self, mcc_celestial_point_c auto const& sky_point)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::timeTo method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).timeTo(sky_point);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// returns a time to exit from the zone
|
||||||
|
auto timeFrom(this auto&& self, mcc_mount_telemetry_data_c auto const& telemetry_data)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::timeFrom method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).timeFrom(telemetry_data);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
auto timeFrom(this auto&& self, mcc_celestial_point_c auto const& sky_point)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPzoneAbstractInterface>) {
|
||||||
|
static_assert(false, "Call an empty MccPzoneAbstractInterface::timeFrom method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).timeFrom(sky_point);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
template <typename T, typename TelemetryDataT>
|
template <typename T, typename TelemetryDataT>
|
||||||
concept mcc_prohibited_zone_c =
|
concept mcc_prohibited_zone_c =
|
||||||
mcc_mount_telemetry_data_c<TelemetryDataT> && std::movable<T> && requires(T t, const T t_const) {
|
mcc_mount_telemetry_data_c<TelemetryDataT> && std::movable<T> && requires(T t, const T t_const) {
|
||||||
typename T::coord_t;
|
typename T::coord_t;
|
||||||
typename T::time_point_t;
|
// typename T::time_point_t;
|
||||||
requires mcc_time_duration_c<typename T::duration_t>;
|
requires mcc_time_duration_c<typename T::duration_t>;
|
||||||
|
|
||||||
// static constexpr member to represent inifite duration
|
// static constexpr member to represent infinite duration
|
||||||
requires requires {
|
requires requires {
|
||||||
requires std::same_as<decltype(T::infiniteDuration), typename T::duration_t>;
|
requires std::same_as<decltype(T::infiniteDuration), typename T::duration_t const>;
|
||||||
[]() {
|
[]() {
|
||||||
constexpr auto val = T::infiniteDuration;
|
constexpr auto val = T::infiniteDuration;
|
||||||
return val;
|
return val;
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// static constexpr member to represent zero duration
|
||||||
// the type 'T' must define a static constexpr member of type MccCoordPairKind
|
|
||||||
// to declarate type of coordinate pair used to describe the zone.
|
|
||||||
// This coordinate pair must be used as input in the 'inZone' class method.
|
|
||||||
requires requires {
|
requires requires {
|
||||||
requires std::same_as<decltype(T::zoneCoordPairKind), const MccCoordPairKind>;
|
requires std::same_as<decltype(T::zeroDuration), typename T::duration_t const>;
|
||||||
[]() {
|
[]() {
|
||||||
constexpr MccCoordPairKind val = T::zoneCoordPairKind;
|
constexpr auto val = T::zeroDuration;
|
||||||
return val;
|
return val;
|
||||||
}(); // to ensure that 'zoneCoordPairKind' can be used at compile-time context
|
|
||||||
};
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
// return a name of the zone
|
// return a name of the zone
|
||||||
{ t_const.name() } -> mcc_formattable;
|
{ t_const.name() } -> mcc_formattable;
|
||||||
|
|
||||||
|
|
||||||
// check if given coordinates are into the zone.
|
// check if given coordinates are into the zone.
|
||||||
// input coordinates interpretation is in according to 'zoneCoordPairKind' static constexpr member
|
|
||||||
{
|
|
||||||
t.inZone(std::declval<typename T::coord_t>(), std::declval<typename T::coord_t>())
|
|
||||||
} -> std::convertible_to<bool>;
|
|
||||||
|
|
||||||
|
|
||||||
// for given coordinates and time the method computes a time to reach the zone.
|
|
||||||
// implementation of the method must assume that input coordinates are apparent RA and DEC at given time
|
|
||||||
// point, while the time point is one from which computation should be performed (e.g. current time moment)
|
|
||||||
{
|
|
||||||
t.timeTo(std::declval<typename T::coord_t>(), std::declval<typename T::coord_t>(),
|
|
||||||
std::declval<typename T::time_point_t>())
|
|
||||||
} -> mcc_time_duration_c;
|
|
||||||
|
|
||||||
|
|
||||||
// for given coordinates and time the method computes a time to exit from the zone
|
|
||||||
{
|
|
||||||
t.timeFrom(std::declval<typename T::coord_t>(), std::declval<typename T::coord_t>(),
|
|
||||||
std::declval<typename T::time_point_t>())
|
|
||||||
} -> mcc_time_duration_c;
|
|
||||||
|
|
||||||
// requires for the methods above with the first argument of type
|
|
||||||
// 'const mcc_mount_telemetry_data_c&' (const lvalue reference)
|
|
||||||
|
|
||||||
{ t.inZone(std::declval<const TelemetryDataT&>()) } -> std::convertible_to<bool>;
|
{ t.inZone(std::declval<const TelemetryDataT&>()) } -> std::convertible_to<bool>;
|
||||||
|
|
||||||
// a time duration to reach the zone.
|
// a time duration to reach the zone.
|
||||||
@@ -470,26 +579,150 @@ concept mcc_prohibited_zone_c =
|
|||||||
{ t.timeFrom(std::declval<const TelemetryDataT&>()) } -> std::same_as<typename T::duration_t>;
|
{ t.timeFrom(std::declval<const TelemetryDataT&>()) } -> std::same_as<typename T::duration_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// an input range of prohibited zones
|
||||||
|
template <typename T, typename TelemetryDataT>
|
||||||
|
concept mcc_irange_of_pzones_c = mcc_mount_telemetry_data_c<TelemetryDataT> && std::ranges::input_range<T> &&
|
||||||
|
mcc_prohibited_zone_c<std::ranges::range_value_t<T>, TelemetryDataT>;
|
||||||
|
|
||||||
|
|
||||||
|
// // a concept for a callable with the first argument of type satisfied to 'mcc_prohibited_zone_c'
|
||||||
|
// template <typename T, typename TelemetryDataT>
|
||||||
|
// concept mcc_pzone_foreach_func_c = mcc_is_callable<T> && mcc_mount_telemetry_data_c<TelemetryDataT> &&
|
||||||
|
// mcc_prohibited_zone_c<mcc_func_arg1_t<T>, TelemetryDataT>;
|
||||||
|
|
||||||
|
|
||||||
|
// There is no way to declare a concept of class with templated method so one needs to define
|
||||||
|
// a generic interface of prohibited zones holder/container explicitly
|
||||||
|
template <mcc_mount_telemetry_data_c TelemetryDataT>
|
||||||
|
struct MccPZoneAbstractContainer {
|
||||||
|
virtual ~MccPZoneAbstractContainer() = default;
|
||||||
|
|
||||||
|
// must return a size of the container after the addition of the given zone
|
||||||
|
template <mcc_prohibited_zone_c<TelemetryDataT> ZT>
|
||||||
|
size_t pzAddZone(this auto&& self, ZT zone)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzAddZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzAddZone(std::move(zone));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// clear the container
|
||||||
|
auto pzClearZones(this auto&& self)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzClearZones method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzClearZones();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// must return the size of the container (number of zones)
|
||||||
|
size_t pzSize(this auto&& self)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzSize method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzSize();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// must return true if the given telemetry coordinates are in any of zones in the container and
|
||||||
|
// false otherwise
|
||||||
|
template <typename RT>
|
||||||
|
bool pzInZone(this auto&& self, const TelemetryDataT& tdata, RT& result)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzInZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzInZone(tdata, result);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename RT>
|
||||||
|
auto pzTimeTo(this auto&& self, const TelemetryDataT& tdata, RT& result)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzInZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzTimeTo(tdata, result);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename RT>
|
||||||
|
auto pzTimeFrom(this auto&& self, const TelemetryDataT& tdata, RT& result)
|
||||||
|
{
|
||||||
|
using self_t = decltype(self);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<std::remove_cvref_t<self_t>, MccPZoneAbstractContainer>) {
|
||||||
|
static_assert(false, "Call an empty MccPZoneAbstractContainer::pzInZone method");
|
||||||
|
} else {
|
||||||
|
return std::forward<self_t>(self).pzTimeFrom(tdata, result);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
MccPZoneAbstractContainer() = default;
|
||||||
|
};
|
||||||
|
|
||||||
|
// a full concept for prohibited zones container
|
||||||
|
template <typename T, typename TelemetryDataT>
|
||||||
|
concept mcc_mount_pzones_container_c = std::derived_from<T, MccPZoneAbstractContainer<TelemetryDataT>> && requires {
|
||||||
|
// common time duration type for zones 'timeTo' and 'timeFrom' methods
|
||||||
|
requires mcc_time_duration_c<typename T::duration_t>;
|
||||||
|
|
||||||
|
// static constexpr member to represent infinite duration
|
||||||
|
requires requires {
|
||||||
|
requires std::same_as<decltype(T::infiniteDuration), typename T::duration_t const>;
|
||||||
|
[]() {
|
||||||
|
constexpr auto val = T::infiniteDuration;
|
||||||
|
return val;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
// static constexpr member to represent zero duration
|
||||||
|
requires requires {
|
||||||
|
requires std::same_as<decltype(T::zeroDuration), typename T::duration_t const>;
|
||||||
|
[]() {
|
||||||
|
constexpr auto val = T::zeroDuration;
|
||||||
|
return val;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
};
|
||||||
|
|
||||||
/* MOUNT GENERIC CONTROLS */
|
/* MOUNT GENERIC CONTROLS */
|
||||||
|
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
concept mcc_mount_controls_c = std::move_constructible<T> && std::movable<T> && requires(T t) {
|
concept mcc_mount_controls_c = requires(T t) {
|
||||||
|
// concept mcc_mount_controls_c = std::move_constructible<T> && std::movable<T> && requires(T t) {
|
||||||
requires mcc_astrom_engine_c<decltype(t.astrometryEngine)>;
|
requires mcc_astrom_engine_c<decltype(t.astrometryEngine)>;
|
||||||
requires mcc_mount_pec_c<decltype(t.PEC)>;
|
requires mcc_mount_pec_c<decltype(t.PEC)>;
|
||||||
requires mcc_mount_hardware_c<decltype(t.hardware)>;
|
requires mcc_mount_hardware_c<decltype(t.hardware)>;
|
||||||
requires mcc_mount_telemetry_c<decltype(t.telemetry)>;
|
requires mcc_mount_telemetry_c<decltype(t.telemetry)>;
|
||||||
|
|
||||||
requires mcc_slew_model_c<decltype(t.slewModel)>;
|
requires mcc_slew_model_c<decltype(t.slewModel)>;
|
||||||
// requires mcc_slew_model_c<decltype(t.slewModel), decltype(t.telemetry)>;
|
|
||||||
requires mcc_guiding_model_c<decltype(t.guidingModel)>;
|
requires mcc_guiding_model_c<decltype(t.guidingModel)>;
|
||||||
// requires mcc_guiding_model_c<decltype(t.guidingModel), decltype(t.telemetry)>;
|
|
||||||
|
|
||||||
// a std::tuple of prohibited zones
|
// a std::tuple of prohibited zones
|
||||||
[]<mcc_prohibited_zone_c<typename decltype(t.telemetry)::mount_telemetry_data_t>... Ts>(std::tuple<Ts...>) {
|
// []<mcc_prohibited_zone_c<typename decltype(t.telemetry)::mount_telemetry_data_t>... Ts>(std::tuple<Ts...>) {
|
||||||
}(t.prohibitedZones);
|
// }(t.prohibitedZones);
|
||||||
|
|
||||||
|
// requires mcc_tuple_c<decltype(t.prohibitedZones)>;
|
||||||
|
requires mcc_irange_of_pzones_c<decltype(t.prohibitedZones),
|
||||||
|
typename decltype(t.telemetry)::mount_telemetry_data_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -508,18 +741,24 @@ concept mcc_mount_c = requires(T t) {
|
|||||||
requires mcc_astrom_engine_c<typename T::astrom_engine_t>;
|
requires mcc_astrom_engine_c<typename T::astrom_engine_t>;
|
||||||
requires mcc_mount_pec_c<typename T::pec_t>;
|
requires mcc_mount_pec_c<typename T::pec_t>;
|
||||||
requires mcc_mount_hardware_c<typename T::hardware_t>;
|
requires mcc_mount_hardware_c<typename T::hardware_t>;
|
||||||
// requires mcc_slew_model_c<typename T::slew_model_t, typename T::mount_telemetry_t>;
|
|
||||||
requires mcc_slew_model_c<typename T::slew_model_t>;
|
requires mcc_slew_model_c<typename T::slew_model_t>;
|
||||||
// requires mcc_guiding_model_c<typename T::guiding_model_t, typename T::mount_telemetry_t>;
|
|
||||||
requires mcc_guiding_model_c<typename T::guiding_model_t>;
|
requires mcc_guiding_model_c<typename T::guiding_model_t>;
|
||||||
|
|
||||||
requires std::same_as<typename T::slew_params_t, typename T::slew_model_t::slew_params_t>;
|
// public methods
|
||||||
|
{
|
||||||
|
t.mountTelemetryData(std::declval<typename T::mount_telemetry_data_t&>())
|
||||||
|
} -> std::same_as<typename T::mount_telemetry_t::error_t>;
|
||||||
|
|
||||||
// public method
|
{
|
||||||
{ t.mountTelemetryData() } -> std::same_as<typename T::mount_telemetry_data_t>;
|
t.slewMount(std::declval<typename T::slew_model_t::slew_point_t>())
|
||||||
|
} -> std::same_as<typename T::slew_model_t::error_t>;
|
||||||
|
|
||||||
|
{
|
||||||
|
t.guidingTarget(std::declval<typename T::guiding_model_t::guiding_point_t>())
|
||||||
|
} -> std::same_as<typename T::guiding_model_t::error_t>;
|
||||||
};
|
};
|
||||||
|
|
||||||
// generic with logging methods
|
// generic with public logging methods
|
||||||
template <typename T>
|
template <typename T>
|
||||||
concept mcc_log_mount_c = mcc_mount_c<T> && mcc_logger_c<T>;
|
concept mcc_log_mount_c = mcc_mount_c<T> && mcc_logger_c<T>;
|
||||||
|
|
||||||
|
|||||||
@@ -28,10 +28,7 @@ public:
|
|||||||
|
|
||||||
virtual ~MccMountEventBase() = default;
|
virtual ~MccMountEventBase() = default;
|
||||||
|
|
||||||
mount_t& mount() const
|
mount_t& mount() const { return _mount; }
|
||||||
{
|
|
||||||
return _mount;
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
MccMountEventBase(mount_t& mount) : _mount(mount) {}
|
MccMountEventBase(mount_t& mount) : _mount(mount) {}
|
||||||
@@ -78,10 +75,7 @@ struct MccMountEventError : public MccMountEventBase<MountT> {
|
|||||||
|
|
||||||
using event_data_t = std::error_code;
|
using event_data_t = std::error_code;
|
||||||
|
|
||||||
event_data_t eventData() const
|
event_data_t eventData() const { return _error; }
|
||||||
{
|
|
||||||
return _error;
|
|
||||||
}
|
|
||||||
|
|
||||||
MccMountEventError(MountT& mount, const event_data_t& error) : base_t(mount), _error(error) {}
|
MccMountEventError(MountT& mount, const event_data_t& error) : base_t(mount), _error(error) {}
|
||||||
|
|
||||||
@@ -98,12 +92,9 @@ struct MccMountEventSlew : public MccMountEventBase<MountT> {
|
|||||||
|
|
||||||
static constexpr std::string_view ID = "MCC-MOUNT-SLEW-EVENT";
|
static constexpr std::string_view ID = "MCC-MOUNT-SLEW-EVENT";
|
||||||
|
|
||||||
using event_data_t = typename MountT::slew_params_t;
|
using event_data_t = typename MountT::slew_model_t::slew_point_t;
|
||||||
|
|
||||||
event_data_t eventData() const
|
event_data_t eventData() const { return _eventData; }
|
||||||
{
|
|
||||||
return _eventData;
|
|
||||||
}
|
|
||||||
|
|
||||||
MccMountEventSlew(MountT& mount, const event_data_t& ev_data) : base_t(mount), _eventData(ev_data) {}
|
MccMountEventSlew(MountT& mount, const event_data_t& ev_data) : base_t(mount), _eventData(ev_data) {}
|
||||||
|
|
||||||
@@ -120,9 +111,14 @@ struct MccMountEventGuiding : public MccMountEventBase<MountT> {
|
|||||||
|
|
||||||
static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT";
|
static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT";
|
||||||
|
|
||||||
// CTAD does not work for clang++ (at least till v. 20 and -std=c++23)!
|
using event_data_t = typename MountT::guiding_model_t::guiding_point_t;
|
||||||
// so, one must explicitly define constructor here
|
|
||||||
MccMountEventGuiding(MountT& mount) : base_t(mount) {}
|
event_data_t eventData() const { return _eventData; }
|
||||||
|
|
||||||
|
MccMountEventGuiding(MountT& mount, const event_data_t& ev_data) : base_t(mount), _eventData(ev_data) {}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
event_data_t _eventData;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -139,10 +135,7 @@ struct MccMountEventStop : public MccMountEventBase<MountT> {
|
|||||||
EVENT_STOP_BUTTON // hardware button
|
EVENT_STOP_BUTTON // hardware button
|
||||||
};
|
};
|
||||||
|
|
||||||
event_data_t eventData() const
|
event_data_t eventData() const { return _reason; }
|
||||||
{
|
|
||||||
return _reason;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string_view reason() const
|
std::string_view reason() const
|
||||||
{
|
{
|
||||||
@@ -286,7 +279,10 @@ struct MccMountStateStop : MccMountStateBase<MountT> {
|
|||||||
|
|
||||||
auto mount = event.mount();
|
auto mount = event.mount();
|
||||||
|
|
||||||
mount.stopMount();
|
// should one check current state (slewing, guiding)?
|
||||||
|
mount.slewModel.stop();
|
||||||
|
mount.guidingModel.stop();
|
||||||
|
mount.hardware.stop();
|
||||||
|
|
||||||
if constexpr (std::same_as<EvT, MccMountEventStop<MountT>>) {
|
if constexpr (std::same_as<EvT, MccMountEventStop<MountT>>) {
|
||||||
mount.logInfo(std::format("Stop reason: {}", event.reason()));
|
mount.logInfo(std::format("Stop reason: {}", event.reason()));
|
||||||
@@ -446,7 +442,8 @@ struct MccMountStateShutdown : MccMountStateBase<MountT> {
|
|||||||
|
|
||||||
|
|
||||||
// slew state
|
// slew state
|
||||||
|
// WARNING: It must be a friend to 'MountT' class if it inherits it base class mount_controls_t
|
||||||
|
// as protected or private!!!
|
||||||
template <traits::mcc_fsm_log_mount_c MountT>
|
template <traits::mcc_fsm_log_mount_c MountT>
|
||||||
struct MccMountStateSlew : MccMountStateBase<MountT> {
|
struct MccMountStateSlew : MccMountStateBase<MountT> {
|
||||||
static constexpr std::string_view ID = "MCC-MOUNT-SLEW-STATE";
|
static constexpr std::string_view ID = "MCC-MOUNT-SLEW-STATE";
|
||||||
@@ -472,7 +469,18 @@ struct MccMountStateSlew : MccMountStateBase<MountT> {
|
|||||||
{
|
{
|
||||||
this->enterLog(event);
|
this->enterLog(event);
|
||||||
|
|
||||||
event.mount().slewMount(event.eventData());
|
auto slew_err = event.mount().slewModel.slew(event.eventData());
|
||||||
|
if (slew_err) {
|
||||||
|
if constexpr (std::same_as<decltype(slew_err), std::error_code>) {
|
||||||
|
event.mount().dispatchEvent(MccMountEventError<MountT>{event.mount(), slew_err});
|
||||||
|
} else {
|
||||||
|
// ...
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// switch to IDLE state
|
||||||
|
event.mount().template dispatchEvent<MccMountEventIDLE<MountT>>({event.mount()});
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -505,7 +513,17 @@ struct MccMountStateGuiding : MccMountStateBase<MountT> {
|
|||||||
{
|
{
|
||||||
this->enterLog(event);
|
this->enterLog(event);
|
||||||
|
|
||||||
event.mount().startGuiding();
|
auto err = event.mount().guidingModel.guiding(event.eventData());
|
||||||
|
if (err) {
|
||||||
|
if constexpr (std::same_as<decltype(err), std::error_code>) {
|
||||||
|
event.mount().dispatchEvent(MccMountEventError<MountT>{event.mount(), err});
|
||||||
|
} else {
|
||||||
|
// ...
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// switch to IDLE state
|
||||||
|
event.mount().template dispatchEvent<MccMountEventIDLE<MountT>>({event.mount()});
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -23,10 +23,7 @@ enum class MccMountDefaultPECErrorCode : int { ERROR_OK, ERROR_INVALID_INPUTS_BI
|
|||||||
struct MccMountDefaultPECCategory : public std::error_category {
|
struct MccMountDefaultPECCategory : public std::error_category {
|
||||||
MccMountDefaultPECCategory() : std::error_category() {}
|
MccMountDefaultPECCategory() : std::error_category() {}
|
||||||
|
|
||||||
const char* name() const noexcept
|
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; }
|
||||||
{
|
|
||||||
return "ADC_GENERIC_DEVICE";
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string message(int ec) const
|
std::string message(int ec) const
|
||||||
{
|
{
|
||||||
@@ -140,14 +137,41 @@ public:
|
|||||||
: _pecData(std::move(pdata)),
|
: _pecData(std::move(pdata)),
|
||||||
_phi(_pecData.siteLatitude),
|
_phi(_pecData.siteLatitude),
|
||||||
_geomCoeffs(_pecData.geomCoefficients),
|
_geomCoeffs(_pecData.geomCoefficients),
|
||||||
_bsplCoeffs(_pecData.bsplineCoefficients)
|
_bsplCoeffs(_pecData.bsplineCoefficients),
|
||||||
|
_pecDataMutex(new std::mutex)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MccMountDefaultPEC(const MccMountDefaultPEC&) = delete;
|
||||||
|
MccMountDefaultPEC& operator=(const MccMountDefaultPEC&) = delete;
|
||||||
|
|
||||||
|
MccMountDefaultPEC(MccMountDefaultPEC&& other)
|
||||||
|
: _pecData(std::move(other._pecData)),
|
||||||
|
_phi(_pecData.siteLatitude),
|
||||||
|
_geomCoeffs(_pecData.geomCoefficients),
|
||||||
|
_bsplCoeffs(_pecData.bsplineCoefficients),
|
||||||
|
_pecDataMutex(std::move(other._pecDataMutex))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
MccMountDefaultPEC& operator=(MccMountDefaultPEC&& other)
|
||||||
|
{
|
||||||
|
if (this == &other) {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
_pecData = std::move(other._pecData);
|
||||||
|
_phi = _pecData.siteLatitude;
|
||||||
|
_geomCoeffs = _pecData.geomCoefficients;
|
||||||
|
_bsplCoeffs = _pecData.bsplineCoefficients;
|
||||||
|
_pecDataMutex = std::move(other._pecDataMutex);
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
void setData(pec_data_t pdata)
|
void setData(pec_data_t pdata)
|
||||||
{
|
{
|
||||||
std::lock_guard lock(_pecDataMutex);
|
std::lock_guard lock(*_pecDataMutex);
|
||||||
|
|
||||||
_pecData = std::move(pdata);
|
_pecData = std::move(pdata);
|
||||||
_phi = _pecData.siteLatitude;
|
_phi = _pecData.siteLatitude;
|
||||||
@@ -158,21 +182,21 @@ public:
|
|||||||
|
|
||||||
pec_data_t getData() const
|
pec_data_t getData() const
|
||||||
{
|
{
|
||||||
std::lock_guard lock(_pecDataMutex);
|
std::lock_guard lock(*_pecDataMutex);
|
||||||
|
|
||||||
return _pecData;
|
return _pecData;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setType(MccMountDefaultPECType type)
|
void setType(MccMountDefaultPECType type)
|
||||||
{
|
{
|
||||||
std::lock_guard lock(_pecDataMutex);
|
std::lock_guard lock(*_pecDataMutex);
|
||||||
|
|
||||||
_pecData.type = type;
|
_pecData.type = type;
|
||||||
}
|
}
|
||||||
|
|
||||||
MccMountDefaultPECType getType() const
|
MccMountDefaultPECType getType() const
|
||||||
{
|
{
|
||||||
std::lock_guard lock(_pecDataMutex);
|
std::lock_guard lock(*_pecDataMutex);
|
||||||
|
|
||||||
return _pecData.type;
|
return _pecData.type;
|
||||||
}
|
}
|
||||||
@@ -183,7 +207,7 @@ public:
|
|||||||
// 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
|
||||||
error_t compute(const coord_t& x, const coord_t& y, pec_result_t& res)
|
error_t compute(const coord_t& x, const coord_t& y, pec_result_t& res)
|
||||||
{
|
{
|
||||||
std::lock_guard lock(_pecDataMutex);
|
std::lock_guard lock(*_pecDataMutex);
|
||||||
|
|
||||||
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) { // equatorial
|
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) { // equatorial
|
||||||
if (_pecData.type == MccMountDefaultPECType::PEC_TYPE_GEOMETRY) {
|
if (_pecData.type == MccMountDefaultPECType::PEC_TYPE_GEOMETRY) {
|
||||||
@@ -249,70 +273,6 @@ public:
|
|||||||
return MccMountDefaultPECErrorCode::ERROR_OK;
|
return MccMountDefaultPECErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
// from celestial to encoder (use of iterative scheme)
|
|
||||||
error_t reverseCompute(const coord_t& x, const coord_t& y, pec_result_t& res, coord_t eps, size_t max_iter = 5)
|
|
||||||
{
|
|
||||||
coord_t e2 = eps * eps;
|
|
||||||
|
|
||||||
coord_t xi = x, yi = y;
|
|
||||||
coord_t xe, ye;
|
|
||||||
size_t iter = 1;
|
|
||||||
|
|
||||||
// the first iteration
|
|
||||||
auto err = compute(x, y, res);
|
|
||||||
|
|
||||||
if (!err) {
|
|
||||||
// 'encoder' cocordinates
|
|
||||||
xe = x - res.dx;
|
|
||||||
ye = y - res.dy;
|
|
||||||
|
|
||||||
err = compute(xe, ye, res); // to celestial
|
|
||||||
if (err) {
|
|
||||||
return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
xi = xe + res.dx; // celestial
|
|
||||||
yi = ye + res.dy;
|
|
||||||
|
|
||||||
auto rx = (x - xi);
|
|
||||||
auto ry = (y - yi);
|
|
||||||
auto d = rx * rx + ry * ry;
|
|
||||||
|
|
||||||
bool ok = d <= e2;
|
|
||||||
if (ok) {
|
|
||||||
return MccMountDefaultPECErrorCode::ERROR_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
while (iter < max_iter) {
|
|
||||||
err = compute(xi, yi, res); // to encoder
|
|
||||||
if (err) {
|
|
||||||
return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
||||||
}
|
|
||||||
xe = x - res.dx;
|
|
||||||
ye = y - res.dy;
|
|
||||||
|
|
||||||
err = compute(xe, ye, res); // to celestial
|
|
||||||
if (err) {
|
|
||||||
return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
xi = xe + res.dx; // celestial
|
|
||||||
yi = ye + res.dy;
|
|
||||||
|
|
||||||
ok = ((x - xi) * (x - xi) + (y - yi) * (y - yi)) <= e2;
|
|
||||||
if (ok) {
|
|
||||||
return MccMountDefaultPECErrorCode::ERROR_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
++iter;
|
|
||||||
}
|
|
||||||
|
|
||||||
err = MccMountDefaultPECErrorCode::ERROR_EXCEED_MAX_ITERS;
|
|
||||||
}
|
|
||||||
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
pec_data_t _pecData;
|
pec_data_t _pecData;
|
||||||
@@ -320,7 +280,7 @@ private:
|
|||||||
pec_geom_coeffs_t& _geomCoeffs;
|
pec_geom_coeffs_t& _geomCoeffs;
|
||||||
pec_bspline_coeffs_t& _bsplCoeffs;
|
pec_bspline_coeffs_t& _bsplCoeffs;
|
||||||
|
|
||||||
mutable std::mutex _pecDataMutex;
|
std::unique_ptr<std::mutex> _pecDataMutex;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -328,6 +288,7 @@ typedef MccMountDefaultPEC<MccMountType::ALTAZ_TYPE> MccMountDefaultAltAzPec;
|
|||||||
typedef MccMountDefaultPEC<MccMountType::FORK_TYPE> MccMountDefaultForkPec;
|
typedef MccMountDefaultPEC<MccMountType::FORK_TYPE> MccMountDefaultForkPec;
|
||||||
|
|
||||||
static_assert(traits::mcc_mount_pec_c<MccMountDefaultForkPec>, "");
|
static_assert(traits::mcc_mount_pec_c<MccMountDefaultForkPec>, "");
|
||||||
|
static_assert(std::movable<MccMountDefaultForkPec>);
|
||||||
|
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -1,10 +1,14 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
|
||||||
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
/* PROHIBITED ZONE IMPLEMENTATION */
|
/* PROHIBITED ZONE IMPLEMENTATION */
|
||||||
|
|
||||||
|
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
|
#include <iostream>
|
||||||
#include <string_view>
|
#include <string_view>
|
||||||
|
|
||||||
#include "mcc_mount_concepts.h"
|
#include "mcc_mount_concepts.h"
|
||||||
@@ -29,6 +33,7 @@ template <MccAltLimitKind KIND = MccAltLimitKind::MIN_ALT_LIMIT>
|
|||||||
class MccAltLimitPZ
|
class MccAltLimitPZ
|
||||||
// class MccAltLimitPZ : public MccProhibitedZone
|
// class MccAltLimitPZ : public MccProhibitedZone
|
||||||
{
|
{
|
||||||
|
protected:
|
||||||
static constexpr auto pi2 = std::numbers::pi * 2.0;
|
static constexpr auto pi2 = std::numbers::pi * 2.0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
@@ -36,25 +41,40 @@ public:
|
|||||||
|
|
||||||
static constexpr MccAltLimitKind altLimitKind = KIND;
|
static constexpr MccAltLimitKind altLimitKind = KIND;
|
||||||
|
|
||||||
|
typedef double coord_t;
|
||||||
|
// typedef MccAngle coord_t;
|
||||||
// floating-point time duration (seconds)
|
// floating-point time duration (seconds)
|
||||||
typedef std::chrono::duration<double> duration_t;
|
typedef std::chrono::duration<double> duration_t;
|
||||||
|
|
||||||
typedef MccAngle coord_t;
|
|
||||||
typedef std::chrono::system_clock::time_point time_point_t;
|
typedef std::chrono::system_clock::time_point time_point_t;
|
||||||
|
|
||||||
static constexpr duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
static constexpr duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
||||||
static constexpr duration_t zeroDuration{0.0};
|
static constexpr duration_t zeroDuration{0.0};
|
||||||
|
|
||||||
//
|
MccAltLimitPZ(const coord_t& alt_limit, const coord_t& lat, traits::mcc_astrom_engine_c auto* astrom_engine)
|
||||||
// TODO: add context (e.g. TT-TAI, UT1-UTC, geo location and so on)!!!
|
|
||||||
MccAltLimitPZ(const MccAngle& alt_limit, const MccAngle& lat)
|
|
||||||
// : MccProhibitedZone(KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE"
|
// : MccProhibitedZone(KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE"
|
||||||
// : KIND == MccAltLimitKind::MAX_ALT_LIMIT ? "MAXALT-ZONE"
|
// : KIND == MccAltLimitKind::MAX_ALT_LIMIT ? "MAXALT-ZONE"
|
||||||
// : "ALTLIMIT-UNKNOWN"),
|
// : "ALTLIMIT-UNKNOWN"),
|
||||||
: _altLimit(alt_limit), _latitude(lat), _abs_lat(std::abs(_latitude)), _lat_lim(pi2 - _abs_lat)
|
: _altLimit(alt_limit), _latitude(lat), _abs_lat(std::abs(_latitude)), _lat_lim(pi2 - _abs_lat)
|
||||||
{
|
{
|
||||||
_lat_lim = pi2 - _abs_lat;
|
_lat_lim = pi2 - _abs_lat;
|
||||||
_altLimit.normalize<MccAngle::NORM_KIND_90_90>();
|
// _altLimit.normalize<MccAngle::NORM_KIND_90_90>();
|
||||||
|
_altLimit = MccAngle(_altLimit).normalize<MccAngle::NORM_KIND_90_90>();
|
||||||
|
|
||||||
|
using astrom_engine_t = std::remove_cvref_t<decltype(*astrom_engine)>;
|
||||||
|
using astrom_coord_t = typename astrom_engine_t::coord_t;
|
||||||
|
|
||||||
|
static_assert(std::convertible_to<coord_t, astrom_coord_t>,
|
||||||
|
"ASTROMETRY ENGINE AND THE ZONE COORDINATE TYPE ARE NOT COMPATIBLE!");
|
||||||
|
static_assert(std::convertible_to<astrom_coord_t, coord_t>,
|
||||||
|
"ASTROMETRY ENGINE AND THE ZONE COORDINATE TYPE ARE NOT COMPATIBLE!");
|
||||||
|
|
||||||
|
_coord2coord = [astrom_engine, this](MccCoordPairKind kind_from, coord_t x_from, coord_t y_from,
|
||||||
|
time_point_t tpoint, MccCoordPairKind kind_to, coord_t& x_to,
|
||||||
|
coord_t& y_to) {
|
||||||
|
auto err = astrom_engine->coord2coord(kind_from, std::move(x_from), std::move(y_from), tpoint, kind_to,
|
||||||
|
x_to, y_to, tpoint);
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -120,59 +140,142 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
bool inZone(traits::mcc_celestial_point_c auto const& target)
|
||||||
// bool inZone(const XT& x, const YT& y, traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
||||||
bool inZone(const XT& x, const YT& y)
|
|
||||||
{
|
{
|
||||||
static constexpr MccCoordPairKind coord_kind = traits::mcc_type_pair_hash<XT, YT>();
|
coord_t alt, az;
|
||||||
|
|
||||||
|
_coord2coord(target.coordPairKind, target.x, target.y, target.time_point, MccCoordPairKind::COORDS_KIND_AZALT,
|
||||||
|
az, alt);
|
||||||
|
|
||||||
if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZALT) { // trivial case
|
|
||||||
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
||||||
return y <= _altLimit;
|
return alt <= _altLimit;
|
||||||
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
||||||
return y >= _altLimit;
|
return alt >= _altLimit;
|
||||||
}
|
}
|
||||||
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZZD) { // trivial case
|
|
||||||
// return inZone(x, MccAngleALT(std::numbers::pi / 2 - (double)y), utc);
|
|
||||||
return inZone(x, MccAngleALT(std::numbers::pi / 2 - (double)y));
|
|
||||||
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
|
||||||
// implementation ...
|
|
||||||
return false;
|
|
||||||
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
|
||||||
// implementation ...
|
|
||||||
return false;
|
|
||||||
} else {
|
|
||||||
throw std::system_error(std::make_error_code(std::errc::operation_not_supported));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
duration_t timeTo(traits::mcc_celestial_point_c auto const& target)
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
|
||||||
duration_t timeTo(const XT& x,
|
|
||||||
const YT& y,
|
|
||||||
traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
||||||
{
|
{
|
||||||
return duration_t{std::numeric_limits<double>::infinity()};
|
coord_t ha, dec;
|
||||||
|
|
||||||
|
if (inZone(target)) {
|
||||||
|
return zeroDuration;
|
||||||
|
}
|
||||||
|
|
||||||
|
_coord2coord(target.coordPairKind, target.x, target.y, target.time_point,
|
||||||
|
MccCoordPairKind::COORDS_KIND_HADEC_APP, ha, dec);
|
||||||
|
|
||||||
|
if (!doesObjectReachZone(dec)) {
|
||||||
|
return infiniteDuration;
|
||||||
|
}
|
||||||
|
|
||||||
|
if constexpr (KIND ==
|
||||||
|
MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination
|
||||||
|
return compute(ha, dec, false);
|
||||||
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper
|
||||||
|
// culmination
|
||||||
|
return compute(ha, dec, true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
duration_t timeFrom(traits::mcc_celestial_point_c auto const& target)
|
||||||
|
{
|
||||||
|
coord_t ha, dec;
|
||||||
|
|
||||||
|
if (!inZone(target)) {
|
||||||
|
return zeroDuration;
|
||||||
|
}
|
||||||
|
|
||||||
|
_coord2coord(target.coordPairKind, target.x, target.y, target.time_point,
|
||||||
|
MccCoordPairKind::COORDS_KIND_HADEC_APP, ha, dec);
|
||||||
|
|
||||||
|
if (!doesObjectExitFromZone(dec)) {
|
||||||
|
return infiniteDuration;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!doesObjectReachZone(dec)) {
|
||||||
|
return zeroDuration;
|
||||||
|
}
|
||||||
|
|
||||||
|
if constexpr (KIND ==
|
||||||
|
MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one before upper culmination
|
||||||
|
return compute(ha, dec, true);
|
||||||
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one after upper
|
||||||
|
// culmination
|
||||||
|
return compute(ha, dec, false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute intersection point for the case of sideral-like moving
|
||||||
|
bool intersectPoint(traits::mcc_celestial_point_c auto const& target, traits::mcc_celestial_point_c auto& int_point)
|
||||||
|
{
|
||||||
|
coord_t ha, dec, az;
|
||||||
|
|
||||||
|
_coord2coord(target.coordPairKind, target.x, target.y, target.time_point,
|
||||||
|
MccCoordPairKind::COORDS_KIND_HADEC_APP, ha, dec);
|
||||||
|
|
||||||
|
// _altLimit = 0.001_degs;
|
||||||
|
// compute HA for intersection point
|
||||||
|
double cos_ha =
|
||||||
|
(std::sin(_altLimit) - std::sin(dec) * std::sin(_latitude)) / std::cos(dec) / std::cos(_latitude);
|
||||||
|
|
||||||
|
std::cout << "HA(inter) = " << MccAngle(ha).sexagesimal(true) << "\n";
|
||||||
|
std::cout << "DEC(inter) = " << MccAngle(dec).sexagesimal() << "\n";
|
||||||
|
std::cout << "HA(85.0) = " << MccAngle(acos(cos_ha)).sexagesimal(true) << "\n";
|
||||||
|
std::cout << "HA(85.0) = " << MccAngle(-acos(cos_ha)).sexagesimal(true) << "\n";
|
||||||
|
std::cout << "PHI = " << MccAngle(_latitude).sexagesimal() << "\n";
|
||||||
|
std::cout << "COS_HA = " << cos_ha << "\n";
|
||||||
|
|
||||||
|
if (cos_ha > 1.0) { // no intersection
|
||||||
|
// compute culmination points?
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
double cosA =
|
||||||
duration_t timeFrom(const XT& x,
|
(-std::sin(dec) * std::cos(_latitude) + std::cos(dec) * std::sin(_latitude) * cos_ha) / cos(_altLimit);
|
||||||
const YT& y,
|
// cosA /= std::cos(_altLimit);
|
||||||
traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
||||||
{
|
std::cout << "COS_A = " << cosA << "\n";
|
||||||
return duration_t{0.0};
|
|
||||||
|
double sinA = std::cos(dec) * sqrt(1.0 - cos_ha * cos_ha) / cos(_altLimit);
|
||||||
|
|
||||||
|
double tgA = sqrt(1.0 - cos_ha * cos_ha) / (cos(_latitude) * tan(dec) - sin(_latitude) * cos_ha);
|
||||||
|
|
||||||
|
auto z = (-std::sin(dec) * std::cos(_latitude) + std::cos(dec) * std::sin(_latitude) * cos_ha) / cosA;
|
||||||
|
// (-std::sin(dec) * std::cos(_latitude) + std::cos(dec) * std::sin(_latitude) * cos_ha) / cos(133.75_degs);
|
||||||
|
std::cout << "Z = " << MccAngle(asin(z)).sexagesimal() << "\n";
|
||||||
|
|
||||||
|
if constexpr (KIND ==
|
||||||
|
MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination
|
||||||
|
az = std::acos(cosA);
|
||||||
|
// az = atan2(sinA, cosA);
|
||||||
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper
|
||||||
|
// culmination
|
||||||
|
az = -std::acos(cosA) + std::numbers::pi; // to system of azimuth started from the North!!!
|
||||||
|
// az = atan(tgA);
|
||||||
|
// az = std::asin(sinA);
|
||||||
|
// az = atan2(sinA, cosA);
|
||||||
|
}
|
||||||
|
|
||||||
|
_coord2coord(MccCoordPairKind::COORDS_KIND_AZALT, az, _altLimit, target.time_point, int_point.coordPairKind,
|
||||||
|
int_point.x, int_point.y);
|
||||||
|
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
MccAngle _altLimit, _latitude, _abs_lat, _lat_lim;
|
coord_t _altLimit, _latitude, _abs_lat, _lat_lim;
|
||||||
|
|
||||||
bool doesObjectReachZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
// wrapper function
|
||||||
|
std::function<void(MccCoordPairKind, coord_t, coord_t, time_point_t, MccCoordPairKind, coord_t&, coord_t&)>
|
||||||
|
_coord2coord{};
|
||||||
|
|
||||||
|
bool doesObjectReachZone(const coord_t& dec_app)
|
||||||
{
|
{
|
||||||
// check for limit conditions
|
// check for limit conditions
|
||||||
auto dd = std::abs(telemetry_data.mntDEC);
|
auto dd = std::abs(dec_app);
|
||||||
|
|
||||||
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
||||||
dd += _altLimit;
|
dd += _altLimit;
|
||||||
@@ -191,11 +294,16 @@ private:
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool doesObjectReachZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
||||||
|
{
|
||||||
|
return doesObjectReachZone(telemetry_data.mntDEC);
|
||||||
|
}
|
||||||
|
|
||||||
bool doesObjectExitFromZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
||||||
|
bool doesObjectExitFromZone(const coord_t& dec_app)
|
||||||
{
|
{
|
||||||
// check for limit conditions
|
// check for limit conditions
|
||||||
auto dd = std::abs(telemetry_data.mntDEC);
|
auto dd = std::abs(dec_app);
|
||||||
|
|
||||||
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
||||||
dd -= _altLimit;
|
dd -= _altLimit;
|
||||||
@@ -214,23 +322,30 @@ private:
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
duration_t compute(traits::mcc_mount_telemetry_data_c auto const& telemetry_data, bool before_upper_culm)
|
bool doesObjectExitFromZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
||||||
{
|
{
|
||||||
double cos_ha = (std::sin(_altLimit) - std::sin(telemetry_data.mntDEC) * std::sin(_latitude)) /
|
return doesObjectExitFromZone(telemetry_data.mntDEC);
|
||||||
std::cos(telemetry_data.mntDEC) / std::cos(_latitude);
|
}
|
||||||
|
|
||||||
|
|
||||||
|
duration_t compute(const coord_t& ha_app, const coord_t& dec_app, bool before_upper_culm)
|
||||||
|
{
|
||||||
|
double cos_ha =
|
||||||
|
(std::sin(_altLimit) - std::sin(dec_app) * std::sin(_latitude)) / std::cos(dec_app) / std::cos(_latitude);
|
||||||
|
|
||||||
if (cos_ha > 1.0) { // should not be!
|
if (cos_ha > 1.0) { // should not be!
|
||||||
return infiniteDuration;
|
return infiniteDuration;
|
||||||
}
|
}
|
||||||
|
|
||||||
double ha;
|
double ha;
|
||||||
|
// WARNING: what about south hemisphere?!!!
|
||||||
if (before_upper_culm) {
|
if (before_upper_culm) {
|
||||||
ha = -std::acos(cos_ha); // HA before upper culmination
|
ha = -std::acos(cos_ha); // HA before upper culmination
|
||||||
} else {
|
} else {
|
||||||
ha = std::acos(cos_ha); // HA after upper culmination!!
|
ha = std::acos(cos_ha); // HA after upper culmination!!
|
||||||
}
|
}
|
||||||
|
|
||||||
MccAngle time_ang = ha - telemetry_data.mntHA; // in sideral time scale
|
coord_t time_ang = ha - ha_app; // in sideral time scale
|
||||||
|
|
||||||
if (time_ang < 0.0) { // next day
|
if (time_ang < 0.0) { // next day
|
||||||
time_ang += pi2;
|
time_ang += pi2;
|
||||||
@@ -238,11 +353,19 @@ private:
|
|||||||
|
|
||||||
time_ang /= mcc_sideral_to_UT1_ratio; // to UT1 time scale
|
time_ang /= mcc_sideral_to_UT1_ratio; // to UT1 time scale
|
||||||
|
|
||||||
return duration_t{time_ang.seconds()};
|
return duration_t{MccAngle(time_ang).seconds()};
|
||||||
|
// return duration_t{time_ang.seconds()};
|
||||||
|
}
|
||||||
|
|
||||||
|
duration_t compute(traits::mcc_mount_telemetry_data_c auto const& telemetry_data, bool before_upper_culm)
|
||||||
|
{
|
||||||
|
return compute(telemetry_data.mntHA, telemetry_data.mntDEC, before_upper_culm);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef MccAltLimitPZ<MccAltLimitKind::MIN_ALT_LIMIT> MccMinAltPZ;
|
typedef MccAltLimitPZ<MccAltLimitKind::MIN_ALT_LIMIT> MccMinAltPZ;
|
||||||
typedef MccAltLimitPZ<MccAltLimitKind::MAX_ALT_LIMIT> MccMaxAltPZ;
|
typedef MccAltLimitPZ<MccAltLimitKind::MAX_ALT_LIMIT> MccMaxAltPZ;
|
||||||
|
|
||||||
|
static_assert(std::movable<MccMinAltPZ>);
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
267
cxx/mcc_mount_pz_container.h
Normal file
267
cxx/mcc_mount_pz_container.h
Normal file
@@ -0,0 +1,267 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
|
/* A DEFAULT IMPLEMENTATION OF PROHIBITED ZONES HOLDER */
|
||||||
|
|
||||||
|
#include "mcc_mount_concepts.h"
|
||||||
|
|
||||||
|
namespace mcc
|
||||||
|
{
|
||||||
|
|
||||||
|
template <traits::mcc_mount_telemetry_data_c TelemetryDataT>
|
||||||
|
class MccPZoneContainer : public traits::MccPZoneAbstractContainer<TelemetryDataT>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
typedef TelemetryDataT telemetry_data_t;
|
||||||
|
|
||||||
|
// a type to which the result of calling prohibited zone class methods 'timeTo' and 'timeFrom' will be converted
|
||||||
|
typedef std::chrono::duration<double> duration_t; // seconds as floating-point number
|
||||||
|
|
||||||
|
// adaptor class for prohibited zones
|
||||||
|
struct MccPZoneWrapper {
|
||||||
|
using duration_t = MccPZoneContainer::duration_t;
|
||||||
|
|
||||||
|
static constexpr duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
||||||
|
static constexpr duration_t zeroDuration{0.0};
|
||||||
|
|
||||||
|
typedef std::function<bool(const TelemetryDataT&)> pz_inzone_func_t;
|
||||||
|
typedef std::function<duration_t(const TelemetryDataT&)> pz_timeto_func_t;
|
||||||
|
typedef std::function<duration_t(const TelemetryDataT&)> pz_timefrom_func_t;
|
||||||
|
|
||||||
|
MccCoordPairKind coordPairKind;
|
||||||
|
const std::function<std::string()> name;
|
||||||
|
pz_inzone_func_t inZone;
|
||||||
|
pz_timeto_func_t timeTo;
|
||||||
|
pz_timefrom_func_t timeFrom;
|
||||||
|
|
||||||
|
MccPZoneWrapper(MccPZoneWrapper&& other)
|
||||||
|
: inZone(std::move(other.inZone)), timeTo(std::move(other.timeTo)), timeFrom(std::move(other.timeFrom)) {};
|
||||||
|
|
||||||
|
MccPZoneWrapper& operator=(MccPZoneWrapper&& other)
|
||||||
|
{
|
||||||
|
if (this != &other) {
|
||||||
|
inZone = std::move(other.inZone);
|
||||||
|
timeTo = std::move(other.timeTo);
|
||||||
|
timeFrom = std::move(other.timeFrom);
|
||||||
|
}
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
};
|
||||||
|
|
||||||
|
MccPZoneWrapper() = default;
|
||||||
|
};
|
||||||
|
|
||||||
|
MccPZoneContainer() = default;
|
||||||
|
virtual ~MccPZoneContainer() = default;
|
||||||
|
|
||||||
|
|
||||||
|
size_t pzSize() const { return _pzWrapperVec.size(); }
|
||||||
|
|
||||||
|
// add zone/zones
|
||||||
|
template <traits::mcc_prohibited_zone_c<telemetry_data_t> ZT,
|
||||||
|
traits::mcc_prohibited_zone_c<telemetry_data_t>... ZTs>
|
||||||
|
size_t pzAddZone(ZT zone, ZTs... zones)
|
||||||
|
{
|
||||||
|
auto zone_ptr = std::make_shared<ZT>(std::move(zone));
|
||||||
|
|
||||||
|
using d_t = typename MccPZoneWrapper::duration_t;
|
||||||
|
|
||||||
|
_pzWrapperVec.emplace_back(
|
||||||
|
{.coordPairKind = ZT::zoneCoordPairKind,
|
||||||
|
.name = [zone_ptr]() { return std::format("{}", zone_ptr->name()); },
|
||||||
|
.inZone = [zone_ptr](const telemetry_data_t& tmry_data) { return zone_ptr->inZone(tmry_data); },
|
||||||
|
.timeTo =
|
||||||
|
[zone_ptr](const telemetry_data_t& tmry_data) {
|
||||||
|
auto d = zone_ptr->timeTo(tmry_data);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<typename ZT::duration_t, d_t>) {
|
||||||
|
return d;
|
||||||
|
} else {
|
||||||
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
}
|
||||||
|
},
|
||||||
|
.timeFrom =
|
||||||
|
[zone_ptr](const telemetry_data_t& tmry_data) {
|
||||||
|
auto d = zone_ptr->timeFrom(tmry_data);
|
||||||
|
|
||||||
|
if constexpr (std::same_as<typename ZT::duration_t, d_t>) {
|
||||||
|
return d;
|
||||||
|
} else {
|
||||||
|
if (d == ZT::infiniteDuration) {
|
||||||
|
return MccPZoneWrapper::infiniteDuration;
|
||||||
|
} else if (d == ZT::zeroDuration) {
|
||||||
|
return MccPZoneWrapper::zeroDuration;
|
||||||
|
}
|
||||||
|
return std::chrono::duration_cast<d_t>(d);
|
||||||
|
}
|
||||||
|
}});
|
||||||
|
|
||||||
|
|
||||||
|
if constexpr (sizeof...(ZTs)) {
|
||||||
|
pzAddZone(std::move(zones)...);
|
||||||
|
}
|
||||||
|
|
||||||
|
return _pzWrapperVec.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
void pzClearZones()
|
||||||
|
{
|
||||||
|
// stop mount here?!!
|
||||||
|
_pzWrapperVec.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// visitors
|
||||||
|
template <std::invocable<MccPZoneWrapper> FT>
|
||||||
|
auto pzForeachZone(FT&& func)
|
||||||
|
requires std::same_as<std::invoke_result_t<FT, MccPZoneWrapper>, void>
|
||||||
|
{
|
||||||
|
for (auto& wr : _pzWrapperVec) {
|
||||||
|
std::forward<FT>(func)(wr);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <std::invocable<MccPZoneWrapper> FT,
|
||||||
|
std::ranges::output_range<std::invoke_result_t<FT, MccPZoneWrapper>> ResT =
|
||||||
|
std::vector<std::invoke_result_t<FT, MccPZoneWrapper>>>
|
||||||
|
auto pzForeachZone(FT&& func)
|
||||||
|
requires(!std::same_as<std::invoke_result_t<FT, MccPZoneWrapper>, void>)
|
||||||
|
{
|
||||||
|
ResT result;
|
||||||
|
|
||||||
|
for (auto& wr : _pzWrapperVec) {
|
||||||
|
std::back_inserter(result) = std::forward<FT>(func)(wr);
|
||||||
|
}
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
template <std::ranges::output_range<bool> RT>
|
||||||
|
bool pzInZone(const telemetry_data_t& tdata, RT& result)
|
||||||
|
{
|
||||||
|
bool in_zone = false;
|
||||||
|
|
||||||
|
auto const p_tdata = &tdata;
|
||||||
|
|
||||||
|
result = pzForeachZone<RT>([&in_zone, p_tdata](auto& wr) {
|
||||||
|
bool r = wr.inZone(*p_tdata);
|
||||||
|
in_zone |= r;
|
||||||
|
|
||||||
|
return r;
|
||||||
|
});
|
||||||
|
|
||||||
|
return in_zone;
|
||||||
|
}
|
||||||
|
|
||||||
|
template <std::ranges::output_range<bool> RT>
|
||||||
|
bool pzInZone(traits::mcc_celestial_point_c auto const& target, RT& result)
|
||||||
|
{
|
||||||
|
bool in_zone = false;
|
||||||
|
|
||||||
|
auto const p_target = ⌖
|
||||||
|
|
||||||
|
result = pzForeachZone<RT>([&in_zone, p_target](auto& wr) {
|
||||||
|
bool r = wr.inZone(*p_target);
|
||||||
|
in_zone |= r;
|
||||||
|
|
||||||
|
return r;
|
||||||
|
});
|
||||||
|
|
||||||
|
return in_zone;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
std::vector<MccPZoneWrapper> _pzWrapperVec{};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class PZC
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
struct point_t {
|
||||||
|
typedef std::chrono::system_clock::time_point time_point_t;
|
||||||
|
typedef double coord_t;
|
||||||
|
|
||||||
|
time_point_t time_point;
|
||||||
|
MccCoordPairKind coordPairKind;
|
||||||
|
coord_t x, y;
|
||||||
|
};
|
||||||
|
|
||||||
|
static_assert(traits::mcc_celestial_point_c<point_t>);
|
||||||
|
|
||||||
|
template <typename ZT>
|
||||||
|
static inline std::unordered_map<const PZC*, std::vector<std::shared_ptr<ZT>>> _zones{};
|
||||||
|
|
||||||
|
std::vector<std::function<void()>> _clearFunc{};
|
||||||
|
|
||||||
|
// template <typename ZT, typename CPT>
|
||||||
|
// static inline std::unordered_map<const PZC*, std::function<std::vector<bool>(const CPT&)>> _inZoneFunc = [](){
|
||||||
|
|
||||||
|
// };
|
||||||
|
// template <typename ZT, typename CPT>
|
||||||
|
// static inline std::function<std::vector<bool>(const PZC*, const CPT&)> _inZoneFunc =
|
||||||
|
// [](const PZC* cont, const CPT& cp) {
|
||||||
|
// std::vector<bool> res;
|
||||||
|
|
||||||
|
// for (ZT& zone : _zones<ZT>[cont]) {
|
||||||
|
// res.emplace_back(zone.inZone(cp));
|
||||||
|
// }
|
||||||
|
|
||||||
|
// return res;
|
||||||
|
// };
|
||||||
|
|
||||||
|
std::vector<std::function<bool(const point_t&)>> _inZoneFunc;
|
||||||
|
|
||||||
|
public:
|
||||||
|
template <typename ZT>
|
||||||
|
size_t addZone(ZT zone)
|
||||||
|
{
|
||||||
|
auto sptr = std::make_shared<ZT>(std::move(zone));
|
||||||
|
|
||||||
|
_zones<ZT>[this].emplace_back(sptr);
|
||||||
|
|
||||||
|
if (_zones<ZT>[this].size() == 1) {
|
||||||
|
_clearFunc.emplace_back([this]() { _zones<ZT>[this].clear(); });
|
||||||
|
|
||||||
|
_inZoneFunc.emplace_back([sptr](const point_t& cp) { return sptr->inZone(cp); });
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename CPT, std::ranges::output_range<bool> RT>
|
||||||
|
bool inZone(const CPT& cp, RT& res)
|
||||||
|
{
|
||||||
|
using tp_t = typename point_t::time_point_t;
|
||||||
|
using cp_tp_t = typename CPT::time_point_t;
|
||||||
|
|
||||||
|
bool in_zone = false, r;
|
||||||
|
|
||||||
|
point_t pt{.coordPairKind = cp.coordPairKind, .x = cp.x, .y = cp.y};
|
||||||
|
if constexpr (traits::mcc_systime_c<cp_tp_t>) {
|
||||||
|
pt.time_point = std::chrono::time_point_cast<tp_t>(cp.time_point);
|
||||||
|
} else if constexpr (std::is_arithmetic_v<cp_tp_t>) {
|
||||||
|
pt.time_point = tp_t{std::chrono::duration<tp_t::rep>(static_cast<tp_t::rep>(cp.time_point))};
|
||||||
|
} else {
|
||||||
|
static_assert(false, "INVALID TYPE!");
|
||||||
|
}
|
||||||
|
|
||||||
|
res = RT();
|
||||||
|
for (auto& func : _inZoneFunc) {
|
||||||
|
r = func(pt);
|
||||||
|
in_zone |= r;
|
||||||
|
|
||||||
|
std::back_inserter(res) = r;
|
||||||
|
}
|
||||||
|
|
||||||
|
return in_zone;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace mcc
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user