...

mcc/mcc_guiding_model.h.split

@@ -0,0 +1,253 @@
+#pragma once
+
+/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */
+
+
+/*                          SIMPLE GUIDING MODEL IMPLEMENTATION                            */
+
+
+#include "mcc_defaults.h"
+#include "mcc_moving_model_common.h"
+
+namespace mcc
+{
+
+enum class MccSimpleGuidingModelErrorCode : int {
+    ERROR_OK,
+    ERROR_HW_GETSTATE,
+    ERROR_HW_SETSTATE,
+    ERROR_PCM_COMP,
+    ERROR_GET_TELEMETRY,
+    ERROR_DIST_TELEMETRY,
+    ERROR_DIFF_TELEMETRY,
+    ERROR_PZONE_CONTAINER_COMP,
+    ERROR_IN_PZONE,
+    ERROR_NEAR_PZONE,
+    ERROR_TIMEOUT,
+    ERROR_UNEXPECTED_AXIS_RATES,
+    ERROR_STOPPED
+};
+
+}  // namespace mcc
+
+
+namespace std
+{
+
+template <>
+class is_error_code_enum<mcc::MccSimpleGuidingModelErrorCode> : public true_type
+{
+};
+
+}  // namespace std
+
+
+
+namespace mcc
+{
+
+class MccSimpleGuidingModel
+{
+public:
+    typedef std::error_code error_t;
+
+    typedef MccSimpleMovingModelParams guiding_params_t;
+
+
+    template <mcc_telemetry_data_c TelemetryT,
+              mcc_hardware_c HardwareT,
+              mcc_PCM_c PcmT,
+              mcc_pzone_container_c PZoneContT>
+    MccSimpleGuidingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
+        : _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
+    {
+        _guidingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
+            typename HardwareT::hardware_state_t hw_state;
+
+            MccTelemetryData tdata;
+            MccEqtHrzCoords intsc_coords;
+            double dist, dx, dy;
+
+            auto t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
+            if (t_err) {
+                return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
+            }
+
+
+            // compute intersection points with the prohibited zones
+            auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords);
+            if (pz_err) {
+                return mcc_deduce_error<error_t>(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
+            }
+
+            bool no_intersects = false;
+
+            if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                if (std::isfinite(intsc_coords.HA)) {
+                    intsc_coords.X = intsc_coords.HA;
+                    intsc_coords.Y = intsc_coords.DEC_APP;
+                } else {
+                    no_intersects = true;
+                    intsc_coords.X = tdata.HA + 710.0_mins;  // 12h - 10min
+                    intsc_coords.Y = tdata.DEC_APP;
+                }
+            } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
+                if (std::isfinite(intsc_coords.AZ)) {
+                    intsc_coords.X = intsc_coords.AZ;
+                    intsc_coords.Y = intsc_coords.ZD;
+                } else {
+                    no_intersects = true;
+                }
+            } else {
+                static_assert(false, "UNKNOW MOUNT TYPE!");
+            }
+
+            // compute position in future
+            auto hw_err = hardware->hardwareGetState(&hw_state);
+            if (hw_err) {
+                return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_GETSTATE);
+            }
+
+            MccPCMResult pcm_inv_res;
+
+            // endpoint of the mount moving
+            auto pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
+            if (pcm_err) {
+                return mcc_deduce_error<error_t>(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
+            }
+
+            if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                hw_state.speedX = _currentParams.trackSpeedX;
+                hw_state.speedY = _currentParams.trackSpeedY;
+            }
+
+            std::chrono::steady_clock::time_point last_corr_tp;
+
+            while (*_stopGuiding) {
+                // wait for updated telemetry data
+                {
+                    std::lock_guard lock{*_currentParamsMutex};
+
+                    t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
+
+                    if (t_err) {
+                        return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
+                    }
+                }
+
+                if (*_stopGuiding) {
+                    break;
+                }
+
+                // control prohibited zones
+                if (mcc_is_near_pzones(pz_cont, tdata, _currentParams.minTimeToPZone, pz_err)) {
+                    return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
+                }
+                if (pz_err) {
+                    return mcc_deduce_error<error_t>(pz_err,
+                                                     MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
+                }
+
+                if (*_stopGuiding) {
+                    break;
+                }
+
+                t_err = telemetry->targetToMountDist(&dist);
+                if (t_err) {
+                    return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_DIST_TELEMETRY);
+                }
+
+                if (*_stopGuiding) {
+                    break;
+                }
+
+                {
+                    std::lock_guard lock{*_currentParamsMutex};
+
+                    if (dist < _currentParams.guidingCorrectionRange[0] ||
+                        dist > _currentParams.guidingCorrectionRange[1]) {  // moving with sideral speed
+
+                        hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
+                        if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                            hw_state.speedX = _currentParams.trackSpeedX;
+                            hw_state.speedY = _currentParams.trackSpeedY;
+                        } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
+                            static_assert(false, "NOT IMPLEMENTED!");
+                        } else {
+                            static_assert(false, "UNKNOW MOUNT TYPE!");
+                        }
+
+                        hw_err = hardware->hardwareSetState(hw_state);
+                        if (hw_err) {
+                            return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
+                        }
+
+                        continue;
+                    }
+
+                    auto now = std::chrono::steady_clock::now();
+                    if ((now - last_corr_tp) < _currentParams.guidingMinInterval) {
+                        continue;
+                    }
+
+                    hw_state.X = tdata.target.X;
+
+                    if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                        if (_currentParams.dualAxisGuiding) {
+                            hw_state.Y = tdata.target.Y;
+                        } else {  // only along HA-axis
+                            hw_state.Y = tdata.Y;
+                        }
+                    } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
+                        hw_state.Y = tdata.target.Y;
+                    } else {
+                        static_assert(false, "UNKNOW MOUNT TYPE!");
+                    }
+                    if (t_err) {
+                        return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_DIFF_TELEMETRY);
+                    }
+
+                    last_corr_tp = now;
+                }
+
+                // send corrections
+                hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_GUIDING;
+                hw_err = hardware->hardwareSetState(hw_state);
+                if (hw_err) {
+                    return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
+                }
+            }
+
+            return MccSimpleGuidingModelErrorCode::ERROR_OK;
+        };
+    }
+
+    MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default;
+    MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default;
+
+    MccSimpleGuidingModel(const MccSimpleGuidingModel&) = delete;
+    MccSimpleGuidingModel& operator=(const MccSimpleGuidingModel&) = delete;
+
+    error_t startGuidingTarget()
+    {
+        *_stopGuiding = false;
+
+        return _guidingFunc();
+    }
+
+    error_t stoptGuidingTarget()
+    {
+        *_stopGuiding = true;
+
+        return MccSimpleGuidingModelErrorCode::ERROR_OK;
+    }
+
+protected:
+    std::function<error_t()> _guidingFunc{};
+    std::unique_ptr<std::atomic_bool> _stopGuiding;
+
+    guiding_params_t _currentParams{};
+    std::unique_ptr<std::mutex> _currentParamsMutex{};
+};
+
+}  // namespace mcc