Compare commits
188 Commits
a96682bc12
...
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 |
@@ -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);
|
||||||
@@ -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
|
||||||
|
|||||||
@@ -1,64 +0,0 @@
|
|||||||
/*
|
|
||||||
* This file is part of the libsidservo project.
|
|
||||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
#include "sidservo.h"
|
|
||||||
|
|
||||||
extern conf_t Conf;
|
|
||||||
|
|
||||||
// unused arguments of functions
|
|
||||||
#define _U_ __attribute__((__unused__))
|
|
||||||
// break absent in `case`
|
|
||||||
#define FALLTHRU __attribute__ ((fallthrough))
|
|
||||||
// and synonym for FALLTHRU
|
|
||||||
#define NOBREAKHERE __attribute__ ((fallthrough))
|
|
||||||
// weak functions
|
|
||||||
#define WEAK __attribute__ ((weak))
|
|
||||||
|
|
||||||
#ifndef DBL_EPSILON
|
|
||||||
#define DBL_EPSILON (2.2204460492503131e-16)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef FALSE
|
|
||||||
#define FALSE (0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef TRUE
|
|
||||||
#define TRUE (1)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
#ifdef EBUG
|
|
||||||
#include <stdio.h>
|
|
||||||
#define COLOR_RED "\033[1;31;40m"
|
|
||||||
#define COLOR_GREEN "\033[1;32;40m"
|
|
||||||
#define COLOR_OLD "\033[0;0;0m"
|
|
||||||
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
|
||||||
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
|
||||||
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
|
|
||||||
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
|
|
||||||
fprintf(stderr, __VA_ARGS__); \
|
|
||||||
fprintf(stderr, "\n");} while(0)
|
|
||||||
|
|
||||||
#else // EBUG
|
|
||||||
#define FNAME()
|
|
||||||
#define DBG(...)
|
|
||||||
#endif // EBUG
|
|
||||||
@@ -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,12 +103,12 @@ 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");
|
green("Other:\n");
|
||||||
@@ -143,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
|
||||||
Mount.moveTo(&tag);
|
if(MCC_E_OK != Mount.moveTo(&tag)){
|
||||||
|
Mount.emergStop();
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
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");
|
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
|
||||||
|
|||||||
@@ -25,46 +25,90 @@
|
|||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "movingmodel.h"
|
#include "movingmodel.h"
|
||||||
#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
|
// parameters for model
|
||||||
static movemodel_t *Xmodel, *Ymodel;
|
static movemodel_t *Xmodel, *Ymodel;
|
||||||
|
// limits for model and/or real mount (in latter case data should be read from mount on init)
|
||||||
// radians, rad/sec, rad/sec^2
|
// radians, rad/sec, rad/sec^2
|
||||||
static limits_t
|
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||||
|
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||||
|
limits_t
|
||||||
Xlimits = {
|
Xlimits = {
|
||||||
.min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
|
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||||
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
|
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
|
||||||
Ylimits = {
|
Ylimits = {
|
||||||
.min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
|
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||||
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
|
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
|
||||||
;
|
;
|
||||||
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
||||||
|
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief nanotime - monotonic time from first run
|
* @brief curtime - monotonic time from first run
|
||||||
* @return time in seconds
|
* @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
|
||||||
|
* @return TRUE if all OK
|
||||||
|
* FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
|
||||||
*/
|
*/
|
||||||
double nanotime(){
|
int curtime(struct timespec *t){
|
||||||
static struct timespec *start = NULL;
|
|
||||||
struct timespec now;
|
struct timespec now;
|
||||||
if(!start){
|
if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
|
||||||
start = malloc(sizeof(struct timespec));
|
now.tv_sec += timeadder.tv_sec;
|
||||||
if(!start) return -1.;
|
now.tv_nsec += timeadder.tv_nsec;
|
||||||
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
|
if(now.tv_nsec > 999999999L){
|
||||||
|
++now.tv_sec;
|
||||||
|
now.tv_nsec -= 1000000000L;
|
||||||
}
|
}
|
||||||
|
if(t) *t = now;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// init starttime; @return TRUE if all OK
|
||||||
|
static int initstarttime(){
|
||||||
|
struct timespec start;
|
||||||
|
if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
|
||||||
|
if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
|
||||||
|
timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
|
||||||
|
timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
|
||||||
|
if(timeadder.tv_nsec < 0){
|
||||||
|
--timeadder.tv_sec;
|
||||||
|
timeadder.tv_nsec += 1000000000L;
|
||||||
|
}
|
||||||
|
curtime(&t0);
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return difference (in seconds) between time1 and time0
|
||||||
|
double timediff(const struct timespec *time1, const struct timespec *time0){
|
||||||
|
if(!time1 || !time0) return -1.;
|
||||||
|
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
|
||||||
|
}
|
||||||
|
// difference between given time and last initstarttime() call
|
||||||
|
double timediff0(const struct timespec *time1){
|
||||||
|
return timediff(time1, &t0);
|
||||||
|
}
|
||||||
|
// time from last initstarttime() call
|
||||||
|
double timefromstart(){
|
||||||
|
struct timespec now;
|
||||||
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
|
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
|
||||||
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
|
||||||
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
|
||||||
return sd + nd;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief quit - close all opened and return to default state
|
* @brief quit - close all opened and return to default state
|
||||||
|
* TODO: close serial devices even in "model" mode
|
||||||
*/
|
*/
|
||||||
static void quit(){
|
static void quit(){
|
||||||
if(Conf.RunModel) return;
|
if(Conf.RunModel) return;
|
||||||
@@ -74,22 +118,67 @@ static void quit(){
|
|||||||
DBG("Exit");
|
DBG("Exit");
|
||||||
}
|
}
|
||||||
|
|
||||||
void getModData(mountdata_t *mountdata){
|
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
|
||||||
if(!mountdata || !Xmodel || !Ymodel) return;
|
if(!c || !Xmodel || !Ymodel) return;
|
||||||
double tnow = nanotime();
|
double tnow = timefromstart();
|
||||||
moveparam_t Xp, Yp;
|
moveparam_t Xp, Yp;
|
||||||
movestate_t Xst = Xmodel->get_state(&Xp);
|
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
|
||||||
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(&Xp, tnow);
|
//DBG("Xstate = %d", Xst);
|
||||||
movestate_t Yst = Ymodel->get_state(&Yp);
|
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
|
||||||
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(&Yp, tnow);
|
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
|
||||||
bzero(mountdata, sizeof(mountdata_t));
|
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
|
||||||
mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
|
c->X = Xp.coord;
|
||||||
mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord;
|
c->Y = Yp.coord;
|
||||||
mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord;
|
if(xst) *xst = Xst;
|
||||||
mountdata->encXspeed.t = mountdata->encYspeed.t = tnow;
|
if(yst) *yst = Yst;
|
||||||
mountdata->encXspeed.val = Xp.speed;
|
}
|
||||||
mountdata->encYspeed.val = Yp.speed;
|
|
||||||
mountdata->millis = (uint32_t)(tnow * 1e3);
|
/**
|
||||||
|
* 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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -100,15 +189,20 @@ void getModData(mountdata_t *mountdata){
|
|||||||
static mcc_errcodes_t init(conf_t *c){
|
static mcc_errcodes_t init(conf_t *c){
|
||||||
FNAME();
|
FNAME();
|
||||||
if(!c) return MCC_E_BADFORMAT;
|
if(!c) return MCC_E_BADFORMAT;
|
||||||
|
if(!initstarttime()) return MCC_E_FAILED;
|
||||||
Conf = *c;
|
Conf = *c;
|
||||||
mcc_errcodes_t ret = MCC_E_OK;
|
mcc_errcodes_t ret = MCC_E_OK;
|
||||||
Xmodel = model_init(&Xlimits);
|
Xmodel = model_init(&Xlimits);
|
||||||
Ymodel = model_init(&Ylimits);
|
Ymodel = model_init(&Ylimits);
|
||||||
|
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||||
|
DBG("Bad value of MountReqInterval");
|
||||||
|
ret = MCC_E_BADFORMAT;
|
||||||
|
}
|
||||||
if(Conf.RunModel){
|
if(Conf.RunModel){
|
||||||
if(!Xmodel || !Ymodel) return MCC_E_FAILED;
|
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
|
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
|
||||||
DBG("Define mount device path and speed");
|
DBG("Define mount device path and speed");
|
||||||
ret = MCC_E_BADFORMAT;
|
ret = MCC_E_BADFORMAT;
|
||||||
}else if(!openMount()){
|
}else if(!openMount()){
|
||||||
@@ -124,53 +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(Conf.RunModel) return ... ;
|
mountdata_t d;
|
||||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
if(MCC_E_OK == getMD(&d)){
|
||||||
//...
|
axis_status_t newx = d.Xstate, newy = d.Ystate;
|
||||||
setStat(MNT_SLEWING, MNT_SLEWING);
|
//DBG("old state: %d/%d", d.Xstate, d.Ystate);
|
||||||
//...
|
if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
|
||||||
return MCC_E_FAILED;
|
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
|
||||||
@@ -180,25 +283,18 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
|||||||
static mcc_errcodes_t move2(const coordpair_t *target){
|
static mcc_errcodes_t move2(const coordpair_t *target){
|
||||||
if(!target) return MCC_E_BADFORMAT;
|
if(!target) 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(Conf.RunModel){
|
|
||||||
double curt = nanotime();
|
|
||||||
moveparam_t param = {0};
|
|
||||||
param.coord = target->X; param.speed = MCC_MAX_X_SPEED;
|
|
||||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
|
||||||
param.coord = target->Y; param.speed = MCC_MAX_Y_SPEED;
|
|
||||||
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
|
||||||
}
|
|
||||||
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};
|
||||||
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -227,14 +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;
|
||||||
if(Conf.RunModel){
|
// updateMotorPos() here can make a problem; TODO: remove?
|
||||||
double curt = nanotime();
|
|
||||||
moveparam_t param = {0};
|
|
||||||
param.coord = target->X; param.speed = speed->X;
|
|
||||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
|
||||||
param.coord = target->Y; param.speed = speed->Y;
|
|
||||||
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
|
||||||
}
|
|
||||||
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;
|
||||||
@@ -243,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;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -252,10 +341,11 @@ 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){
|
if(Conf.RunModel){
|
||||||
double curt = nanotime();
|
double curt = timefromstart();
|
||||||
Xmodel->emergency_stop(curt);
|
Xmodel->emergency_stop(Xmodel, curt);
|
||||||
Ymodel->emergency_stop(curt);
|
Ymodel->emergency_stop(Ymodel, curt);
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
||||||
@@ -263,10 +353,11 @@ static mcc_errcodes_t emstop(){
|
|||||||
}
|
}
|
||||||
// normal stop
|
// normal stop
|
||||||
static mcc_errcodes_t stop(){
|
static mcc_errcodes_t stop(){
|
||||||
|
FNAME();
|
||||||
if(Conf.RunModel){
|
if(Conf.RunModel){
|
||||||
double curt = nanotime();
|
double curt = timefromstart();
|
||||||
Xmodel->stop(curt);
|
Xmodel->stop(Xmodel, curt);
|
||||||
Ymodel->stop(curt);
|
Ymodel->stop(Ymodel,curt);
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
||||||
@@ -280,7 +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) return MCC_E_FAILED;
|
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);
|
||||||
@@ -292,6 +392,7 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -302,7 +403,16 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
|
|||||||
*/
|
*/
|
||||||
static mcc_errcodes_t longcmd(long_command_t *cmd){
|
static mcc_errcodes_t longcmd(long_command_t *cmd){
|
||||||
if(!cmd) return MCC_E_BADFORMAT;
|
if(!cmd) return MCC_E_BADFORMAT;
|
||||||
if(Conf.RunModel) return MCC_E_FAILED;
|
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);
|
||||||
@@ -313,6 +423,7 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -320,6 +431,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
if(!hwConfig) return MCC_E_BADFORMAT;
|
if(!hwConfig) return MCC_E_BADFORMAT;
|
||||||
if(Conf.RunModel) return MCC_E_FAILED;
|
if(Conf.RunModel) return MCC_E_FAILED;
|
||||||
SSconfig config;
|
SSconfig config;
|
||||||
|
DBG("Read HW configuration");
|
||||||
if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
|
if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
|
||||||
// Convert acceleration (ticks per loop^2 to rad/s^2)
|
// Convert acceleration (ticks per loop^2 to rad/s^2)
|
||||||
hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
|
hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
|
||||||
@@ -359,8 +471,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
// Copy ticks per revolution
|
// Copy ticks per revolution
|
||||||
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
|
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
|
||||||
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
|
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
|
||||||
hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less
|
hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
|
||||||
hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
|
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
|
||||||
// Convert slew rates (ticks per loop to rad/s)
|
// Convert slew rates (ticks per loop to rad/s)
|
||||||
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
||||||
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
||||||
@@ -378,6 +490,30 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
|
hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
|
||||||
// Convert backlash speed (ticks per loop to rad/s)
|
// Convert backlash speed (ticks per loop to rad/s)
|
||||||
hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
|
hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
|
||||||
|
// now read text commands
|
||||||
|
int64_t i64;
|
||||||
|
double Xticks, Yticks;
|
||||||
|
DBG("SERIAL");
|
||||||
|
// motor's encoder ticks per rev
|
||||||
|
if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED;
|
||||||
|
Xticks = ((double) i64); // divide by 4 as these values stored ???
|
||||||
|
if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED;
|
||||||
|
Yticks = ((double) i64);
|
||||||
|
X_ENC_ZERO = Conf.XEncZero;
|
||||||
|
Y_ENC_ZERO = Conf.YEncZero;
|
||||||
|
DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
|
||||||
|
X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
|
||||||
|
Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
|
||||||
|
DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
|
||||||
|
// axis encoder ticks per rev
|
||||||
|
if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
|
||||||
|
Xticks = (double) i64;
|
||||||
|
if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
|
||||||
|
Yticks = (double) i64;
|
||||||
|
DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
|
||||||
|
X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
|
||||||
|
Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
|
||||||
|
DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -433,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,
|
||||||
@@ -453,5 +609,13 @@ mount_t Mount = {
|
|||||||
.longCmd = longcmd,
|
.longCmd = longcmd,
|
||||||
.getHWconfig = get_hwconf,
|
.getHWconfig = get_hwconf,
|
||||||
.saveHWconfig = write_hwconf,
|
.saveHWconfig = write_hwconf,
|
||||||
.currentT = nanotime,
|
.currentT = curtime,
|
||||||
|
.timeFromStart = timefromstart,
|
||||||
|
.timeDiff = timediff,
|
||||||
|
.timeDiff0 = timediff0,
|
||||||
|
.correctTo = correct2,
|
||||||
|
.getMaxSpeed = maxspeed,
|
||||||
|
.getMinSpeed = minspeed,
|
||||||
|
.getAcceleration = acceleration,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -24,18 +24,29 @@
|
|||||||
|
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "movingmodel.h"
|
||||||
#include "sidservo.h"
|
#include "sidservo.h"
|
||||||
|
|
||||||
extern conf_t Conf;
|
extern conf_t Conf;
|
||||||
double nanotime();
|
extern limits_t Xlimits, Ylimits;
|
||||||
void getModData(mountdata_t *mountdata);
|
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))
|
||||||
|
|
||||||
@@ -59,7 +70,7 @@ void getModData(mountdata_t *mountdata);
|
|||||||
#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
|
|
||||||
|
|
||||||
@@ -36,9 +36,11 @@ static void chkminmax(double *min, double *max){
|
|||||||
|
|
||||||
movemodel_t *model_init(limits_t *l){
|
movemodel_t *model_init(limits_t *l){
|
||||||
if(!l) return FALSE;
|
if(!l) return FALSE;
|
||||||
movemodel_t *model = calloc(1, sizeof(movemodel_t));
|
movemodel_t *m = calloc(1, sizeof(movemodel_t));
|
||||||
memcpy(model, &trapez, sizeof(movemodel_t));
|
// we can't use memcpy or assign as Times/Params would be common for all
|
||||||
if(!model->init_limits) goto fail;
|
*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;
|
moveparam_t *max = &l->max, *min = &l->min;
|
||||||
if(min->speed < 0.) min->speed = -min->speed;
|
if(min->speed < 0.) min->speed = -min->speed;
|
||||||
if(max->speed < 0.) max->speed = -max->speed;
|
if(max->speed < 0.) max->speed = -max->speed;
|
||||||
@@ -47,17 +49,25 @@ movemodel_t *model_init(limits_t *l){
|
|||||||
chkminmax(&min->coord, &max->coord);
|
chkminmax(&min->coord, &max->coord);
|
||||||
chkminmax(&min->speed, &max->speed);
|
chkminmax(&min->speed, &max->speed);
|
||||||
chkminmax(&min->accel, &max->accel);
|
chkminmax(&min->accel, &max->accel);
|
||||||
if(!model->init_limits(l)) return NULL;
|
m->Min = l->min;
|
||||||
return model;
|
m->Max = l->max;
|
||||||
fail:
|
m->movingstage = STAGE_STOPPED;
|
||||||
free(model); return NULL;
|
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){
|
int model_move2(movemodel_t *model, moveparam_t *target, double t){
|
||||||
if(!target || !model) return FALSE;
|
if(!target || !model) return FALSE;
|
||||||
DBG("MOVE to %g at speed %g", target->coord, target->speed);
|
DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
|
||||||
// only positive velocity
|
// only positive velocity
|
||||||
if(target->speed < 0.) target->speed = -target->speed;
|
if(target->speed < 0.) target->speed = -target->speed;
|
||||||
|
if(fabs(target->speed) < model->Min.speed){
|
||||||
|
DBG("STOP");
|
||||||
|
model->stop(model, t);
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
// don't mind about acceleration - user cannot set it now
|
// don't mind about acceleration - user cannot set it now
|
||||||
return model->calculate(target, t);
|
return model->calculate(model, target, t);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -17,12 +17,13 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
#include "sidservo.h"
|
#include "sidservo.h"
|
||||||
|
|
||||||
// tolerance, time ticks
|
// tolerance, time ticks
|
||||||
#define COORD_TOLERANCE_DEFAULT (0.01)
|
#define COORD_TOLERANCE_DEFAULT (1e-8)
|
||||||
#define COORD_TOLERANCE_MIN (0.0001)
|
#define COORD_TOLERANCE_MIN (1e-12)
|
||||||
#define COORD_TOLERANCE_MAX (10.)
|
#define COORD_TOLERANCE_MAX (10.)
|
||||||
#define TIME_TICK_DEFAULT (0.0001)
|
#define TIME_TICK_DEFAULT (0.0001)
|
||||||
#define TIME_TICK_MIN (1e-9)
|
#define TIME_TICK_MIN (1e-9)
|
||||||
@@ -43,17 +44,32 @@ typedef struct{
|
|||||||
typedef struct{
|
typedef struct{
|
||||||
moveparam_t min;
|
moveparam_t min;
|
||||||
moveparam_t max;
|
moveparam_t max;
|
||||||
double acceleration;
|
//double acceleration;
|
||||||
} limits_t;
|
} limits_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef enum{
|
||||||
int (*init_limits)(limits_t *lim); // init values of limits, jerk
|
STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed
|
||||||
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
STAGE_MAXSPEED, // go with target speed
|
||||||
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
|
STAGE_DECEL, // go from target speed to zero
|
||||||
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
|
STAGE_STOPPED, // stop
|
||||||
void (*stop)(double t); // stop by ramp
|
STAGE_AMOUNT
|
||||||
void (*emergency_stop)(double t); // stop with highest acceleration
|
} movingstage_t;
|
||||||
double (*stoppenanotime)(); // time when moving will ends
|
|
||||||
|
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;
|
||||||
|
|
||||||
movemodel_t *model_init(limits_t *l);
|
movemodel_t *model_init(limits_t *l);
|
||||||
|
|||||||
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;
|
||||||
|
}
|
||||||
@@ -24,54 +24,148 @@
|
|||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "ramp.h"
|
#include "ramp.h"
|
||||||
|
|
||||||
|
#ifdef EBUG
|
||||||
|
#undef DBG
|
||||||
|
#define DBG(...)
|
||||||
|
#undef FNAME
|
||||||
|
#define FNAME()
|
||||||
|
#endif
|
||||||
|
|
||||||
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||||
static movestate_t state = ST_STOP;
|
|
||||||
static moveparam_t Min, Max;
|
|
||||||
|
|
||||||
typedef enum{
|
static void emstop(movemodel_t *m, double _U_ t){
|
||||||
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
|
FNAME();
|
||||||
STAGE_MAXSPEED, // go with target speed
|
pthread_mutex_lock(&m->mutex);
|
||||||
STAGE_DECEL, // go from target speed to zero
|
m->curparams.accel = 0.;
|
||||||
STAGE_STOPPED, // stop
|
m->curparams.speed = 0.;
|
||||||
STAGE_AMOUNT
|
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
|
||||||
} movingstage_t;
|
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
|
||||||
|
m->state = ST_STOP;
|
||||||
static movingstage_t movingstage = STAGE_STOPPED;
|
m->movingstage = STAGE_STOPPED;
|
||||||
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
|
pthread_mutex_unlock(&m->mutex);
|
||||||
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){
|
static void stop(movemodel_t *m, double t){
|
||||||
curparams.accel = 0.;
|
FNAME();
|
||||||
curparams.speed = 0.;
|
pthread_mutex_lock(&m->mutex);
|
||||||
bzero(Times, sizeof(Times));
|
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
|
||||||
bzero(Params, sizeof(Params));
|
m->movingstage = STAGE_DECEL;
|
||||||
state = ST_STOP;
|
m->state = ST_MOVE;
|
||||||
movingstage = STAGE_STOPPED;
|
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;
|
||||||
static void stop(double t){
|
else m->Params[STAGE_DECEL].accel = m->Max.accel;
|
||||||
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
|
m->Params[STAGE_DECEL].coord = m->curparams.coord;
|
||||||
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
|
// 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;
|
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
|
// 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;
|
double dt = m->Times[STAGE_STOPPED] - t;
|
||||||
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
|
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
|
||||||
Params[STAGE_DECEL].accel * dt * dt / 2.;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -80,141 +174,86 @@ static void stop(double t){
|
|||||||
* @param t - current time value
|
* @param t - current time value
|
||||||
* @return FALSE if can't move with given parameters
|
* @return FALSE if can't move with given parameters
|
||||||
*/
|
*/
|
||||||
static int calc(moveparam_t *x, double t){
|
static int calc(movemodel_t *m, moveparam_t *x, double t) {
|
||||||
if(!x) return FALSE;
|
//DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
|
||||||
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
if (!x || !m) return FALSE;
|
||||||
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
|
pthread_mutex_lock(&m->mutex);
|
||||||
double Dx = fabs(x->coord - curparams.coord); // full distance
|
int ret = FALSE;
|
||||||
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
|
// Validate input parameters
|
||||||
// we have two variants: with or without stage with constant speed
|
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
|
||||||
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
|
DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
|
||||||
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
|
goto ret;
|
||||||
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){
|
|
||||||
DBG("distance too short");
|
|
||||||
return FALSE;
|
|
||||||
}
|
}
|
||||||
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
|
if(x->speed < m->Min.speed || x->speed > m->Max.speed){
|
||||||
DBG("Distance good to just stop");
|
DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
|
||||||
stop(t);
|
goto ret;
|
||||||
return TRUE;
|
|
||||||
}
|
}
|
||||||
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
|
ret = TRUE; // now there's no chanses to make error
|
||||||
// after stop we will have full profile
|
unlockedcalc(m, x, t);
|
||||||
double dxs3 = Dx - dx0s;
|
// Debug output
|
||||||
double newspeed = sqrt(Max.accel * dxs3);
|
/*for(int i = 0; i < STAGE_AMOUNT; i++){
|
||||||
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
|
DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
|
||||||
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
|
i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
|
||||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
}*/
|
||||||
Params[0].accel = sign * Max.accel;
|
ret:
|
||||||
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
|
pthread_mutex_unlock(&m->mutex);
|
||||||
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
|
return ret;
|
||||||
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){
|
|
||||||
DBG("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){
|
|
||||||
DBG("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){
|
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
|
||||||
if(state == ST_STOP) goto ret;
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
if(m->state == ST_STOP) goto ret;
|
||||||
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||||
if(Times[s] <= t){ // check time for current stage
|
if(m->Times[s] <= t){ // check time for current stage
|
||||||
movingstage = s;
|
m->movingstage = s;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(movingstage == STAGE_STOPPED){
|
if(m->movingstage == STAGE_STOPPED){
|
||||||
curparams.coord = Params[STAGE_STOPPED].coord;
|
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
|
||||||
emstop(t);
|
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;
|
goto ret;
|
||||||
}
|
}
|
||||||
// calculate current parameters
|
// calculate current parameters
|
||||||
double dt = t - Times[movingstage];
|
double dt = t - m->Times[m->movingstage];
|
||||||
double a = Params[movingstage].accel;
|
double a = m->Params[m->movingstage].accel;
|
||||||
double v0 = Params[movingstage].speed;
|
double v0 = m->Params[m->movingstage].speed;
|
||||||
double x0 = Params[movingstage].coord;
|
double x0 = m->Params[m->movingstage].coord;
|
||||||
curparams.accel = a;
|
m->curparams.accel = a;
|
||||||
curparams.speed = v0 + a * dt;
|
m->curparams.speed = v0 + a * dt;
|
||||||
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||||
ret:
|
ret:
|
||||||
if(next) *next = curparams;
|
if(next) *next = m->curparams;
|
||||||
return state;
|
movestate_t st = m->state;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return st;
|
||||||
}
|
}
|
||||||
|
|
||||||
static movestate_t getst(moveparam_t *cur){
|
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
|
||||||
if(cur) *cur = curparams;
|
pthread_mutex_lock(&m->mutex);
|
||||||
return state;
|
if(cur) *cur = m->curparams;
|
||||||
|
movestate_t st = m->state;
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return st;
|
||||||
}
|
}
|
||||||
|
|
||||||
static double gettstop(){
|
static double gettstop(movemodel_t *m){
|
||||||
return Times[STAGE_STOPPED];
|
pthread_mutex_lock(&m->mutex);
|
||||||
|
double r = m->Times[STAGE_STOPPED];
|
||||||
|
pthread_mutex_unlock(&m->mutex);
|
||||||
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
movemodel_t trapez = {
|
movemodel_t trapez = {
|
||||||
.init_limits = initlims,
|
|
||||||
.stop = stop,
|
.stop = stop,
|
||||||
.emergency_stop = emstop,
|
.emergency_stop = emstop,
|
||||||
.get_state = getst,
|
.get_state = getst,
|
||||||
.calculate = calc,
|
.calculate = calc,
|
||||||
.proc_move = proc,
|
.proc_move = proc,
|
||||||
.stoppenanotime = gettstop,
|
.stoppedtime = gettstop,
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -20,6 +20,7 @@
|
|||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
#include <poll.h>
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
#include <signal.h>
|
#include <signal.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
@@ -33,13 +34,14 @@
|
|||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "movingmodel.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,
|
||||||
@@ -47,7 +49,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
|||||||
// encoders thread and mount thread
|
// encoders thread and mount thread
|
||||||
static pthread_t encthread, mntthread;
|
static pthread_t encthread, mntthread;
|
||||||
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
|
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
|
||||||
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
|
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
|
||||||
// encoders raw data
|
// encoders raw data
|
||||||
typedef struct __attribute__((packed)){
|
typedef struct __attribute__((packed)){
|
||||||
uint8_t magick;
|
uint8_t magick;
|
||||||
@@ -57,17 +59,31 @@ typedef struct __attribute__((packed)){
|
|||||||
} enc_t;
|
} enc_t;
|
||||||
|
|
||||||
// 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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -75,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
|
||||||
@@ -110,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
|
||||||
@@ -128,33 +145,53 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
|
|||||||
* @param t - measurement time
|
* @param t - measurement time
|
||||||
* @return amount of data read or 0 if problem
|
* @return amount of data read or 0 if problem
|
||||||
*/
|
*/
|
||||||
static int getencval(int fd, double *val, double *t){
|
static int getencval(int fd, double *val, struct timespec *t){
|
||||||
if(fd < 0) return FALSE;
|
if(fd < 0){
|
||||||
|
DBG("Encoder fd < 0!");
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
char buf[128];
|
char buf[128];
|
||||||
int got = 0, Lmax = 127;
|
int got = 0, Lmax = 127;
|
||||||
double t0 = nanotime();
|
double t0 = timefromstart();
|
||||||
|
//DBG("start: %.6g", t0);
|
||||||
do{
|
do{
|
||||||
fd_set rfds;
|
fd_set rfds;
|
||||||
FD_ZERO(&rfds);
|
FD_ZERO(&rfds);
|
||||||
FD_SET(fd, &rfds);
|
FD_SET(fd, &rfds);
|
||||||
struct timeval tv = encRtmout;
|
struct timeval tv = encRtmout;
|
||||||
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
|
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
|
||||||
if(!retval) continue;
|
if(!retval){
|
||||||
|
//DBG("select()==0 - timeout, %.6g", timefromstart());
|
||||||
|
break;
|
||||||
|
}
|
||||||
if(retval < 0){
|
if(retval < 0){
|
||||||
if(errno == EINTR) continue;
|
if(errno == EINTR){
|
||||||
|
DBG("EINTR");
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
DBG("select() < 0");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
if(FD_ISSET(fd, &rfds)){
|
if(FD_ISSET(fd, &rfds)){
|
||||||
ssize_t l = read(fd, &buf[got], Lmax);
|
ssize_t l = read(fd, &buf[got], Lmax);
|
||||||
if(l < 1) return 0; // disconnected ??
|
if(l < 1){
|
||||||
|
DBG("read() < 0");
|
||||||
|
return 0; // disconnected ??
|
||||||
|
}
|
||||||
got += l; Lmax -= l;
|
got += l; Lmax -= l;
|
||||||
buf[got] = 0;
|
buf[got] = 0;
|
||||||
} else continue;
|
} else continue;
|
||||||
if(strchr(buf, '\n')) break;
|
if(buf[got-1] == '\n') break; // got EOL as last symbol
|
||||||
}while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval);
|
}while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
|
||||||
if(got == 0) return 0; // WTF?
|
if(got == 0){
|
||||||
|
//DBG("No data from encoder, tfs=%.6g", timefromstart());
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
char *estr = strrchr(buf, '\n');
|
char *estr = strrchr(buf, '\n');
|
||||||
if(!estr) return 0;
|
if(!estr){
|
||||||
|
DBG("No EOL");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
*estr = 0;
|
*estr = 0;
|
||||||
char *bgn = strrchr(buf, '\n');
|
char *bgn = strrchr(buf, '\n');
|
||||||
if(bgn) ++bgn;
|
if(bgn) ++bgn;
|
||||||
@@ -166,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;
|
||||||
@@ -216,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;
|
||||||
@@ -239,13 +302,14 @@ static void *encoderthread1(void _U_ *u){
|
|||||||
if((uint8_t)b == ENC_MAGICK){
|
if((uint8_t)b == ENC_MAGICK){
|
||||||
// DBG("Got magic -> start filling packet");
|
// DBG("Got magic -> start filling packet");
|
||||||
databuf[wridx++] = (uint8_t) b;
|
databuf[wridx++] = (uint8_t) b;
|
||||||
t = nanotime();
|
|
||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
}else databuf[wridx++] = (uint8_t) b;
|
}else databuf[wridx++] = (uint8_t) b;
|
||||||
if(wridx == ENC_DATALEN){
|
if(wridx == ENC_DATALEN){
|
||||||
parse_encbuf(databuf, t);
|
if(curtime(&tcur)){
|
||||||
wridx = 0;
|
parse_encbuf(databuf, &tcur);
|
||||||
|
wridx = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(encfd[0] > -1){
|
if(encfd[0] > -1){
|
||||||
@@ -255,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 = nanotime();
|
do{ // main cycle
|
||||||
const char *req = "\n";
|
if(poll(pfds, 2, 0) < 0){
|
||||||
int need2ask = 0; // need or not to ask encoder for new data
|
DBG("poll()");
|
||||||
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
|
break;
|
||||||
if(need2ask){
|
|
||||||
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
|
|
||||||
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
|
|
||||||
}
|
}
|
||||||
double v, t;
|
int got = 0;
|
||||||
if(getencval(encfd[0], &v, &t)){
|
for(int i = 0; i < 2; ++i){
|
||||||
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);
|
double curt = timefromstart();
|
||||||
mountdata.encYposition.t = t;
|
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
|
||||||
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
|
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
|
||||||
errctr = 0;
|
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
|
||||||
need2ask = 0;
|
pthread_mutex_lock(&datamutex);
|
||||||
} else {
|
if(i == 0){
|
||||||
if(need2ask) ++errctr;
|
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
|
||||||
else need2ask = 1;
|
curtime(&mountdata.encXposition.t);
|
||||||
continue;
|
/*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();
|
||||||
|
}
|
||||||
|
pthread_mutex_unlock(&datamutex);
|
||||||
|
}
|
||||||
|
if(!asknext(encfd[i])){
|
||||||
|
++errctr;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
|
||||||
|
++got;
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
if(need2ask) ++errctr;
|
|
||||||
else need2ask = 1;
|
|
||||||
continue;
|
|
||||||
}
|
}
|
||||||
while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); }
|
if(got == 2) errctr = 0;
|
||||||
//DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval);
|
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
|
||||||
t0 = nanotime();
|
DBG("\n\nEXIT: ERRCTR=%d", errctr);
|
||||||
}
|
|
||||||
DBG("ERRCTR=%d", errctr);
|
|
||||||
for(int i = 0; i < 2; ++i){
|
for(int i = 0; i < 2; ++i){
|
||||||
if(encfd[i] > -1){
|
if(encfd[i] > -1){
|
||||||
close(encfd[i]);
|
close(encfd[i]);
|
||||||
@@ -321,19 +476,67 @@ void data_free(data_t **x){
|
|||||||
*x = NULL;
|
*x = NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
|
||||||
|
if(!prev || !nstopped || !stat) return;
|
||||||
|
if(isnan(*prev)){
|
||||||
|
*stat = AXIS_STOPPED;
|
||||||
|
DBG("START");
|
||||||
|
}else if(*stat != AXIS_STOPPED){
|
||||||
|
if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||||
|
*stat = AXIS_STOPPED;
|
||||||
|
DBG("AXIS stopped; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped);
|
||||||
|
}
|
||||||
|
}else if(*prev != cur){
|
||||||
|
DBG("AXIS moving");
|
||||||
|
*nstopped = 0;
|
||||||
|
}
|
||||||
|
*prev = cur;
|
||||||
|
}
|
||||||
|
|
||||||
// main mount thread
|
// main mount thread
|
||||||
static void *mountthread(void _U_ *u){
|
static void *mountthread(void _U_ *u){
|
||||||
int errctr = 0;
|
int errctr = 0;
|
||||||
uint8_t buf[2*sizeof(SSstat)];
|
uint8_t buf[2*sizeof(SSstat)];
|
||||||
SSstat *status = (SSstat*) buf;
|
SSstat *status = (SSstat*) buf;
|
||||||
if(Conf.RunModel) while(1){
|
bzero(&mountdata, sizeof(mountdata));
|
||||||
pthread_mutex_lock(&datamutex);
|
double t0 = timefromstart(), tstart = t0, tcur = t0;
|
||||||
// now change data
|
double oldmt = -100.; // old `millis measurement` time
|
||||||
getModData(&mountdata);
|
static uint32_t oldmillis = 0;
|
||||||
pthread_mutex_unlock(&datamutex);
|
if(Conf.RunModel){
|
||||||
double t0 = nanotime();
|
double Xprev = NAN, Yprev = NAN; // previous coordinates
|
||||||
while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
|
int xcnt = 0, ycnt = 0;
|
||||||
t0 = nanotime();
|
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)};
|
||||||
@@ -342,8 +545,9 @@ static void *mountthread(void _U_ *u){
|
|||||||
if(!cmd_getstat) goto failed;
|
if(!cmd_getstat) goto failed;
|
||||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||||
// read data to status
|
// read data to status
|
||||||
double tgot = nanotime(), t0 = tgot;
|
struct timespec tcur;
|
||||||
//DBG("tgot=%g", tgot);
|
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);
|
||||||
@@ -359,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
|
||||||
do{
|
do{
|
||||||
usleep(5000);
|
usleep(500);
|
||||||
}while(nanotime() - t0 < Conf.MountReqInterval);
|
}while(timefromstart() - t0 < Conf.MountReqInterval);
|
||||||
t0 = nanotime();
|
t0 = timefromstart();
|
||||||
}
|
}
|
||||||
data_free(&cmd_getstat);
|
data_free(&cmd_getstat);
|
||||||
failed:
|
failed:
|
||||||
@@ -381,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
|
||||||
@@ -391,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
|
||||||
@@ -424,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]);
|
||||||
@@ -439,7 +654,7 @@ int openEncoder(){
|
|||||||
|
|
||||||
// return FALSE if failed
|
// return FALSE if failed
|
||||||
int openMount(){
|
int openMount(){
|
||||||
if(Conf.RunModel) return TRUE;
|
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);
|
||||||
@@ -456,10 +671,13 @@ int openMount(){
|
|||||||
}while(1);*/
|
}while(1);*/
|
||||||
mntRtmout.tv_sec = 0;
|
mntRtmout.tv_sec = 0;
|
||||||
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
|
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");
|
||||||
close(mntfd);
|
if(!Conf.RunModel){
|
||||||
mntfd = -1;
|
close(mntfd);
|
||||||
|
mntfd = -1;
|
||||||
|
}
|
||||||
return FALSE;
|
return FALSE;
|
||||||
}
|
}
|
||||||
DBG("Mount opened, thread started");
|
DBG("Mount opened, thread started");
|
||||||
@@ -468,6 +686,7 @@ 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(Conf.RunModel) return;
|
||||||
if(mntfd > -1){
|
if(mntfd > -1){
|
||||||
DBG("Cancel mount thread");
|
DBG("Cancel mount thread");
|
||||||
@@ -486,7 +705,7 @@ void closeSerial(){
|
|||||||
DBG("close encoder's 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;
|
||||||
}
|
}
|
||||||
@@ -499,42 +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){
|
||||||
//double t0 = nanotime();
|
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
|
||||||
}
|
}
|
||||||
//DBG("sent by %g", nanotime() - t0);
|
if(!in) return TRUE;
|
||||||
uint8_t buf[256];
|
|
||||||
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("got %zd bytes by %g", in->len, nanotime() - t0);
|
|
||||||
while(getmntbyte() > -1);
|
while(getmntbyte() > -1);
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
@@ -585,26 +806,25 @@ static int bincmd(uint8_t *cmd, int len){
|
|||||||
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;
|
||||||
@@ -639,16 +859,23 @@ int cmdC(SSconfig *conf, int rw){
|
|||||||
}else{ // read
|
}else{ // read
|
||||||
data_t d;
|
data_t d;
|
||||||
d.buf = (uint8_t *) conf;
|
d.buf = (uint8_t *) conf;
|
||||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
d.len = 0; d.maxlen = 0;
|
||||||
|
ret = wr(rcmd, &d, 1);
|
||||||
|
DBG("write command: %s", ret ? "TRUE" : "FALSE");
|
||||||
|
if(!ret) goto rtn;
|
||||||
|
// make a huge pause for stupid SSII
|
||||||
|
usleep(100000);
|
||||||
|
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||||
ret = wr(rcmd, &d, 1);
|
ret = wr(rcmd, &d, 1);
|
||||||
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
||||||
if(d.len != d.maxlen) return FALSE;
|
if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
|
||||||
// simplest checksum
|
// simplest checksum
|
||||||
uint16_t sum = 0;
|
uint16_t sum = 0;
|
||||||
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
||||||
if(sum != conf->checksum){
|
if(sum != conf->checksum){
|
||||||
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
||||||
return FALSE;
|
ret = FALSE;
|
||||||
|
goto rtn;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
rtn:
|
rtn:
|
||||||
|
|||||||
@@ -34,9 +34,11 @@ 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();
|
||||||
|
|||||||
@@ -32,19 +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)
|
|
||||||
// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
|
|
||||||
// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
|
||||||
#define MCC_X_ACCELERATION (0.219911)
|
|
||||||
#define MCC_Y_ACCELERATION (0.165806)
|
|
||||||
|
|
||||||
// max speed interval, seconds
|
// max speed interval, seconds
|
||||||
#define MCC_CONF_MAX_SPEEDINT (2.)
|
#define MCC_CONF_MAX_SPEEDINT (2.)
|
||||||
// minimal speed interval in parts of EncoderReqInterval
|
// minimal speed interval in parts of EncoderReqInterval
|
||||||
#define MCC_CONF_MIN_SPEEDC (3.)
|
#define MCC_CONF_MIN_SPEEDC (3.)
|
||||||
|
|
||||||
|
|
||||||
// error codes
|
// error codes
|
||||||
typedef enum{
|
typedef enum{
|
||||||
MCC_E_OK = 0, // all OK
|
MCC_E_OK = 0, // all OK
|
||||||
@@ -53,20 +48,38 @@ typedef enum{
|
|||||||
MCC_E_ENCODERDEV, // encoder device error or can't open
|
MCC_E_ENCODERDEV, // encoder device error or can't open
|
||||||
MCC_E_MOUNTDEV, // mount device error or can't open
|
MCC_E_MOUNTDEV, // mount device error or can't open
|
||||||
MCC_E_FAILED, // failed to run command - protocol error
|
MCC_E_FAILED, // failed to run command - protocol error
|
||||||
|
MCC_E_AMOUNT // Just amount of errors
|
||||||
} mcc_errcodes_t;
|
} mcc_errcodes_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
char* MountDevPath; // path to mount device
|
double P, I, D;
|
||||||
int MountDevSpeed; // serial speed
|
} PIDpar_t;
|
||||||
char* EncoderDevPath; // path to encoder device
|
|
||||||
int EncoderDevSpeed; // serial speed
|
typedef struct{
|
||||||
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
char* MountDevPath; // path to mount device
|
||||||
char* EncoderXDevPath; // paths to new controller devices
|
int MountDevSpeed; // serial speed
|
||||||
|
char* EncoderDevPath; // path to encoder device
|
||||||
|
int EncoderDevSpeed; // serial speed
|
||||||
|
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
||||||
|
char* EncoderXDevPath; // paths to new controller devices
|
||||||
char* EncoderYDevPath;
|
char* EncoderYDevPath;
|
||||||
double MountReqInterval; // interval between subsequent mount requests (seconds)
|
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
|
||||||
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
|
double MountReqInterval; // interval between subsequent mount requests (seconds)
|
||||||
double EncoderSpeedInterval;// interval between speed calculations
|
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
|
||||||
int RunModel; // == 1 if you want to use model instead of real mount
|
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
|
||||||
@@ -77,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{
|
||||||
@@ -123,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;
|
||||||
@@ -167,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 (???)
|
||||||
@@ -178,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 (???)
|
||||||
@@ -207,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
|
||||||
@@ -229,7 +245,13 @@ typedef struct{
|
|||||||
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
|
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
|
||||||
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
||||||
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
|
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
|
||||||
double (*currentT)(); // current time
|
int (*currentT)(struct timespec *t); // current time
|
||||||
|
double (*timeFromStart)(); // amount of seconds from last init
|
||||||
|
double (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times
|
||||||
|
double (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time
|
||||||
|
mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
|
||||||
|
mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//-
|
||||||
|
mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
|
||||||
} mount_t;
|
} mount_t;
|
||||||
|
|
||||||
extern mount_t Mount;
|
extern mount_t Mount;
|
||||||
|
|||||||
@@ -18,6 +18,7 @@
|
|||||||
|
|
||||||
#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 <unistd.h>
|
||||||
|
|
||||||
@@ -25,6 +26,13 @@
|
|||||||
#include "serial.h"
|
#include "serial.h"
|
||||||
#include "ssii.h"
|
#include "ssii.h"
|
||||||
|
|
||||||
|
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
|
||||||
|
// defaults until read from controller
|
||||||
|
double X_MOT_STEPSPERREV = 13312000.,
|
||||||
|
Y_MOT_STEPSPERREV = 17578668.,
|
||||||
|
X_ENC_STEPSPERREV = 67108864.,
|
||||||
|
Y_ENC_STEPSPERREV = 67108864.;
|
||||||
|
|
||||||
uint16_t SScalcChecksum(uint8_t *buf, int len){
|
uint16_t SScalcChecksum(uint8_t *buf, int len){
|
||||||
uint16_t checksum = 0;
|
uint16_t checksum = 0;
|
||||||
for(int i = 0; i < len; i++){
|
for(int i = 0; i < len; i++){
|
||||||
@@ -36,19 +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;
|
||||||
DBG("START");
|
DBG("START");
|
||||||
}else if(*stat != MNT_STOPPED){
|
}else if(*stat != AXIS_STOPPED){
|
||||||
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||||
*stat = MNT_STOPPED;
|
*stat = AXIS_STOPPED;
|
||||||
DBG("AXE stopped");
|
DBG("AXIS stopped");
|
||||||
}
|
}
|
||||||
}else if(*prev != cur){
|
}else if(*prev != cur){
|
||||||
DBG("AXE moving");
|
DBG("AXIS moving");
|
||||||
//*stat = MNT_SLEWING;
|
|
||||||
*nstopped = 0;
|
*nstopped = 0;
|
||||||
}
|
}
|
||||||
*prev = cur;
|
*prev = cur;
|
||||||
@@ -57,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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -67,17 +74,19 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
|
|||||||
* @param m (o) - output
|
* @param m (o) - output
|
||||||
* @param t - measurement time
|
* @param t - measurement time
|
||||||
*/
|
*/
|
||||||
void SSconvstat(const SSstat *s, mountdata_t *m, double t){
|
void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
|
||||||
if(!s || !m) return;
|
if(!s || !m || !t) return;
|
||||||
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
||||||
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
||||||
ChkStopped(s, m);
|
ChkStopped(s, m);
|
||||||
m->motXposition.t = m->motYposition.t = t;
|
m->motXposition.t = m->motYposition.t = *t;
|
||||||
// fill encoder data from here, as there's no separate enc thread
|
// fill encoder data from here, as there's no separate enc thread
|
||||||
if(!Conf.SepEncoder){
|
if(!Conf.SepEncoder){
|
||||||
m->encXposition.val = X_ENC2RAD(s->Xenc);
|
m->encXposition.val = Xenc2rad(s->Xenc);
|
||||||
m->encYposition.val = Y_ENC2RAD(s->Yenc);
|
DBG("encx: %g", m->encXposition.val);
|
||||||
m->encXposition.t = m->encYposition.t = t;
|
m->encYposition.val = Yenc2rad(s->Yenc);
|
||||||
|
m->encXposition.t = m->encYposition.t = *t;
|
||||||
|
getXspeed(); getYspeed();
|
||||||
}
|
}
|
||||||
m->keypad = s->keypad;
|
m->keypad = s->keypad;
|
||||||
m->extradata.ExtraBits = s->ExtraBits;
|
m->extradata.ExtraBits = s->ExtraBits;
|
||||||
@@ -164,9 +173,7 @@ int SSstop(int emerg){
|
|||||||
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
|
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
|
||||||
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
|
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
|
||||||
for(; i < 10; ++i){
|
for(; i < 10; ++i){
|
||||||
DBG("t1: %g", nanotime());
|
|
||||||
if(!SStextcmd(cmdx, NULL)) continue;
|
if(!SStextcmd(cmdx, NULL)) continue;
|
||||||
DBG("t2: %g", nanotime());
|
|
||||||
if(SStextcmd(cmdy, NULL)) break;
|
if(SStextcmd(cmdy, NULL)) break;
|
||||||
}
|
}
|
||||||
if(i == 10) return FALSE;
|
if(i == 10) return FALSE;
|
||||||
@@ -176,29 +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 = nanotime(), 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 = nanotime();
|
t = timefromstart();
|
||||||
|
if(!curtime(&curt)){
|
||||||
|
usleep(10000);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
//DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
|
||||||
if(MCC_E_OK == getMD(&md)){
|
if(MCC_E_OK == getMD(&md)){
|
||||||
if(md.encXposition.t == 0 || md.encYposition.t == 0){
|
if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
|
||||||
DBG("Just started, t-t0 = %g!", t - t0);
|
DBG("Just started? t-t0 = %g!", t - t0);
|
||||||
sleep(1);
|
usleep(10000);
|
||||||
DBG("t-t0 = %g", nanotime() - t0);
|
|
||||||
//usleep(10000);
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
|
if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
|
||||||
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
|
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
|
||||||
DBG("FIX motors position to encoders");
|
mcc_errcodes_t OK = MCC_E_OK;
|
||||||
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
|
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
|
||||||
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
|
DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
|
||||||
DBG("All OK, Dt=%g", nanotime() - t0);
|
DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
|
||||||
return MCC_E_OK;
|
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
|
||||||
}
|
DBG("Xpos sync failed!");
|
||||||
}else{
|
OK = MCC_E_FAILED;
|
||||||
DBG("on position");
|
}else DBG("Xpos sync OK, Dt=%g", t - t0);
|
||||||
return MCC_E_OK;
|
}
|
||||||
|
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);
|
||||||
|
|||||||
@@ -173,64 +173,74 @@
|
|||||||
#define SITECH_LOOP_FREQUENCY (1953.)
|
#define SITECH_LOOP_FREQUENCY (1953.)
|
||||||
|
|
||||||
// amount of consequent same coordinates to detect stop
|
// amount of consequent same coordinates to detect stop
|
||||||
#define MOTOR_STOPPED_CNT (4)
|
#define MOTOR_STOPPED_CNT (19)
|
||||||
|
|
||||||
|
// replace macros with global variables inited when config read
|
||||||
|
extern int X_ENC_ZERO, Y_ENC_ZERO;
|
||||||
|
extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
|
||||||
|
|
||||||
// TODO: take it from settings?
|
// TODO: take it from settings?
|
||||||
// steps per revolution (SSI - x4 - for SSI)
|
// steps per revolution (SSI - x4 - for SSI)
|
||||||
#define X_MOT_STEPSPERREV_SSI (13312000.)
|
// -> hwconf.Xconf.mot/enc_stepsperrev
|
||||||
|
//#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||||
// 13312000 / 4 = 3328000
|
// 13312000 / 4 = 3328000
|
||||||
#define X_MOT_STEPSPERREV (3328000.)
|
//#define X_MOT_STEPSPERREV (3328000.)
|
||||||
#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
//#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
||||||
// 17578668 / 4 = 4394667
|
// 17578668 / 4 = 4394667
|
||||||
#define Y_MOT_STEPSPERREV (4394667.)
|
//#define Y_MOT_STEPSPERREV (4394667.)
|
||||||
|
|
||||||
// encoder per revolution
|
// encoder per revolution
|
||||||
#define X_ENC_STEPSPERREV (67108864.)
|
//#define X_ENC_STEPSPERREV (67108864.)
|
||||||
#define Y_ENC_STEPSPERREV (67108864.)
|
//#define Y_ENC_STEPSPERREV (67108864.)
|
||||||
// encoder zero position
|
// encoder zero position
|
||||||
#define X_ENC_ZERO (61245239)
|
// -> conf.XEncZero/YEncZero
|
||||||
#define Y_ENC_ZERO (36999830)
|
//#define X_ENC_ZERO (61245239)
|
||||||
// encoder reversed (no: +1)
|
//#define Y_ENC_ZERO (36999830)
|
||||||
#define X_ENC_SIGN (-1.)
|
// encoder reversed (no: +1) -> sign of ...stepsperrev
|
||||||
#define Y_ENC_SIGN (-1.)
|
//#define X_ENC_SIGN (-1.)
|
||||||
|
//#define Y_ENC_SIGN (-1.)
|
||||||
|
|
||||||
|
|
||||||
// encoder position to radians and back
|
// encoder position to radians and back
|
||||||
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
|
#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
|
||||||
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
|
#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
|
||||||
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
|
#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
|
||||||
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
|
#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
|
||||||
|
|
||||||
// convert angle in radians to +-pi
|
// convert angle in radians to +-pi
|
||||||
static inline double ang2half(double ang){
|
static inline __attribute__((always_inline)) double ang2half(double ang){
|
||||||
|
ang = fmod(ang, 2.*M_PI);
|
||||||
if(ang < -M_PI) ang += 2.*M_PI;
|
if(ang < -M_PI) ang += 2.*M_PI;
|
||||||
else if(ang > M_PI) ang -= 2.*M_PI;
|
else if(ang > M_PI) ang -= 2.*M_PI;
|
||||||
return ang;
|
return ang;
|
||||||
}
|
}
|
||||||
// convert to only positive: 0..2pi
|
// convert to only positive: 0..2pi
|
||||||
static inline double ang2full(double ang){
|
static inline __attribute__((always_inline)) double ang2full(double ang){
|
||||||
|
ang = fmod(ang, 2.*M_PI);
|
||||||
if(ang < 0.) ang += 2.*M_PI;
|
if(ang < 0.) ang += 2.*M_PI;
|
||||||
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
|
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
|
||||||
return ang;
|
return ang;
|
||||||
}
|
}
|
||||||
|
|
||||||
// motor position to radians and back
|
// motor position to radians and back
|
||||||
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
|
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
|
||||||
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
|
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
|
||||||
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
|
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
|
||||||
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
|
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
|
||||||
// motor speed in rad/s and back
|
// motor speed in rad/s and back
|
||||||
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
|
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||||
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
|
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||||
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
|
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||||
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
|
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||||
// motor acceleration -//-
|
// motor acceleration -//-
|
||||||
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
|
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||||
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
|
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||||
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
|
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||||
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
|
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||||
|
|
||||||
// adder time to seconds vice versa
|
// adder time to seconds vice versa
|
||||||
#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY)
|
#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY))
|
||||||
#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY)
|
#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY))
|
||||||
|
|
||||||
// encoder's tolerance (ticks)
|
// encoder's tolerance (ticks)
|
||||||
#define YencTOL (25.)
|
#define YencTOL (25.)
|
||||||
@@ -331,7 +341,7 @@ typedef struct{
|
|||||||
} __attribute__((packed)) SSconfig;
|
} __attribute__((packed)) SSconfig;
|
||||||
|
|
||||||
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
||||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t);
|
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
|
||||||
int SStextcmd(const char *cmd, data_t *answer);
|
int SStextcmd(const char *cmd, data_t *answer);
|
||||||
int SSrawcmd(const char *cmd, data_t *answer);
|
int SSrawcmd(const char *cmd, data_t *answer);
|
||||||
int SSgetint(const char *cmd, int64_t *ans);
|
int SSgetint(const char *cmd, int64_t *ans);
|
||||||
|
|||||||
88
asibfm700/CMakeLists.txt
Normal file
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.state) {
|
switch (pos.moving_type) {
|
||||||
case hw_state_t::HW_STATE_SLEW: // slew mount
|
case hw_moving_type_t::HW_MOVE_SLEWING: // slew mount
|
||||||
err = static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
|
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));
|
||||||
|
}
|
||||||
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,302 +189,146 @@ 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) {
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
_stopRequested = false;
|
||||||
}
|
|
||||||
|
|
||||||
const auto p_mount_controls = &mount_controls;
|
if (guiding_point.correctionRange[0] >= guiding_point.correctionRange[1]) {
|
||||||
|
|
||||||
auto check_zones = [p_mount_controls, this]() {
|
|
||||||
return [this]<size_t... Is>(std::index_sequence<Is...>) {
|
|
||||||
error_t ret;
|
|
||||||
|
|
||||||
(
|
|
||||||
[&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;
|
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto low_corr_limit = context.correctionRange[0] * context.correctionRange[0];
|
auto low_corr_limit = guiding_point.correctionRange[0] * guiding_point.correctionRange[0];
|
||||||
auto high_corr_limit = context.correctionRange[1] * context.correctionRange[1];
|
auto high_corr_limit = guiding_point.correctionRange[1] * guiding_point.correctionRange[1];
|
||||||
|
|
||||||
auto& astrom_engine = p_mount_controls->astrometryEngine;
|
// using coord_t = typename astrom_engine_t::coord_t;
|
||||||
auto& hardware = p_mount_controls->hardware;
|
// using jd_t = typename astrom_engine_t::juldate_t;
|
||||||
auto& pec = p_mount_controls->PEC;
|
// jd_t jd;
|
||||||
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) {
|
if (t_err) {
|
||||||
ast_err = astrom_engine.icrs2obs(ra_icrs, dec_icrs, jd, ra_app, dec_app, ha, az, alt, eo);
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||||
}
|
logError(std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
||||||
if (ast_err) {
|
return t_err;
|
||||||
if constexpr (std::same_as<decltype(ast_err), error_t>) {
|
} else {
|
||||||
logError(
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||||
std::format("An error occured while performing astrometry computations: code = {} ({})",
|
logError(std::format("An telemetry error occured: code = {}", t_err));
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<bool, Nzones> in_zone_flag;
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
if (_stopRequested) {
|
||||||
|
// return MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED;
|
||||||
|
// interpetate stoping as 'no error' exit
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
t_err = telemetry.data(t_data);
|
|
||||||
|
// 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) {
|
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>) {
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||||
logError(
|
std::format_to(std::back_inserter(err_str), ": code = {} ({})", t_err.value(), t_err.message());
|
||||||
std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
logError(err_str);
|
||||||
return t_err;
|
return t_err;
|
||||||
} else {
|
} else {
|
||||||
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||||
logError(std::format("An telemetry error occured: code = {}", t_err));
|
std::format_to(std::back_inserter(err_str), ": code = {}", t_err.value());
|
||||||
}
|
}
|
||||||
|
logError(err_str);
|
||||||
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_stopRequested) {
|
||||||
|
// interpetate stoping as 'no error' exit
|
||||||
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
||||||
|
}
|
||||||
|
|
||||||
// compare t_data with computed coordinates ...
|
// compare t_data with computed coordinates ...
|
||||||
if (_doCorrection) {
|
if (_doCorrection) {
|
||||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
auto coord_diff = p_telemetry->targetToMountDiff();
|
||||||
xr = ha - t_data.mntHA;
|
|
||||||
yr = dec_app - t_data.mntDEC;
|
|
||||||
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
|
||||||
xr = az - t_data.mntAZ;
|
|
||||||
yr = alt - t_data.mntALT;
|
|
||||||
} else {
|
|
||||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
|
||||||
}
|
|
||||||
|
|
||||||
coord_diff = xr * xr + yr * yr;
|
if (coord_diff.r2 < low_corr_limit) {
|
||||||
|
logWarn(
|
||||||
if (coord_diff < low_corr_limit) {
|
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,9 +137,9 @@ 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{};
|
||||||
mcc::astrom::iers::MccIersBulletinA _bulletinA{};
|
mcc::astrom::iers::MccIersBulletinA _bulletinA{};
|
||||||
@@ -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 */
|
|
||||||
|
|
||||||
/* PROHIBITED ZONE IMPLEMENTATION */
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||||
|
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
|
||||||
if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZALT) { // trivial case
|
_coord2coord(target.coordPairKind, target.x, target.y, target.time_point, MccCoordPairKind::COORDS_KIND_AZALT,
|
||||||
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
az, alt);
|
||||||
return y <= _altLimit;
|
|
||||||
} else if constexpr (KIND == MccAltLimitKind::MAX_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 (coord_kind == MccCoordPairKind::COORDS_KIND_AZZD) { // trivial case
|
return alt >= _altLimit;
|
||||||
// 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 ...
|
duration_t timeTo(traits::mcc_celestial_point_c auto const& target)
|
||||||
return false;
|
{
|
||||||
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
coord_t ha, dec;
|
||||||
// implementation ...
|
|
||||||
return false;
|
if (inZone(target)) {
|
||||||
} else {
|
return zeroDuration;
|
||||||
throw std::system_error(std::make_error_code(std::errc::operation_not_supported));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
_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)
|
||||||
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 (!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
|
||||||
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
bool intersectPoint(traits::mcc_celestial_point_c auto const& target, traits::mcc_celestial_point_c auto& int_point)
|
||||||
duration_t timeFrom(const XT& x,
|
|
||||||
const YT& y,
|
|
||||||
traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
||||||
{
|
{
|
||||||
return duration_t{0.0};
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
double cosA =
|
||||||
|
(-std::sin(dec) * std::cos(_latitude) + std::cos(dec) * std::sin(_latitude) * cos_ha) / cos(_altLimit);
|
||||||
|
// cosA /= std::cos(_altLimit);
|
||||||
|
|
||||||
|
std::cout << "COS_A = " << cosA << "\n";
|
||||||
|
|
||||||
|
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
|
||||||
@@ -5,14 +5,13 @@
|
|||||||
|
|
||||||
/* MOUNT TELEMETRY OBJECT POSSIBLE GENERIC IMPLEMENTATION */
|
/* MOUNT TELEMETRY OBJECT POSSIBLE GENERIC IMPLEMENTATION */
|
||||||
|
|
||||||
#include <list>
|
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
|
|
||||||
#include "mcc_mount_telemetry_astrom.h"
|
#include "mcc_mount_telemetry_astrom.h"
|
||||||
|
|
||||||
namespace mcc
|
namespace mcc
|
||||||
{
|
{
|
||||||
enum class MccMountTelemetryErrorCode : int { ERROR_OK, ERROR_HARDWARE };
|
enum class MccMountTelemetryErrorCode : int { ERROR_OK, ERROR_HARDWARE, ERROR_DATA_TIMEOUT };
|
||||||
|
|
||||||
/* error category definition */
|
/* error category definition */
|
||||||
|
|
||||||
@@ -34,6 +33,8 @@ struct MccMountTelemetryCategory : public std::error_category {
|
|||||||
return "OK";
|
return "OK";
|
||||||
case MccMountTelemetryErrorCode::ERROR_HARDWARE:
|
case MccMountTelemetryErrorCode::ERROR_HARDWARE:
|
||||||
return "hardware request failed";
|
return "hardware request failed";
|
||||||
|
case MccMountTelemetryErrorCode::ERROR_DATA_TIMEOUT:
|
||||||
|
return "an timeout occured while waiting for new data";
|
||||||
default:
|
default:
|
||||||
return "UNKNOWN";
|
return "UNKNOWN";
|
||||||
}
|
}
|
||||||
@@ -73,7 +74,7 @@ namespace mcc
|
|||||||
|
|
||||||
/* DEFAULT TELEMETRY DATA CLASS */
|
/* DEFAULT TELEMETRY DATA CLASS */
|
||||||
|
|
||||||
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_mount_pec_c PEC_T>
|
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T>
|
||||||
struct MccMountTelemetryData {
|
struct MccMountTelemetryData {
|
||||||
typedef typename ASTROM_ENGINE_T::coord_t coord_t;
|
typedef typename ASTROM_ENGINE_T::coord_t coord_t;
|
||||||
typedef typename ASTROM_ENGINE_T::time_point_t time_point_t;
|
typedef typename ASTROM_ENGINE_T::time_point_t time_point_t;
|
||||||
@@ -105,7 +106,7 @@ struct MccMountTelemetryData {
|
|||||||
coord_t mntRateX, mntRateY;
|
coord_t mntRateX, mntRateY;
|
||||||
|
|
||||||
// current refraction coefficients
|
// current refraction coefficients
|
||||||
typename PEC_T::pec_result_t currRefrCoeffs;
|
typename ASTROM_ENGINE_T::refract_result_t currRefrCoeffs;
|
||||||
|
|
||||||
// current refraction correction (for mntALT)
|
// current refraction correction (for mntALT)
|
||||||
coord_t currRefr;
|
coord_t currRefr;
|
||||||
@@ -122,8 +123,7 @@ struct MccMountTelemetryData {
|
|||||||
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T,
|
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T,
|
||||||
traits::mcc_mount_pec_c PEC_T,
|
traits::mcc_mount_pec_c PEC_T,
|
||||||
traits::mcc_mount_hardware_c HARDWARE_T,
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
||||||
std::derived_from<MccMountTelemetryData<ASTROM_ENGINE_T, PEC_T>> DATA_T =
|
std::derived_from<MccMountTelemetryData<ASTROM_ENGINE_T>> DATA_T = MccMountTelemetryData<ASTROM_ENGINE_T>>
|
||||||
MccMountTelemetryData<ASTROM_ENGINE_T, PEC_T>>
|
|
||||||
class MccMountTelemetry : public MccMountTelemetryAstromTransform<ASTROM_ENGINE_T, PEC_T>
|
class MccMountTelemetry : public MccMountTelemetryAstromTransform<ASTROM_ENGINE_T, PEC_T>
|
||||||
{
|
{
|
||||||
typedef MccMountTelemetryAstromTransform<ASTROM_ENGINE_T, PEC_T> base_t;
|
typedef MccMountTelemetryAstromTransform<ASTROM_ENGINE_T, PEC_T> base_t;
|
||||||
@@ -139,16 +139,44 @@ public:
|
|||||||
typedef HARDWARE_T hardware_t;
|
typedef HARDWARE_T hardware_t;
|
||||||
typedef DATA_T mount_telemetry_data_t;
|
typedef DATA_T mount_telemetry_data_t;
|
||||||
|
|
||||||
|
|
||||||
typedef std::error_code error_t;
|
typedef std::error_code error_t;
|
||||||
|
|
||||||
typedef std::function<void(mount_telemetry_data_t)> update_callback_func_t;
|
struct coord_diff_t {
|
||||||
typedef std::list<update_callback_func_t> update_callback_container_t;
|
typename ASTROM_ENGINE_T::coord_t xdiff, ydiff, r2;
|
||||||
|
};
|
||||||
|
|
||||||
MccMountTelemetry(astrom_engine_t& astrom_engine, pec_t& pec, hardware_t& hardware)
|
MccMountTelemetry(astrom_engine_t* astrom_engine, pec_t* pec, hardware_t* hardware)
|
||||||
: base_t(astrom_engine, pec), _hardware(hardware)
|
: base_t(astrom_engine, pec), _hardware(hardware)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MccMountTelemetry(MccMountTelemetry&& other)
|
||||||
|
: base_t(std::move(other)),
|
||||||
|
_data(std::move(other._data)),
|
||||||
|
_hardware(other._hardware),
|
||||||
|
_updateMutex(std::move(other._updateMutex)),
|
||||||
|
_updateCondVar(std::move(other._updateCondVar))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
MccMountTelemetry& operator=(MccMountTelemetry&& other)
|
||||||
|
{
|
||||||
|
if (this == &other) {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
base_t::operator=(std::move(other));
|
||||||
|
|
||||||
|
_data = std::move(other._data);
|
||||||
|
_hardware = other._hardware;
|
||||||
|
_updateMutex = std::move(other._updateMutex);
|
||||||
|
_updateCondVar = std::move(other._updateCondVar);
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
virtual ~MccMountTelemetry() = default;
|
virtual ~MccMountTelemetry() = default;
|
||||||
|
|
||||||
|
|
||||||
@@ -156,7 +184,7 @@ public:
|
|||||||
template <traits::mcc_celestial_point_c PointT>
|
template <traits::mcc_celestial_point_c PointT>
|
||||||
error_t setTarget(PointT tag_point)
|
error_t setTarget(PointT tag_point)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_updateMutex};
|
std::lock_guard lock{*_updateMutex};
|
||||||
|
|
||||||
auto err = this->toICRS(tag_point, _data.utc, _data.tagRA_ICRS, _data.tagDEC_ICRS);
|
auto err = this->toICRS(tag_point, _data.utc, _data.tagRA_ICRS, _data.tagDEC_ICRS);
|
||||||
if (!err) {
|
if (!err) {
|
||||||
@@ -166,14 +194,37 @@ public:
|
|||||||
return err;
|
return err;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// target - mount coordinate difference
|
||||||
|
auto targetToMountDiff()
|
||||||
|
{
|
||||||
|
std::lock_guard lk(*_updateMutex);
|
||||||
|
|
||||||
|
coord_diff_t result;
|
||||||
|
|
||||||
|
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
||||||
|
result.xdiff = _data.tagRA - _data.mntHA;
|
||||||
|
result.ydiff = _data.tagDEC - _data.mntDEC;
|
||||||
|
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
||||||
|
result.xdiff = _data.tagAZ - _data.mntAZ;
|
||||||
|
result.ydiff = _data.tagALT - _data.mntALT;
|
||||||
|
} else {
|
||||||
|
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
||||||
|
}
|
||||||
|
|
||||||
|
result.r2 = result.xdiff * result.xdiff + result.ydiff * result.ydiff;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// update telemetry right now
|
||||||
error_t update()
|
error_t update()
|
||||||
{
|
{
|
||||||
mount_telemetry_data_t current_data;
|
mount_telemetry_data_t current_data;
|
||||||
|
|
||||||
typename hardware_t::axes_pos_t ax_pos;
|
typename hardware_t::axes_pos_t ax_pos;
|
||||||
|
|
||||||
auto err = _hardware.getPos(ax_pos);
|
auto err = _hardware->getPos(ax_pos);
|
||||||
if (err) {
|
if (err) {
|
||||||
if constexpr (std::same_as<error_t, decltype(err)>) {
|
if constexpr (std::same_as<error_t, decltype(err)>) {
|
||||||
return err;
|
return err;
|
||||||
@@ -210,13 +261,16 @@ public:
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
// correction for PEC
|
// compute corrections for PEC and celestial apparent coordinates
|
||||||
if constexpr (base_t::equatorialMount) {
|
if constexpr (base_t::equatorialMount) {
|
||||||
res_err = this->toApparent(point_t{.coordPairKind = MccCoordPairKind::COORDS_KIND_XY,
|
res_err = this->toApparent(point_t{.coordPairKind = MccCoordPairKind::COORDS_KIND_XY,
|
||||||
.x = current_data.mntPosX,
|
.x = current_data.mntPosX,
|
||||||
.y = current_data.mntPosY},
|
.y = current_data.mntPosY},
|
||||||
current_data.utc, current_data.mntHA, current_data.mntDEC);
|
current_data.utc, current_data.mntHA, current_data.mntDEC);
|
||||||
if (!res_err) {
|
if (!res_err) {
|
||||||
|
current_data.pecX = current_data.mntHA - current_data.mntPosX;
|
||||||
|
current_data.pecY = current_data.mntDEC - current_data.mntPosY;
|
||||||
|
|
||||||
ast_err = this->_astromEngine.hadec2azalt(current_data.mntHA, current_data.mntDEC, current_data.mntAZ,
|
ast_err = this->_astromEngine.hadec2azalt(current_data.mntHA, current_data.mntDEC, current_data.mntAZ,
|
||||||
current_data.mntALT);
|
current_data.mntALT);
|
||||||
}
|
}
|
||||||
@@ -226,6 +280,9 @@ public:
|
|||||||
.y = current_data.mntPosY},
|
.y = current_data.mntPosY},
|
||||||
current_data.utc, current_data.mntAZ, current_data.mntALT);
|
current_data.utc, current_data.mntAZ, current_data.mntALT);
|
||||||
if (!res_err) {
|
if (!res_err) {
|
||||||
|
current_data.pecX = current_data.mntAZ - current_data.mntPosX;
|
||||||
|
current_data.pecY = current_data.mntALT - current_data.mntPosY;
|
||||||
|
|
||||||
ast_err = this->_astromEngine.azalt2hadec(current_data.mntAZ, current_data.mntALT, current_data.mntHA,
|
ast_err = this->_astromEngine.azalt2hadec(current_data.mntAZ, current_data.mntALT, current_data.mntHA,
|
||||||
current_data.mntDEC);
|
current_data.mntDEC);
|
||||||
}
|
}
|
||||||
@@ -242,7 +299,7 @@ public:
|
|||||||
ast_err = this->_astromEngine.hadec2pa(current_data.mntHA, current_data.mntDEC, current_data.mntPA);
|
ast_err = this->_astromEngine.hadec2pa(current_data.mntHA, current_data.mntDEC, current_data.mntPA);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
// target coordinates (assuming its ICRS RA-DEC are already given or computed)
|
// target coordinates (assuming its ICRS RA-DEC are already given or computed)
|
||||||
typename mount_telemetry_data_t::eo_t eo;
|
typename astrom_engine_t::eo_t eo;
|
||||||
ast_err = this->_astromEngine.icrs2obs(current_data.tagRA_ICRS, current_data.tagDEC_ICRS,
|
ast_err = this->_astromEngine.icrs2obs(current_data.tagRA_ICRS, current_data.tagDEC_ICRS,
|
||||||
current_data.jd, current_data.tagRA, current_data.tagDEC,
|
current_data.jd, current_data.tagRA, current_data.tagDEC,
|
||||||
current_data.tagHA, current_data.tagAZ, current_data.tagALT, eo);
|
current_data.tagHA, current_data.tagAZ, current_data.tagALT, eo);
|
||||||
@@ -262,65 +319,68 @@ public:
|
|||||||
return MccMountTelemetryAstromTransformErrorCode::ERROR_ASTROMETRY_COMP;
|
return MccMountTelemetryAstromTransformErrorCode::ERROR_ASTROMETRY_COMP;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::lock_guard lock{_updateMutex};
|
std::lock_guard lock{*_updateMutex};
|
||||||
|
|
||||||
_data = std::move(current_data);
|
_data = std::move(current_data);
|
||||||
|
|
||||||
std::lock_guard cl_lock{_callbackMutex};
|
// notify all threads for new telemetry data
|
||||||
|
_updateCondVar->notify_all();
|
||||||
// callbacks will be invoked with its own copy of telemetry data!
|
|
||||||
for (auto& func : _callbackFuncs) {
|
|
||||||
std::thread t([this, &func] {
|
|
||||||
auto d = _data;
|
|
||||||
func(std::move(d));
|
|
||||||
});
|
|
||||||
t.detach();
|
|
||||||
}
|
|
||||||
|
|
||||||
return MccMountTelemetryErrorCode::ERROR_OK;
|
return MccMountTelemetryErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
update_callback_container_t::iterator addCallbackFunc(update_callback_func_t func)
|
|
||||||
{
|
|
||||||
std::lock_guard lock{_callbackMutex};
|
|
||||||
|
|
||||||
return _callbackFuncs.emplace_back(std::move(func));
|
|
||||||
}
|
|
||||||
|
|
||||||
void delCallbackFunc(update_callback_container_t::iterator iter)
|
|
||||||
{
|
|
||||||
std::lock_guard lock{_callbackMutex};
|
|
||||||
|
|
||||||
if (_callbackFuncs.size()) {
|
|
||||||
_callbackFuncs.erase(iter);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void clearCallbackFuncs()
|
|
||||||
{
|
|
||||||
std::lock_guard lock{_callbackMutex};
|
|
||||||
|
|
||||||
_callbackFuncs.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
error_t data(mount_telemetry_data_t& data)
|
error_t data(mount_telemetry_data_t& data)
|
||||||
{
|
{
|
||||||
std::lock_guard lock{_updateMutex};
|
std::lock_guard lock{*_updateMutex};
|
||||||
|
|
||||||
data = std::move(_data);
|
data = _data;
|
||||||
|
|
||||||
|
return MccMountTelemetryErrorCode::ERROR_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
// wait (block current thread) until data is updated or given timeout occurs
|
||||||
|
template <traits::mcc_time_duration_c DT>
|
||||||
|
error_t waitForUpdatedData(mount_telemetry_data_t& data, const DT& timeout)
|
||||||
|
{
|
||||||
|
auto timeout_tp = std::chrono::steady_clock::now() + timeout;
|
||||||
|
|
||||||
|
std::unique_lock lk(*_updateMutex);
|
||||||
|
|
||||||
|
auto res = _updateCondVar->wait_until(
|
||||||
|
lk, timeout_tp, [last_time_point = _data.time_point, this]() { return last_time_point < _data.timepoint; });
|
||||||
|
if (res == std::cv_status::timeout) {
|
||||||
|
return MccMountTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||||
|
}
|
||||||
|
|
||||||
|
data = _data;
|
||||||
|
|
||||||
return MccMountTelemetryErrorCode::ERROR_OK;
|
return MccMountTelemetryErrorCode::ERROR_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
mount_telemetry_data_t _data{};
|
mount_telemetry_data_t _data{};
|
||||||
hardware_t& _hardware;
|
hardware_t* _hardware;
|
||||||
update_callback_container_t _callbackFuncs{};
|
|
||||||
|
|
||||||
std::mutex _updateMutex;
|
std::unique_ptr<std::mutex> _updateMutex;
|
||||||
std::mutex _callbackMutex;
|
std::unique_ptr<std::condition_variable> _updateCondVar;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
namespace traits
|
||||||
|
{
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
concept mcc_mount_default_telemetry_c = requires {
|
||||||
|
requires mcc_astrom_engine_c<typename T::astrom_engine_t>;
|
||||||
|
requires mcc_mount_pec_c<typename T::pec_t>;
|
||||||
|
requires mcc_mount_hardware_c<typename T::hardware_t>;
|
||||||
|
requires mcc_mount_telemetry_data_c<typename T::mount_telemetry_data_t>;
|
||||||
|
|
||||||
|
requires std::derived_from<T, mcc::MccMountTelemetry<typename T::astrom_engine_t, typename T::pec_t,
|
||||||
|
typename T::hardware_t, typename T::mount_telemetry_data_t>>;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace traits
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -26,10 +26,7 @@ enum class MccMountTelemetryAstromTransformErrorCode : int {
|
|||||||
struct MccMountTelemetryAstromTransformCategory : public std::error_category {
|
struct MccMountTelemetryAstromTransformCategory : public std::error_category {
|
||||||
MccMountTelemetryAstromTransformCategory() : std::error_category() {}
|
MccMountTelemetryAstromTransformCategory() : 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
|
||||||
{
|
{
|
||||||
@@ -83,7 +80,7 @@ namespace mcc
|
|||||||
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_mount_pec_c PEC_T>
|
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_mount_pec_c PEC_T>
|
||||||
class MccMountTelemetryAstromTransform
|
class MccMountTelemetryAstromTransform
|
||||||
{
|
{
|
||||||
static typename ASTROM_ENGINE_T::coord_t dummyCoord{};
|
inline static typename ASTROM_ENGINE_T::coord_t dummyCoord{};
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef ASTROM_ENGINE_T astrom_engine_t;
|
typedef ASTROM_ENGINE_T astrom_engine_t;
|
||||||
@@ -107,11 +104,42 @@ public:
|
|||||||
|
|
||||||
typedef std::error_code error_t;
|
typedef std::error_code error_t;
|
||||||
|
|
||||||
MccMountTelemetryAstromTransform(astrom_engine_t& astrom_engine, pec_t& pec)
|
MccMountTelemetryAstromTransform(astrom_engine_t* astrom_engine, pec_t* pec)
|
||||||
: _astromEngine(astrom_engine), _pec(pec)
|
: _astromEngine(astrom_engine), _pec(pec)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
MccMountTelemetryAstromTransform(MccMountTelemetryAstromTransform&& other)
|
||||||
|
: _pec(other._pec), _astromEngine(other._astromEngine)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
MccMountTelemetryAstromTransform& operator=(MccMountTelemetryAstromTransform&& other)
|
||||||
|
{
|
||||||
|
if (this != &other) {
|
||||||
|
_pec = other._pec;
|
||||||
|
_astromEngine = other._astromEngine;
|
||||||
|
}
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
MccMountTelemetryAstromTransform(const MccMountTelemetryAstromTransform& other)
|
||||||
|
: _pec(other._pec), _astromEngine(other._astromEngine)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
MccMountTelemetryAstromTransform& operator=(const MccMountTelemetryAstromTransform& other)
|
||||||
|
{
|
||||||
|
if (this != &other) {
|
||||||
|
_pec = other._pec;
|
||||||
|
_astromEngine = other._astromEngine;
|
||||||
|
}
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
virtual ~MccMountTelemetryAstromTransform() = default;
|
virtual ~MccMountTelemetryAstromTransform() = default;
|
||||||
|
|
||||||
template <traits::mcc_celestial_point_c CT>
|
template <traits::mcc_celestial_point_c CT>
|
||||||
@@ -134,9 +162,9 @@ public:
|
|||||||
typename pec_t::error_t pec_err;
|
typename pec_t::error_t pec_err;
|
||||||
|
|
||||||
auto get_jd_lst_eo = [&time_point, this](jd_t& jd, sideral_time_t& lst, eo_t& eo) {
|
auto get_jd_lst_eo = [&time_point, this](jd_t& jd, sideral_time_t& lst, eo_t& eo) {
|
||||||
auto ast_err = _astromEngine.greg2jul(time_point, jd);
|
auto ast_err = _astromEngine->greg2jul(time_point, jd);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine.apparentSiderTime(jd, lst, true);
|
ast_err = _astromEngine->apparentSiderTime(jd, lst, true);
|
||||||
|
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine->eqOrigins(jd, eo);
|
ast_err = _astromEngine->eqOrigins(jd, eo);
|
||||||
@@ -148,7 +176,7 @@ public:
|
|||||||
|
|
||||||
if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's
|
if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's
|
||||||
typename pec_t::pec_result_t pec_res;
|
typename pec_t::pec_result_t pec_res;
|
||||||
pec_err = _pec.compute(coord.x, coord.y, pec_res);
|
pec_err = _pec->compute(coord.x, coord.y, pec_res);
|
||||||
if (!pec_err) {
|
if (!pec_err) {
|
||||||
X_app = coord.x + pec_res.dx;
|
X_app = coord.x + pec_res.dx;
|
||||||
Y_app = coord.y + pec_res.dy;
|
Y_app = coord.y + pec_res.dy;
|
||||||
@@ -170,7 +198,7 @@ public:
|
|||||||
Y_app = coord.y;
|
Y_app = coord.y;
|
||||||
XX_app = HA;
|
XX_app = HA;
|
||||||
} else if constexpr (altAzMount) {
|
} else if constexpr (altAzMount) {
|
||||||
ast_err = _astromEngine.hadec2azalt(HA, coord.y, X_app, Y_app);
|
ast_err = _astromEngine->hadec2azalt(HA, coord.y, X_app, Y_app);
|
||||||
} else {
|
} else {
|
||||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
||||||
}
|
}
|
||||||
@@ -185,16 +213,19 @@ public:
|
|||||||
Y_app = coord.y;
|
Y_app = coord.y;
|
||||||
XX_app = lst - coord.x + eo;
|
XX_app = lst - coord.x + eo;
|
||||||
} else if constexpr (altAzMount) {
|
} else if constexpr (altAzMount) {
|
||||||
ast_err = _astromEngine.hadec2azalt(coord.x, coord.y, X_app, Y_app);
|
ast_err = _astromEngine->hadec2azalt(coord.x, coord.y, X_app, Y_app);
|
||||||
} else {
|
} else {
|
||||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT
|
||||||
if constexpr (equatorialMount) {
|
if constexpr (equatorialMount) {
|
||||||
ast_err = azalt2hadec(coord.x, coord.y, X_app, Y_app); // compute HA-DEC
|
ast_err = _astromEngine->azalt2hadec(coord.x, coord.y, X_app, Y_app); // compute HA-DEC
|
||||||
if (!ast_err) { // compute CIO RA (as XX_app)
|
if (!ast_err) { // compute CIO RA (as XX_app)
|
||||||
ast_err = toApparent(X_app, Y_app, X_app, Y_app, XX_app);
|
coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||||
|
coord.x = X_app;
|
||||||
|
coord.y = Y_app;
|
||||||
|
ast_err = toApparent(std::move(coord), std::move(time_point), X_app, Y_app, XX_app);
|
||||||
}
|
}
|
||||||
} else if (altAzMount) {
|
} else if (altAzMount) {
|
||||||
X_app = coord.x;
|
X_app = coord.x;
|
||||||
@@ -215,9 +246,9 @@ public:
|
|||||||
// for alt-azimuthal mount:
|
// for alt-azimuthal mount:
|
||||||
// X_app = AZ, Y_app = ALT
|
// X_app = AZ, Y_app = ALT
|
||||||
|
|
||||||
ast_err = _astromEngine.greg2jul(time_point, jd);
|
ast_err = _astromEngine->greg2jul(time_point, jd);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine.icrs2obs(coord.x, coord.y, jd, X_app, Y_app, XX_app, az, alt, eo);
|
ast_err = _astromEngine->icrs2obs(coord.x, coord.y, jd, X_app, Y_app, XX_app, az, alt, eo);
|
||||||
|
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
if constexpr (equatorialMount) {
|
if constexpr (equatorialMount) {
|
||||||
@@ -262,7 +293,7 @@ public:
|
|||||||
|
|
||||||
if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's
|
if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's
|
||||||
typename pec_t::pec_result_t pec_res;
|
typename pec_t::pec_result_t pec_res;
|
||||||
pec_err = _pec.compute(coord.x, coord.y, pec_res);
|
pec_err = _pec->compute(coord.x, coord.y, pec_res);
|
||||||
if (!pec_err) {
|
if (!pec_err) {
|
||||||
coord.x += pec_res.dx;
|
coord.x += pec_res.dx;
|
||||||
coord.y += pec_res.dy;
|
coord.y += pec_res.dy;
|
||||||
@@ -297,9 +328,9 @@ public:
|
|||||||
if (coord.coordPairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
if (coord.coordPairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||||
typename astrom_engine_t::jd_t jd;
|
typename astrom_engine_t::jd_t jd;
|
||||||
|
|
||||||
ast_err = _astromEngine.greg2jul(astrom_engine_t::timePointNow(), jd);
|
ast_err = _astromEngine->greg2jul(astrom_engine_t::timePointNow(), jd);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine.obs2icrs(coord.coordPairKind, coord.x, coord.y, jd, RA, DEC);
|
ast_err = _astromEngine->obs2icrs(coord.coordPairKind, coord.x, coord.y, jd, RA, DEC);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -336,9 +367,9 @@ public:
|
|||||||
sideral_time_t lst;
|
sideral_time_t lst;
|
||||||
|
|
||||||
auto get_jd_lst_eo = [&time_point, this](jd_t& jd, sideral_time_t& lst, eo_t& eo) {
|
auto get_jd_lst_eo = [&time_point, this](jd_t& jd, sideral_time_t& lst, eo_t& eo) {
|
||||||
auto ast_err = _astromEngine.greg2jul(time_point, jd);
|
auto ast_err = _astromEngine->greg2jul(time_point, jd);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine.apparentSiderTime(jd, lst, true);
|
ast_err = _astromEngine->apparentSiderTime(jd, lst, true);
|
||||||
|
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = _astromEngine->eqOrigins(jd, eo);
|
ast_err = _astromEngine->eqOrigins(jd, eo);
|
||||||
@@ -369,7 +400,7 @@ public:
|
|||||||
if constexpr (equatorialMount) {
|
if constexpr (equatorialMount) {
|
||||||
typename pec_t::pec_result_t pec_res;
|
typename pec_t::pec_result_t pec_res;
|
||||||
|
|
||||||
pec_err = _pec.compute(coord.x, coord.y, pec_res);
|
pec_err = _pec->compute(coord.x, coord.y, pec_res);
|
||||||
if (!pec_err) {
|
if (!pec_err) {
|
||||||
X = coord.x - pec_res.dx;
|
X = coord.x - pec_res.dx;
|
||||||
Y = coord.y - pec_res.dy;
|
Y = coord.y - pec_res.dy;
|
||||||
@@ -378,7 +409,7 @@ public:
|
|||||||
}
|
}
|
||||||
} else if constexpr (altAzMount) {
|
} else if constexpr (altAzMount) {
|
||||||
coord_t az, alt;
|
coord_t az, alt;
|
||||||
ast_err = _astromEngine.hadec2azalt(coord.x, coord.y, az, alt);
|
ast_err = _astromEngine->hadec2azalt(coord.x, coord.y, az, alt);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT;
|
coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT;
|
||||||
coord.x = az;
|
coord.x = az;
|
||||||
@@ -392,7 +423,7 @@ public:
|
|||||||
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT
|
} else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT
|
||||||
if constexpr (equatorialMount) {
|
if constexpr (equatorialMount) {
|
||||||
coord_t ha, dec;
|
coord_t ha, dec;
|
||||||
ast_err = _astromEngine.azalt2hadec(coord.x, coord.y, ha, dec);
|
ast_err = _astromEngine->azalt2hadec(coord.x, coord.y, ha, dec);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||||
coord.x = ha;
|
coord.x = ha;
|
||||||
@@ -403,7 +434,7 @@ public:
|
|||||||
} else if constexpr (altAzMount) {
|
} else if constexpr (altAzMount) {
|
||||||
typename pec_t::pec_result_t pec_res;
|
typename pec_t::pec_result_t pec_res;
|
||||||
|
|
||||||
pec_err = _pec.compute(coord.x, coord.y, pec_res);
|
pec_err = _pec->compute(coord.x, coord.y, pec_res);
|
||||||
if (!pec_err) {
|
if (!pec_err) {
|
||||||
X = coord.x - pec_res.dx;
|
X = coord.x - pec_res.dx;
|
||||||
Y = coord.y - pec_res.dy;
|
Y = coord.y - pec_res.dy;
|
||||||
@@ -421,7 +452,7 @@ public:
|
|||||||
coord_t ra, dec, ha, az, alt;
|
coord_t ra, dec, ha, az, alt;
|
||||||
eo_t eo;
|
eo_t eo;
|
||||||
|
|
||||||
ast_err = _astromEngine.greg2jul(time_point, jd);
|
ast_err = _astromEngine->greg2jul(time_point, jd);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
ast_err = icrs2obs(coord.x, coord.y, jd, ra, dec, ha, az, alt, eo);
|
ast_err = icrs2obs(coord.x, coord.y, jd, ra, dec, ha, az, alt, eo);
|
||||||
if (!ast_err) {
|
if (!ast_err) {
|
||||||
@@ -464,8 +495,9 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
astrom_engine_t& _astromEngine;
|
astrom_engine_t* _astromEngine;
|
||||||
pec_t& _pec;
|
pec_t* _pec;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -16,6 +16,7 @@ namespace mcc
|
|||||||
|
|
||||||
struct MccCelestialPoint {
|
struct MccCelestialPoint {
|
||||||
typedef double coord_t;
|
typedef double coord_t;
|
||||||
|
typedef std::chrono::system_clock::time_point time_point_t;
|
||||||
|
|
||||||
MccCoordPairKind coordPairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
|
MccCoordPairKind coordPairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
|
||||||
coord_t x{0.0}, y{0.0};
|
coord_t x{0.0}, y{0.0};
|
||||||
@@ -25,43 +26,113 @@ static_assert(traits::mcc_celestial_point_c<MccCelestialPoint>, "MccCelestialPoi
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* COMMON SLEW-AND-GUIDING POINT CLASS DEFINITION */
|
||||||
|
|
||||||
|
template <typename coord_t>
|
||||||
|
struct MccSimpleSlewAndGuidingModelParams {
|
||||||
|
// ******* default constants *******
|
||||||
|
|
||||||
|
static constexpr size_t defaultWithinToleranceCycleNumber = 10;
|
||||||
|
static constexpr size_t defaultMaxAdjustingCycleNumber = 100;
|
||||||
|
|
||||||
|
// common parameters
|
||||||
|
|
||||||
|
// timeout to wait telemetry update (in seconds, as floating-point)
|
||||||
|
std::chrono::duration<double> telemetryUpdateTimeout{1.0};
|
||||||
|
|
||||||
|
|
||||||
|
// ******* slewing-related parameters *******
|
||||||
|
|
||||||
|
// target-mount coordinate difference to start adjusting of slewing (in radians)
|
||||||
|
coord_t adjustCoordDiff{(double)MccAngle{10.0_degs}};
|
||||||
|
|
||||||
|
// coordinates difference to stop slewing (in radians)
|
||||||
|
coord_t slewToleranceRadius{(double)MccAngle{5.0_arcsecs}};
|
||||||
|
|
||||||
|
// slew process timeout
|
||||||
|
std::chrono::seconds slewTimeout{3600};
|
||||||
|
|
||||||
|
std::chrono::duration<double> telemetryPollingInterval{0.1};
|
||||||
|
|
||||||
|
// if true - stop mount after the slewing
|
||||||
|
bool stopAfterSlew{false};
|
||||||
|
|
||||||
|
coord_t slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
||||||
|
coord_t slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
||||||
|
|
||||||
|
coord_t adjustXRate{(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||||
|
coord_t adjustYRate{(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||||
|
|
||||||
|
// number of consecutive measurements within slewToleranceRadius radius to stop adjusting of slewing
|
||||||
|
size_t withinToleranceCycleNumber{defaultWithinToleranceCycleNumber};
|
||||||
|
|
||||||
|
// maximal allowed number of adjusting cycles
|
||||||
|
size_t maxAdjustingCycleNumber{defaultMaxAdjustingCycleNumber};
|
||||||
|
|
||||||
|
|
||||||
|
// ******* guiding-related parameters *******
|
||||||
|
|
||||||
|
coord_t correctionRange[2]{(double)MccAngle(0.3_arcsecs), (double)MccAngle(5.0_arcsecs)};
|
||||||
|
bool dualAxisGuiding{true}; // mount must be of an equatorial type: false means guiding along only HA-axis
|
||||||
|
};
|
||||||
|
|
||||||
|
struct MccSlewAndGuidingPoint : MccCelestialPoint,
|
||||||
|
MccSimpleSlewAndGuidingModelParams<typename MccCelestialPoint::coord_t> {
|
||||||
|
typedef MccSimpleSlewAndGuidingModelParams<typename MccCelestialPoint::coord_t> slew_guiding_params_t;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
/* CHECK FOR CURRENT MOUNT POSITION IN PROHIBITED ZONES */
|
/* CHECK FOR CURRENT MOUNT POSITION IN PROHIBITED ZONES */
|
||||||
|
|
||||||
/*
|
template <traits::mcc_mount_telemetry_data_c TelemetryDataT, traits::mcc_prohibited_zone_c<TelemetryDataT>... ZTs>
|
||||||
* WARNING: if an error occured during telemetry data request
|
auto mccCheckInZonePZTuple(const TelemetryDataT& telemetry_data,
|
||||||
* result 'in zone' flags cannot be interpretated correctly!
|
const std::tuple<ZTs...>& tuple_zones,
|
||||||
*/
|
|
||||||
template <traits::mcc_mount_telemetry_c TelemetryT,
|
|
||||||
traits::mcc_prohibited_zone_c<typename TelemetryT::mount_telemetry_data_t>... ZTs>
|
|
||||||
auto mccCheckInZonePZTuple(TelemetryT& telemetry,
|
|
||||||
std::tuple<ZTs...>& tuple_zones,
|
|
||||||
std::array<bool, sizeof...(ZTs)>& in_zone)
|
std::array<bool, sizeof...(ZTs)>& in_zone)
|
||||||
{
|
{
|
||||||
const auto p_telemetry = &telemetry;
|
const auto p_tdata = &telemetry_data;
|
||||||
const auto p_tuple_zones = &tuple_zones;
|
const auto p_tuple_zones = &tuple_zones;
|
||||||
const auto p_in_zone = &in_zone;
|
const auto p_in_zone = &in_zone;
|
||||||
|
|
||||||
return [p_telemetry, p_tuple_zones, p_in_zone]<size_t... Is>(std::index_sequence<Is...>) {
|
[p_tdata, p_tuple_zones, p_in_zone]<size_t... Is>(std::index_sequence<Is...>) {
|
||||||
typename TelemetryT::error_t t_err;
|
(((*p_in_zone)[Is] = std::get<Is>(*p_tuple_zones).inZone(*p_tdata)), ...);
|
||||||
(
|
}(std::make_index_sequence<sizeof...(ZTs)>{});
|
||||||
[&t_err]() {
|
|
||||||
if constexpr (Is) {
|
|
||||||
if (t_err) {
|
|
||||||
(*p_in_zone)[Is] = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
typename TelemetryT::mount_telemetry_data_t tdata;
|
return [p_in_zone]<size_t... Is>(std::index_sequence<Is...>) {
|
||||||
t_err = p_telemetry->data(tdata);
|
return ((*p_in_zone)[Is] || ...);
|
||||||
|
|
||||||
if (!t_err) {
|
|
||||||
(*p_in_zone)[Is] = std::get<Is>(p_tuple_zones).inZone(tdata);
|
|
||||||
}
|
|
||||||
}(),
|
|
||||||
...);
|
|
||||||
}(std::make_index_sequence<sizeof...(ZTs)>{});
|
}(std::make_index_sequence<sizeof...(ZTs)>{});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template <traits::mcc_mount_telemetry_data_c TelemetryDataT,
|
||||||
|
traits::mcc_irange_of_pzones_c<TelemetryDataT> RT,
|
||||||
|
std::ranges::output_range<bool> ResT>
|
||||||
|
auto mccCheckInZonePZRange(const TelemetryDataT& telemetry_data, const RT& pzones, ResT& result)
|
||||||
|
{
|
||||||
|
bool in_zone = false;
|
||||||
|
|
||||||
|
auto Npz = std::ranges::distance(pzones);
|
||||||
|
if (!Npz) {
|
||||||
|
return in_zone;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto res_sz = std::ranges::distance(result);
|
||||||
|
bool ok;
|
||||||
|
|
||||||
|
size_t i = 1;
|
||||||
|
auto res_iter = result.begin();
|
||||||
|
for (auto& el : pzones) {
|
||||||
|
ok = el.inZone(telemetry_data);
|
||||||
|
in_zone |= ok;
|
||||||
|
|
||||||
|
if (i > res_sz) {
|
||||||
|
std::back_inserter(result) = ok;
|
||||||
|
} else {
|
||||||
|
std::ranges::advance(res_iter, 1);
|
||||||
|
*res_iter = ok;
|
||||||
|
}
|
||||||
|
++i;
|
||||||
|
}
|
||||||
|
|
||||||
|
return in_zone;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -24,6 +24,8 @@ enum class MccSimpleSlewModelErrorCode : int {
|
|||||||
ERROR_PEC_COMP,
|
ERROR_PEC_COMP,
|
||||||
ERROR_HARDWARE_SETPOS,
|
ERROR_HARDWARE_SETPOS,
|
||||||
ERROR_HARDWARE_GETPOS,
|
ERROR_HARDWARE_GETPOS,
|
||||||
|
ERROR_SLEW_STOPPED,
|
||||||
|
ERROR_SLEW_ADJ_MAXITER,
|
||||||
ERROR_SLEW_TIMEOUT
|
ERROR_SLEW_TIMEOUT
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -76,6 +78,12 @@ struct MccSimpleSlewModelCategory : public std::error_category {
|
|||||||
return "slew model: cannot set position";
|
return "slew model: cannot set position";
|
||||||
case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS:
|
case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS:
|
||||||
return "slew model: cannot get position";
|
return "slew model: cannot get position";
|
||||||
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED:
|
||||||
|
return "slew model: stopped";
|
||||||
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER:
|
||||||
|
return "slew model: max number of adjusting iteration was exceeded";
|
||||||
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT:
|
||||||
|
return "slew model: timeout occured while slewing";
|
||||||
default:
|
default:
|
||||||
return "UNKNOWN";
|
return "UNKNOWN";
|
||||||
}
|
}
|
||||||
@@ -116,50 +124,57 @@ public:
|
|||||||
|
|
||||||
static constexpr size_t defaultAdjustSuccessCycles = 5;
|
static constexpr size_t defaultAdjustSuccessCycles = 5;
|
||||||
|
|
||||||
struct slew_point_t : MccCelestialPoint {
|
using slew_point_t = MccSlewAndGuidingPoint;
|
||||||
// target-mount coordinate difference to start adjusting of slewing (in radians)
|
|
||||||
coord_t adjustCoordDiff{(double)MccAngle{10.0_degs}};
|
|
||||||
|
|
||||||
// coordinates difference to stop slewing (in radians)
|
|
||||||
coord_t slewToleranceRadius{(double)MccAngle{5.0_arcsecs}};
|
|
||||||
|
|
||||||
// coordinates polling interval in seconds
|
|
||||||
std::chrono::duration<double> coordPollingInterval{0.1};
|
|
||||||
bool stopAfterSlew{false};
|
|
||||||
std::chrono::seconds slewTimeout{3600};
|
|
||||||
|
|
||||||
coord_t slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
|
||||||
coord_t slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
|
||||||
|
|
||||||
coord_t adjustXRate{
|
|
||||||
(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
|
||||||
coord_t adjustYRate{
|
|
||||||
(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
|
||||||
|
|
||||||
// number of consecutive measurements within slewPrecision radius to stop adjusting of slewing
|
|
||||||
size_t withinToleranceCycleNumber{10};
|
|
||||||
// maximal allowed number of adjusting cycles
|
|
||||||
size_t maxAdjustingCycleNumber{100};
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T, typename... LoggerCtorArgTs>
|
template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
||||||
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, LoggerCtorArgTs&&... ctor_args)
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
||||||
|
traits::mcc_irange_of_pzones_c<typename TELEMETRY_T::mount_telemetry_data_t> PZ_T,
|
||||||
|
// traits::mcc_tuple_c PZ_T, // std::tuple of prohibited zones
|
||||||
|
typename... LoggerCtorArgTs>
|
||||||
|
MccSimpleSlewModel(TELEMETRY_T* telemetry,
|
||||||
|
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 'MccSimpleSlewModel' class instance ({})", (void*)this));
|
logDebug(std::format("Create 'MccSimpleSlewModel' class instance ({})", (void*)this));
|
||||||
|
|
||||||
init(mount_controls);
|
init(telemetry, hardware, prohibited_zone);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
|
// template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
||||||
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls)
|
// 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 // std::tuple of prohibited zones
|
||||||
|
// >
|
||||||
|
// MccSimpleSlewModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
|
||||||
|
// requires(std::same_as<LoggerT, MccNullLogger>)
|
||||||
|
// {
|
||||||
|
// init(telemetry, hardware, prohibited_zone);
|
||||||
|
// }
|
||||||
|
|
||||||
|
MccSimpleSlewModel(MccSimpleSlewModel&& other)
|
||||||
|
: _stopRequested(other._stopRequested.load()), _slewFunc(std::move(other._slewFunc))
|
||||||
{
|
{
|
||||||
init(mount_controls);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MccSimpleSlewModel& operator=(MccSimpleSlewModel&& other)
|
||||||
|
{
|
||||||
|
if (this == &other) {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
_stopRequested = other._stopRequested.load();
|
||||||
|
_slewFunc = std::move(other._slewFunc);
|
||||||
|
|
||||||
|
return *this;
|
||||||
|
};
|
||||||
|
|
||||||
|
MccSimpleSlewModel(const MccSimpleSlewModel&) = delete;
|
||||||
|
MccSimpleSlewModel& operator=(const MccSimpleSlewModel&) = delete;
|
||||||
|
|
||||||
virtual ~MccSimpleSlewModel()
|
virtual ~MccSimpleSlewModel()
|
||||||
{
|
{
|
||||||
logDebug(std::format("Delete 'MccSimpleSlewModel' class instance ({})", (void*)this));
|
logDebug(std::format("Delete 'MccSimpleSlewModel' class instance ({})", (void*)this));
|
||||||
@@ -167,62 +182,59 @@ public:
|
|||||||
|
|
||||||
error_t slew(slew_point_t pars)
|
error_t slew(slew_point_t pars)
|
||||||
{
|
{
|
||||||
|
_stopRequested = false;
|
||||||
error_t res_err = _slewFunc(std::move(pars));
|
error_t res_err = _slewFunc(std::move(pars));
|
||||||
|
|
||||||
return res_err;
|
return res_err;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
error_t stop()
|
||||||
|
{
|
||||||
|
_stopRequested = true;
|
||||||
|
return MccSimpleSlewModelErrorCode::ERROR_OK;
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
std::atomic_bool _stopRequested{false};
|
||||||
std::function<error_t(const slew_point_t&)> _slewFunc{};
|
std::function<error_t(const slew_point_t&)> _slewFunc{};
|
||||||
|
|
||||||
void init(auto& mount_controls)
|
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);
|
using hardware_t = decltype(*p_hardware);
|
||||||
using hardware_t = decltype(mount_controls.hardware);
|
using telemetry_t = decltype(*p_telemetry);
|
||||||
using pec_t = decltype(mount_controls.PEC);
|
static_assert(traits::mcc_mount_default_telemetry_c<telemetry_t>,
|
||||||
using telemetry_t = decltype(mount_controls.telemetry);
|
"TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry'!");
|
||||||
static_assert(std::derived_from<telemetry_t, MccMountTelemetry<astrom_engine_t, pec_t, hardware_t,
|
|
||||||
typename telemetry_t::mount_telemetry_data_t>>,
|
|
||||||
"TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry' ONE!");
|
|
||||||
|
|
||||||
using tpl_pz_t = decltype(mount_controls.prohibitedZones);
|
using astrom_engine_t = typename telemetry_t::astrom_engine_t;
|
||||||
|
|
||||||
static constexpr size_t Nzones = std::tuple_size_v<tpl_pz_t>;
|
static constexpr size_t Nzones = std::tuple_size_v<decltype(*p_prohibited_zones)>;
|
||||||
|
|
||||||
const auto p_mount_controls = &mount_controls;
|
// const auto p_telemetry = &telemetry;
|
||||||
|
// const auto p_hardware = &hardware;
|
||||||
|
// const auto p_prohibited_zones = &prohibited_zones;
|
||||||
|
|
||||||
|
_slewFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](slew_point_t slew_point) {
|
||||||
_slewFunc = [p_mount_controls, this](slew_point_t slew_point) {
|
_stopRequested = false;
|
||||||
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;
|
|
||||||
|
|
||||||
typename hardware_t::axes_pos_t ax_pos;
|
typename hardware_t::axes_pos_t ax_pos;
|
||||||
|
|
||||||
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 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;
|
||||||
|
|
||||||
coord_t ra_icrs, dec_icrs;
|
if (slew_point.withinToleranceCycleNumber == 0) {
|
||||||
|
slew_point.withinToleranceCycleNumber = MccSlewAndGuidingPoint::defaultWithinToleranceCycleNumber;
|
||||||
if (slew_point.adjustSuccessCycles == 0) {
|
|
||||||
slew_point.adjustSuccessCycles = 5;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// first, compute encoder coordinates
|
// first, compute encoder coordinates
|
||||||
ax_pos.time_point = astrom_engine_t::timePointNow();
|
ax_pos.time_point = astrom_engine_t::timePointNow();
|
||||||
t_err = telemetry.toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
|
t_err = p_telemetry->toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
|
||||||
if (!t_err) {
|
if (!t_err) {
|
||||||
// SETUP TARGET SKY POINT
|
// SETUP TARGET SKY POINT
|
||||||
t_err = telemetry.setTarget(slew_point);
|
t_err = p_telemetry->setTarget(slew_point);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (t_err) {
|
if (t_err) {
|
||||||
@@ -240,7 +252,8 @@ protected:
|
|||||||
// start moving the mount (it is assumed this is asynchronous operation!!!)
|
// start moving the mount (it is assumed this is asynchronous operation!!!)
|
||||||
ax_pos.xrate = slew_point.slewXRate;
|
ax_pos.xrate = slew_point.slewXRate;
|
||||||
ax_pos.yrate = slew_point.slewYRate;
|
ax_pos.yrate = slew_point.slewYRate;
|
||||||
typename hardware_t::error_t hw_err = hardware->setPos(ax_pos);
|
ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_SLEWING;
|
||||||
|
typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);
|
||||||
|
|
||||||
if (hw_err) {
|
if (hw_err) {
|
||||||
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
||||||
@@ -256,15 +269,17 @@ protected:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
|
// typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
|
||||||
|
|
||||||
auto adj_ax_pos = ax_pos; // to prevent possible effects in hardware 'setPos' method
|
auto adj_ax_pos = ax_pos; // to prevent possible effects in hardware 'setPos' method
|
||||||
adj_ax_pos.xrate = slew_point.adjustXRate;
|
adj_ax_pos.xrate = slew_point.adjustXRate;
|
||||||
adj_ax_pos.yrate = slew_point.adjustYRate;
|
adj_ax_pos.yrate = slew_point.adjustYRate;
|
||||||
|
adj_ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_ADJUSTING;
|
||||||
|
|
||||||
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
|
typename telemetry_t::mount_telemetry_data_t::coord_t adj_rad2 = slew_point.adjustCoordDiff *
|
||||||
adj_rad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff,
|
slew_point.adjustCoordDiff,
|
||||||
tol_rad2 = slew_point.slewToleranceRadius * slew_point.slewToleranceRadius;
|
tol_rad2 = slew_point.slewToleranceRadius *
|
||||||
|
slew_point.slewToleranceRadius;
|
||||||
|
|
||||||
std::array<bool, Nzones> in_zone_flag;
|
std::array<bool, Nzones> in_zone_flag;
|
||||||
|
|
||||||
@@ -272,49 +287,54 @@ protected:
|
|||||||
size_t i_in_tol_cycle = 0;
|
size_t i_in_tol_cycle = 0;
|
||||||
bool in_adj_mode = false;
|
bool in_adj_mode = false;
|
||||||
|
|
||||||
auto coord_diff_func = [](auto& t_data) {
|
// compute new hardware coordinate of target
|
||||||
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr;
|
// auto compute_new_coord = [](auto const& t_data, typename hardware_t::axes_pos_t& hw_pos) {
|
||||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
// // current celestial position of target is already computed for given time point
|
||||||
xr = t_data.tagRA - t_data.mntHA;
|
// // so one needs only correct apparent coordinates for PEC corrections
|
||||||
yr = t_data.tagDEC - t_data.mntDEC;
|
// hw_pos.time_point = t_data.time_point;
|
||||||
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
// if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
||||||
xr = t_data.tagAZ - t_data.mntAZ;
|
// hw_pos.x = t_data.tagHA - t_data.pecX;
|
||||||
yr = t_data.tagALT - t_data.mntALT;
|
// hw_pos.y = t_data.tagDEC - t_data.pecY;
|
||||||
} else {
|
// } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
||||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
// hw_pos.x = t_data.tagAZ - t_data.pecX;
|
||||||
|
// hw_pos.y = t_data.tagALT - t_data.pecY;
|
||||||
|
// } else {
|
||||||
|
// static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
||||||
|
// }
|
||||||
|
// };
|
||||||
|
|
||||||
|
auto cycle_func = [&](auto& t_data) mutable {
|
||||||
|
if (_stopRequested) {
|
||||||
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
|
||||||
}
|
}
|
||||||
|
|
||||||
return xr * xr + yr * yr;
|
|
||||||
};
|
|
||||||
|
|
||||||
auto cycle_func = [&](auto t_data) mutable {
|
|
||||||
// check for prohibited zones
|
// check for prohibited zones
|
||||||
auto t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);
|
if (mccCheckInZonePZTuple(t_data, *p_prohibited_zones, in_zone_flag)) {
|
||||||
|
return MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE;
|
||||||
|
};
|
||||||
|
|
||||||
if (t_err) {
|
|
||||||
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
|
||||||
logError(
|
|
||||||
std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
|
||||||
res_err = t_err;
|
|
||||||
} else {
|
|
||||||
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
|
||||||
logError(std::format("An telemetry error occured: code = {}", t_err));
|
|
||||||
}
|
|
||||||
res_err = MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// t_data was updated in caller!!!
|
// t_data was updated in caller!!!
|
||||||
coord_diff2 = coord_diff_func(t_data);
|
auto coord_diff = p_telemetry->targetToMountDiff();
|
||||||
|
|
||||||
if (coord_diff2 < adj_rad2) { // adjusting mode
|
if (_stopRequested) {
|
||||||
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (coord_diff.r2 < adj_rad2) { // adjusting mode
|
||||||
in_adj_mode = true;
|
in_adj_mode = true;
|
||||||
hw_err = hardware->setPos(adj_ax_pos);
|
|
||||||
|
// compute_new_coord(t_data, adj_ax_pos);
|
||||||
|
|
||||||
|
adj_ax_pos.time_point = t_data.time_point;
|
||||||
|
adj_ax_pos.x += coord_diff.xdiff;
|
||||||
|
adj_ax_pos.y += coord_diff.ydiff;
|
||||||
|
|
||||||
|
hw_err = p_hardware->setPos(adj_ax_pos);
|
||||||
|
|
||||||
if (!hw_err) {
|
if (!hw_err) {
|
||||||
++i_adj_cycle;
|
++i_adj_cycle;
|
||||||
if (coord_diff2 < tol_rad2) {
|
if (coord_diff.r2 < tol_rad2) {
|
||||||
++i_in_tol_cycle;
|
++i_in_tol_cycle;
|
||||||
|
|
||||||
if (i_in_tol_cycle == slew_point.withinToleranceCycleNumber) {
|
if (i_in_tol_cycle == slew_point.withinToleranceCycleNumber) {
|
||||||
@@ -324,7 +344,7 @@ protected:
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (i_adj_cycle == slew_point.maxAdjustingCycleNumber) {
|
if (i_adj_cycle == slew_point.maxAdjustingCycleNumber) {
|
||||||
// res_err = max iter namber was exceeded
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
@@ -342,8 +362,41 @@ protected:
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else { // continue to slewing
|
||||||
if (in_adj_mode) { // ?!!!!!!!!!!!!!
|
if (in_adj_mode) { // ?!!!!!!!!!!!!! slew again?!!!
|
||||||
|
logWarn(std::format(
|
||||||
|
"The slewing is in adjusting mode but computed target-to-mount coordinate difference "
|
||||||
|
"'{}' is greater than limit '{}' for adjusting mode!",
|
||||||
|
coord_diff.r2, adj_rad2));
|
||||||
|
|
||||||
|
in_adj_mode = false;
|
||||||
|
i_adj_cycle = 0;
|
||||||
|
i_in_tol_cycle = 0;
|
||||||
|
|
||||||
|
// compute_new_coord(t_data, ax_pos);
|
||||||
|
adj_ax_pos.time_point = t_data.time_point;
|
||||||
|
adj_ax_pos.x += coord_diff.xdiff;
|
||||||
|
adj_ax_pos.y += coord_diff.ydiff;
|
||||||
|
|
||||||
|
ax_pos.time_point = t_data.time_point;
|
||||||
|
ax_pos.x = adj_ax_pos.x;
|
||||||
|
ax_pos.y = adj_ax_pos.y;
|
||||||
|
// send command for slewing
|
||||||
|
typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);
|
||||||
|
|
||||||
|
|
||||||
|
if (hw_err) {
|
||||||
|
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
||||||
|
logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
|
||||||
|
hw_err.message()));
|
||||||
|
return hw_err;
|
||||||
|
} else {
|
||||||
|
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
||||||
|
logError(std::format("An hardware error occured: code = {}", hw_err));
|
||||||
|
}
|
||||||
|
return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@@ -353,10 +406,30 @@ protected:
|
|||||||
|
|
||||||
// NOTE: TARGET COORDINATES WILL BE UPDATED FOR CURRENT TIME-POINT IN TELEMETRY-CLASS!!!
|
// NOTE: TARGET COORDINATES WILL BE UPDATED FOR CURRENT TIME-POINT IN TELEMETRY-CLASS!!!
|
||||||
|
|
||||||
auto iter = telemetry.addCallbackFunc(cycle_func);
|
|
||||||
|
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
|
t_err = p_telemetry->waitForUpdatedData(t_data, slew_point.telemetryUpdateTimeout);
|
||||||
|
|
||||||
|
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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
cycle_func(t_data);
|
||||||
|
|
||||||
|
if (res_err) {
|
||||||
|
return res_err;
|
||||||
|
}
|
||||||
|
|
||||||
if ((std::chrono::steady_clock::now() - start_poll_tm) > slew_point.slewTimeout) {
|
if ((std::chrono::steady_clock::now() - start_poll_tm) > slew_point.slewTimeout) {
|
||||||
logError("Waiting time for completion of slewing expired!");
|
logError("Waiting time for completion of slewing expired!");
|
||||||
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
|
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
|
||||||
@@ -369,7 +442,6 @@ protected:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
static_assert(std::movable<MccSimpleSlewModel<>>);
|
||||||
// static_assert(traits::mcc_slew_model_c<>);
|
|
||||||
|
|
||||||
} // namespace mcc
|
} // namespace mcc
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ public:
|
|||||||
|
|
||||||
template <traits::mcc_range_of_input_char_range R = decltype(LOGGER_DEFAULT_FORMAT)>
|
template <traits::mcc_range_of_input_char_range R = decltype(LOGGER_DEFAULT_FORMAT)>
|
||||||
MccSpdlogLogger(std::shared_ptr<spdlog::logger> logger, const R& pattern_range = LOGGER_DEFAULT_FORMAT)
|
MccSpdlogLogger(std::shared_ptr<spdlog::logger> logger, const R& pattern_range = LOGGER_DEFAULT_FORMAT)
|
||||||
: _loggerSPtr(logger), _currentLogPatternRange(), _currentLogPattern()
|
: _currentLogPatternRange(), _currentLogPattern(), _loggerSPtr(logger)
|
||||||
{
|
{
|
||||||
if (std::distance(pattern_range.begin(), pattern_range.end())) {
|
if (std::distance(pattern_range.begin(), pattern_range.end())) {
|
||||||
std::ranges::copy(
|
std::ranges::copy(
|
||||||
@@ -178,10 +178,8 @@ protected:
|
|||||||
|
|
||||||
void addMarkToPatternIdx(const char* mark, size_t after_idx = 1)
|
void addMarkToPatternIdx(const char* mark, size_t after_idx = 1)
|
||||||
{
|
{
|
||||||
addMarkToPatternIdx(std::string_view{mark});
|
addMarkToPatternIdx(std::string_view{mark}, after_idx);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
} // namespace mcc::utils
|
} // namespace mcc::utils
|
||||||
|
|||||||
@@ -1,23 +1,28 @@
|
|||||||
#include <fstream>
|
// #include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
// #include "../mcc_coord.h"
|
// #include "../mcc_coord.h"
|
||||||
#include "../mcc_mount_astro_erfa.h"
|
#include "../mcc_mount_astro_erfa.h"
|
||||||
#include "../mcc_mount_pz.h"
|
#include "../mcc_mount_pz.h"
|
||||||
#include "../mount_astrom.h"
|
// #include "../mount_astrom.h"
|
||||||
|
#include "../mcc_mount_pec.h"
|
||||||
|
#include "../mcc_mount_telemetry.h"
|
||||||
|
|
||||||
// BTA coords from maps.yandex.ru: 43.646711, 41.440732
|
// BTA coords from maps.yandex.ru: 43.646711, 41.440732
|
||||||
|
|
||||||
struct tel_data_t {
|
// struct tel_data_t {
|
||||||
typedef mcc::MccAngle coord_t;
|
// typedef mcc::MccAngle coord_t;
|
||||||
typedef std::chrono::system_clock::time_point time_point_t;
|
// typedef std::chrono::system_clock::time_point time_point_t;
|
||||||
|
|
||||||
time_point_t time_point;
|
// time_point_t time_point;
|
||||||
coord_t mntRA, mntDEC, mntHA;
|
// coord_t tagRA, tagDEC, tagHA;
|
||||||
coord_t mntAZ, mntALT;
|
// coord_t tagAZ, tagALT;
|
||||||
coord_t mntPosX, mntPosY;
|
|
||||||
coord_t mntRateX, mntRateY;
|
// coord_t mntRA, mntDEC, mntHA;
|
||||||
};
|
// coord_t mntAZ, mntALT;
|
||||||
|
// coord_t mntPosX, mntPosY;
|
||||||
|
// coord_t mntRateX, mntRateY;
|
||||||
|
// };
|
||||||
|
|
||||||
int main(int argc, char* argv[])
|
int main(int argc, char* argv[])
|
||||||
{
|
{
|
||||||
@@ -158,7 +163,9 @@ int main(int argc, char* argv[])
|
|||||||
|
|
||||||
// std::cout << "\n\n\n\n";
|
// std::cout << "\n\n\n\n";
|
||||||
|
|
||||||
using engine_t = mcc::astrom::erfa::MccMountAstromEngineERFA<>;
|
auto as_sgs = [](double v, bool hms = false) { return mcc::MccAngle(v).sexagesimal(hms); };
|
||||||
|
|
||||||
|
using engine_t = mcc::astrom::erfa::MccMountAstromEngineERFA;
|
||||||
engine_t::engine_state_t state;
|
engine_t::engine_state_t state;
|
||||||
state.lon = 41.440732_degs;
|
state.lon = 41.440732_degs;
|
||||||
state.lat = 43.646711_degs;
|
state.lat = 43.646711_degs;
|
||||||
@@ -166,8 +173,8 @@ int main(int argc, char* argv[])
|
|||||||
|
|
||||||
state.meteo = {10.0, 0.5, 1010.0};
|
state.meteo = {10.0, 0.5, 1010.0};
|
||||||
|
|
||||||
std::cout << "LON = " << state.lon.sexagesimal() << "\n";
|
std::cout << "LON = " << mcc::MccAngle(state.lon).sexagesimal() << "\n";
|
||||||
std::cout << "LAT = " << state.lat.sexagesimal() << "\n\n";
|
std::cout << "LAT = " << mcc::MccAngle(state.lat).sexagesimal() << "\n\n";
|
||||||
|
|
||||||
engine_t erfa(state);
|
engine_t erfa(state);
|
||||||
engine_t::juldate_t jd{60861.72};
|
engine_t::juldate_t jd{60861.72};
|
||||||
@@ -176,40 +183,46 @@ int main(int argc, char* argv[])
|
|||||||
erfa.greg2jul(now, jd);
|
erfa.greg2jul(now, jd);
|
||||||
std::cout << "MJD(" << now << ") = " << jd.mjd << "\n";
|
std::cout << "MJD(" << now << ") = " << jd.mjd << "\n";
|
||||||
|
|
||||||
mcc::MccAngle lst;
|
double lst;
|
||||||
erfa.apparentSiderTime(jd, lst, true);
|
erfa.apparentSiderTime(jd, lst, true);
|
||||||
std::cout << "LST(MJD = " << jd.mjd << ") = " << lst.sexagesimal(true) << "\n\n";
|
std::cout << "LST(MJD = " << jd.mjd << ") = " << mcc::MccAngle(lst).sexagesimal(true) << "\n\n";
|
||||||
|
|
||||||
|
|
||||||
// mcc::MccAngle ra1{"10:00:00", mcc::mcc_hms}, dec1{"68:25:10.43"}, ra_o, dec_o, ha1, az1, alt1;
|
// mcc::MccAngle ra1{"10:00:00", mcc::mcc_hms}, dec1{"68:25:10.43"}, ra_o, dec_o, ha1, az1, alt1;
|
||||||
mcc::MccAngle ra1{"5:00:00", mcc::mcc_hms}, dec1{"38:25:10.43"}, ra_o, dec_o, ha1, az1, alt1;
|
mcc::MccAngle ra1{"5:00:00", mcc::mcc_hms}, dec1{"38:25:10.43"};
|
||||||
mcc::MccAngle eor;
|
double ra_o, dec_o, ha1, az1, alt1, eor;
|
||||||
|
|
||||||
std::cout << "RA = " << ra1.sexagesimal(true) << ", DEC = " << dec1.sexagesimal() << "\n";
|
std::cout << "RA = " << ra1.sexagesimal(true) << ", DEC = " << dec1.sexagesimal() << "\n";
|
||||||
|
|
||||||
auto res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
auto res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
||||||
std::cout << "eq of origins (from icrs2obs) = " << eor.sexagesimal(true) << "\n";
|
std::cout << "eq of origins (from icrs2obs) = " << mcc::MccAngle(eor).sexagesimal(true) << "\n";
|
||||||
|
|
||||||
std::cout << "ret code (icrs2obs) = " << res.message() << "\n";
|
std::cout << "ret code (icrs2obs) = " << res.message() << "\n";
|
||||||
std::cout << "alt = " << alt1.sexagesimal() << "\n";
|
std::cout << "alt = " << mcc::MccAngle(alt1).sexagesimal() << "\n";
|
||||||
std::cout << "az = " << az1.sexagesimal() << "\n";
|
std::cout << "az = " << mcc::MccAngle(az1).sexagesimal() << "\n";
|
||||||
|
|
||||||
std::cout << "HA_app = " << ha1.sexagesimal(true) << "\n";
|
std::cout << "HA_app = " << mcc::MccAngle(ha1).sexagesimal(true) << "\n";
|
||||||
std::cout << "RA_app = " << ra_o.sexagesimal(true) << "\n";
|
std::cout << "RA_app = " << mcc::MccAngle(ra_o).sexagesimal(true) << "\n";
|
||||||
std::cout << "DEC_app = " << dec_o.sexagesimal() << "\n";
|
std::cout << "DEC_app = " << mcc::MccAngle(dec_o).sexagesimal() << "\n";
|
||||||
|
|
||||||
res = erfa.eqOrigins(jd, eor);
|
res = erfa.eqOrigins(jd, eor);
|
||||||
|
|
||||||
std::cout << "eq of origins (from eqOrigins) = " << eor.sexagesimal(true) << "\n";
|
std::cout << "eq of origins (from eqOrigins) = " << mcc::MccAngle(eor).sexagesimal(true) << "\n";
|
||||||
std::cout << "RA_app_comp = " << (lst - ha1 + eor).sexagesimal(true) << "\n";
|
std::cout << "RA_app_comp = " << mcc::MccAngle(lst - ha1 + eor).sexagesimal(true) << "\n";
|
||||||
|
|
||||||
|
|
||||||
std::cout << "\n\n\n\n";
|
std::cout << "\n\n\n\n";
|
||||||
|
|
||||||
mcc::MccMinAltPZ minalt_pz(10.0_degs, state.lat);
|
mcc::MccMinAltPZ minalt_pz(10.0_degs, state.lat, &erfa);
|
||||||
mcc::MccMaxAltPZ maxalt_pz(85.0_degs, state.lat);
|
mcc::MccMaxAltPZ maxalt_pz(85.0_degs, state.lat, &erfa);
|
||||||
|
|
||||||
tel_data_t tdata{std::chrono::system_clock::now(), ra_o, dec_o, ha1, az1, alt1, 0.0, 0.0, 0.0, 0.0};
|
// tel_data_t tdata{std::chrono::system_clock::now(), ra_o, dec_o, ha1, az1, alt1, 0.0, 0.0, 0.0, 0.0};
|
||||||
|
mcc::MccMountTelemetryData<engine_t> tdata{.time_point = engine_t::timePointNow(),
|
||||||
|
.mntRA = ra_o,
|
||||||
|
.mntDEC = dec_o,
|
||||||
|
.mntHA = ha1,
|
||||||
|
.mntAZ = az1,
|
||||||
|
.mntALT = alt1};
|
||||||
|
|
||||||
bool ask = minalt_pz.inZone(tdata);
|
bool ask = minalt_pz.inZone(tdata);
|
||||||
std::cout << "in zone? " << std::boolalpha << ask << "\n";
|
std::cout << "in zone? " << std::boolalpha << ask << "\n";
|
||||||
@@ -236,19 +249,25 @@ int main(int argc, char* argv[])
|
|||||||
dec1 = "40:25:10.43"_dms;
|
dec1 = "40:25:10.43"_dms;
|
||||||
res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
||||||
std::cout << "ret code (icrs2obs) = " << res.message() << "\n";
|
std::cout << "ret code (icrs2obs) = " << res.message() << "\n";
|
||||||
std::cout << "alt = " << alt1.sexagesimal() << "\n";
|
std::cout << "alt = " << as_sgs(alt1) << "\n";
|
||||||
std::cout << "az = " << az1.sexagesimal() << "\n";
|
std::cout << "az = " << as_sgs(az1) << "\n";
|
||||||
|
|
||||||
std::cout << "HA_app = " << ha1.sexagesimal(true) << "\n";
|
std::cout << "HA_app = " << as_sgs(ha1, true) << "\n";
|
||||||
std::cout << "RA_app = " << ra_o.sexagesimal(true) << "\n";
|
std::cout << "RA_app = " << as_sgs(ra_o, true) << "\n";
|
||||||
std::cout << "DEC_app = " << dec_o.sexagesimal() << "\n";
|
std::cout << "DEC_app = " << as_sgs(dec_o) << "\n";
|
||||||
|
|
||||||
|
|
||||||
tdata = {std::chrono::system_clock::now(), ra_o, dec_o, ha1, az1, alt1, 0.0, 0.0, 0.0, 0.0};
|
// tdata = {std::chrono::system_clock::now(), ra_o, dec_o, ha1, az1, alt1, 0.0, 0.0, 0.0, 0.0};
|
||||||
|
tdata = {.time_point = engine_t::timePointNow(),
|
||||||
|
.mntRA = ra_o,
|
||||||
|
.mntDEC = dec_o,
|
||||||
|
.mntHA = ha1,
|
||||||
|
.mntAZ = az1,
|
||||||
|
.mntALT = alt1};
|
||||||
|
|
||||||
jd.mjd += 1.0;
|
jd.mjd += 1.0;
|
||||||
res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
res = erfa.icrs2obs(ra1, dec1, jd, ra_o, dec_o, ha1, az1, alt1, eor);
|
||||||
std::cout << "RA_app (+24h) = " << ra_o.sexagesimal(true) << "\n";
|
std::cout << "RA_app (+24h) = " << as_sgs(ra_o, true) << "\n";
|
||||||
|
|
||||||
tm = maxalt_pz.timeTo(tdata);
|
tm = maxalt_pz.timeTo(tdata);
|
||||||
if (std::isinf(tm.count())) {
|
if (std::isinf(tm.count())) {
|
||||||
@@ -268,10 +287,35 @@ int main(int argc, char* argv[])
|
|||||||
std::cout << "(MAXALT) time from zone: " << std::chrono::hh_mm_ss(tm) << " (" << now1 << ")\n";
|
std::cout << "(MAXALT) time from zone: " << std::chrono::hh_mm_ss(tm) << " (" << now1 << ")\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
struct cp_t {
|
||||||
|
typedef double coord_t;
|
||||||
|
typedef std::chrono::system_clock::time_point time_point_t;
|
||||||
|
|
||||||
|
mcc::MccCoordPairKind coordPairKind;
|
||||||
|
std::chrono::system_clock::time_point time_point;
|
||||||
|
double x, y;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
cp_t tcp{.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS,
|
||||||
|
.time_point = engine_t::timePointNow(),
|
||||||
|
.x = "17:00:00"_hms,
|
||||||
|
.y = "40:25:10.43"_dms},
|
||||||
|
icp{.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT};
|
||||||
|
|
||||||
|
|
||||||
|
// bool ok = minalt_pz.intersectPoint(tcp, icp);
|
||||||
|
bool ok = maxalt_pz.intersectPoint(tcp, icp);
|
||||||
|
|
||||||
|
std::cout << "\nINTERSECT Az = " << as_sgs(icp.x) << "\n";
|
||||||
|
std::cout << "INTERSECT Alt = " << as_sgs(icp.y) << "\n";
|
||||||
|
|
||||||
|
|
||||||
ra_o = 1.0;
|
ra_o = 1.0;
|
||||||
dec_o = 2.0;
|
dec_o = 2.0;
|
||||||
ha1 = ra_o * dec_o;
|
ha1 = ra_o * dec_o;
|
||||||
std::cout << "mult: " << ha1.degrees() << "\n";
|
std::cout << "mult: " << mcc::MccAngle(ha1).degrees() << "\n";
|
||||||
|
|
||||||
return ecode;
|
return ecode;
|
||||||
}
|
}
|
||||||
|
|||||||
110
mcc/CMakeLists.txt
Normal file
110
mcc/CMakeLists.txt
Normal file
@@ -0,0 +1,110 @@
|
|||||||
|
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(Threads REQUIRED)
|
||||||
|
|
||||||
|
# ******* SPDLOG LIBRARY *******
|
||||||
|
|
||||||
|
include(FetchContent)
|
||||||
|
include(ExternalProject)
|
||||||
|
|
||||||
|
set(SPDLOG_USE_STD_FORMAT ON CACHE INTERNAL "Use of C++20 std::format")
|
||||||
|
set(SPDLOG_FMT_EXTERNAL OFF CACHE INTERNAL "Turn off external fmt library")
|
||||||
|
|
||||||
|
find_package(spdlog CONFIG)
|
||||||
|
if (NOT ${spdlog_FOUND})
|
||||||
|
FetchContent_Declare(spdlog
|
||||||
|
# ExternalProject_Add(spdlog
|
||||||
|
# SOURCE_DIR ${CMAKE_BINARY_DIR}/spdlog_lib
|
||||||
|
# BINARY_DIR ${CMAKE_BINARY_DIR}/spdlog_lib/build
|
||||||
|
GIT_REPOSITORY "https://github.com/gabime/spdlog.git"
|
||||||
|
GIT_TAG "v1.15.1"
|
||||||
|
GIT_SHALLOW TRUE
|
||||||
|
GIT_SUBMODULES ""
|
||||||
|
GIT_PROGRESS TRUE
|
||||||
|
CMAKE_ARGS "-DSPDLOG_USE_STD_FORMAT=ON -DSPDLOG_FMT_EXTERNAL=OFF"
|
||||||
|
# CONFIGURE_COMMAND ""
|
||||||
|
# BUILD_COMMAND ""
|
||||||
|
# INSTALL_COMMAND ""
|
||||||
|
# UPDATE_COMMAND ""
|
||||||
|
# SOURCE_SUBDIR cmake # turn off building
|
||||||
|
OVERRIDE_FIND_PACKAGE
|
||||||
|
)
|
||||||
|
find_package(spdlog CONFIG)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# ******* ERFA LIBRARY *******
|
||||||
|
|
||||||
|
ExternalProject_Add(erfalib
|
||||||
|
PREFIX ${CMAKE_BINARY_DIR}/erfa_lib
|
||||||
|
GIT_REPOSITORY "https://github.com/liberfa/erfa.git"
|
||||||
|
GIT_TAG "v2.0.1"
|
||||||
|
UPDATE_COMMAND ""
|
||||||
|
PATCH_COMMAND ""
|
||||||
|
# BINARY_DIR erfa_build
|
||||||
|
# SOURCE_DIR erfa
|
||||||
|
# INSTALL_DIR
|
||||||
|
LOG_CONFIGURE 1
|
||||||
|
CONFIGURE_COMMAND meson setup --reconfigure -Ddefault_library=static -Dbuildtype=release
|
||||||
|
-Dprefix=${CMAKE_BINARY_DIR}/erfa_lib -Dlibdir= -Dincludedir= -Ddatadir= <SOURCE_DIR>
|
||||||
|
BUILD_COMMAND ninja -C <BINARY_DIR>
|
||||||
|
INSTALL_COMMAND meson install -C <BINARY_DIR>
|
||||||
|
BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a
|
||||||
|
)
|
||||||
|
add_library(ERFA_LIB STATIC IMPORTED GLOBAL)
|
||||||
|
set_target_properties(ERFA_LIB PROPERTIES IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a)
|
||||||
|
add_dependencies(ERFA_LIB erfalib)
|
||||||
|
set(ERFA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/erfa_lib)
|
||||||
|
# set(ERFA_LIBFILE ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a PARENT_SCOPE)
|
||||||
|
include_directories(${ERFA_INCLUDE_DIR})
|
||||||
|
message(STATUS "ERFA INCLUDE DIR: " ${ERFA_INCLUDE_DIR})
|
||||||
|
|
||||||
|
add_subdirectory(bsplines)
|
||||||
|
message(STATUS "BSPLINES_INCLUDE_DIR: " ${BSPLINES_INCLUDE_DIR})
|
||||||
|
include_directories(${BSPLINES_INCLUDE_DIR})
|
||||||
|
|
||||||
|
set(MCC_LIBRARY_SRC mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h
|
||||||
|
mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h
|
||||||
|
mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h
|
||||||
|
mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_moving_model_common.h
|
||||||
|
mcc_netserver_endpoint.h mcc_netserver.h mcc_netserver_proto.h)
|
||||||
|
|
||||||
|
list(APPEND MCC_LIBRARY_SRC mcc_spdlog.h)
|
||||||
|
|
||||||
|
set(MCC_LIBRARY mcc)
|
||||||
|
add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC})
|
||||||
|
target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23)
|
||||||
|
target_compile_definitions(${MCC_LIBRARY} INTERFACE SPDLOG_USE_STD_FORMAT=1 SPDLOG_FMT_EXTERNAL=0)
|
||||||
|
target_link_libraries(${MCC_LIBRARY} INTERFACE spdlog Threads::Threads atomic ${ERFA_LIB})
|
||||||
|
target_include_directories(${MCC_LIBRARY} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR})
|
||||||
|
target_include_directories(${MCC_LIBRARY} INTERFACE
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||||
|
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/mcc>
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
option(WITH_TESTS "Build tests" ON)
|
||||||
|
|
||||||
|
if (WITH_TESTS)
|
||||||
|
set(CTTE_TEST_APP ccte_test)
|
||||||
|
add_executable(${CTTE_TEST_APP} tests/ccte_test.cpp)
|
||||||
|
target_include_directories(${CTTE_TEST_APP} PRIVATE ${ERFA_INCLUDE_DIR})
|
||||||
|
target_link_libraries(${CTTE_TEST_APP} ERFA_LIB bsplines)
|
||||||
|
|
||||||
|
set(NETMSG_TESTS_APP netmsg_test)
|
||||||
|
add_executable(${NETMSG_TESTS_APP} tests/netmsg_test.cpp)
|
||||||
|
target_link_libraries(${NETMSG_TESTS_APP} mcc)
|
||||||
|
|
||||||
|
|
||||||
|
set(MCCCOORD_TEST_APP mcc_coord_test)
|
||||||
|
add_executable(${MCCCOORD_TEST_APP} tests/mcc_coord_test.cpp)
|
||||||
|
target_link_libraries(${MCCCOORD_TEST_APP} mcc ERFA_LIB)
|
||||||
|
|
||||||
|
enable_testing()
|
||||||
|
endif()
|
||||||
35
mcc/bsplines/CMakeLists.txt
Normal file
35
mcc/bsplines/CMakeLists.txt
Normal file
@@ -0,0 +1,35 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.20)
|
||||||
|
|
||||||
|
set(func_name "")
|
||||||
|
|
||||||
|
file(GLOB src_files "*.f")
|
||||||
|
|
||||||
|
foreach(ff IN LISTS src_files)
|
||||||
|
get_filename_component(sn ${ff} NAME_WE)
|
||||||
|
list(APPEND func_name ${sn})
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
# message(STATUS "${func_name}")
|
||||||
|
|
||||||
|
string(REPLACE ";" " " func_str "${func_name}")
|
||||||
|
|
||||||
|
# message(STATUS ${func_str})
|
||||||
|
|
||||||
|
enable_language(Fortran CXX)
|
||||||
|
|
||||||
|
include(FortranCInterface)
|
||||||
|
FortranCInterface_HEADER(FortranCInterface.h
|
||||||
|
MACRO_NAMESPACE "FC_"
|
||||||
|
# SYMBOL_NAMESPACE "fp_"
|
||||||
|
SYMBOL_NAMESPACE ""
|
||||||
|
# SYMBOLS ${func_str}
|
||||||
|
SYMBOLS ${func_name}
|
||||||
|
)
|
||||||
|
FortranCInterface_VERIFY(CXX)
|
||||||
|
|
||||||
|
set(BSPLINES_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR} PARENT_SCOPE)
|
||||||
|
include_directories(${BSPLINES_INCLUDE_DIR})
|
||||||
|
|
||||||
|
add_library(bsplines STATIC ${src_files} mcc_bsplines.h)
|
||||||
|
|
||||||
|
|
||||||
19
mcc/bsplines/Makefile
Normal file
19
mcc/bsplines/Makefile
Normal file
@@ -0,0 +1,19 @@
|
|||||||
|
# Makefile that builts a library lib$(LIB).a from all
|
||||||
|
# of the Fortran files found in the current directory.
|
||||||
|
# Usage: make LIB=<libname>
|
||||||
|
# Pearu
|
||||||
|
|
||||||
|
OBJ=$(patsubst %.f,%.o,$(shell ls *.f))
|
||||||
|
all: lib$(LIB).a
|
||||||
|
$(OBJ):
|
||||||
|
$(FC) -c $(FFLAGS) $(FSHARED) $(patsubst %.o,%.f,$(@F)) -o $@
|
||||||
|
lib$(LIB).a: $(OBJ)
|
||||||
|
$(AR) rus lib$(LIB).a $?
|
||||||
|
clean:
|
||||||
|
rm *.o
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
3
mcc/bsplines/README
Normal file
3
mcc/bsplines/README
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
- ddierckx is a 'real*8' version of dierckx
|
||||||
|
generated by Pearu Peterson <pearu@ioc.ee>.
|
||||||
|
- dierckx (in netlib) is fitpack by P. Dierckx
|
||||||
66
mcc/bsplines/bispeu.f
Normal file
66
mcc/bsplines/bispeu.f
Normal file
@@ -0,0 +1,66 @@
|
|||||||
|
recursive subroutine bispeu(tx,nx,ty,ny,c,kx,ky,x,y,z,m,wrk,
|
||||||
|
* lwrk, ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine bispeu evaluates on a set of points (x(i),y(i)),i=1,...,m
|
||||||
|
c a bivariate spline s(x,y) of degrees kx and ky, given in the
|
||||||
|
c b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call bispeu(tx,nx,ty,ny,c,kx,ky,x,y,z,m,wrk,lwrk,
|
||||||
|
c * iwrk,kwrk,ier)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c tx : real array, length nx, which contains the position of the
|
||||||
|
c knots in the x-direction.
|
||||||
|
c nx : integer, giving the total number of knots in the x-direction
|
||||||
|
c ty : real array, length ny, which contains the position of the
|
||||||
|
c knots in the y-direction.
|
||||||
|
c ny : integer, giving the total number of knots in the y-direction
|
||||||
|
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||||
|
c b-spline coefficients.
|
||||||
|
c kx,ky : integer values, giving the degrees of the spline.
|
||||||
|
c x : real array of dimension (mx).
|
||||||
|
c y : real array of dimension (my).
|
||||||
|
c m : on entry m must specify the number points. m >= 1.
|
||||||
|
c wrk : real array of dimension lwrk. used as workspace.
|
||||||
|
c lwrk : integer, specifying the dimension of wrk.
|
||||||
|
c lwrk >= kx+ky+2
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c z : real array of dimension m.
|
||||||
|
c on successful exit z(i) contains the value of s(x,y)
|
||||||
|
c at the point (x(i),y(i)), i=1,...,m.
|
||||||
|
c ier : integer error flag
|
||||||
|
c ier=0 : normal return
|
||||||
|
c ier=10: invalid input data (see restrictions)
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c m >=1, lwrk>=mx*(kx+1)+my*(ky+1), kwrk>=mx+my
|
||||||
|
c tx(kx+1) <= x(i-1) <= x(i) <= tx(nx-kx), i=2,...,mx
|
||||||
|
c ty(ky+1) <= y(j-1) <= y(j) <= ty(ny-ky), j=2,...,my
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpbisp,fpbspl
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer nx,ny,kx,ky,m,lwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(m),y(m),z(m),
|
||||||
|
* wrk(lwrk)
|
||||||
|
c ..local scalars..
|
||||||
|
integer iwrk(2)
|
||||||
|
integer i, lwest
|
||||||
|
c ..
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
lwest = kx+ky+2
|
||||||
|
if (lwrk.lt.lwest) go to 100
|
||||||
|
if (m.lt.1) go to 100
|
||||||
|
ier = 0
|
||||||
|
do 10 i=1,m
|
||||||
|
call fpbisp(tx,nx,ty,ny,c,kx,ky,x(i),1,y(i),1,z(i),wrk(1),
|
||||||
|
* wrk(kx+2),iwrk(1),iwrk(2))
|
||||||
|
10 continue
|
||||||
|
100 return
|
||||||
|
end
|
||||||
104
mcc/bsplines/bispev.f
Normal file
104
mcc/bsplines/bispev.f
Normal file
@@ -0,0 +1,104 @@
|
|||||||
|
recursive subroutine bispev(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,
|
||||||
|
* wrk,lwrk,iwrk,kwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine bispev evaluates on a grid (x(i),y(j)),i=1,...,mx; j=1,...
|
||||||
|
c ,my a bivariate spline s(x,y) of degrees kx and ky, given in the
|
||||||
|
c b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call bispev(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,wrk,lwrk,
|
||||||
|
c * iwrk,kwrk,ier)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c tx : real array, length nx, which contains the position of the
|
||||||
|
c knots in the x-direction.
|
||||||
|
c nx : integer, giving the total number of knots in the x-direction
|
||||||
|
c ty : real array, length ny, which contains the position of the
|
||||||
|
c knots in the y-direction.
|
||||||
|
c ny : integer, giving the total number of knots in the y-direction
|
||||||
|
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||||
|
c b-spline coefficients.
|
||||||
|
c kx,ky : integer values, giving the degrees of the spline.
|
||||||
|
c x : real array of dimension (mx).
|
||||||
|
c before entry x(i) must be set to the x co-ordinate of the
|
||||||
|
c i-th grid point along the x-axis.
|
||||||
|
c tx(kx+1)<=x(i-1)<=x(i)<=tx(nx-kx), i=2,...,mx.
|
||||||
|
c mx : on entry mx must specify the number of grid points along
|
||||||
|
c the x-axis. mx >=1.
|
||||||
|
c y : real array of dimension (my).
|
||||||
|
c before entry y(j) must be set to the y co-ordinate of the
|
||||||
|
c j-th grid point along the y-axis.
|
||||||
|
c ty(ky+1)<=y(j-1)<=y(j)<=ty(ny-ky), j=2,...,my.
|
||||||
|
c my : on entry my must specify the number of grid points along
|
||||||
|
c the y-axis. my >=1.
|
||||||
|
c wrk : real array of dimension lwrk. used as workspace.
|
||||||
|
c lwrk : integer, specifying the dimension of wrk.
|
||||||
|
c lwrk >= mx*(kx+1)+my*(ky+1)
|
||||||
|
c iwrk : integer array of dimension kwrk. used as workspace.
|
||||||
|
c kwrk : integer, specifying the dimension of iwrk. kwrk >= mx+my.
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c z : real array of dimension (mx*my).
|
||||||
|
c on successful exit z(my*(i-1)+j) contains the value of s(x,y)
|
||||||
|
c at the point (x(i),y(j)),i=1,...,mx;j=1,...,my.
|
||||||
|
c ier : integer error flag
|
||||||
|
c ier=0 : normal return
|
||||||
|
c ier=10: invalid input data (see restrictions)
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c mx >=1, my >=1, lwrk>=mx*(kx+1)+my*(ky+1), kwrk>=mx+my
|
||||||
|
c tx(kx+1) <= x(i-1) <= x(i) <= tx(nx-kx), i=2,...,mx
|
||||||
|
c ty(ky+1) <= y(j-1) <= y(j) <= ty(ny-ky), j=2,...,my
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpbisp,fpbspl
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c de boor c : on calculating with b-splines, j. approximation theory
|
||||||
|
c 6 (1972) 50-62.
|
||||||
|
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||||
|
c applics 10 (1972) 134-149.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer nx,ny,kx,ky,mx,my,lwrk,kwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
integer iwrk(kwrk)
|
||||||
|
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(mx),y(my),z(mx*my),
|
||||||
|
* wrk(lwrk)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,iw,lwest
|
||||||
|
c ..
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
lwest = (kx+1)*mx+(ky+1)*my
|
||||||
|
if(lwrk.lt.lwest) go to 100
|
||||||
|
if(kwrk.lt.(mx+my)) go to 100
|
||||||
|
if (mx.lt.1) go to 100
|
||||||
|
if (mx.eq.1) go to 30
|
||||||
|
go to 10
|
||||||
|
10 do 20 i=2,mx
|
||||||
|
if(x(i).lt.x(i-1)) go to 100
|
||||||
|
20 continue
|
||||||
|
30 if (my.lt.1) go to 100
|
||||||
|
if (my.eq.1) go to 60
|
||||||
|
go to 40
|
||||||
|
40 do 50 i=2,my
|
||||||
|
if(y(i).lt.y(i-1)) go to 100
|
||||||
|
50 continue
|
||||||
|
60 ier = 0
|
||||||
|
iw = mx*(kx+1)+1
|
||||||
|
call fpbisp(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,wrk(1),wrk(iw),
|
||||||
|
* iwrk(1),iwrk(mx+1))
|
||||||
|
100 return
|
||||||
|
end
|
||||||
353
mcc/bsplines/clocur.f
Normal file
353
mcc/bsplines/clocur.f
Normal file
@@ -0,0 +1,353 @@
|
|||||||
|
recursive subroutine clocur(iopt,ipar,idim,m,u,mx,x,w,k,s,nest,
|
||||||
|
* n,t,nc,c,fp,wrk,lwrk,iwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c given the ordered set of m points x(i) in the idim-dimensional space
|
||||||
|
c with x(1)=x(m), and given also a corresponding set of strictly in-
|
||||||
|
c creasing values u(i) and the set of positive numbers w(i),i=1,2,...,m
|
||||||
|
c subroutine clocur determines a smooth approximating closed spline
|
||||||
|
c curve s(u), i.e.
|
||||||
|
c x1 = s1(u)
|
||||||
|
c x2 = s2(u) u(1) <= u <= u(m)
|
||||||
|
c .........
|
||||||
|
c xidim = sidim(u)
|
||||||
|
c with sj(u),j=1,2,...,idim periodic spline functions of degree k with
|
||||||
|
c common knots t(j),j=1,2,...,n.
|
||||||
|
c if ipar=1 the values u(i),i=1,2,...,m must be supplied by the user.
|
||||||
|
c if ipar=0 these values are chosen automatically by clocur as
|
||||||
|
c v(1) = 0
|
||||||
|
c v(i) = v(i-1) + dist(x(i),x(i-1)) ,i=2,3,...,m
|
||||||
|
c u(i) = v(i)/v(m) ,i=1,2,...,m
|
||||||
|
c if iopt=-1 clocur calculates the weighted least-squares closed spline
|
||||||
|
c curve according to a given set of knots.
|
||||||
|
c if iopt>=0 the number of knots of the splines sj(u) and the position
|
||||||
|
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||||
|
c ness of s(u) is then achieved by minimalizing the discontinuity
|
||||||
|
c jumps of the k-th derivative of s(u) at the knots t(j),j=k+2,k+3,...,
|
||||||
|
c n-k-1. the amount of smoothness is determined by the condition that
|
||||||
|
c f(p)=sum((w(i)*dist(x(i),s(u(i))))**2) be <= s, with s a given non-
|
||||||
|
c negative constant, called the smoothing factor.
|
||||||
|
c the fit s(u) is given in the b-spline representation and can be
|
||||||
|
c evaluated by means of subroutine curev.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call clocur(iopt,ipar,idim,m,u,mx,x,w,k,s,nest,n,t,nc,c,
|
||||||
|
c * fp,wrk,lwrk,iwrk,ier)
|
||||||
|
c
|
||||||
|
c parameters:
|
||||||
|
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||||
|
c least-squares closed spline curve (iopt=-1) or a smoothing
|
||||||
|
c closed spline curve (iopt=0 or 1) must be determined. if
|
||||||
|
c iopt=0 the routine will start with an initial set of knots
|
||||||
|
c t(i)=u(1)+(u(m)-u(1))*(i-k-1),i=1,2,...,2*k+2. if iopt=1 the
|
||||||
|
c routine will continue with the knots found at the last call.
|
||||||
|
c attention: a call with iopt=1 must always be immediately
|
||||||
|
c preceded by another call with iopt=1 or iopt=0.
|
||||||
|
c unchanged on exit.
|
||||||
|
c ipar : integer flag. on entry ipar must specify whether (ipar=1)
|
||||||
|
c the user will supply the parameter values u(i),or whether
|
||||||
|
c (ipar=0) these values are to be calculated by clocur.
|
||||||
|
c unchanged on exit.
|
||||||
|
c idim : integer. on entry idim must specify the dimension of the
|
||||||
|
c curve. 0 < idim < 11.
|
||||||
|
c unchanged on exit.
|
||||||
|
c m : integer. on entry m must specify the number of data points.
|
||||||
|
c m > 1. unchanged on exit.
|
||||||
|
c u : real array of dimension at least (m). in case ipar=1,before
|
||||||
|
c entry, u(i) must be set to the i-th value of the parameter
|
||||||
|
c variable u for i=1,2,...,m. these values must then be
|
||||||
|
c supplied in strictly ascending order and will be unchanged
|
||||||
|
c on exit. in case ipar=0, on exit,the array will contain the
|
||||||
|
c values u(i) as determined by clocur.
|
||||||
|
c mx : integer. on entry mx must specify the actual dimension of
|
||||||
|
c the array x as declared in the calling (sub)program. mx must
|
||||||
|
c not be too small (see x). unchanged on exit.
|
||||||
|
c x : real array of dimension at least idim*m.
|
||||||
|
c before entry, x(idim*(i-1)+j) must contain the j-th coord-
|
||||||
|
c inate of the i-th data point for i=1,2,...,m and j=1,2,...,
|
||||||
|
c idim. since first and last data point must coincide it
|
||||||
|
c means that x(j)=x(idim*(m-1)+j),j=1,2,...,idim.
|
||||||
|
c unchanged on exit.
|
||||||
|
c w : real array of dimension at least (m). before entry, w(i)
|
||||||
|
c must be set to the i-th value in the set of weights. the
|
||||||
|
c w(i) must be strictly positive. w(m) is not used.
|
||||||
|
c unchanged on exit. see also further comments.
|
||||||
|
c k : integer. on entry k must specify the degree of the splines.
|
||||||
|
c 1<=k<=5. it is recommended to use cubic splines (k=3).
|
||||||
|
c the user is strongly dissuaded from choosing k even,together
|
||||||
|
c with a small s-value. unchanged on exit.
|
||||||
|
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||||
|
c factor. s >=0. unchanged on exit.
|
||||||
|
c for advice on the choice of s see further comments.
|
||||||
|
c nest : integer. on entry nest must contain an over-estimate of the
|
||||||
|
c total number of knots of the splines returned, to indicate
|
||||||
|
c the storage space available to the routine. nest >=2*k+2.
|
||||||
|
c in most practical situation nest=m/2 will be sufficient.
|
||||||
|
c always large enough is nest=m+2*k, the number of knots
|
||||||
|
c needed for interpolation (s=0). unchanged on exit.
|
||||||
|
c n : integer.
|
||||||
|
c unless ier = 10 (in case iopt >=0), n will contain the
|
||||||
|
c total number of knots of the smoothing spline curve returned
|
||||||
|
c if the computation mode iopt=1 is used this value of n
|
||||||
|
c should be left unchanged between subsequent calls.
|
||||||
|
c in case iopt=-1, the value of n must be specified on entry.
|
||||||
|
c t : real array of dimension at least (nest).
|
||||||
|
c on successful exit, this array will contain the knots of the
|
||||||
|
c spline curve,i.e. the position of the interior knots t(k+2),
|
||||||
|
c t(k+3),..,t(n-k-1) as well as the position of the additional
|
||||||
|
c t(1),t(2),..,t(k+1)=u(1) and u(m)=t(n-k),...,t(n) needed for
|
||||||
|
c the b-spline representation.
|
||||||
|
c if the computation mode iopt=1 is used, the values of t(1),
|
||||||
|
c t(2),...,t(n) should be left unchanged between subsequent
|
||||||
|
c calls. if the computation mode iopt=-1 is used, the values
|
||||||
|
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||||
|
c entry. see also the restrictions (ier=10).
|
||||||
|
c nc : integer. on entry nc must specify the actual dimension of
|
||||||
|
c the array c as declared in the calling (sub)program. nc
|
||||||
|
c must not be too small (see c). unchanged on exit.
|
||||||
|
c c : real array of dimension at least (nest*idim).
|
||||||
|
c on successful exit, this array will contain the coefficients
|
||||||
|
c in the b-spline representation of the spline curve s(u),i.e.
|
||||||
|
c the b-spline coefficients of the spline sj(u) will be given
|
||||||
|
c in c(n*(j-1)+i),i=1,2,...,n-k-1 for j=1,2,...,idim.
|
||||||
|
c fp : real. unless ier = 10, fp contains the weighted sum of
|
||||||
|
c squared residuals of the spline curve returned.
|
||||||
|
c wrk : real array of dimension at least m*(k+1)+nest*(7+idim+5*k).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||||
|
c the array wrk as declared in the calling (sub)program. lwrk
|
||||||
|
c must not be too small (see wrk). unchanged on exit.
|
||||||
|
c iwrk : integer array of dimension at least (nest).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c ier : integer. unless the routine detects an error, ier contains a
|
||||||
|
c non-positive value on exit, i.e.
|
||||||
|
c ier=0 : normal return. the close curve returned has a residual
|
||||||
|
c sum of squares fp such that abs(fp-s)/s <= tol with tol a
|
||||||
|
c relative tolerance set to 0.001 by the program.
|
||||||
|
c ier=-1 : normal return. the curve returned is an interpolating
|
||||||
|
c spline curve (fp=0).
|
||||||
|
c ier=-2 : normal return. the curve returned is the weighted least-
|
||||||
|
c squares point,i.e. each spline sj(u) is a constant. in
|
||||||
|
c this extreme case fp gives the upper bound fp0 for the
|
||||||
|
c smoothing factor s.
|
||||||
|
c ier=1 : error. the required storage space exceeds the available
|
||||||
|
c storage space, as specified by the parameter nest.
|
||||||
|
c probably causes : nest too small. if nest is already
|
||||||
|
c large (say nest > m/2), it may also indicate that s is
|
||||||
|
c too small
|
||||||
|
c the approximation returned is the least-squares closed
|
||||||
|
c curve according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||||
|
c the parameter fp gives the corresponding weighted sum of
|
||||||
|
c squared residuals (fp>s).
|
||||||
|
c ier=2 : error. a theoretically impossible result was found during
|
||||||
|
c the iteration process for finding a smoothing curve with
|
||||||
|
c fp = s. probably causes : s too small.
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||||
|
c by the program) allowed for finding a smoothing curve
|
||||||
|
c with fp=s has been reached. probably causes : s too small
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=10 : error. on entry, the input data are controlled on validity
|
||||||
|
c the following restrictions must be satisfied.
|
||||||
|
c -1<=iopt<=1, 1<=k<=5, m>1, nest>2*k+2, w(i)>0,i=1,2,...,m
|
||||||
|
c 0<=ipar<=1, 0<idim<=10, lwrk>=(k+1)*m+nest*(7+idim+5*k),
|
||||||
|
c nc>=nest*idim, x(j)=x(idim*(m-1)+j), j=1,2,...,idim
|
||||||
|
c if ipar=0: sum j=1,idim (x(i*idim+j)-x((i-1)*idim+j))**2>0
|
||||||
|
c i=1,2,...,m-1.
|
||||||
|
c if ipar=1: u(1)<u(2)<...<u(m)
|
||||||
|
c if iopt=-1: 2*k+2<=n<=min(nest,m+2*k)
|
||||||
|
c u(1)<t(k+2)<t(k+3)<...<t(n-k-1)<u(m)
|
||||||
|
c (u(1)=0 and u(m)=1 in case ipar=0)
|
||||||
|
c the schoenberg-whitney conditions, i.e. there
|
||||||
|
c must be a subset of data points uu(j) with
|
||||||
|
c uu(j) = u(i) or u(i)+(u(m)-u(1)) such that
|
||||||
|
c t(j) < uu(j) < t(j+k+1), j=k+1,...,n-k-1
|
||||||
|
c if iopt>=0: s>=0
|
||||||
|
c if s=0 : nest >= m+2*k
|
||||||
|
c if one of these conditions is found to be violated,control
|
||||||
|
c is immediately repassed to the calling program. in that
|
||||||
|
c case there is no approximation returned.
|
||||||
|
c
|
||||||
|
c further comments:
|
||||||
|
c by means of the parameter s, the user can control the tradeoff
|
||||||
|
c between closeness of fit and smoothness of fit of the approximation.
|
||||||
|
c if s is too large, the curve will be too smooth and signal will be
|
||||||
|
c lost ; if s is too small the curve will pick up too much noise. in
|
||||||
|
c the extreme cases the program will return an interpolating curve if
|
||||||
|
c s=0 and the weighted least-squares point if s is very large.
|
||||||
|
c between these extremes, a properly chosen s will result in a good
|
||||||
|
c compromise between closeness of fit and smoothness of fit.
|
||||||
|
c to decide whether an approximation, corresponding to a certain s is
|
||||||
|
c satisfactory the user is highly recommended to inspect the fits
|
||||||
|
c graphically.
|
||||||
|
c recommended values for s depend on the weights w(i). if these are
|
||||||
|
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||||
|
c x(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||||
|
c sqrt(2*m)). if nothing is known about the statistical error in x(i)
|
||||||
|
c each w(i) can be set equal to one and s determined by trial and
|
||||||
|
c error, taking account of the comments above. the best is then to
|
||||||
|
c start with a very large value of s ( to determine the weighted
|
||||||
|
c least-squares point and the upper bound fp0 for s) and then to
|
||||||
|
c progressively decrease the value of s ( say by a factor 10 in the
|
||||||
|
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||||
|
c approximating curve shows more detail) to obtain closer fits.
|
||||||
|
c to economize the search for a good s-value the program provides with
|
||||||
|
c different modes of computation. at the first call of the routine, or
|
||||||
|
c whenever he wants to restart with the initial set of knots the user
|
||||||
|
c must set iopt=0.
|
||||||
|
c if iopt=1 the program will continue with the set of knots found at
|
||||||
|
c the last call of the routine. this will save a lot of computation
|
||||||
|
c time if clocur is called repeatedly for different values of s.
|
||||||
|
c the number of knots of the spline returned and their location will
|
||||||
|
c depend on the value of s and on the complexity of the shape of the
|
||||||
|
c curve underlying the data. but, if the computation mode iopt=1 is
|
||||||
|
c used, the knots returned may also depend on the s-values at previous
|
||||||
|
c calls (if these were smaller). therefore, if after a number of
|
||||||
|
c trials with different s-values and iopt=1, the user can finally
|
||||||
|
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||||
|
c clocur once more with the selected value for s but now with iopt=0.
|
||||||
|
c indeed, clocur may then return an approximation of the same quality
|
||||||
|
c of fit but with fewer knots and therefore better if data reduction
|
||||||
|
c is also an important objective for the user.
|
||||||
|
c
|
||||||
|
c the form of the approximating curve can strongly be affected by
|
||||||
|
c the choice of the parameter values u(i). if there is no physical
|
||||||
|
c reason for choosing a particular parameter u, often good results
|
||||||
|
c will be obtained with the choice of clocur(in case ipar=0), i.e.
|
||||||
|
c v(1)=0, v(i)=v(i-1)+q(i), i=2,...,m, u(i)=v(i)/v(m), i=1,..,m
|
||||||
|
c where
|
||||||
|
c q(i)= sqrt(sum j=1,idim (xj(i)-xj(i-1))**2 )
|
||||||
|
c other possibilities for q(i) are
|
||||||
|
c q(i)= sum j=1,idim (xj(i)-xj(i-1))**2
|
||||||
|
c q(i)= sum j=1,idim abs(xj(i)-xj(i-1))
|
||||||
|
c q(i)= max j=1,idim abs(xj(i)-xj(i-1))
|
||||||
|
c q(i)= 1
|
||||||
|
c
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpbacp,fpbspl,fpchep,fpclos,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||||
|
c
|
||||||
|
c references:
|
||||||
|
c dierckx p. : algorithms for smoothing data with periodic and
|
||||||
|
c parametric splines, computer graphics and image
|
||||||
|
c processing 20 (1982) 171-184.
|
||||||
|
c dierckx p. : algorithms for smoothing data with periodic and param-
|
||||||
|
c etric splines, report tw55, dept. computer science,
|
||||||
|
c k.u.leuven, 1981.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author:
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u. leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c creation date : may 1979
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 s,fp
|
||||||
|
integer iopt,ipar,idim,m,mx,k,nest,n,nc,lwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 u(m),x(mx),w(m),t(nest),c(nc),wrk(lwrk)
|
||||||
|
integer iwrk(nest)
|
||||||
|
c ..local scalars..
|
||||||
|
real*8 per,tol,dist
|
||||||
|
integer i,ia1,ia2,ib,ifp,ig1,ig2,iq,iz,i1,i2,j1,j2,k1,k2,lwest,
|
||||||
|
* maxit,m1,nmin,ncc,j
|
||||||
|
c ..function references..
|
||||||
|
real*8 sqrt
|
||||||
|
c we set up the parameters tol and maxit
|
||||||
|
maxit = 20
|
||||||
|
tol = 0.1e-02
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(iopt.lt.(-1) .or. iopt.gt.1) go to 90
|
||||||
|
if(ipar.lt.0 .or. ipar.gt.1) go to 90
|
||||||
|
if(idim.le.0 .or. idim.gt.10) go to 90
|
||||||
|
if(k.le.0 .or. k.gt.5) go to 90
|
||||||
|
k1 = k+1
|
||||||
|
k2 = k1+1
|
||||||
|
nmin = 2*k1
|
||||||
|
if(m.lt.2 .or. nest.lt.nmin) go to 90
|
||||||
|
ncc = nest*idim
|
||||||
|
if(mx.lt.m*idim .or. nc.lt.ncc) go to 90
|
||||||
|
lwest = m*k1+nest*(7+idim+5*k)
|
||||||
|
if(lwrk.lt.lwest) go to 90
|
||||||
|
i1 = idim
|
||||||
|
i2 = m*idim
|
||||||
|
do 5 j=1,idim
|
||||||
|
if(x(i1).ne.x(i2)) go to 90
|
||||||
|
i1 = i1-1
|
||||||
|
i2 = i2-1
|
||||||
|
5 continue
|
||||||
|
if(ipar.ne.0 .or. iopt.gt.0) go to 40
|
||||||
|
i1 = 0
|
||||||
|
i2 = idim
|
||||||
|
u(1) = 0.
|
||||||
|
do 20 i=2,m
|
||||||
|
dist = 0.
|
||||||
|
do 10 j1=1,idim
|
||||||
|
i1 = i1+1
|
||||||
|
i2 = i2+1
|
||||||
|
dist = dist+(x(i2)-x(i1))**2
|
||||||
|
10 continue
|
||||||
|
u(i) = u(i-1)+sqrt(dist)
|
||||||
|
20 continue
|
||||||
|
if(u(m).le.0.) go to 90
|
||||||
|
do 30 i=2,m
|
||||||
|
u(i) = u(i)/u(m)
|
||||||
|
30 continue
|
||||||
|
u(m) = 0.1e+01
|
||||||
|
40 if(w(1).le.0.) go to 90
|
||||||
|
m1 = m-1
|
||||||
|
do 50 i=1,m1
|
||||||
|
if(u(i).ge.u(i+1) .or. w(i).le.0.) go to 90
|
||||||
|
50 continue
|
||||||
|
if(iopt.ge.0) go to 70
|
||||||
|
if(n.le.nmin .or. n.gt.nest) go to 90
|
||||||
|
per = u(m)-u(1)
|
||||||
|
j1 = k1
|
||||||
|
t(j1) = u(1)
|
||||||
|
i1 = n-k
|
||||||
|
t(i1) = u(m)
|
||||||
|
j2 = j1
|
||||||
|
i2 = i1
|
||||||
|
do 60 i=1,k
|
||||||
|
i1 = i1+1
|
||||||
|
i2 = i2-1
|
||||||
|
j1 = j1+1
|
||||||
|
j2 = j2-1
|
||||||
|
t(j2) = t(i2)-per
|
||||||
|
t(i1) = t(j1)+per
|
||||||
|
60 continue
|
||||||
|
call fpchep(u,m,t,n,k,ier)
|
||||||
|
if (ier.eq.0) go to 80
|
||||||
|
go to 90
|
||||||
|
70 if(s.lt.0.) go to 90
|
||||||
|
if(s.eq.0. .and. nest.lt.(m+2*k)) go to 90
|
||||||
|
ier = 0
|
||||||
|
c we partition the working space and determine the spline approximation.
|
||||||
|
80 ifp = 1
|
||||||
|
iz = ifp+nest
|
||||||
|
ia1 = iz+ncc
|
||||||
|
ia2 = ia1+nest*k1
|
||||||
|
ib = ia2+nest*k
|
||||||
|
ig1 = ib+nest*k2
|
||||||
|
ig2 = ig1+nest*k2
|
||||||
|
iq = ig2+nest*k1
|
||||||
|
call fpclos(iopt,idim,m,u,mx,x,w,k,s,nest,tol,maxit,k1,k2,n,t,
|
||||||
|
* ncc,c,fp,wrk(ifp),wrk(iz),wrk(ia1),wrk(ia2),wrk(ib),wrk(ig1),
|
||||||
|
* wrk(ig2),wrk(iq),iwrk,ier)
|
||||||
|
90 return
|
||||||
|
end
|
||||||
181
mcc/bsplines/cocosp.f
Normal file
181
mcc/bsplines/cocosp.f
Normal file
@@ -0,0 +1,181 @@
|
|||||||
|
recursive subroutine cocosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,
|
||||||
|
* sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c given the set of data points (x(i),y(i)) and the set of positive
|
||||||
|
c numbers w(i),i=1,2,...,m, subroutine cocosp determines the weighted
|
||||||
|
c least-squares cubic spline s(x) with given knots t(j),j=1,2,...,n
|
||||||
|
c which satisfies the following concavity/convexity conditions
|
||||||
|
c s''(t(j+3))*e(j) <= 0, j=1,2,...n-6
|
||||||
|
c the fit is given in the b-spline representation( b-spline coef-
|
||||||
|
c ficients c(j),j=1,2,...n-4) and can be evaluated by means of
|
||||||
|
c subroutine splev.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call cocosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,bind,wrk,
|
||||||
|
c * lwrk,iwrk,kwrk,ier)
|
||||||
|
c
|
||||||
|
c parameters:
|
||||||
|
c m : integer. on entry m must specify the number of data points.
|
||||||
|
c m > 3. unchanged on exit.
|
||||||
|
c x : real array of dimension at least (m). before entry, x(i)
|
||||||
|
c must be set to the i-th value of the independent variable x,
|
||||||
|
c for i=1,2,...,m. these values must be supplied in strictly
|
||||||
|
c ascending order. unchanged on exit.
|
||||||
|
c y : real array of dimension at least (m). before entry, y(i)
|
||||||
|
c must be set to the i-th value of the dependent variable y,
|
||||||
|
c for i=1,2,...,m. unchanged on exit.
|
||||||
|
c w : real array of dimension at least (m). before entry, w(i)
|
||||||
|
c must be set to the i-th value in the set of weights. the
|
||||||
|
c w(i) must be strictly positive. unchanged on exit.
|
||||||
|
c n : integer. on entry n must contain the total number of knots
|
||||||
|
c of the cubic spline. m+4>=n>=8. unchanged on exit.
|
||||||
|
c t : real array of dimension at least (n). before entry, this
|
||||||
|
c array must contain the knots of the spline, i.e. the position
|
||||||
|
c of the interior knots t(5),t(6),...,t(n-4) as well as the
|
||||||
|
c position of the boundary knots t(1),t(2),t(3),t(4) and t(n-3)
|
||||||
|
c t(n-2),t(n-1),t(n) needed for the b-spline representation.
|
||||||
|
c unchanged on exit. see also the restrictions (ier=10).
|
||||||
|
c e : real array of dimension at least (n). before entry, e(j)
|
||||||
|
c must be set to 1 if s(x) must be locally concave at t(j+3),
|
||||||
|
c to (-1) if s(x) must be locally convex at t(j+3) and to 0
|
||||||
|
c if no convexity constraint is imposed at t(j+3),j=1,2,..,n-6.
|
||||||
|
c e(n-5),...,e(n) are not used. unchanged on exit.
|
||||||
|
c maxtr : integer. on entry maxtr must contain an over-estimate of the
|
||||||
|
c total number of records in the used tree structure, to indic-
|
||||||
|
c ate the storage space available to the routine. maxtr >=1
|
||||||
|
c in most practical situation maxtr=100 will be sufficient.
|
||||||
|
c always large enough is
|
||||||
|
c n-5 n-6
|
||||||
|
c maxtr = ( ) + ( ) with l the greatest
|
||||||
|
c l l+1
|
||||||
|
c integer <= (n-6)/2 . unchanged on exit.
|
||||||
|
c maxbin: integer. on entry maxbin must contain an over-estimate of the
|
||||||
|
c number of knots where s(x) will have a zero second derivative
|
||||||
|
c maxbin >=1. in most practical situation maxbin = 10 will be
|
||||||
|
c sufficient. always large enough is maxbin=n-6.
|
||||||
|
c unchanged on exit.
|
||||||
|
c c : real array of dimension at least (n).
|
||||||
|
c on successful exit, this array will contain the coefficients
|
||||||
|
c c(1),c(2),..,c(n-4) in the b-spline representation of s(x)
|
||||||
|
c sq : real. on successful exit, sq contains the weighted sum of
|
||||||
|
c squared residuals of the spline approximation returned.
|
||||||
|
c sx : real array of dimension at least m. on successful exit
|
||||||
|
c this array will contain the spline values s(x(i)),i=1,...,m
|
||||||
|
c bind : logical array of dimension at least (n). on successful exit
|
||||||
|
c this array will indicate the knots where s''(x)=0, i.e.
|
||||||
|
c s''(t(j+3)) .eq. 0 if bind(j) = .true.
|
||||||
|
c s''(t(j+3)) .ne. 0 if bind(j) = .false., j=1,2,...,n-6
|
||||||
|
c wrk : real array of dimension at least m*4+n*7+maxbin*(maxbin+n+1)
|
||||||
|
c used as working space.
|
||||||
|
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||||
|
c the array wrk as declared in the calling (sub)program.lwrk
|
||||||
|
c must not be too small (see wrk). unchanged on exit.
|
||||||
|
c iwrk : integer array of dimension at least (maxtr*4+2*(maxbin+1))
|
||||||
|
c used as working space.
|
||||||
|
c kwrk : integer. on entry,kwrk must specify the actual dimension of
|
||||||
|
c the array iwrk as declared in the calling (sub)program. kwrk
|
||||||
|
c must not be too small (see iwrk). unchanged on exit.
|
||||||
|
c ier : integer. error flag
|
||||||
|
c ier=0 : successful exit.
|
||||||
|
c ier>0 : abnormal termination: no approximation is returned
|
||||||
|
c ier=1 : the number of knots where s''(x)=0 exceeds maxbin.
|
||||||
|
c probably causes : maxbin too small.
|
||||||
|
c ier=2 : the number of records in the tree structure exceeds
|
||||||
|
c maxtr.
|
||||||
|
c probably causes : maxtr too small.
|
||||||
|
c ier=3 : the algorithm finds no solution to the posed quadratic
|
||||||
|
c programming problem.
|
||||||
|
c probably causes : rounding errors.
|
||||||
|
c ier=10 : on entry, the input data are controlled on validity.
|
||||||
|
c the following restrictions must be satisfied
|
||||||
|
c m>3, maxtr>=1, maxbin>=1, 8<=n<=m+4,w(i) > 0,
|
||||||
|
c x(1)<x(2)<...<x(m), t(1)<=t(2)<=t(3)<=t(4)<=x(1),
|
||||||
|
c x(1)<t(5)<t(6)<...<t(n-4)<x(m)<=t(n-3)<=...<=t(n),
|
||||||
|
c kwrk>=maxtr*4+2*(maxbin+1),
|
||||||
|
c lwrk>=m*4+n*7+maxbin*(maxbin+n+1),
|
||||||
|
c the schoenberg-whitney conditions, i.e. there must
|
||||||
|
c be a subset of data points xx(j) such that
|
||||||
|
c t(j) < xx(j) < t(j+4), j=1,2,...,n-4
|
||||||
|
c if one of these restrictions is found to be violated
|
||||||
|
c control is immediately repassed to the calling program
|
||||||
|
c
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpcosp,fpbspl,fpadno,fpdeno,fpseno,fpfrno,fpchec
|
||||||
|
c
|
||||||
|
c references:
|
||||||
|
c dierckx p. : an algorithm for cubic spline fitting with convexity
|
||||||
|
c constraints, computing 24 (1980) 349-371.
|
||||||
|
c dierckx p. : an algorithm for least-squares cubic spline fitting
|
||||||
|
c with convexity and concavity constraints, report tw39,
|
||||||
|
c dept. computer science, k.u.leuven, 1978.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author:
|
||||||
|
c p. dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c creation date : march 1978
|
||||||
|
c latest update : march 1987.
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 sq
|
||||||
|
integer m,n,maxtr,maxbin,lwrk,kwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),y(m),w(m),t(n),e(n),c(n),sx(m),wrk(lwrk)
|
||||||
|
integer iwrk(kwrk)
|
||||||
|
logical bind(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,ia,ib,ic,iq,iu,iz,izz,ji,jib,jjb,jl,jr,ju,kwest,
|
||||||
|
* lwest,mb,nm,n6
|
||||||
|
real*8 one
|
||||||
|
c ..
|
||||||
|
c set constant
|
||||||
|
one = 0.1e+01
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(m.lt.4 .or. n.lt.8) go to 40
|
||||||
|
if(maxtr.lt.1 .or. maxbin.lt.1) go to 40
|
||||||
|
lwest = 7*n+m*4+maxbin*(1+n+maxbin)
|
||||||
|
kwest = 4*maxtr+2*(maxbin+1)
|
||||||
|
if(lwrk.lt.lwest .or. kwrk.lt.kwest) go to 40
|
||||||
|
if(w(1).le.0.) go to 40
|
||||||
|
do 10 i=2,m
|
||||||
|
if(x(i-1).ge.x(i) .or. w(i).le.0.) go to 40
|
||||||
|
10 continue
|
||||||
|
call fpchec(x,m,t,n,3,ier)
|
||||||
|
if (ier.eq.0) go to 20
|
||||||
|
go to 40
|
||||||
|
c set numbers e(i)
|
||||||
|
20 n6 = n-6
|
||||||
|
do 30 i=1,n6
|
||||||
|
if(e(i).gt.0.) e(i) = one
|
||||||
|
if(e(i).lt.0.) e(i) = -one
|
||||||
|
30 continue
|
||||||
|
c we partition the working space and determine the spline approximation
|
||||||
|
nm = n+maxbin
|
||||||
|
mb = maxbin+1
|
||||||
|
ia = 1
|
||||||
|
ib = ia+4*n
|
||||||
|
ic = ib+nm*maxbin
|
||||||
|
iz = ic+n
|
||||||
|
izz = iz+n
|
||||||
|
iu = izz+n
|
||||||
|
iq = iu+maxbin
|
||||||
|
ji = 1
|
||||||
|
ju = ji+maxtr
|
||||||
|
jl = ju+maxtr
|
||||||
|
jr = jl+maxtr
|
||||||
|
jjb = jr+maxtr
|
||||||
|
jib = jjb+mb
|
||||||
|
call fpcosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,bind,nm,mb,wrk(ia),
|
||||||
|
*
|
||||||
|
* wrk(ib),wrk(ic),wrk(iz),wrk(izz),wrk(iu),wrk(iq),iwrk(ji),
|
||||||
|
* iwrk(ju),iwrk(jl),iwrk(jr),iwrk(jjb),iwrk(jib),ier)
|
||||||
|
40 return
|
||||||
|
end
|
||||||
234
mcc/bsplines/concon.f
Normal file
234
mcc/bsplines/concon.f
Normal file
@@ -0,0 +1,234 @@
|
|||||||
|
recursive subroutine concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,
|
||||||
|
* n,t,c,sq,sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c given the set of data points (x(i),y(i)) and the set of positive
|
||||||
|
c numbers w(i), i=1,2,...,m,subroutine concon determines a cubic spline
|
||||||
|
c approximation s(x) which satisfies the following local convexity
|
||||||
|
c constraints s''(x(i))*v(i) <= 0, i=1,2,...,m.
|
||||||
|
c the number of knots n and the position t(j),j=1,2,...n is chosen
|
||||||
|
c automatically by the routine in a way that
|
||||||
|
c sq = sum((w(i)*(y(i)-s(x(i))))**2) be <= s.
|
||||||
|
c the fit is given in the b-spline representation (b-spline coef-
|
||||||
|
c ficients c(j),j=1,2,...n-4) and can be evaluated by means of
|
||||||
|
c subroutine splev.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c
|
||||||
|
c call concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,
|
||||||
|
c * sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||||
|
c
|
||||||
|
c parameters:
|
||||||
|
c iopt: integer flag.
|
||||||
|
c if iopt=0, the routine will start with the minimal number of
|
||||||
|
c knots to guarantee that the convexity conditions will be
|
||||||
|
c satisfied. if iopt=1, the routine will continue with the set
|
||||||
|
c of knots found at the last call of the routine.
|
||||||
|
c attention: a call with iopt=1 must always be immediately
|
||||||
|
c preceded by another call with iopt=1 or iopt=0.
|
||||||
|
c unchanged on exit.
|
||||||
|
c m : integer. on entry m must specify the number of data points.
|
||||||
|
c m > 3. unchanged on exit.
|
||||||
|
c x : real array of dimension at least (m). before entry, x(i)
|
||||||
|
c must be set to the i-th value of the independent variable x,
|
||||||
|
c for i=1,2,...,m. these values must be supplied in strictly
|
||||||
|
c ascending order. unchanged on exit.
|
||||||
|
c y : real array of dimension at least (m). before entry, y(i)
|
||||||
|
c must be set to the i-th value of the dependent variable y,
|
||||||
|
c for i=1,2,...,m. unchanged on exit.
|
||||||
|
c w : real array of dimension at least (m). before entry, w(i)
|
||||||
|
c must be set to the i-th value in the set of weights. the
|
||||||
|
c w(i) must be strictly positive. unchanged on exit.
|
||||||
|
c v : real array of dimension at least (m). before entry, v(i)
|
||||||
|
c must be set to 1 if s(x) must be locally concave at x(i),
|
||||||
|
c to (-1) if s(x) must be locally convex at x(i) and to 0
|
||||||
|
c if no convexity constraint is imposed at x(i).
|
||||||
|
c s : real. on entry s must specify an over-estimate for the
|
||||||
|
c the weighted sum of squared residuals sq of the requested
|
||||||
|
c spline. s >=0. unchanged on exit.
|
||||||
|
c nest : integer. on entry nest must contain an over-estimate of the
|
||||||
|
c total number of knots of the spline returned, to indicate
|
||||||
|
c the storage space available to the routine. nest >=8.
|
||||||
|
c in most practical situation nest=m/2 will be sufficient.
|
||||||
|
c always large enough is nest=m+4. unchanged on exit.
|
||||||
|
c maxtr : integer. on entry maxtr must contain an over-estimate of the
|
||||||
|
c total number of records in the used tree structure, to indic-
|
||||||
|
c ate the storage space available to the routine. maxtr >=1
|
||||||
|
c in most practical situation maxtr=100 will be sufficient.
|
||||||
|
c always large enough is
|
||||||
|
c nest-5 nest-6
|
||||||
|
c maxtr = ( ) + ( ) with l the greatest
|
||||||
|
c l l+1
|
||||||
|
c integer <= (nest-6)/2 . unchanged on exit.
|
||||||
|
c maxbin: integer. on entry maxbin must contain an over-estimate of the
|
||||||
|
c number of knots where s(x) will have a zero second derivative
|
||||||
|
c maxbin >=1. in most practical situation maxbin = 10 will be
|
||||||
|
c sufficient. always large enough is maxbin=nest-6.
|
||||||
|
c unchanged on exit.
|
||||||
|
c n : integer.
|
||||||
|
c on exit with ier <=0, n will contain the total number of
|
||||||
|
c knots of the spline approximation returned. if the comput-
|
||||||
|
c ation mode iopt=1 is used this value of n should be left
|
||||||
|
c unchanged between subsequent calls.
|
||||||
|
c t : real array of dimension at least (nest).
|
||||||
|
c on exit with ier<=0, this array will contain the knots of the
|
||||||
|
c spline,i.e. the position of the interior knots t(5),t(6),...,
|
||||||
|
c t(n-4) as well as the position of the additional knots
|
||||||
|
c t(1)=t(2)=t(3)=t(4)=x(1) and t(n-3)=t(n-2)=t(n-1)=t(n)=x(m)
|
||||||
|
c needed for the b-spline representation.
|
||||||
|
c if the computation mode iopt=1 is used, the values of t(1),
|
||||||
|
c t(2),...,t(n) should be left unchanged between subsequent
|
||||||
|
c calls.
|
||||||
|
c c : real array of dimension at least (nest).
|
||||||
|
c on successful exit, this array will contain the coefficients
|
||||||
|
c c(1),c(2),..,c(n-4) in the b-spline representation of s(x)
|
||||||
|
c sq : real. unless ier>0 , sq contains the weighted sum of
|
||||||
|
c squared residuals of the spline approximation returned.
|
||||||
|
c sx : real array of dimension at least m. on exit with ier<=0
|
||||||
|
c this array will contain the spline values s(x(i)),i=1,...,m
|
||||||
|
c if the computation mode iopt=1 is used, the values of sx(1),
|
||||||
|
c sx(2),...,sx(m) should be left unchanged between subsequent
|
||||||
|
c calls.
|
||||||
|
c bind: logical array of dimension at least nest. on exit with ier<=0
|
||||||
|
c this array will indicate the knots where s''(x)=0, i.e.
|
||||||
|
c s''(t(j+3)) .eq. 0 if bind(j) = .true.
|
||||||
|
c s''(t(j+3)) .ne. 0 if bind(j) = .false., j=1,2,...,n-6
|
||||||
|
c if the computation mode iopt=1 is used, the values of bind(1)
|
||||||
|
c ,...,bind(n-6) should be left unchanged between subsequent
|
||||||
|
c calls.
|
||||||
|
c wrk : real array of dimension at least (m*4+nest*8+maxbin*(maxbin+
|
||||||
|
c nest+1)). used as working space.
|
||||||
|
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||||
|
c the array wrk as declared in the calling (sub)program.lwrk
|
||||||
|
c must not be too small (see wrk). unchanged on exit.
|
||||||
|
c iwrk : integer array of dimension at least (maxtr*4+2*(maxbin+1))
|
||||||
|
c used as working space.
|
||||||
|
c kwrk : integer. on entry,kwrk must specify the actual dimension of
|
||||||
|
c the array iwrk as declared in the calling (sub)program. kwrk
|
||||||
|
c must not be too small (see iwrk). unchanged on exit.
|
||||||
|
c ier : integer. error flag
|
||||||
|
c ier=0 : normal return, s(x) satisfies the concavity/convexity
|
||||||
|
c constraints and sq <= s.
|
||||||
|
c ier<0 : abnormal termination: s(x) satisfies the concavity/
|
||||||
|
c convexity constraints but sq > s.
|
||||||
|
c ier=-3 : the requested storage space exceeds the available
|
||||||
|
c storage space as specified by the parameter nest.
|
||||||
|
c probably causes: nest too small. if nest is already
|
||||||
|
c large (say nest > m/2), it may also indicate that s
|
||||||
|
c is too small.
|
||||||
|
c the approximation returned is the least-squares cubic
|
||||||
|
c spline according to the knots t(1),...,t(n) (n=nest)
|
||||||
|
c which satisfies the convexity constraints.
|
||||||
|
c ier=-2 : the maximal number of knots n=m+4 has been reached.
|
||||||
|
c probably causes: s too small.
|
||||||
|
c ier=-1 : the number of knots n is less than the maximal number
|
||||||
|
c m+4 but concon finds that adding one or more knots
|
||||||
|
c will not further reduce the value of sq.
|
||||||
|
c probably causes : s too small.
|
||||||
|
c ier>0 : abnormal termination: no approximation is returned
|
||||||
|
c ier=1 : the number of knots where s''(x)=0 exceeds maxbin.
|
||||||
|
c probably causes : maxbin too small.
|
||||||
|
c ier=2 : the number of records in the tree structure exceeds
|
||||||
|
c maxtr.
|
||||||
|
c probably causes : maxtr too small.
|
||||||
|
c ier=3 : the algorithm finds no solution to the posed quadratic
|
||||||
|
c programming problem.
|
||||||
|
c probably causes : rounding errors.
|
||||||
|
c ier=4 : the minimum number of knots (given by n) to guarantee
|
||||||
|
c that the concavity/convexity conditions will be
|
||||||
|
c satisfied is greater than nest.
|
||||||
|
c probably causes: nest too small.
|
||||||
|
c ier=5 : the minimum number of knots (given by n) to guarantee
|
||||||
|
c that the concavity/convexity conditions will be
|
||||||
|
c satisfied is greater than m+4.
|
||||||
|
c probably causes: strongly alternating convexity and
|
||||||
|
c concavity conditions. normally the situation can be
|
||||||
|
c coped with by adding n-m-4 extra data points (found
|
||||||
|
c by linear interpolation e.g.) with a small weight w(i)
|
||||||
|
c and a v(i) number equal to zero.
|
||||||
|
c ier=10 : on entry, the input data are controlled on validity.
|
||||||
|
c the following restrictions must be satisfied
|
||||||
|
c 0<=iopt<=1, m>3, nest>=8, s>=0, maxtr>=1, maxbin>=1,
|
||||||
|
c kwrk>=maxtr*4+2*(maxbin+1), w(i)>0, x(i) < x(i+1),
|
||||||
|
c lwrk>=m*4+nest*8+maxbin*(maxbin+nest+1)
|
||||||
|
c if one of these restrictions is found to be violated
|
||||||
|
c control is immediately repassed to the calling program
|
||||||
|
c
|
||||||
|
c further comments:
|
||||||
|
c as an example of the use of the computation mode iopt=1, the
|
||||||
|
c following program segment will cause concon to return control
|
||||||
|
c each time a spline with a new set of knots has been computed.
|
||||||
|
c .............
|
||||||
|
c iopt = 0
|
||||||
|
c s = 0.1e+60 (s very large)
|
||||||
|
c do 10 i=1,m
|
||||||
|
c call concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,sx,
|
||||||
|
c * bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||||
|
c ......
|
||||||
|
c s = sq
|
||||||
|
c iopt=1
|
||||||
|
c 10 continue
|
||||||
|
c .............
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpcoco,fpcosp,fpbspl,fpadno,fpdeno,fpseno,fpfrno
|
||||||
|
c
|
||||||
|
c references:
|
||||||
|
c dierckx p. : an algorithm for cubic spline fitting with convexity
|
||||||
|
c constraints, computing 24 (1980) 349-371.
|
||||||
|
c dierckx p. : an algorithm for least-squares cubic spline fitting
|
||||||
|
c with convexity and concavity constraints, report tw39,
|
||||||
|
c dept. computer science, k.u.leuven, 1978.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author:
|
||||||
|
c p. dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c creation date : march 1978
|
||||||
|
c latest update : march 1987.
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 s,sq
|
||||||
|
integer iopt,m,nest,maxtr,maxbin,n,lwrk,kwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),y(m),w(m),v(m),t(nest),c(nest),sx(m),wrk(lwrk)
|
||||||
|
integer iwrk(kwrk)
|
||||||
|
logical bind(nest)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,lwest,kwest,ie,iw,lww
|
||||||
|
real*8 one
|
||||||
|
c ..
|
||||||
|
c set constant
|
||||||
|
one = 0.1e+01
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(iopt.lt.0 .or. iopt.gt.1) go to 30
|
||||||
|
if(m.lt.4 .or. nest.lt.8) go to 30
|
||||||
|
if(s.lt.0.) go to 30
|
||||||
|
if(maxtr.lt.1 .or. maxbin.lt.1) go to 30
|
||||||
|
lwest = 8*nest+m*4+maxbin*(1+nest+maxbin)
|
||||||
|
kwest = 4*maxtr+2*(maxbin+1)
|
||||||
|
if(lwrk.lt.lwest .or. kwrk.lt.kwest) go to 30
|
||||||
|
if(iopt.gt.0) go to 20
|
||||||
|
if(w(1).le.0.) go to 30
|
||||||
|
if(v(1).gt.0.) v(1) = one
|
||||||
|
if(v(1).lt.0.) v(1) = -one
|
||||||
|
do 10 i=2,m
|
||||||
|
if(x(i-1).ge.x(i) .or. w(i).le.0.) go to 30
|
||||||
|
if(v(i).gt.0.) v(i) = one
|
||||||
|
if(v(i).lt.0.) v(i) = -one
|
||||||
|
10 continue
|
||||||
|
20 ier = 0
|
||||||
|
c we partition the working space and determine the spline approximation
|
||||||
|
ie = 1
|
||||||
|
iw = ie+nest
|
||||||
|
lww = lwrk-nest
|
||||||
|
call fpcoco(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,sx,
|
||||||
|
* bind,wrk(ie),wrk(iw),lww,iwrk,kwrk,ier)
|
||||||
|
30 return
|
||||||
|
end
|
||||||
371
mcc/bsplines/concur.f
Normal file
371
mcc/bsplines/concur.f
Normal file
@@ -0,0 +1,371 @@
|
|||||||
|
recursive subroutine concur(iopt,idim,m,u,mx,x,xx,w,ib,db,nb,
|
||||||
|
* ie,de,ne,k,s,nest,n,t,nc,c,np,cp,fp,wrk,lwrk,iwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c given the ordered set of m points x(i) in the idim-dimensional space
|
||||||
|
c and given also a corresponding set of strictly increasing values u(i)
|
||||||
|
c and the set of positive numbers w(i),i=1,2,...,m, subroutine concur
|
||||||
|
c determines a smooth approximating spline curve s(u), i.e.
|
||||||
|
c x1 = s1(u)
|
||||||
|
c x2 = s2(u) ub = u(1) <= u <= u(m) = ue
|
||||||
|
c .........
|
||||||
|
c xidim = sidim(u)
|
||||||
|
c with sj(u),j=1,2,...,idim spline functions of odd degree k with
|
||||||
|
c common knots t(j),j=1,2,...,n.
|
||||||
|
c in addition these splines will satisfy the following boundary
|
||||||
|
c constraints (l)
|
||||||
|
c if ib > 0 : sj (u(1)) = db(idim*l+j) ,l=0,1,...,ib-1
|
||||||
|
c and (l)
|
||||||
|
c if ie > 0 : sj (u(m)) = de(idim*l+j) ,l=0,1,...,ie-1.
|
||||||
|
c if iopt=-1 concur calculates the weighted least-squares spline curve
|
||||||
|
c according to a given set of knots.
|
||||||
|
c if iopt>=0 the number of knots of the splines sj(u) and the position
|
||||||
|
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||||
|
c ness of s(u) is then achieved by minimalizing the discontinuity
|
||||||
|
c jumps of the k-th derivative of s(u) at the knots t(j),j=k+2,k+3,...,
|
||||||
|
c n-k-1. the amount of smoothness is determined by the condition that
|
||||||
|
c f(p)=sum((w(i)*dist(x(i),s(u(i))))**2) be <= s, with s a given non-
|
||||||
|
c negative constant, called the smoothing factor.
|
||||||
|
c the fit s(u) is given in the b-spline representation and can be
|
||||||
|
c evaluated by means of subroutine curev.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call concur(iopt,idim,m,u,mx,x,xx,w,ib,db,nb,ie,de,ne,k,s,nest,n,
|
||||||
|
c * t,nc,c,np,cp,fp,wrk,lwrk,iwrk,ier)
|
||||||
|
c
|
||||||
|
c parameters:
|
||||||
|
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||||
|
c least-squares spline curve (iopt=-1) or a smoothing spline
|
||||||
|
c curve (iopt=0 or 1) must be determined.if iopt=0 the routine
|
||||||
|
c will start with an initial set of knots t(i)=ub,t(i+k+1)=ue,
|
||||||
|
c i=1,2,...,k+1. if iopt=1 the routine will continue with the
|
||||||
|
c knots found at the last call of the routine.
|
||||||
|
c attention: a call with iopt=1 must always be immediately
|
||||||
|
c preceded by another call with iopt=1 or iopt=0.
|
||||||
|
c unchanged on exit.
|
||||||
|
c idim : integer. on entry idim must specify the dimension of the
|
||||||
|
c curve. 0 < idim < 11.
|
||||||
|
c unchanged on exit.
|
||||||
|
c m : integer. on entry m must specify the number of data points.
|
||||||
|
c m > k-max(ib-1,0)-max(ie-1,0). unchanged on exit.
|
||||||
|
c u : real array of dimension at least (m). before entry,
|
||||||
|
c u(i) must be set to the i-th value of the parameter variable
|
||||||
|
c u for i=1,2,...,m. these values must be supplied in
|
||||||
|
c strictly ascending order and will be unchanged on exit.
|
||||||
|
c mx : integer. on entry mx must specify the actual dimension of
|
||||||
|
c the arrays x and xx as declared in the calling (sub)program
|
||||||
|
c mx must not be too small (see x). unchanged on exit.
|
||||||
|
c x : real array of dimension at least idim*m.
|
||||||
|
c before entry, x(idim*(i-1)+j) must contain the j-th coord-
|
||||||
|
c inate of the i-th data point for i=1,2,...,m and j=1,2,...,
|
||||||
|
c idim. unchanged on exit.
|
||||||
|
c xx : real array of dimension at least idim*m.
|
||||||
|
c used as working space. on exit xx contains the coordinates
|
||||||
|
c of the data points to which a spline curve with zero deriv-
|
||||||
|
c ative constraints has been determined.
|
||||||
|
c if the computation mode iopt =1 is used xx should be left
|
||||||
|
c unchanged between calls.
|
||||||
|
c w : real array of dimension at least (m). before entry, w(i)
|
||||||
|
c must be set to the i-th value in the set of weights. the
|
||||||
|
c w(i) must be strictly positive. unchanged on exit.
|
||||||
|
c see also further comments.
|
||||||
|
c ib : integer. on entry ib must specify the number of derivative
|
||||||
|
c constraints for the curve at the begin point. 0<=ib<=(k+1)/2
|
||||||
|
c unchanged on exit.
|
||||||
|
c db : real array of dimension nb. before entry db(idim*l+j) must
|
||||||
|
c contain the l-th order derivative of sj(u) at u=u(1) for
|
||||||
|
c j=1,2,...,idim and l=0,1,...,ib-1 (if ib>0).
|
||||||
|
c unchanged on exit.
|
||||||
|
c nb : integer, specifying the dimension of db. nb>=max(1,idim*ib)
|
||||||
|
c unchanged on exit.
|
||||||
|
c ie : integer. on entry ie must specify the number of derivative
|
||||||
|
c constraints for the curve at the end point. 0<=ie<=(k+1)/2
|
||||||
|
c unchanged on exit.
|
||||||
|
c de : real array of dimension ne. before entry de(idim*l+j) must
|
||||||
|
c contain the l-th order derivative of sj(u) at u=u(m) for
|
||||||
|
c j=1,2,...,idim and l=0,1,...,ie-1 (if ie>0).
|
||||||
|
c unchanged on exit.
|
||||||
|
c ne : integer, specifying the dimension of de. ne>=max(1,idim*ie)
|
||||||
|
c unchanged on exit.
|
||||||
|
c k : integer. on entry k must specify the degree of the splines.
|
||||||
|
c k=1,3 or 5.
|
||||||
|
c unchanged on exit.
|
||||||
|
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||||
|
c factor. s >=0. unchanged on exit.
|
||||||
|
c for advice on the choice of s see further comments.
|
||||||
|
c nest : integer. on entry nest must contain an over-estimate of the
|
||||||
|
c total number of knots of the splines returned, to indicate
|
||||||
|
c the storage space available to the routine. nest >=2*k+2.
|
||||||
|
c in most practical situation nest=m/2 will be sufficient.
|
||||||
|
c always large enough is nest=m+k+1+max(0,ib-1)+max(0,ie-1),
|
||||||
|
c the number of knots needed for interpolation (s=0).
|
||||||
|
c unchanged on exit.
|
||||||
|
c n : integer.
|
||||||
|
c unless ier = 10 (in case iopt >=0), n will contain the
|
||||||
|
c total number of knots of the smoothing spline curve returned
|
||||||
|
c if the computation mode iopt=1 is used this value of n
|
||||||
|
c should be left unchanged between subsequent calls.
|
||||||
|
c in case iopt=-1, the value of n must be specified on entry.
|
||||||
|
c t : real array of dimension at least (nest).
|
||||||
|
c on successful exit, this array will contain the knots of the
|
||||||
|
c spline curve,i.e. the position of the interior knots t(k+2),
|
||||||
|
c t(k+3),..,t(n-k-1) as well as the position of the additional
|
||||||
|
c t(1)=t(2)=...=t(k+1)=ub and t(n-k)=...=t(n)=ue needed for
|
||||||
|
c the b-spline representation.
|
||||||
|
c if the computation mode iopt=1 is used, the values of t(1),
|
||||||
|
c t(2),...,t(n) should be left unchanged between subsequent
|
||||||
|
c calls. if the computation mode iopt=-1 is used, the values
|
||||||
|
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||||
|
c entry. see also the restrictions (ier=10).
|
||||||
|
c nc : integer. on entry nc must specify the actual dimension of
|
||||||
|
c the array c as declared in the calling (sub)program. nc
|
||||||
|
c must not be too small (see c). unchanged on exit.
|
||||||
|
c c : real array of dimension at least (nest*idim).
|
||||||
|
c on successful exit, this array will contain the coefficients
|
||||||
|
c in the b-spline representation of the spline curve s(u),i.e.
|
||||||
|
c the b-spline coefficients of the spline sj(u) will be given
|
||||||
|
c in c(n*(j-1)+i),i=1,2,...,n-k-1 for j=1,2,...,idim.
|
||||||
|
c cp : real array of dimension at least 2*(k+1)*idim.
|
||||||
|
c on exit cp will contain the b-spline coefficients of a
|
||||||
|
c polynomial curve which satisfies the boundary constraints.
|
||||||
|
c if the computation mode iopt =1 is used cp should be left
|
||||||
|
c unchanged between calls.
|
||||||
|
c np : integer. on entry np must specify the actual dimension of
|
||||||
|
c the array cp as declared in the calling (sub)program. np
|
||||||
|
c must not be too small (see cp). unchanged on exit.
|
||||||
|
c fp : real. unless ier = 10, fp contains the weighted sum of
|
||||||
|
c squared residuals of the spline curve returned.
|
||||||
|
c wrk : real array of dimension at least m*(k+1)+nest*(6+idim+3*k).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||||
|
c the array wrk as declared in the calling (sub)program. lwrk
|
||||||
|
c must not be too small (see wrk). unchanged on exit.
|
||||||
|
c iwrk : integer array of dimension at least (nest).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c ier : integer. unless the routine detects an error, ier contains a
|
||||||
|
c non-positive value on exit, i.e.
|
||||||
|
c ier=0 : normal return. the curve returned has a residual sum of
|
||||||
|
c squares fp such that abs(fp-s)/s <= tol with tol a relat-
|
||||||
|
c ive tolerance set to 0.001 by the program.
|
||||||
|
c ier=-1 : normal return. the curve returned is an interpolating
|
||||||
|
c spline curve, satisfying the constraints (fp=0).
|
||||||
|
c ier=-2 : normal return. the curve returned is the weighted least-
|
||||||
|
c squares polynomial curve of degree k, satisfying the
|
||||||
|
c constraints. in this extreme case fp gives the upper
|
||||||
|
c bound fp0 for the smoothing factor s.
|
||||||
|
c ier=1 : error. the required storage space exceeds the available
|
||||||
|
c storage space, as specified by the parameter nest.
|
||||||
|
c probably causes : nest too small. if nest is already
|
||||||
|
c large (say nest > m/2), it may also indicate that s is
|
||||||
|
c too small
|
||||||
|
c the approximation returned is the least-squares spline
|
||||||
|
c curve according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||||
|
c the parameter fp gives the corresponding weighted sum of
|
||||||
|
c squared residuals (fp>s).
|
||||||
|
c ier=2 : error. a theoretically impossible result was found during
|
||||||
|
c the iteration process for finding a smoothing spline curve
|
||||||
|
c with fp = s. probably causes : s too small.
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||||
|
c by the program) allowed for finding a smoothing curve
|
||||||
|
c with fp=s has been reached. probably causes : s too small
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=10 : error. on entry, the input data are controlled on validity
|
||||||
|
c the following restrictions must be satisfied.
|
||||||
|
c -1<=iopt<=1, k = 1,3 or 5, m>k-max(0,ib-1)-max(0,ie-1),
|
||||||
|
c nest>=2k+2, 0<idim<=10, lwrk>=(k+1)*m+nest*(6+idim+3*k),
|
||||||
|
c nc >=nest*idim ,u(1)<u(2)<...<u(m),w(i)>0 i=1,2,...,m,
|
||||||
|
c mx>=idim*m,0<=ib<=(k+1)/2,0<=ie<=(k+1)/2,nb>=1,ne>=1,
|
||||||
|
c nb>=ib*idim,ne>=ib*idim,np>=2*(k+1)*idim,
|
||||||
|
c if iopt=-1:2*k+2<=n<=min(nest,mmax) with mmax = m+k+1+
|
||||||
|
c max(0,ib-1)+max(0,ie-1)
|
||||||
|
c u(1)<t(k+2)<t(k+3)<...<t(n-k-1)<u(m)
|
||||||
|
c the schoenberg-whitney conditions, i.e. there
|
||||||
|
c must be a subset of data points uu(j) such that
|
||||||
|
c t(j) < uu(j) < t(j+k+1), j=1+max(0,ib-1),...
|
||||||
|
c ,n+k-1-max(0,ie-1)
|
||||||
|
c if iopt>=0: s>=0
|
||||||
|
c if s=0 : nest >=mmax (see above)
|
||||||
|
c if one of these conditions is found to be violated,control
|
||||||
|
c is immediately repassed to the calling program. in that
|
||||||
|
c case there is no approximation returned.
|
||||||
|
c
|
||||||
|
c further comments:
|
||||||
|
c by means of the parameter s, the user can control the tradeoff
|
||||||
|
c between closeness of fit and smoothness of fit of the approximation.
|
||||||
|
c if s is too large, the curve will be too smooth and signal will be
|
||||||
|
c lost ; if s is too small the curve will pick up too much noise. in
|
||||||
|
c the extreme cases the program will return an interpolating curve if
|
||||||
|
c s=0 and the least-squares polynomial curve of degree k if s is
|
||||||
|
c very large. between these extremes, a properly chosen s will result
|
||||||
|
c in a good compromise between closeness of fit and smoothness of fit.
|
||||||
|
c to decide whether an approximation, corresponding to a certain s is
|
||||||
|
c satisfactory the user is highly recommended to inspect the fits
|
||||||
|
c graphically.
|
||||||
|
c recommended values for s depend on the weights w(i). if these are
|
||||||
|
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||||
|
c x(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||||
|
c sqrt(2*m)). if nothing is known about the statistical error in x(i)
|
||||||
|
c each w(i) can be set equal to one and s determined by trial and
|
||||||
|
c error, taking account of the comments above. the best is then to
|
||||||
|
c start with a very large value of s ( to determine the least-squares
|
||||||
|
c polynomial curve and the upper bound fp0 for s) and then to
|
||||||
|
c progressively decrease the value of s ( say by a factor 10 in the
|
||||||
|
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||||
|
c approximating curve shows more detail) to obtain closer fits.
|
||||||
|
c to economize the search for a good s-value the program provides with
|
||||||
|
c different modes of computation. at the first call of the routine, or
|
||||||
|
c whenever he wants to restart with the initial set of knots the user
|
||||||
|
c must set iopt=0.
|
||||||
|
c if iopt=1 the program will continue with the set of knots found at
|
||||||
|
c the last call of the routine. this will save a lot of computation
|
||||||
|
c time if concur is called repeatedly for different values of s.
|
||||||
|
c the number of knots of the spline returned and their location will
|
||||||
|
c depend on the value of s and on the complexity of the shape of the
|
||||||
|
c curve underlying the data. but, if the computation mode iopt=1 is
|
||||||
|
c used, the knots returned may also depend on the s-values at previous
|
||||||
|
c calls (if these were smaller). therefore, if after a number of
|
||||||
|
c trials with different s-values and iopt=1, the user can finally
|
||||||
|
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||||
|
c concur once more with the selected value for s but now with iopt=0.
|
||||||
|
c indeed, concur may then return an approximation of the same quality
|
||||||
|
c of fit but with fewer knots and therefore better if data reduction
|
||||||
|
c is also an important objective for the user.
|
||||||
|
c
|
||||||
|
c the form of the approximating curve can strongly be affected by
|
||||||
|
c the choice of the parameter values u(i). if there is no physical
|
||||||
|
c reason for choosing a particular parameter u, often good results
|
||||||
|
c will be obtained with the choice
|
||||||
|
c v(1)=0, v(i)=v(i-1)+q(i), i=2,...,m, u(i)=v(i)/v(m), i=1,..,m
|
||||||
|
c where
|
||||||
|
c q(i)= sqrt(sum j=1,idim (xj(i)-xj(i-1))**2 )
|
||||||
|
c other possibilities for q(i) are
|
||||||
|
c q(i)= sum j=1,idim (xj(i)-xj(i-1))**2
|
||||||
|
c q(i)= sum j=1,idim abs(xj(i)-xj(i-1))
|
||||||
|
c q(i)= max j=1,idim abs(xj(i)-xj(i-1))
|
||||||
|
c q(i)= 1
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpback,fpbspl,fpched,fpcons,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||||
|
c curev,fppocu,fpadpo,fpinst
|
||||||
|
c
|
||||||
|
c references:
|
||||||
|
c dierckx p. : algorithms for smoothing data with periodic and
|
||||||
|
c parametric splines, computer graphics and image
|
||||||
|
c processing 20 (1982) 171-184.
|
||||||
|
c dierckx p. : algorithms for smoothing data with periodic and param-
|
||||||
|
c etric splines, report tw55, dept. computer science,
|
||||||
|
c k.u.leuven, 1981.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author:
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u. leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c creation date : may 1979
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 s,fp
|
||||||
|
integer iopt,idim,m,mx,ib,nb,ie,ne,k,nest,n,nc,np,lwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 u(m),x(mx),xx(mx),db(nb),de(ne),w(m),t(nest),c(nc),wrk(lwrk
|
||||||
|
*)
|
||||||
|
real*8 cp(np)
|
||||||
|
integer iwrk(nest)
|
||||||
|
c ..local scalars..
|
||||||
|
real*8 tol
|
||||||
|
integer i,ib1,ie1,ja,jb,jfp,jg,jq,jz,j,k1,k2,lwest,maxit,nmin,
|
||||||
|
* ncc,kk,mmin,nmax,mxx
|
||||||
|
c ..function references
|
||||||
|
integer max0
|
||||||
|
c ..
|
||||||
|
c we set up the parameters tol and maxit
|
||||||
|
maxit = 20
|
||||||
|
tol = 0.1e-02
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(iopt.lt.(-1) .or. iopt.gt.1) go to 90
|
||||||
|
if(idim.le.0 .or. idim.gt.10) go to 90
|
||||||
|
if(k.le.0 .or. k.gt.5) go to 90
|
||||||
|
k1 = k+1
|
||||||
|
kk = k1/2
|
||||||
|
if(kk*2.ne.k1) go to 90
|
||||||
|
k2 = k1+1
|
||||||
|
if(ib.lt.0 .or. ib.gt.kk) go to 90
|
||||||
|
if(ie.lt.0 .or. ie.gt.kk) go to 90
|
||||||
|
nmin = 2*k1
|
||||||
|
ib1 = max0(0,ib-1)
|
||||||
|
ie1 = max0(0,ie-1)
|
||||||
|
mmin = k1-ib1-ie1
|
||||||
|
if(m.lt.mmin .or. nest.lt.nmin) go to 90
|
||||||
|
if(nb.lt.(idim*ib) .or. ne.lt.(idim*ie)) go to 90
|
||||||
|
if(np.lt.(2*k1*idim)) go to 90
|
||||||
|
mxx = m*idim
|
||||||
|
ncc = nest*idim
|
||||||
|
if(mx.lt.mxx .or. nc.lt.ncc) go to 90
|
||||||
|
lwest = m*k1+nest*(6+idim+3*k)
|
||||||
|
if(lwrk.lt.lwest) go to 90
|
||||||
|
if(w(1).le.0.) go to 90
|
||||||
|
do 10 i=2,m
|
||||||
|
if(u(i-1).ge.u(i) .or. w(i).le.0.) go to 90
|
||||||
|
10 continue
|
||||||
|
if(iopt.ge.0) go to 30
|
||||||
|
if(n.lt.nmin .or. n.gt.nest) go to 90
|
||||||
|
j = n
|
||||||
|
do 20 i=1,k1
|
||||||
|
t(i) = u(1)
|
||||||
|
t(j) = u(m)
|
||||||
|
j = j-1
|
||||||
|
20 continue
|
||||||
|
call fpched(u,m,t,n,k,ib,ie,ier)
|
||||||
|
if (ier.eq.0) go to 40
|
||||||
|
go to 90
|
||||||
|
30 if(s.lt.0.) go to 90
|
||||||
|
nmax = m+k1+ib1+ie1
|
||||||
|
if(s.eq.0. .and. nest.lt.nmax) go to 90
|
||||||
|
ier = 0
|
||||||
|
if(iopt.gt.0) go to 70
|
||||||
|
c we determine a polynomial curve satisfying the boundary constraints.
|
||||||
|
40 call fppocu(idim,k,u(1),u(m),ib,db,nb,ie,de,ne,cp,np)
|
||||||
|
c we generate new data points which will be approximated by a spline
|
||||||
|
c with zero derivative constraints.
|
||||||
|
j = nmin
|
||||||
|
do 50 i=1,k1
|
||||||
|
wrk(i) = u(1)
|
||||||
|
wrk(j) = u(m)
|
||||||
|
j = j-1
|
||||||
|
50 continue
|
||||||
|
c evaluate the polynomial curve
|
||||||
|
call curev(idim,wrk,nmin,cp,np,k,u,m,xx,mxx,ier)
|
||||||
|
c subtract from the old data, the values of the polynomial curve
|
||||||
|
do 60 i=1,mxx
|
||||||
|
xx(i) = x(i)-xx(i)
|
||||||
|
60 continue
|
||||||
|
c we partition the working space and determine the spline curve.
|
||||||
|
70 jfp = 1
|
||||||
|
jz = jfp+nest
|
||||||
|
ja = jz+ncc
|
||||||
|
jb = ja+nest*k1
|
||||||
|
jg = jb+nest*k2
|
||||||
|
jq = jg+nest*k2
|
||||||
|
call fpcons(iopt,idim,m,u,mxx,xx,w,ib,ie,k,s,nest,tol,maxit,k1,
|
||||||
|
* k2,n,t,ncc,c,fp,wrk(jfp),wrk(jz),wrk(ja),wrk(jb),wrk(jg),wrk(jq),
|
||||||
|
*
|
||||||
|
* iwrk,ier)
|
||||||
|
c add the polynomial curve to the calculated spline.
|
||||||
|
call fpadpo(idim,t,n,c,ncc,k,cp,np,wrk(jz),wrk(ja),wrk(jb))
|
||||||
|
90 return
|
||||||
|
end
|
||||||
92
mcc/bsplines/cualde.f
Normal file
92
mcc/bsplines/cualde.f
Normal file
@@ -0,0 +1,92 @@
|
|||||||
|
recursive subroutine cualde(idim,t,n,c,nc,k1,u,d,nd,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine cualde evaluates at the point u all the derivatives
|
||||||
|
c (l)
|
||||||
|
c d(idim*l+j) = sj (u) ,l=0,1,...,k, j=1,2,...,idim
|
||||||
|
c of a spline curve s(u) of order k1 (degree k=k1-1) and dimension idim
|
||||||
|
c given in its b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call cualde(idim,t,n,c,nc,k1,u,d,nd,ier)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c idim : integer, giving the dimension of the spline curve.
|
||||||
|
c t : array,length n, which contains the position of the knots.
|
||||||
|
c n : integer, giving the total number of knots of s(u).
|
||||||
|
c c : array,length nc, which contains the b-spline coefficients.
|
||||||
|
c nc : integer, giving the total number of coefficients of s(u).
|
||||||
|
c k1 : integer, giving the order of s(u) (order=degree+1).
|
||||||
|
c u : real, which contains the point where the derivatives must
|
||||||
|
c be evaluated.
|
||||||
|
c nd : integer, giving the dimension of the array d. nd >= k1*idim
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c d : array,length nd,giving the different curve derivatives.
|
||||||
|
c d(idim*l+j) will contain the j-th coordinate of the l-th
|
||||||
|
c derivative of the curve at the point u.
|
||||||
|
c ier : error flag
|
||||||
|
c ier = 0 : normal return
|
||||||
|
c ier =10 : invalid input data (see restrictions)
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c nd >= k1*idim
|
||||||
|
c t(k1) <= u <= t(n-k1+1)
|
||||||
|
c
|
||||||
|
c further comments:
|
||||||
|
c if u coincides with a knot, right derivatives are computed
|
||||||
|
c ( left derivatives if u = t(n-k1+1) ).
|
||||||
|
c
|
||||||
|
c other subroutines required: fpader.
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c de boor c : on calculating with b-splines, j. approximation theory
|
||||||
|
c 6 (1972) 50-62.
|
||||||
|
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||||
|
c applics 10 (1972) 134-149.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer idim,n,nc,k1,nd,ier
|
||||||
|
real*8 u
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),c(nc),d(nd)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,j,kk,l,m,nk1
|
||||||
|
c ..local array..
|
||||||
|
real*8 h(6)
|
||||||
|
c ..
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(nd.lt.(k1*idim)) go to 500
|
||||||
|
nk1 = n-k1
|
||||||
|
if(u.lt.t(k1) .or. u.gt.t(nk1+1)) go to 500
|
||||||
|
c search for knot interval t(l) <= u < t(l+1)
|
||||||
|
l = k1
|
||||||
|
100 if(u.lt.t(l+1) .or. l.eq.nk1) go to 200
|
||||||
|
l = l+1
|
||||||
|
go to 100
|
||||||
|
200 if(t(l).ge.t(l+1)) go to 500
|
||||||
|
ier = 0
|
||||||
|
c calculate the derivatives.
|
||||||
|
j = 1
|
||||||
|
do 400 i=1,idim
|
||||||
|
call fpader(t,n,c(j),k1,u,l,h)
|
||||||
|
m = i
|
||||||
|
do 300 kk=1,k1
|
||||||
|
d(m) = h(kk)
|
||||||
|
m = m+idim
|
||||||
|
300 continue
|
||||||
|
j = j+n
|
||||||
|
400 continue
|
||||||
|
500 return
|
||||||
|
end
|
||||||
111
mcc/bsplines/curev.f
Normal file
111
mcc/bsplines/curev.f
Normal file
@@ -0,0 +1,111 @@
|
|||||||
|
recursive subroutine curev(idim,t,n,c,nc,k,u,m,x,mx,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine curev evaluates in a number of points u(i),i=1,2,...,m
|
||||||
|
c a spline curve s(u) of degree k and dimension idim, given in its
|
||||||
|
c b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call curev(idim,t,n,c,nc,k,u,m,x,mx,ier)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c idim : integer, giving the dimension of the spline curve.
|
||||||
|
c t : array,length n, which contains the position of the knots.
|
||||||
|
c n : integer, giving the total number of knots of s(u).
|
||||||
|
c c : array,length nc, which contains the b-spline coefficients.
|
||||||
|
c nc : integer, giving the total number of coefficients of s(u).
|
||||||
|
c k : integer, giving the degree of s(u).
|
||||||
|
c u : array,length m, which contains the points where s(u) must
|
||||||
|
c be evaluated.
|
||||||
|
c m : integer, giving the number of points where s(u) must be
|
||||||
|
c evaluated.
|
||||||
|
c mx : integer, giving the dimension of the array x. mx >= m*idim
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c x : array,length mx,giving the value of s(u) at the different
|
||||||
|
c points. x(idim*(i-1)+j) will contain the j-th coordinate
|
||||||
|
c of the i-th point on the curve.
|
||||||
|
c ier : error flag
|
||||||
|
c ier = 0 : normal return
|
||||||
|
c ier =10 : invalid input data (see restrictions)
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c m >= 1
|
||||||
|
c mx >= m*idim
|
||||||
|
c t(k+1) <= u(i) <= u(i+1) <= t(n-k) , i=1,2,...,m-1.
|
||||||
|
c
|
||||||
|
c other subroutines required: fpbspl.
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c de boor c : on calculating with b-splines, j. approximation theory
|
||||||
|
c 6 (1972) 50-62.
|
||||||
|
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||||
|
c applics 10 (1972) 134-149.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer idim,n,nc,k,m,mx,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),c(nc),u(m),x(mx)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,j,jj,j1,k1,l,ll,l1,mm,nk1
|
||||||
|
real*8 arg,sp,tb,te
|
||||||
|
c ..local array..
|
||||||
|
real*8 h(6)
|
||||||
|
c ..
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if (m.lt.1) go to 100
|
||||||
|
if (m.eq.1) go to 30
|
||||||
|
go to 10
|
||||||
|
10 do 20 i=2,m
|
||||||
|
if(u(i).lt.u(i-1)) go to 100
|
||||||
|
20 continue
|
||||||
|
30 if(mx.lt.(m*idim)) go to 100
|
||||||
|
ier = 0
|
||||||
|
c fetch tb and te, the boundaries of the approximation interval.
|
||||||
|
k1 = k+1
|
||||||
|
nk1 = n-k1
|
||||||
|
tb = t(k1)
|
||||||
|
te = t(nk1+1)
|
||||||
|
l = k1
|
||||||
|
l1 = l+1
|
||||||
|
c main loop for the different points.
|
||||||
|
mm = 0
|
||||||
|
do 80 i=1,m
|
||||||
|
c fetch a new u-value arg.
|
||||||
|
arg = u(i)
|
||||||
|
if(arg.lt.tb) arg = tb
|
||||||
|
if(arg.gt.te) arg = te
|
||||||
|
c search for knot interval t(l) <= arg < t(l+1)
|
||||||
|
40 if(arg.lt.t(l1) .or. l.eq.nk1) go to 50
|
||||||
|
l = l1
|
||||||
|
l1 = l+1
|
||||||
|
go to 40
|
||||||
|
c evaluate the non-zero b-splines at arg.
|
||||||
|
50 call fpbspl(t,n,k,arg,l,h)
|
||||||
|
c find the value of s(u) at u=arg.
|
||||||
|
ll = l-k1
|
||||||
|
do 70 j1=1,idim
|
||||||
|
jj = ll
|
||||||
|
sp = 0.
|
||||||
|
do 60 j=1,k1
|
||||||
|
jj = jj+1
|
||||||
|
sp = sp+c(jj)*h(j)
|
||||||
|
60 continue
|
||||||
|
mm = mm+1
|
||||||
|
x(mm) = sp
|
||||||
|
ll = ll+n
|
||||||
|
70 continue
|
||||||
|
80 continue
|
||||||
|
100 return
|
||||||
|
end
|
||||||
261
mcc/bsplines/curfit.f
Normal file
261
mcc/bsplines/curfit.f
Normal file
@@ -0,0 +1,261 @@
|
|||||||
|
recursive subroutine curfit(iopt,m,x,y,w,xb,xe,k,s,nest,n,
|
||||||
|
* t,c,fp,wrk,lwrk,iwrk,ier)
|
||||||
|
implicit none
|
||||||
|
c given the set of data points (x(i),y(i)) and the set of positive
|
||||||
|
c numbers w(i),i=1,2,...,m,subroutine curfit determines a smooth spline
|
||||||
|
c approximation of degree k on the interval xb <= x <= xe.
|
||||||
|
c if iopt=-1 curfit calculates the weighted least-squares spline
|
||||||
|
c according to a given set of knots.
|
||||||
|
c if iopt>=0 the number of knots of the spline s(x) and the position
|
||||||
|
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||||
|
c ness of s(x) is then achieved by minimalizing the discontinuity
|
||||||
|
c jumps of the k-th derivative of s(x) at the knots t(j),j=k+2,k+3,...,
|
||||||
|
c n-k-1. the amount of smoothness is determined by the condition that
|
||||||
|
c f(p)=sum((w(i)*(y(i)-s(x(i))))**2) be <= s, with s a given non-
|
||||||
|
c negative constant, called the smoothing factor.
|
||||||
|
c the fit s(x) is given in the b-spline representation (b-spline coef-
|
||||||
|
c ficients c(j),j=1,2,...,n-k-1) and can be evaluated by means of
|
||||||
|
c subroutine splev.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call curfit(iopt,m,x,y,w,xb,xe,k,s,nest,n,t,c,fp,wrk,
|
||||||
|
c * lwrk,iwrk,ier)
|
||||||
|
c
|
||||||
|
c parameters:
|
||||||
|
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||||
|
c least-squares spline (iopt=-1) or a smoothing spline (iopt=
|
||||||
|
c 0 or 1) must be determined. if iopt=0 the routine will start
|
||||||
|
c with an initial set of knots t(i)=xb, t(i+k+1)=xe, i=1,2,...
|
||||||
|
c k+1. if iopt=1 the routine will continue with the knots
|
||||||
|
c found at the last call of the routine.
|
||||||
|
c attention: a call with iopt=1 must always be immediately
|
||||||
|
c preceded by another call with iopt=1 or iopt=0.
|
||||||
|
c unchanged on exit.
|
||||||
|
c m : integer. on entry m must specify the number of data points.
|
||||||
|
c m > k. unchanged on exit.
|
||||||
|
c x : real array of dimension at least (m). before entry, x(i)
|
||||||
|
c must be set to the i-th value of the independent variable x,
|
||||||
|
c for i=1,2,...,m. these values must be supplied in strictly
|
||||||
|
c ascending order. unchanged on exit.
|
||||||
|
c y : real array of dimension at least (m). before entry, y(i)
|
||||||
|
c must be set to the i-th value of the dependent variable y,
|
||||||
|
c for i=1,2,...,m. unchanged on exit.
|
||||||
|
c w : real array of dimension at least (m). before entry, w(i)
|
||||||
|
c must be set to the i-th value in the set of weights. the
|
||||||
|
c w(i) must be strictly positive. unchanged on exit.
|
||||||
|
c see also further comments.
|
||||||
|
c xb,xe : real values. on entry xb and xe must specify the boundaries
|
||||||
|
c of the approximation interval. xb<=x(1), xe>=x(m).
|
||||||
|
c unchanged on exit.
|
||||||
|
c k : integer. on entry k must specify the degree of the spline.
|
||||||
|
c 1<=k<=5. it is recommended to use cubic splines (k=3).
|
||||||
|
c the user is strongly dissuaded from choosing k even,together
|
||||||
|
c with a small s-value. unchanged on exit.
|
||||||
|
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||||
|
c factor. s >=0. unchanged on exit.
|
||||||
|
c for advice on the choice of s see further comments.
|
||||||
|
c nest : integer. on entry nest must contain an over-estimate of the
|
||||||
|
c total number of knots of the spline returned, to indicate
|
||||||
|
c the storage space available to the routine. nest >=2*k+2.
|
||||||
|
c in most practical situation nest=m/2 will be sufficient.
|
||||||
|
c always large enough is nest=m+k+1, the number of knots
|
||||||
|
c needed for interpolation (s=0). unchanged on exit.
|
||||||
|
c n : integer.
|
||||||
|
c unless ier =10 (in case iopt >=0), n will contain the
|
||||||
|
c total number of knots of the spline approximation returned.
|
||||||
|
c if the computation mode iopt=1 is used this value of n
|
||||||
|
c should be left unchanged between subsequent calls.
|
||||||
|
c in case iopt=-1, the value of n must be specified on entry.
|
||||||
|
c t : real array of dimension at least (nest).
|
||||||
|
c on successful exit, this array will contain the knots of the
|
||||||
|
c spline,i.e. the position of the interior knots t(k+2),t(k+3)
|
||||||
|
c ...,t(n-k-1) as well as the position of the additional knots
|
||||||
|
c t(1)=t(2)=...=t(k+1)=xb and t(n-k)=...=t(n)=xe needed for
|
||||||
|
c the b-spline representation.
|
||||||
|
c if the computation mode iopt=1 is used, the values of t(1),
|
||||||
|
c t(2),...,t(n) should be left unchanged between subsequent
|
||||||
|
c calls. if the computation mode iopt=-1 is used, the values
|
||||||
|
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||||
|
c entry. see also the restrictions (ier=10).
|
||||||
|
c c : real array of dimension at least (nest).
|
||||||
|
c on successful exit, this array will contain the coefficients
|
||||||
|
c c(1),c(2),..,c(n-k-1) in the b-spline representation of s(x)
|
||||||
|
c fp : real. unless ier=10, fp contains the weighted sum of
|
||||||
|
c squared residuals of the spline approximation returned.
|
||||||
|
c wrk : real array of dimension at least (m*(k+1)+nest*(7+3*k)).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||||
|
c the array wrk as declared in the calling (sub)program.lwrk
|
||||||
|
c must not be too small (see wrk). unchanged on exit.
|
||||||
|
c iwrk : integer array of dimension at least (nest).
|
||||||
|
c used as working space. if the computation mode iopt=1 is
|
||||||
|
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||||
|
c between subsequent calls.
|
||||||
|
c ier : integer. unless the routine detects an error, ier contains a
|
||||||
|
c non-positive value on exit, i.e.
|
||||||
|
c ier=0 : normal return. the spline returned has a residual sum of
|
||||||
|
c squares fp such that abs(fp-s)/s <= tol with tol a relat-
|
||||||
|
c ive tolerance set to 0.001 by the program.
|
||||||
|
c ier=-1 : normal return. the spline returned is an interpolating
|
||||||
|
c spline (fp=0).
|
||||||
|
c ier=-2 : normal return. the spline returned is the weighted least-
|
||||||
|
c squares polynomial of degree k. in this extreme case fp
|
||||||
|
c gives the upper bound fp0 for the smoothing factor s.
|
||||||
|
c ier=1 : error. the required storage space exceeds the available
|
||||||
|
c storage space, as specified by the parameter nest.
|
||||||
|
c probably causes : nest too small. if nest is already
|
||||||
|
c large (say nest > m/2), it may also indicate that s is
|
||||||
|
c too small
|
||||||
|
c the approximation returned is the weighted least-squares
|
||||||
|
c spline according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||||
|
c the parameter fp gives the corresponding weighted sum of
|
||||||
|
c squared residuals (fp>s).
|
||||||
|
c ier=2 : error. a theoretically impossible result was found during
|
||||||
|
c the iteration process for finding a smoothing spline with
|
||||||
|
c fp = s. probably causes : s too small.
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||||
|
c by the program) allowed for finding a smoothing spline
|
||||||
|
c with fp=s has been reached. probably causes : s too small
|
||||||
|
c there is an approximation returned but the corresponding
|
||||||
|
c weighted sum of squared residuals does not satisfy the
|
||||||
|
c condition abs(fp-s)/s < tol.
|
||||||
|
c ier=10 : error. on entry, the input data are controlled on validity
|
||||||
|
c the following restrictions must be satisfied.
|
||||||
|
c -1<=iopt<=1, 1<=k<=5, m>k, nest>2*k+2, w(i)>0,i=1,2,...,m
|
||||||
|
c xb<=x(1)<x(2)<...<x(m)<=xe, lwrk>=(k+1)*m+nest*(7+3*k)
|
||||||
|
c if iopt=-1: 2*k+2<=n<=min(nest,m+k+1)
|
||||||
|
c xb<t(k+2)<t(k+3)<...<t(n-k-1)<xe
|
||||||
|
c the schoenberg-whitney conditions, i.e. there
|
||||||
|
c must be a subset of data points xx(j) such that
|
||||||
|
c t(j) < xx(j) < t(j+k+1), j=1,2,...,n-k-1
|
||||||
|
c if iopt>=0: s>=0
|
||||||
|
c if s=0 : nest >= m+k+1
|
||||||
|
c if one of these conditions is found to be violated,control
|
||||||
|
c is immediately repassed to the calling program. in that
|
||||||
|
c case there is no approximation returned.
|
||||||
|
c
|
||||||
|
c further comments:
|
||||||
|
c by means of the parameter s, the user can control the tradeoff
|
||||||
|
c between closeness of fit and smoothness of fit of the approximation.
|
||||||
|
c if s is too large, the spline will be too smooth and signal will be
|
||||||
|
c lost ; if s is too small the spline will pick up too much noise. in
|
||||||
|
c the extreme cases the program will return an interpolating spline if
|
||||||
|
c s=0 and the weighted least-squares polynomial of degree k if s is
|
||||||
|
c very large. between these extremes, a properly chosen s will result
|
||||||
|
c in a good compromise between closeness of fit and smoothness of fit.
|
||||||
|
c to decide whether an approximation, corresponding to a certain s is
|
||||||
|
c satisfactory the user is highly recommended to inspect the fits
|
||||||
|
c graphically.
|
||||||
|
c recommended values for s depend on the weights w(i). if these are
|
||||||
|
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||||
|
c y(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||||
|
c sqrt(2*m)). if nothing is known about the statistical error in y(i)
|
||||||
|
c each w(i) can be set equal to one and s determined by trial and
|
||||||
|
c error, taking account of the comments above. the best is then to
|
||||||
|
c start with a very large value of s ( to determine the least-squares
|
||||||
|
c polynomial and the corresponding upper bound fp0 for s) and then to
|
||||||
|
c progressively decrease the value of s ( say by a factor 10 in the
|
||||||
|
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||||
|
c approximation shows more detail) to obtain closer fits.
|
||||||
|
c to economize the search for a good s-value the program provides with
|
||||||
|
c different modes of computation. at the first call of the routine, or
|
||||||
|
c whenever he wants to restart with the initial set of knots the user
|
||||||
|
c must set iopt=0.
|
||||||
|
c if iopt=1 the program will continue with the set of knots found at
|
||||||
|
c the last call of the routine. this will save a lot of computation
|
||||||
|
c time if curfit is called repeatedly for different values of s.
|
||||||
|
c the number of knots of the spline returned and their location will
|
||||||
|
c depend on the value of s and on the complexity of the shape of the
|
||||||
|
c function underlying the data. but, if the computation mode iopt=1
|
||||||
|
c is used, the knots returned may also depend on the s-values at
|
||||||
|
c previous calls (if these were smaller). therefore, if after a number
|
||||||
|
c of trials with different s-values and iopt=1, the user can finally
|
||||||
|
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||||
|
c curfit once more with the selected value for s but now with iopt=0.
|
||||||
|
c indeed, curfit may then return an approximation of the same quality
|
||||||
|
c of fit but with fewer knots and therefore better if data reduction
|
||||||
|
c is also an important objective for the user.
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c fpback,fpbspl,fpchec,fpcurf,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||||
|
c
|
||||||
|
c references:
|
||||||
|
c dierckx p. : an algorithm for smoothing, differentiation and integ-
|
||||||
|
c ration of experimental data using spline functions,
|
||||||
|
c j.comp.appl.maths 1 (1975) 165-184.
|
||||||
|
c dierckx p. : a fast algorithm for smoothing data on a rectangular
|
||||||
|
c grid while using spline functions, siam j.numer.anal.
|
||||||
|
c 19 (1982) 1286-1304.
|
||||||
|
c dierckx p. : an improved algorithm for curve fitting with spline
|
||||||
|
c functions, report tw54, dept. computer science,k.u.
|
||||||
|
c leuven, 1981.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author:
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u. leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c creation date : may 1979
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 xb,xe,s,fp
|
||||||
|
integer iopt,m,k,nest,n,lwrk,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),y(m),w(m),t(nest),c(nest),wrk(lwrk)
|
||||||
|
integer iwrk(nest)
|
||||||
|
c ..local scalars..
|
||||||
|
real*8 tol
|
||||||
|
integer i,ia,ib,ifp,ig,iq,iz,j,k1,k2,lwest,maxit,nmin
|
||||||
|
c ..
|
||||||
|
c we set up the parameters tol and maxit
|
||||||
|
maxit = 20
|
||||||
|
tol = 0.1d-02
|
||||||
|
c before starting computations a data check is made. if the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(k.le.0 .or. k.gt.5) go to 50
|
||||||
|
k1 = k+1
|
||||||
|
k2 = k1+1
|
||||||
|
if(iopt.lt.(-1) .or. iopt.gt.1) go to 50
|
||||||
|
nmin = 2*k1
|
||||||
|
if(m.lt.k1 .or. nest.lt.nmin) go to 50
|
||||||
|
lwest = m*k1+nest*(7+3*k)
|
||||||
|
if(lwrk.lt.lwest) go to 50
|
||||||
|
if(xb.gt.x(1) .or. xe.lt.x(m)) go to 50
|
||||||
|
do 10 i=2,m
|
||||||
|
if(x(i-1).gt.x(i)) go to 50
|
||||||
|
10 continue
|
||||||
|
if(iopt.ge.0) go to 30
|
||||||
|
if(n.lt.nmin .or. n.gt.nest) go to 50
|
||||||
|
j = n
|
||||||
|
do 20 i=1,k1
|
||||||
|
t(i) = xb
|
||||||
|
t(j) = xe
|
||||||
|
j = j-1
|
||||||
|
20 continue
|
||||||
|
call fpchec(x,m,t,n,k,ier)
|
||||||
|
if (ier.eq.0) go to 40
|
||||||
|
go to 50
|
||||||
|
30 if(s.lt.0.) go to 50
|
||||||
|
if(s.eq.0. .and. nest.lt.(m+k1)) go to 50
|
||||||
|
c we partition the working space and determine the spline approximation.
|
||||||
|
40 ifp = 1
|
||||||
|
iz = ifp+nest
|
||||||
|
ia = iz+nest
|
||||||
|
ib = ia+nest*k1
|
||||||
|
ig = ib+nest*k2
|
||||||
|
iq = ig+nest*k2
|
||||||
|
call fpcurf(iopt,x,y,w,m,xb,xe,k,s,nest,tol,maxit,k1,k2,n,t,c,fp,
|
||||||
|
* wrk(ifp),wrk(iz),wrk(ia),wrk(ib),wrk(ig),wrk(iq),iwrk,ier)
|
||||||
|
50 return
|
||||||
|
end
|
||||||
91
mcc/bsplines/dblint.f
Normal file
91
mcc/bsplines/dblint.f
Normal file
@@ -0,0 +1,91 @@
|
|||||||
|
recursive function dblint(tx,nx,ty,ny,c,kx,ky,xb,xe,yb,
|
||||||
|
* ye,wrk) result(dblint_res)
|
||||||
|
implicit none
|
||||||
|
real*8 :: dblint_res
|
||||||
|
c function dblint calculates the double integral
|
||||||
|
c / xe / ye
|
||||||
|
c | | s(x,y) dx dy
|
||||||
|
c xb / yb /
|
||||||
|
c with s(x,y) a bivariate spline of degrees kx and ky, given in the
|
||||||
|
c b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c aint = dblint(tx,nx,ty,ny,c,kx,ky,xb,xe,yb,ye,wrk)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c tx : real array, length nx, which contains the position of the
|
||||||
|
c knots in the x-direction.
|
||||||
|
c nx : integer, giving the total number of knots in the x-direction
|
||||||
|
c ty : real array, length ny, which contains the position of the
|
||||||
|
c knots in the y-direction.
|
||||||
|
c ny : integer, giving the total number of knots in the y-direction
|
||||||
|
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||||
|
c b-spline coefficients.
|
||||||
|
c kx,ky : integer values, giving the degrees of the spline.
|
||||||
|
c xb,xe : real values, containing the boundaries of the integration
|
||||||
|
c yb,ye domain. s(x,y) is considered to be identically zero out-
|
||||||
|
c side the rectangle (tx(kx+1),tx(nx-kx))*(ty(ky+1),ty(ny-ky))
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c aint : real , containing the double integral of s(x,y).
|
||||||
|
c wrk : real array of dimension at least (nx+ny-kx-ky-2).
|
||||||
|
c used as working space.
|
||||||
|
c on exit, wrk(i) will contain the integral
|
||||||
|
c / xe
|
||||||
|
c | ni,kx+1(x) dx , i=1,2,...,nx-kx-1
|
||||||
|
c xb /
|
||||||
|
c with ni,kx+1(x) the normalized b-spline defined on
|
||||||
|
c the knots tx(i),...,tx(i+kx+1)
|
||||||
|
c wrk(j+nx-kx-1) will contain the integral
|
||||||
|
c / ye
|
||||||
|
c | nj,ky+1(y) dy , j=1,2,...,ny-ky-1
|
||||||
|
c yb /
|
||||||
|
c with nj,ky+1(y) the normalized b-spline defined on
|
||||||
|
c the knots ty(j),...,ty(j+ky+1)
|
||||||
|
c
|
||||||
|
c other subroutines required: fpintb
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c gaffney p.w. : the calculation of indefinite integrals of b-splines
|
||||||
|
c j. inst. maths applics 17 (1976) 37-41.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1989
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer nx,ny,kx,ky
|
||||||
|
real*8 xb,xe,yb,ye
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),wrk(nx+ny-kx-ky-2)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,j,l,m,nkx1,nky1
|
||||||
|
real*8 res
|
||||||
|
c ..
|
||||||
|
nkx1 = nx-kx-1
|
||||||
|
nky1 = ny-ky-1
|
||||||
|
c we calculate the integrals of the normalized b-splines ni,kx+1(x)
|
||||||
|
call fpintb(tx,nx,wrk,nkx1,xb,xe)
|
||||||
|
c we calculate the integrals of the normalized b-splines nj,ky+1(y)
|
||||||
|
call fpintb(ty,ny,wrk(nkx1+1),nky1,yb,ye)
|
||||||
|
c calculate the integral of s(x,y)
|
||||||
|
dblint_res = 0.
|
||||||
|
do 200 i=1,nkx1
|
||||||
|
res = wrk(i)
|
||||||
|
if(res.eq.0.) go to 200
|
||||||
|
m = (i-1)*nky1
|
||||||
|
l = nkx1
|
||||||
|
do 100 j=1,nky1
|
||||||
|
m = m+1
|
||||||
|
l = l+1
|
||||||
|
dblint_res = dblint_res + res*wrk(l)*c(m)
|
||||||
|
100 continue
|
||||||
|
200 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
84
mcc/bsplines/evapol.f
Normal file
84
mcc/bsplines/evapol.f
Normal file
@@ -0,0 +1,84 @@
|
|||||||
|
recursive function evapol(tu,nu,tv,nv,c,rad,x,y) result(e_res)
|
||||||
|
implicit none
|
||||||
|
real*8 :: e_res
|
||||||
|
c function program evacir evaluates the function f(x,y) = s(u,v),
|
||||||
|
c defined through the transformation
|
||||||
|
c x = u*rad(v)*cos(v) y = u*rad(v)*sin(v)
|
||||||
|
c and where s(u,v) is a bicubic spline ( 0<=u<=1 , -pi<=v<=pi ), given
|
||||||
|
c in its standard b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c f = evapol(tu,nu,tv,nv,c,rad,x,y)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c tu : real array, length nu, which contains the position of the
|
||||||
|
c knots in the u-direction.
|
||||||
|
c nu : integer, giving the total number of knots in the u-direction
|
||||||
|
c tv : real array, length nv, which contains the position of the
|
||||||
|
c knots in the v-direction.
|
||||||
|
c nv : integer, giving the total number of knots in the v-direction
|
||||||
|
c c : real array, length (nu-4)*(nv-4), which contains the
|
||||||
|
c b-spline coefficients.
|
||||||
|
c rad : real function subprogram, defining the boundary of the
|
||||||
|
c approximation domain. must be declared external in the
|
||||||
|
c calling (sub)-program
|
||||||
|
c x,y : real values.
|
||||||
|
c before entry x and y must be set to the co-ordinates of
|
||||||
|
c the point where f(x,y) must be evaluated.
|
||||||
|
c
|
||||||
|
c output parameter:
|
||||||
|
c f : real
|
||||||
|
c on exit f contains the value of f(x,y)
|
||||||
|
c
|
||||||
|
c other subroutines required:
|
||||||
|
c bispev,fpbisp,fpbspl
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c de boor c : on calculating with b-splines, j. approximation theory
|
||||||
|
c 6 (1972) 50-62.
|
||||||
|
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||||
|
c applics 10 (1972) 134-149.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1989
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer nu,nv
|
||||||
|
real*8 x,y
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 tu(nu),tv(nv),c((nu-4)*(nv-4))
|
||||||
|
c ..user specified function
|
||||||
|
real*8 rad
|
||||||
|
c ..local scalars..
|
||||||
|
integer ier
|
||||||
|
real*8 u,v,r,f,one,dist
|
||||||
|
c ..local arrays
|
||||||
|
real*8 wrk(8)
|
||||||
|
integer iwrk(2)
|
||||||
|
c ..function references
|
||||||
|
real*8 atan2,sqrt
|
||||||
|
c ..
|
||||||
|
c calculate the (u,v)-coordinates of the given point.
|
||||||
|
one = 1
|
||||||
|
u = 0.
|
||||||
|
v = 0.
|
||||||
|
dist = x**2+y**2
|
||||||
|
if(dist.le.0.) go to 10
|
||||||
|
v = atan2(y,x)
|
||||||
|
r = rad(v)
|
||||||
|
if(r.le.0.) go to 10
|
||||||
|
u = sqrt(dist)/r
|
||||||
|
if(u.gt.one) u = one
|
||||||
|
c evaluate s(u,v)
|
||||||
|
10 call bispev(tu,nu,tv,nv,c,3,3,u,1,v,1,f,wrk,8,iwrk,2,ier)
|
||||||
|
e_res = f
|
||||||
|
return
|
||||||
|
end
|
||||||
|
|
||||||
97
mcc/bsplines/fourco.f
Normal file
97
mcc/bsplines/fourco.f
Normal file
@@ -0,0 +1,97 @@
|
|||||||
|
recursive subroutine fourco(t,n,c,alfa,m,ress,resc,wrk1,wrk2,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine fourco calculates the integrals
|
||||||
|
c /t(n-3)
|
||||||
|
c ress(i) = ! s(x)*sin(alfa(i)*x) dx and
|
||||||
|
c t(4)/
|
||||||
|
c /t(n-3)
|
||||||
|
c resc(i) = ! s(x)*cos(alfa(i)*x) dx, i=1,...,m,
|
||||||
|
c t(4)/
|
||||||
|
c where s(x) denotes a cubic spline which is given in its
|
||||||
|
c b-spline representation.
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call fourco(t,n,c,alfa,m,ress,resc,wrk1,wrk2,ier)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c t : real array,length n, containing the knots of s(x).
|
||||||
|
c n : integer, containing the total number of knots. n>=10.
|
||||||
|
c c : real array,length n, containing the b-spline coefficients.
|
||||||
|
c alfa : real array,length m, containing the parameters alfa(i).
|
||||||
|
c m : integer, specifying the number of integrals to be computed.
|
||||||
|
c wrk1 : real array,length n. used as working space
|
||||||
|
c wrk2 : real array,length n. used as working space
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c ress : real array,length m, containing the integrals ress(i).
|
||||||
|
c resc : real array,length m, containing the integrals resc(i).
|
||||||
|
c ier : error flag:
|
||||||
|
c ier=0 : normal return.
|
||||||
|
c ier=10: invalid input data (see restrictions).
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c n >= 10
|
||||||
|
c t(4) < t(5) < ... < t(n-4) < t(n-3).
|
||||||
|
c t(1) <= t(2) <= t(3) <= t(4).
|
||||||
|
c t(n-3) <= t(n-2) <= t(n-1) <= t(n).
|
||||||
|
c
|
||||||
|
c other subroutines required: fpbfou,fpcsin
|
||||||
|
c
|
||||||
|
c references :
|
||||||
|
c dierckx p. : calculation of fouriercoefficients of discrete
|
||||||
|
c functions using cubic splines. j. computational
|
||||||
|
c and applied mathematics 3 (1977) 207-209.
|
||||||
|
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||||
|
c numerical analysis, oxford university press, 1993.
|
||||||
|
c
|
||||||
|
c author :
|
||||||
|
c p.dierckx
|
||||||
|
c dept. computer science, k.u.leuven
|
||||||
|
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||||
|
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||||
|
c
|
||||||
|
c latest update : march 1987
|
||||||
|
c
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer n,m,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),c(n),wrk1(n),wrk2(n),alfa(m),ress(m),resc(m)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,j,n4
|
||||||
|
real*8 rs,rc
|
||||||
|
c ..
|
||||||
|
n4 = n-4
|
||||||
|
c before starting computations a data check is made. in the input data
|
||||||
|
c are invalid, control is immediately repassed to the calling program.
|
||||||
|
ier = 10
|
||||||
|
if(n.lt.10) go to 50
|
||||||
|
j = n
|
||||||
|
do 10 i=1,3
|
||||||
|
if(t(i).gt.t(i+1)) go to 50
|
||||||
|
if(t(j).lt.t(j-1)) go to 50
|
||||||
|
j = j-1
|
||||||
|
10 continue
|
||||||
|
do 20 i=4,n4
|
||||||
|
if(t(i).ge.t(i+1)) go to 50
|
||||||
|
20 continue
|
||||||
|
ier = 0
|
||||||
|
c main loop for the different alfa(i).
|
||||||
|
do 40 i=1,m
|
||||||
|
c calculate the integrals
|
||||||
|
c wrk1(j) = integral(nj,4(x)*sin(alfa*x)) and
|
||||||
|
c wrk2(j) = integral(nj,4(x)*cos(alfa*x)), j=1,2,...,n-4,
|
||||||
|
c where nj,4(x) denotes the normalised cubic b-spline defined on the
|
||||||
|
c knots t(j),t(j+1),...,t(j+4).
|
||||||
|
call fpbfou(t,n,alfa(i),wrk1,wrk2)
|
||||||
|
c calculate the integrals ress(i) and resc(i).
|
||||||
|
rs = 0.
|
||||||
|
rc = 0.
|
||||||
|
do 30 j=1,n4
|
||||||
|
rs = rs+c(j)*wrk1(j)
|
||||||
|
rc = rc+c(j)*wrk2(j)
|
||||||
|
30 continue
|
||||||
|
ress(i) = rs
|
||||||
|
resc(i) = rc
|
||||||
|
40 continue
|
||||||
|
50 return
|
||||||
|
end
|
||||||
57
mcc/bsplines/fpader.f
Normal file
57
mcc/bsplines/fpader.f
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
recursive subroutine fpader(t,n,c,k1,x,l,d)
|
||||||
|
c subroutine fpader calculates the derivatives
|
||||||
|
c (j-1)
|
||||||
|
c d(j) = s (x) , j=1,2,...,k1
|
||||||
|
c of a spline of order k1 at the point t(l)<=x<t(l+1), using the
|
||||||
|
c stable recurrence scheme of de boor
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 x
|
||||||
|
integer n,k1,l
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),c(n),d(k1)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,ik,j,jj,j1,j2,ki,kj,li,lj,lk
|
||||||
|
real*8 ak,fac,one
|
||||||
|
c ..local array..
|
||||||
|
real*8 h(20)
|
||||||
|
c ..
|
||||||
|
one = 0.1d+01
|
||||||
|
lk = l-k1
|
||||||
|
do 100 i=1,k1
|
||||||
|
ik = i+lk
|
||||||
|
h(i) = c(ik)
|
||||||
|
100 continue
|
||||||
|
kj = k1
|
||||||
|
fac = one
|
||||||
|
do 700 j=1,k1
|
||||||
|
ki = kj
|
||||||
|
j1 = j+1
|
||||||
|
if(j.eq.1) go to 300
|
||||||
|
i = k1
|
||||||
|
do 200 jj=j,k1
|
||||||
|
li = i+lk
|
||||||
|
lj = li+kj
|
||||||
|
h(i) = (h(i)-h(i-1))/(t(lj)-t(li))
|
||||||
|
i = i-1
|
||||||
|
200 continue
|
||||||
|
300 do 400 i=j,k1
|
||||||
|
d(i) = h(i)
|
||||||
|
400 continue
|
||||||
|
if(j.eq.k1) go to 600
|
||||||
|
do 500 jj=j1,k1
|
||||||
|
ki = ki-1
|
||||||
|
i = k1
|
||||||
|
do 500 j2=jj,k1
|
||||||
|
li = i+lk
|
||||||
|
lj = li+ki
|
||||||
|
d(i) = ((x-t(li))*d(i)+(t(lj)-x)*d(i-1))/(t(lj)-t(li))
|
||||||
|
i = i-1
|
||||||
|
500 continue
|
||||||
|
600 d(j) = d(k1)*fac
|
||||||
|
ak = k1-j
|
||||||
|
fac = fac*ak
|
||||||
|
kj = kj-1
|
||||||
|
700 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
60
mcc/bsplines/fpadno.f
Normal file
60
mcc/bsplines/fpadno.f
Normal file
@@ -0,0 +1,60 @@
|
|||||||
|
recursive subroutine fpadno(maxtr,up,left,right,info,count,
|
||||||
|
* merk,jbind,n1,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpadno adds a branch of length n1 to the triply linked
|
||||||
|
c tree,the information of which is kept in the arrays up,left,right
|
||||||
|
c and info. the information field of the nodes of this new branch is
|
||||||
|
c given in the array jbind. in linking the new branch fpadno takes
|
||||||
|
c account of the property of the tree that
|
||||||
|
c info(k) < info(right(k)) ; info(k) < info(left(k))
|
||||||
|
c if necessary the subroutine calls subroutine fpfrno to collect the
|
||||||
|
c free nodes of the tree. if no computer words are available at that
|
||||||
|
c moment, the error parameter ier is set to 1.
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer maxtr,count,merk,n1,ier
|
||||||
|
c ..array arguments..
|
||||||
|
integer up(maxtr),left(maxtr),right(maxtr),info(maxtr),jbind(n1)
|
||||||
|
c ..local scalars..
|
||||||
|
integer k,niveau,point
|
||||||
|
logical bool
|
||||||
|
c ..subroutine references..
|
||||||
|
c fpfrno
|
||||||
|
c ..
|
||||||
|
point = 1
|
||||||
|
niveau = 1
|
||||||
|
10 k = left(point)
|
||||||
|
bool = .true.
|
||||||
|
20 if(k.eq.0) go to 50
|
||||||
|
if (info(k)-jbind(niveau).lt.0) go to 30
|
||||||
|
if (info(k)-jbind(niveau).eq.0) go to 40
|
||||||
|
go to 50
|
||||||
|
30 point = k
|
||||||
|
k = right(point)
|
||||||
|
bool = .false.
|
||||||
|
go to 20
|
||||||
|
40 point = k
|
||||||
|
niveau = niveau+1
|
||||||
|
go to 10
|
||||||
|
50 if(niveau.gt.n1) go to 90
|
||||||
|
count = count+1
|
||||||
|
if(count.le.maxtr) go to 60
|
||||||
|
call fpfrno(maxtr,up,left,right,info,point,merk,n1,count,ier)
|
||||||
|
if(ier.ne.0) go to 100
|
||||||
|
60 info(count) = jbind(niveau)
|
||||||
|
left(count) = 0
|
||||||
|
right(count) = k
|
||||||
|
if(bool) go to 70
|
||||||
|
bool = .true.
|
||||||
|
right(point) = count
|
||||||
|
up(count) = up(point)
|
||||||
|
go to 80
|
||||||
|
70 up(count) = point
|
||||||
|
left(point) = count
|
||||||
|
80 point = count
|
||||||
|
niveau = niveau+1
|
||||||
|
k = 0
|
||||||
|
go to 50
|
||||||
|
90 ier = 0
|
||||||
|
100 return
|
||||||
|
end
|
||||||
71
mcc/bsplines/fpadpo.f
Normal file
71
mcc/bsplines/fpadpo.f
Normal file
@@ -0,0 +1,71 @@
|
|||||||
|
recursive subroutine fpadpo(idim,t,n,c,nc,k,cp,np,cc,t1,t2)
|
||||||
|
implicit none
|
||||||
|
c given a idim-dimensional spline curve of degree k, in its b-spline
|
||||||
|
c representation ( knots t(j),j=1,...,n , b-spline coefficients c(j),
|
||||||
|
c j=1,...,nc) and given also a polynomial curve in its b-spline
|
||||||
|
c representation ( coefficients cp(j), j=1,...,np), subroutine fpadpo
|
||||||
|
c calculates the b-spline representation (coefficients c(j),j=1,...,nc)
|
||||||
|
c of the sum of the two curves.
|
||||||
|
c
|
||||||
|
c other subroutine required : fpinst
|
||||||
|
c
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer idim,k,n,nc,np
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),c(nc),cp(np),cc(nc),t1(n),t2(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,ii,j,jj,k1,l,l1,n1,n2,nk1,nk2
|
||||||
|
c ..
|
||||||
|
k1 = k+1
|
||||||
|
nk1 = n-k1
|
||||||
|
c initialization
|
||||||
|
j = 1
|
||||||
|
l = 1
|
||||||
|
do 20 jj=1,idim
|
||||||
|
l1 = j
|
||||||
|
do 10 ii=1,k1
|
||||||
|
cc(l1) = cp(l)
|
||||||
|
l1 = l1+1
|
||||||
|
l = l+1
|
||||||
|
10 continue
|
||||||
|
j = j+n
|
||||||
|
l = l+k1
|
||||||
|
20 continue
|
||||||
|
if(nk1.eq.k1) go to 70
|
||||||
|
n1 = k1*2
|
||||||
|
j = n
|
||||||
|
l = n1
|
||||||
|
do 30 i=1,k1
|
||||||
|
t1(i) = t(i)
|
||||||
|
t1(l) = t(j)
|
||||||
|
l = l-1
|
||||||
|
j = j-1
|
||||||
|
30 continue
|
||||||
|
c find the b-spline representation of the given polynomial curve
|
||||||
|
c according to the given set of knots.
|
||||||
|
nk2 = nk1-1
|
||||||
|
do 60 l=k1,nk2
|
||||||
|
l1 = l+1
|
||||||
|
j = 1
|
||||||
|
do 40 i=1,idim
|
||||||
|
call fpinst(0,t1,n1,cc(j),k,t(l1),l,t2,n2,cc(j),n)
|
||||||
|
j = j+n
|
||||||
|
40 continue
|
||||||
|
do 50 i=1,n2
|
||||||
|
t1(i) = t2(i)
|
||||||
|
50 continue
|
||||||
|
n1 = n2
|
||||||
|
60 continue
|
||||||
|
c find the b-spline representation of the resulting curve.
|
||||||
|
70 j = 1
|
||||||
|
do 90 jj=1,idim
|
||||||
|
l = j
|
||||||
|
do 80 i=1,nk1
|
||||||
|
c(l) = cc(l)+c(l)
|
||||||
|
l = l+1
|
||||||
|
80 continue
|
||||||
|
j = j+n
|
||||||
|
90 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
32
mcc/bsplines/fpback.f
Normal file
32
mcc/bsplines/fpback.f
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
recursive subroutine fpback(a,z,n,k,c,nest)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpback calculates the solution of the system of
|
||||||
|
c equations a*c = z with a a n x n upper triangular matrix
|
||||||
|
c of bandwidth k.
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer n,k,nest
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 a(nest,k),z(n),c(n)
|
||||||
|
c ..local scalars..
|
||||||
|
real*8 store
|
||||||
|
integer i,i1,j,k1,l,m
|
||||||
|
c ..
|
||||||
|
k1 = k-1
|
||||||
|
c(n) = z(n)/a(n,1)
|
||||||
|
i = n-1
|
||||||
|
if(i.eq.0) go to 30
|
||||||
|
do 20 j=2,n
|
||||||
|
store = z(i)
|
||||||
|
i1 = k1
|
||||||
|
if(j.le.k1) i1 = j-1
|
||||||
|
m = i
|
||||||
|
do 10 l=1,i1
|
||||||
|
m = m+1
|
||||||
|
store = store-c(m)*a(i,l+1)
|
||||||
|
10 continue
|
||||||
|
c(i) = store/a(i,1)
|
||||||
|
i = i-1
|
||||||
|
20 continue
|
||||||
|
30 return
|
||||||
|
end
|
||||||
59
mcc/bsplines/fpbacp.f
Normal file
59
mcc/bsplines/fpbacp.f
Normal file
@@ -0,0 +1,59 @@
|
|||||||
|
recursive subroutine fpbacp(a,b,z,n,k,c,k1,nest)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpbacp calculates the solution of the system of equations
|
||||||
|
c g * c = z with g a n x n upper triangular matrix of the form
|
||||||
|
c ! a ' !
|
||||||
|
c g = ! ' b !
|
||||||
|
c ! 0 ' !
|
||||||
|
c with b a n x k matrix and a a (n-k) x (n-k) upper triangular
|
||||||
|
c matrix of bandwidth k1.
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer n,k,k1,nest
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 a(nest,k1),b(nest,k),z(n),c(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,i1,j,l,l0,l1,n2
|
||||||
|
real*8 store
|
||||||
|
c ..
|
||||||
|
n2 = n-k
|
||||||
|
l = n
|
||||||
|
do 30 i=1,k
|
||||||
|
store = z(l)
|
||||||
|
j = k+2-i
|
||||||
|
if(i.eq.1) go to 20
|
||||||
|
l0 = l
|
||||||
|
do 10 l1=j,k
|
||||||
|
l0 = l0+1
|
||||||
|
store = store-c(l0)*b(l,l1)
|
||||||
|
10 continue
|
||||||
|
20 c(l) = store/b(l,j-1)
|
||||||
|
l = l-1
|
||||||
|
if(l.eq.0) go to 80
|
||||||
|
30 continue
|
||||||
|
do 50 i=1,n2
|
||||||
|
store = z(i)
|
||||||
|
l = n2
|
||||||
|
do 40 j=1,k
|
||||||
|
l = l+1
|
||||||
|
store = store-c(l)*b(i,j)
|
||||||
|
40 continue
|
||||||
|
c(i) = store
|
||||||
|
50 continue
|
||||||
|
i = n2
|
||||||
|
c(i) = c(i)/a(i,1)
|
||||||
|
if(i.eq.1) go to 80
|
||||||
|
do 70 j=2,n2
|
||||||
|
i = i-1
|
||||||
|
store = c(i)
|
||||||
|
i1 = k
|
||||||
|
if(j.le.k) i1=j-1
|
||||||
|
l = i
|
||||||
|
do 60 l0=1,i1
|
||||||
|
l = l+1
|
||||||
|
store = store-c(l)*a(i,l0+1)
|
||||||
|
60 continue
|
||||||
|
c(i) = store/a(i,1)
|
||||||
|
70 continue
|
||||||
|
80 return
|
||||||
|
end
|
||||||
198
mcc/bsplines/fpbfout.f
Normal file
198
mcc/bsplines/fpbfout.f
Normal file
@@ -0,0 +1,198 @@
|
|||||||
|
recursive subroutine fpbfou(t,n,par,ress,resc)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpbfou calculates the integrals
|
||||||
|
c /t(n-3)
|
||||||
|
c ress(j) = ! nj,4(x)*sin(par*x) dx and
|
||||||
|
c t(4)/
|
||||||
|
c /t(n-3)
|
||||||
|
c resc(j) = ! nj,4(x)*cos(par*x) dx , j=1,2,...n-4
|
||||||
|
c t(4)/
|
||||||
|
c where nj,4(x) denotes the cubic b-spline defined on the knots
|
||||||
|
c t(j),t(j+1),...,t(j+4).
|
||||||
|
c
|
||||||
|
c calling sequence:
|
||||||
|
c call fpbfou(t,n,par,ress,resc)
|
||||||
|
c
|
||||||
|
c input parameters:
|
||||||
|
c t : real array,length n, containing the knots.
|
||||||
|
c n : integer, containing the number of knots.
|
||||||
|
c par : real, containing the value of the parameter par.
|
||||||
|
c
|
||||||
|
c output parameters:
|
||||||
|
c ress : real array,length n, containing the integrals ress(j).
|
||||||
|
c resc : real array,length n, containing the integrals resc(j).
|
||||||
|
c
|
||||||
|
c restrictions:
|
||||||
|
c n >= 10, t(4) < t(5) < ... < t(n-4) < t(n-3).
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer n
|
||||||
|
real*8 par
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),ress(n),resc(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,ic,ipj,is,j,jj,jp1,jp4,k,li,lj,ll,nmj,nm3,nm7
|
||||||
|
real*8 ak,beta,con1,con2,c1,c2,delta,eps,fac,f1,f2,f3,one,quart,
|
||||||
|
* sign,six,s1,s2,term
|
||||||
|
c ..local arrays..
|
||||||
|
real*8 co(5),si(5),hs(5),hc(5),rs(3),rc(3)
|
||||||
|
c ..function references..
|
||||||
|
real*8 cos,sin,abs
|
||||||
|
c ..
|
||||||
|
c initialization.
|
||||||
|
one = 0.1e+01
|
||||||
|
six = 0.6e+01
|
||||||
|
eps = 0.1e-07
|
||||||
|
quart = 0.25e0
|
||||||
|
con1 = 0.5e-01
|
||||||
|
con2 = 0.12e+03
|
||||||
|
nm3 = n-3
|
||||||
|
nm7 = n-7
|
||||||
|
if(par.ne.0.) term = six/par
|
||||||
|
beta = par*t(4)
|
||||||
|
co(1) = cos(beta)
|
||||||
|
si(1) = sin(beta)
|
||||||
|
c calculate the integrals ress(j) and resc(j), j=1,2,3 by setting up
|
||||||
|
c a divided difference table.
|
||||||
|
do 30 j=1,3
|
||||||
|
jp1 = j+1
|
||||||
|
jp4 = j+4
|
||||||
|
beta = par*t(jp4)
|
||||||
|
co(jp1) = cos(beta)
|
||||||
|
si(jp1) = sin(beta)
|
||||||
|
call fpcsin(t(4),t(jp4),par,si(1),co(1),si(jp1),co(jp1),
|
||||||
|
* rs(j),rc(j))
|
||||||
|
i = 5-j
|
||||||
|
hs(i) = 0.
|
||||||
|
hc(i) = 0.
|
||||||
|
do 10 jj=1,j
|
||||||
|
ipj = i+jj
|
||||||
|
hs(ipj) = rs(jj)
|
||||||
|
hc(ipj) = rc(jj)
|
||||||
|
10 continue
|
||||||
|
do 20 jj=1,3
|
||||||
|
if(i.lt.jj) i = jj
|
||||||
|
k = 5
|
||||||
|
li = jp4
|
||||||
|
do 20 ll=i,4
|
||||||
|
lj = li-jj
|
||||||
|
fac = t(li)-t(lj)
|
||||||
|
hs(k) = (hs(k)-hs(k-1))/fac
|
||||||
|
hc(k) = (hc(k)-hc(k-1))/fac
|
||||||
|
k = k-1
|
||||||
|
li = li-1
|
||||||
|
20 continue
|
||||||
|
ress(j) = hs(5)-hs(4)
|
||||||
|
resc(j) = hc(5)-hc(4)
|
||||||
|
30 continue
|
||||||
|
if(nm7.lt.4) go to 160
|
||||||
|
c calculate the integrals ress(j) and resc(j),j=4,5,...,n-7.
|
||||||
|
do 150 j=4,nm7
|
||||||
|
jp4 = j+4
|
||||||
|
beta = par*t(jp4)
|
||||||
|
co(5) = cos(beta)
|
||||||
|
si(5) = sin(beta)
|
||||||
|
delta = t(jp4)-t(j)
|
||||||
|
c the way of computing ress(j) and resc(j) depends on the value of
|
||||||
|
c beta = par*(t(j+4)-t(j)).
|
||||||
|
beta = delta*par
|
||||||
|
if(abs(beta).le.one) go to 60
|
||||||
|
c if !beta! > 1 the integrals are calculated by setting up a divided
|
||||||
|
c difference table.
|
||||||
|
do 40 k=1,5
|
||||||
|
hs(k) = si(k)
|
||||||
|
hc(k) = co(k)
|
||||||
|
40 continue
|
||||||
|
do 50 jj=1,3
|
||||||
|
k = 5
|
||||||
|
li = jp4
|
||||||
|
do 50 ll=jj,4
|
||||||
|
lj = li-jj
|
||||||
|
fac = par*(t(li)-t(lj))
|
||||||
|
hs(k) = (hs(k)-hs(k-1))/fac
|
||||||
|
hc(k) = (hc(k)-hc(k-1))/fac
|
||||||
|
k = k-1
|
||||||
|
li = li-1
|
||||||
|
50 continue
|
||||||
|
s2 = (hs(5)-hs(4))*term
|
||||||
|
c2 = (hc(5)-hc(4))*term
|
||||||
|
go to 130
|
||||||
|
c if !beta! <= 1 the integrals are calculated by evaluating a series
|
||||||
|
c expansion.
|
||||||
|
60 f3 = 0.
|
||||||
|
do 70 i=1,4
|
||||||
|
ipj = i+j
|
||||||
|
hs(i) = par*(t(ipj)-t(j))
|
||||||
|
hc(i) = hs(i)
|
||||||
|
f3 = f3+hs(i)
|
||||||
|
70 continue
|
||||||
|
f3 = f3*con1
|
||||||
|
c1 = quart
|
||||||
|
s1 = f3
|
||||||
|
if(abs(f3).le.eps) go to 120
|
||||||
|
sign = one
|
||||||
|
fac = con2
|
||||||
|
k = 5
|
||||||
|
is = 0
|
||||||
|
do 110 ic=1,20
|
||||||
|
k = k+1
|
||||||
|
ak = k
|
||||||
|
fac = fac*ak
|
||||||
|
f1 = 0.
|
||||||
|
f3 = 0.
|
||||||
|
do 80 i=1,4
|
||||||
|
f1 = f1+hc(i)
|
||||||
|
f2 = f1*hs(i)
|
||||||
|
hc(i) = f2
|
||||||
|
f3 = f3+f2
|
||||||
|
80 continue
|
||||||
|
f3 = f3*six/fac
|
||||||
|
if(is.eq.0) go to 90
|
||||||
|
is = 0
|
||||||
|
s1 = s1+f3*sign
|
||||||
|
go to 100
|
||||||
|
90 sign = -sign
|
||||||
|
is = 1
|
||||||
|
c1 = c1+f3*sign
|
||||||
|
100 if(abs(f3).le.eps) go to 120
|
||||||
|
110 continue
|
||||||
|
120 s2 = delta*(co(1)*s1+si(1)*c1)
|
||||||
|
c2 = delta*(co(1)*c1-si(1)*s1)
|
||||||
|
130 ress(j) = s2
|
||||||
|
resc(j) = c2
|
||||||
|
do 140 i=1,4
|
||||||
|
co(i) = co(i+1)
|
||||||
|
si(i) = si(i+1)
|
||||||
|
140 continue
|
||||||
|
150 continue
|
||||||
|
c calculate the integrals ress(j) and resc(j),j=n-6,n-5,n-4 by setting
|
||||||
|
c up a divided difference table.
|
||||||
|
160 do 190 j=1,3
|
||||||
|
nmj = nm3-j
|
||||||
|
i = 5-j
|
||||||
|
call fpcsin(t(nm3),t(nmj),par,si(4),co(4),si(i-1),co(i-1),
|
||||||
|
* rs(j),rc(j))
|
||||||
|
hs(i) = 0.
|
||||||
|
hc(i) = 0.
|
||||||
|
do 170 jj=1,j
|
||||||
|
ipj = i+jj
|
||||||
|
hc(ipj) = rc(jj)
|
||||||
|
hs(ipj) = rs(jj)
|
||||||
|
170 continue
|
||||||
|
do 180 jj=1,3
|
||||||
|
if(i.lt.jj) i = jj
|
||||||
|
k = 5
|
||||||
|
li = nmj
|
||||||
|
do 180 ll=i,4
|
||||||
|
lj = li+jj
|
||||||
|
fac = t(lj)-t(li)
|
||||||
|
hs(k) = (hs(k-1)-hs(k))/fac
|
||||||
|
hc(k) = (hc(k-1)-hc(k))/fac
|
||||||
|
k = k-1
|
||||||
|
li = li+1
|
||||||
|
180 continue
|
||||||
|
ress(nmj) = hs(4)-hs(5)
|
||||||
|
resc(nmj) = hc(4)-hc(5)
|
||||||
|
190 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
81
mcc/bsplines/fpbisp.f
Normal file
81
mcc/bsplines/fpbisp.f
Normal file
@@ -0,0 +1,81 @@
|
|||||||
|
recursive subroutine fpbisp(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,
|
||||||
|
* z,wx,wy,lx,ly)
|
||||||
|
implicit none
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer nx,ny,kx,ky,mx,my
|
||||||
|
c ..array arguments..
|
||||||
|
integer lx(mx),ly(my)
|
||||||
|
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(mx),y(my),z(mx*my),
|
||||||
|
* wx(mx,kx+1),wy(my,ky+1)
|
||||||
|
c ..local scalars..
|
||||||
|
integer kx1,ky1,l,l1,l2,m,nkx1,nky1, i, i1, j, j1
|
||||||
|
real*8 arg,sp,tb,te
|
||||||
|
c ..local arrays..
|
||||||
|
real*8 h(6)
|
||||||
|
c ..subroutine references..
|
||||||
|
c fpbspl
|
||||||
|
c ..
|
||||||
|
kx1 = kx+1
|
||||||
|
nkx1 = nx-kx1
|
||||||
|
tb = tx(kx1)
|
||||||
|
te = tx(nkx1+1)
|
||||||
|
l = kx1
|
||||||
|
l1 = l+1
|
||||||
|
do 40 i=1,mx
|
||||||
|
arg = x(i)
|
||||||
|
if(arg.lt.tb) arg = tb
|
||||||
|
if(arg.gt.te) arg = te
|
||||||
|
10 if(arg.lt.tx(l1) .or. l.eq.nkx1) go to 20
|
||||||
|
l = l1
|
||||||
|
l1 = l+1
|
||||||
|
go to 10
|
||||||
|
20 call fpbspl(tx,nx,kx,arg,l,h)
|
||||||
|
lx(i) = l-kx1
|
||||||
|
do 30 j=1,kx1
|
||||||
|
wx(i,j) = h(j)
|
||||||
|
30 continue
|
||||||
|
40 continue
|
||||||
|
ky1 = ky+1
|
||||||
|
nky1 = ny-ky1
|
||||||
|
tb = ty(ky1)
|
||||||
|
te = ty(nky1+1)
|
||||||
|
l = ky1
|
||||||
|
l1 = l+1
|
||||||
|
do 80 i=1,my
|
||||||
|
arg = y(i)
|
||||||
|
if(arg.lt.tb) arg = tb
|
||||||
|
if(arg.gt.te) arg = te
|
||||||
|
50 if(arg.lt.ty(l1) .or. l.eq.nky1) go to 60
|
||||||
|
l = l1
|
||||||
|
l1 = l+1
|
||||||
|
go to 50
|
||||||
|
60 call fpbspl(ty,ny,ky,arg,l,h)
|
||||||
|
ly(i) = l-ky1
|
||||||
|
do 70 j=1,ky1
|
||||||
|
wy(i,j) = h(j)
|
||||||
|
70 continue
|
||||||
|
80 continue
|
||||||
|
m = 0
|
||||||
|
do 130 i=1,mx
|
||||||
|
l = lx(i)*nky1
|
||||||
|
do 90 i1=1,kx1
|
||||||
|
h(i1) = wx(i,i1)
|
||||||
|
90 continue
|
||||||
|
do 120 j=1,my
|
||||||
|
l1 = l+ly(j)
|
||||||
|
sp = 0.
|
||||||
|
do 110 i1=1,kx1
|
||||||
|
l2 = l1
|
||||||
|
do 100 j1=1,ky1
|
||||||
|
l2 = l2+1
|
||||||
|
sp = sp+c(l2)*h(i1)*wy(j,j1)
|
||||||
|
100 continue
|
||||||
|
l1 = l1+nky1
|
||||||
|
110 continue
|
||||||
|
m = m+1
|
||||||
|
z(m) = sp
|
||||||
|
120 continue
|
||||||
|
130 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
|
|
||||||
42
mcc/bsplines/fpbspl.f
Normal file
42
mcc/bsplines/fpbspl.f
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
recursive subroutine fpbspl(t,n,k,x,l,h)
|
||||||
|
c subroutine fpbspl evaluates the (k+1) non-zero b-splines of
|
||||||
|
c degree k at t(l) <= x < t(l+1) using the stable recurrence
|
||||||
|
c relation of de boor and cox.
|
||||||
|
c Travis Oliphant 2007
|
||||||
|
c changed so that weighting of 0 is used when knots with
|
||||||
|
c multiplicity are present.
|
||||||
|
c Also, notice that l+k <= n and 1 <= l+1-k
|
||||||
|
c or else the routine will be accessing memory outside t
|
||||||
|
c Thus it is imperative that that k <= l <= n-k but this
|
||||||
|
c is not checked.
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
real*8 x
|
||||||
|
integer n,k,l
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 t(n),h(20)
|
||||||
|
c ..local scalars..
|
||||||
|
real*8 f,one
|
||||||
|
integer i,j,li,lj
|
||||||
|
c ..local arrays..
|
||||||
|
real*8 hh(19)
|
||||||
|
c ..
|
||||||
|
one = 0.1d+01
|
||||||
|
h(1) = one
|
||||||
|
do 20 j=1,k
|
||||||
|
do 10 i=1,j
|
||||||
|
hh(i) = h(i)
|
||||||
|
10 continue
|
||||||
|
h(1) = 0.0d0
|
||||||
|
do 20 i=1,j
|
||||||
|
li = l+i
|
||||||
|
lj = li-j
|
||||||
|
if (t(li).ne.t(lj)) goto 15
|
||||||
|
h(i+1) = 0.0d0
|
||||||
|
goto 20
|
||||||
|
15 f = hh(i)/(t(li)-t(lj))
|
||||||
|
h(i) = h(i)+f*(t(li)-x)
|
||||||
|
h(i+1) = f*(x-t(lj))
|
||||||
|
20 continue
|
||||||
|
return
|
||||||
|
end
|
||||||
87
mcc/bsplines/fpchec.f
Normal file
87
mcc/bsplines/fpchec.f
Normal file
@@ -0,0 +1,87 @@
|
|||||||
|
recursive subroutine fpchec(x,m,t,n,k,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpchec verifies the number and the position of the knots
|
||||||
|
c t(j),j=1,2,...,n of a spline of degree k, in relation to the number
|
||||||
|
c and the position of the data points x(i),i=1,2,...,m. if all of the
|
||||||
|
c following conditions are fulfilled, the error parameter ier is set
|
||||||
|
c to zero. if one of the conditions is violated ier is set to ten.
|
||||||
|
c 1) k+1 <= n-k-1 <= m
|
||||||
|
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||||
|
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||||
|
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||||
|
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||||
|
c 5) the conditions specified by schoenberg and whitney must hold
|
||||||
|
c for at least one subset of data points, i.e. there must be a
|
||||||
|
c subset of data points y(j) such that
|
||||||
|
c t(j) < y(j) < t(j+k+1), j=1,2,...,n-k-1
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer m,n,k,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),t(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,j,k1,k2,l,nk1,nk2,nk3
|
||||||
|
real*8 tj,tl
|
||||||
|
c ..
|
||||||
|
k1 = k+1
|
||||||
|
k2 = k1+1
|
||||||
|
nk1 = n-k1
|
||||||
|
nk2 = nk1+1
|
||||||
|
ier = 10
|
||||||
|
c check condition no 1
|
||||||
|
if (nk1.lt.k1 .or. nk1.gt.m) then
|
||||||
|
ier = 10
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
c check condition no 2
|
||||||
|
j = n
|
||||||
|
do 20 i=1,k
|
||||||
|
if (t(i) .gt. t(i+1)) then
|
||||||
|
ier = 20
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
if (t(j) .lt. t(j-1)) then
|
||||||
|
ier = 20
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
j = j-1
|
||||||
|
20 continue
|
||||||
|
c check condition no 3
|
||||||
|
do 30 i=k2,nk2
|
||||||
|
if (t(i) .le. t(i-1)) then
|
||||||
|
ier = 30
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
30 continue
|
||||||
|
c check condition no 4
|
||||||
|
if (x(1).lt.t(k1) .or. x(m).gt.t(nk2)) then
|
||||||
|
ier = 40
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
c check condition no 5
|
||||||
|
if (x(1).ge.t(k2) .or. x(m).le.t(nk1)) then
|
||||||
|
ier = 50
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
i = 1
|
||||||
|
l = k2
|
||||||
|
nk3 = nk1-1
|
||||||
|
if (nk3 .lt. 2) go to 70
|
||||||
|
do 60 j=2,nk3
|
||||||
|
tj = t(j)
|
||||||
|
l = l+1
|
||||||
|
tl = t(l)
|
||||||
|
40 i = i+1
|
||||||
|
if (i .ge. m) then
|
||||||
|
ier = 50
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
if (x(i) .le. tj) go to 40
|
||||||
|
if (x(i) .ge. tl) then
|
||||||
|
ier = 50
|
||||||
|
go to 80
|
||||||
|
endif
|
||||||
|
60 continue
|
||||||
|
70 ier = 0
|
||||||
|
80 return
|
||||||
|
end
|
||||||
70
mcc/bsplines/fpched.f
Normal file
70
mcc/bsplines/fpched.f
Normal file
@@ -0,0 +1,70 @@
|
|||||||
|
recursive subroutine fpched(x,m,t,n,k,ib,ie,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpched verifies the number and the position of the knots
|
||||||
|
c t(j),j=1,2,...,n of a spline of degree k,with ib derative constraints
|
||||||
|
c at x(1) and ie constraints at x(m), in relation to the number and
|
||||||
|
c the position of the data points x(i),i=1,2,...,m. if all of the
|
||||||
|
c following conditions are fulfilled, the error parameter ier is set
|
||||||
|
c to zero. if one of the conditions is violated ier is set to ten.
|
||||||
|
c 1) k+1 <= n-k-1 <= m + max(0,ib-1) + max(0,ie-1)
|
||||||
|
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||||
|
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||||
|
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||||
|
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||||
|
c 5) the conditions specified by schoenberg and whitney must hold
|
||||||
|
c for at least one subset of data points, i.e. there must be a
|
||||||
|
c subset of data points y(j) such that
|
||||||
|
c t(j) < y(j) < t(j+k+1), j=1+ib1,2+ib1,...,n-k-1-ie1
|
||||||
|
c with ib1 = max(0,ib-1), ie1 = max(0,ie-1)
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer m,n,k,ib,ie,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),t(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,ib1,ie1,j,jj,k1,k2,l,nk1,nk2,nk3
|
||||||
|
real*8 tj,tl
|
||||||
|
c ..
|
||||||
|
k1 = k+1
|
||||||
|
k2 = k1+1
|
||||||
|
nk1 = n-k1
|
||||||
|
nk2 = nk1+1
|
||||||
|
ib1 = ib-1
|
||||||
|
if(ib1.lt.0) ib1 = 0
|
||||||
|
ie1 = ie-1
|
||||||
|
if(ie1.lt.0) ie1 = 0
|
||||||
|
ier = 10
|
||||||
|
c check condition no 1
|
||||||
|
if(nk1.lt.k1 .or. nk1.gt.(m+ib1+ie1)) go to 80
|
||||||
|
c check condition no 2
|
||||||
|
j = n
|
||||||
|
do 20 i=1,k
|
||||||
|
if(t(i).gt.t(i+1)) go to 80
|
||||||
|
if(t(j).lt.t(j-1)) go to 80
|
||||||
|
j = j-1
|
||||||
|
20 continue
|
||||||
|
c check condition no 3
|
||||||
|
do 30 i=k2,nk2
|
||||||
|
if(t(i).le.t(i-1)) go to 80
|
||||||
|
30 continue
|
||||||
|
c check condition no 4
|
||||||
|
if(x(1).lt.t(k1) .or. x(m).gt.t(nk2)) go to 80
|
||||||
|
c check condition no 5
|
||||||
|
if(x(1).ge.t(k2) .or. x(m).le.t(nk1)) go to 80
|
||||||
|
i = 1
|
||||||
|
jj = 2+ib1
|
||||||
|
l = jj+k
|
||||||
|
nk3 = nk1-1-ie1
|
||||||
|
if(nk3.lt.jj) go to 70
|
||||||
|
do 60 j=jj,nk3
|
||||||
|
tj = t(j)
|
||||||
|
l = l+1
|
||||||
|
tl = t(l)
|
||||||
|
40 i = i+1
|
||||||
|
if(i.ge.m) go to 80
|
||||||
|
if(x(i).le.tj) go to 40
|
||||||
|
if(x(i).ge.tl) go to 80
|
||||||
|
60 continue
|
||||||
|
70 ier = 0
|
||||||
|
80 return
|
||||||
|
end
|
||||||
82
mcc/bsplines/fpchep.f
Normal file
82
mcc/bsplines/fpchep.f
Normal file
@@ -0,0 +1,82 @@
|
|||||||
|
recursive subroutine fpchep(x,m,t,n,k,ier)
|
||||||
|
implicit none
|
||||||
|
c subroutine fpchep verifies the number and the position of the knots
|
||||||
|
c t(j),j=1,2,...,n of a periodic spline of degree k, in relation to
|
||||||
|
c the number and the position of the data points x(i),i=1,2,...,m.
|
||||||
|
c if all of the following conditions are fulfilled, ier is set
|
||||||
|
c to zero. if one of the conditions is violated ier is set to ten.
|
||||||
|
c 1) k+1 <= n-k-1 <= m+k-1
|
||||||
|
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||||
|
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||||
|
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||||
|
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||||
|
c 5) the conditions specified by schoenberg and whitney must hold
|
||||||
|
c for at least one subset of data points, i.e. there must be a
|
||||||
|
c subset of data points y(j) such that
|
||||||
|
c t(j) < y(j) < t(j+k+1), j=k+1,...,n-k-1
|
||||||
|
c ..
|
||||||
|
c ..scalar arguments..
|
||||||
|
integer m,n,k,ier
|
||||||
|
c ..array arguments..
|
||||||
|
real*8 x(m),t(n)
|
||||||
|
c ..local scalars..
|
||||||
|
integer i,i1,i2,j,j1,k1,k2,l,l1,l2,mm,m1,nk1,nk2
|
||||||
|
real*8 per,tj,tl,xi
|
||||||
|
c ..
|
||||||
|
k1 = k+1
|
||||||
|
k2 = k1+1
|
||||||
|
nk1 = n-k1
|
||||||
|
nk2 = nk1+1
|
||||||
|
m1 = m-1
|
||||||
|
ier = 10
|
||||||
|
c check condition no 1
|
||||||
|
if(nk1.lt.k1 .or. n.gt.m+2*k) go to 130
|
||||||
|
c check condition no 2
|
||||||
|
j = n
|
||||||
|
do 20 i=1,k
|
||||||
|
if(t(i).gt.t(i+1)) go to 130
|
||||||
|
if(t(j).lt.t(j-1)) go to 130
|
||||||
|
j = j-1
|
||||||
|
20 continue
|
||||||
|
c check condition no 3
|
||||||
|
do 30 i=k2,nk2
|
||||||
|
if(t(i).le.t(i-1)) go to 130
|
||||||
|
30 continue
|
||||||
|
c check condition no 4
|
||||||
|
if(x(1).lt.t(k1) .or. x(m).gt.t(nk2)) go to 130
|
||||||
|
c check condition no 5
|
||||||
|
l1 = k1
|
||||||
|
l2 = 1
|
||||||
|
do 50 l=1,m
|
||||||
|
xi = x(l)
|
||||||
|
40 if(xi.lt.t(l1+1) .or. l.eq.nk1) go to 50
|
||||||
|
l1 = l1+1
|
||||||
|
l2 = l2+1
|
||||||
|
if(l2.gt.k1) go to 60
|
||||||
|
go to 40
|
||||||
|
50 continue
|
||||||
|
l = m
|
||||||
|
60 per = t(nk2)-t(k1)
|
||||||
|
do 120 i1=2,l
|
||||||
|
i = i1-1
|
||||||
|
mm = i+m1
|
||||||
|
do 110 j=k1,nk1
|
||||||
|
tj = t(j)
|
||||||
|
j1 = j+k1
|
||||||
|
tl = t(j1)
|
||||||
|
70 i = i+1
|
||||||
|
if(i.gt.mm) go to 120
|
||||||
|
i2 = i-m1
|
||||||
|
if (i2.le.0) go to 80
|
||||||
|
go to 90
|
||||||
|
80 xi = x(i)
|
||||||
|
go to 100
|
||||||
|
90 xi = x(i2)+per
|
||||||
|
100 if(xi.le.tj) go to 70
|
||||||
|
if(xi.ge.tl) go to 120
|
||||||
|
110 continue
|
||||||
|
ier = 0
|
||||||
|
go to 130
|
||||||
|
120 continue
|
||||||
|
130 return
|
||||||
|
end
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user