...

mcc/mcc_generics.h

@@ -420,6 +420,14 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
 
     { t_const.hardwareName() } -> std::formattable<char>;
 
+    // the 'T' class must contain static constexpr member of 'MccMountType' type
+    requires std::same_as<decltype(T::mountType), const MccMountType>;
+    []() {
+        static constexpr MccMountType val = T::mountType;
+        return val;
+    }();  // to ensure 'mountType' can be used in compile-time context
+
+
     // a type that defines at least HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING
     // and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a
     // possible tunning of hardware hardwareSetState-related commands and detect stop-state
@@ -657,6 +665,13 @@ struct mcc_telemetry_interface_t {
         std::forward<SelfT>(self).targetToMountDiff(pair_kind, dx, dy);
     }
 
+    // compute distance between target and actual mount celestial points
+    template <std::derived_from<mcc_telemetry_interface_t> SelfT>
+    RetT targetToMountDist(this SelfT&& self, mcc_angle_c auto* dist)
+    {
+        std::forward<SelfT>(self).targetToMountDist(dist);
+    }
+
 protected:
     mcc_telemetry_interface_t() = default;
 };

mcc/mcc_pzone_utils.h

@@ -0,0 +1,69 @@
+#pragma once
+
+/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */
+
+
+/*                          PROHIBITED ZONE CONTAINER UTILITIES                         */
+
+
+#include "mcc_generics.h"
+
+namespace mcc
+{
+
+
+template <typename T, traits::mcc_callable_c FuncT>
+void MccPZoneContainerForEach(std::ranges::output_range<T> auto const& input_range, FuncT&& func)
+    requires requires(std::remove_cvref_t<FuncT> f, T v) { [](std::remove_cvref_t<FuncT> ff, T vv) { ff(vv); }(f, v); }
+{
+    for (auto const& el : input_range) {
+        std::forward<FuncT>(func)(el);
+    }
+}
+
+
+template <typename T>
+auto MccPZoneContainerTimeStat(const T& durations,
+                               traits::mcc_time_duration_c auto* min_time,
+                               traits::mcc_time_duration_c auto* max_time)
+    requires traits::mcc_output_duration_range_c<T>
+{
+    if (min_time == nullptr && max_time == nullptr) {
+        return;
+    }
+
+    using min_t = std::decay_t<decltype(*min_time)>;
+    using max_t = std::decay_t<decltype(*max_time)>;
+
+    using duration_t = std::ranges::range_value_t<T>;
+    duration_t mint = duration_t::max();
+    duration_t maxt = duration_t::min();
+
+    MccPZoneContainerForEach(durations, [&mint, maxt](auto const& d) {
+        if (d < mint) {
+            mint = d;
+        }
+
+        if (d > maxt) {
+            maxt = d;
+        }
+    });
+
+    if (min_time != nullptr) {
+        *min_time = std::chrono::duration_cast<min_t>(mint);
+    }
+
+    if (max_time != nullptr) {
+        *max_time = std::chrono::duration_cast<max_t>(maxt);
+    }
+}
+
+
+template <mcc_celestial_point_c CPT>
+auto MccPZoneContainerIntersectStat(std::ranges::output_range<CPT> auto const& points,
+                                    mcc_celestial_point_c auto* first)
+{
+}
+
+
+}  // namespace mcc

mcc/mcc_slewing_model.h

@@ -0,0 +1,267 @@
+#pragma once
+
+/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */
+
+
+/*                          SIMPLE SLEWING MODEL IMPLEMENTATION                            */
+
+
+#include "mcc_defaults.h"
+#include "mcc_generics.h"
+
+
+namespace mcc
+{
+
+enum class MccSimpleSlewingModelErrorCode : 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_UNEXPECTED_AXIS_RATES
+};
+
+}  // namespace mcc
+
+
+namespace std
+{
+
+template <>
+class is_error_code_enum<mcc::MccSimpleSlewingModelErrorCode> : public true_type
+{
+};
+
+}  // namespace std
+
+
+
+namespace mcc
+{
+
+/*
+    The target celestial point must be set in telemetry->target
+ */
+
+class MccSimpleSlewingModel
+{
+public:
+    typedef std::error_code error_t;
+
+    struct slewing_params_t {
+        bool slewAndStop{false};  // slew to target and stop mount
+
+        std::chrono::seconds telemetryTimeout{3};
+
+        // minimal time to prohibited zone at current speed. if it is lesser then exit with error
+        std::chrono::seconds minTimeToPZone{10};
+
+        // target-mount coordinate difference to start adjusting of slewing (in radians)
+        double adjustCoordDiff{10.0_degs};
+
+        // coordinates difference to stop slewing (in radians)
+        double slewToleranceRadius{5.0_arcsecs};
+
+        // slew process timeout
+        std::chrono::seconds slewTimeout{3600};
+
+        double slewXRate{0.0};  // maximal slewing rate (0 means move with maximal allowed rate)
+        double slewYRate{0.0};  // maximal slewing rate (0 means move with maximal allowed rate)
+
+        double adjustXRate{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+        double adjustYRate{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+    };
+
+    template <mcc_telemetry_data_c TelemetryT,
+              mcc_hardware_c HardwareT,
+              mcc_PCM_c PcmT,
+              mcc_pzone_container_c PZoneContT>
+    MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
+        : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
+    {
+        _slewingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
+            *_stopSlewing = false;
+
+            // first, check target coordinates
+            typename TelemetryT::error_t t_err;
+            MccTelemetryData tdata;
+
+            {
+                std::lock_guard lock{*_currentParamsMutex};
+                t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
+
+                if (t_err) {
+                    return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
+                }
+            }
+
+            bool in_zone;
+            auto pz_err = pz_cont->inPZone(tdata.target, &in_zone);
+            if (pz_err) {
+                return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
+            }
+
+            if (in_zone) {
+                return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
+            }
+
+            MccCelestialPoint cpt;
+            mcc_tp2tp(tdata.time_point, cpt.time_point);
+
+            if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
+            } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
+                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
+            } else {
+                static_assert(false, "UNKNOWN MOUNT TYPE!");
+            }
+
+            std::vector<MccCelestialPoint> isct_pt(pz_cont->sizePZones, cpt);
+            pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt);
+            if (pz_err) {
+                return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
+            }
+
+            typename HardwareT::hardware_state_t hw_state;
+
+            auto hw_err = hardware->hardwareGetState(&hw_state);
+            if (hw_err) {
+                return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
+            }
+
+            hw_state.X = (double)tdata.target.X;
+            hw_state.Y = (double)tdata.target.Y;
+            {
+                std::lock_guard lock{*_currentParamsMutex};
+
+                hw_state.speedX = _currentParams.slewXRate;
+                hw_state.speedY = _currentParams.slewYRate;
+            }
+            hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING;
+
+            // start slewing
+            hw_err = hardware->hardwareSetState(hw_state);
+            if (hw_err) {
+                return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
+            }
+
+            double dist, dx, dy, sinY;
+            std::chrono::duration<double> dtx, dty;  // seconds in double
+            while (!*_stopSlewing) {
+                // 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, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
+                    }
+                }
+
+
+                // compute time to prohibited zones at current speed
+                for (auto const& pt : isct_pt) {
+                    if (std::isfinite(pt.X) && std::isfinite(pt.Y)) {
+                        if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
+                            sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
+                            dx = pt.X - tdata.HA;
+                            dy = pt.Y - tdata.DEC_APP;
+                        } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
+                            sinY = sin(tdata.ZD);
+                            dx = pt.X - tdata.AZ;
+                            dy = pt.Y - tdata.ZD;
+                        }
+
+                        if (utils::isEqual(sinY, 0.0)) {
+                            dtx = decltype(dtx){std::numeric_limits<double>::infinity()};
+                        } else {
+                            dtx = decltype(dtx){std::abs(dx / tdata.speedX / sinY)};
+                        }
+                        dty = decltype(dty){std::abs(dy / tdata.speedY)};
+                        if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
+                            return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
+                        }
+                    }
+                }
+
+
+                t_err = telemetry->targetToMountDist(&dist);
+                if (t_err) {
+                    return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
+                }
+
+                if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
+                    break;
+                }
+
+                if (dist <= _currentParams.adjustCoordDiff) {
+                }
+
+                // check for current axis speed
+                if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) {
+                    // unhandled stop state?!!!
+                    return MccSimpleSlewingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES;
+                }
+            }
+
+            return MccSimpleSlewingModelErrorCode::ERROR_OK;
+        };
+    }
+
+
+    MccSimpleSlewingModel(MccSimpleSlewingModel&&) = default;
+    MccSimpleSlewingModel& operator=(MccSimpleSlewingModel&&) = default;
+
+    MccSimpleSlewingModel(const MccSimpleSlewingModel&) = delete;
+    MccSimpleSlewingModel& operator=(const MccSimpleSlewingModel&) = delete;
+
+
+    virtual ~MccSimpleSlewingModel() = default;
+
+    error_t slewToTarget()
+    {
+        return _slewingFunc();
+    }
+
+
+    error_t stopSlewing()
+    {
+        *_stopSlewing = true;
+
+        return MccSimpleSlewingModelErrorCode::ERROR_OK;
+    }
+
+
+    error_t setSlewingParams(slewing_params_t pars)
+    {
+        std::lock_guard lock{*_currentParamsMutex};
+
+        _currentParams = std::move(pars);
+
+        return MccSimpleSlewingModelErrorCode::ERROR_OK;
+    }
+
+
+    slewing_params_t getSlewingParams() const
+    {
+        std::lock_guard lock{*_currentParamsMutex};
+
+        return _currentParams;
+    }
+
+protected:
+    std::function<error_t()> _slewingFunc{};
+    std::unique_ptr<std::atomic_bool> _stopSlewing;
+
+    slewing_params_t _currentParams{};
+    std::unique_ptr<std::mutex> _currentParamsMutex{};
+};
+
+
+}  // namespace mcc

mcc/mcc_telemetry.h

@@ -124,7 +124,7 @@ public:
 
         _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
 
-        using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
+        // using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
         using pcm_t = std::remove_cvref_t<decltype(*pcm)>;
         using hardware_t = std::remove_cvref_t<decltype(*hardware)>;
 
@@ -414,7 +414,8 @@ public:
 
         if (_internalUpdatingFuture.valid()) {
             // try to exit correctly
-            auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
+            // auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
+            _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
             // _internalUpdatingFuture.get();
         }
     };
@@ -622,6 +623,31 @@ public:
         return MccTelemetryErrorCode::ERROR_OK;
     }
 
+
+    error_t targetToMountDist(mcc_angle_c auto* dist)
+    {
+        if (dist == nullptr) {
+            return MccTelemetryErrorCode::ERROR_NULLPTR;
+        }
+
+        std::lock_guard lock{*_updateMutex};
+
+        double dHA = _data.HA - _data.target.HA;
+        double cosDHA = cos(dHA);
+
+        double cosT = cos(_data.target.DEC_APP);
+        double sinT = sin(_data.target.DEC_APP);
+        double cosM = cos(_data.DEC_APP);
+        double sinM = sin(_data.DEC_APP);
+
+        double term1 = cosT * sin(dHA);
+        double term2 = cosM * sinT - sinM * cosT * cosDHA;
+
+        *dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA)));
+
+        return MccTelemetryErrorCode::ERROR_OK;
+    }
+
 protected:
     std::unique_ptr<std::atomic_bool> _isDataUpdated;
     MccTelemetryData _data;

mcc/mcc_tracking_model.h

@@ -64,25 +64,23 @@ public:
               mcc_PCM_c PcmT,
               mcc_pzone_container_c PZoneContT>
     MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
-        : _stopTracking(new std::atomic_bool())
+        : _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex)
     {
         *_stopTracking = false;
 
+        // set default values
         if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
             _currentTrackParams.trackSpeedX = tracking_params_t::sideralRate;  // move along HA-axis with sideral rate
             _currentTrackParams.trackSpeedY = 0.0;
+
+            _currentTrackParams.telemetryTimeout = std::chrono::seconds(3);
+            _currentTrackParams.minTimeToPZone = std::chrono::seconds(10);
         }
 
         _trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
             MccCelestialPoint cpt;
             typename HardwareT::hardware_state_t hw_state;
 
-            // compute position in future
-            auto err = hardware->hardwareGetState(&hw_state);
-            if (err) {
-                return mcc_deduce_error<error_t>(err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
-            }
-
             if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
                 cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
             } else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
@@ -138,6 +136,12 @@ public:
                     cpt.Y = tdata.DEC_APP;
                 }
 
+                // compute position in future
+                auto err = hardware->hardwareGetState(&hw_state);
+                if (err) {
+                    return mcc_deduce_error<error_t>(err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
+                }
+
                 MccPCMResult pcm_inv_res;
 
                 // endpoint of the mount moving
@@ -148,8 +152,12 @@ public:
 
                 // just set sideral rate once
                 mcc_tp2tp(cpt.time_point, hw_state.time_point);
-                hw_state.speedX = _currentTrackParams.trackSpeedX;
-                hw_state.speedY = _currentTrackParams.trackSpeedY;
+                {
+                    std::lock_guard lock{*_currentTrackParamsMutex};
+
+                    hw_state.speedX = _currentTrackParams.trackSpeedX;
+                    hw_state.speedY = _currentTrackParams.trackSpeedY;
+                }
                 hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
 
                 // start tracking
@@ -169,7 +177,7 @@ public:
 
                     min_time = std::chrono::duration<double>{0};
                     for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
-                        if (pz_timeto[i] <= _currentTrackParams.minTimeToPZone) {
+                        if (pz_timeto[i] < _currentTrackParams.minTimeToPZone) {
                             return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
                         }
                         if (pz_timeto[i] < min_time) {
@@ -222,11 +230,17 @@ public:
 
     error_t setTrackingParams(tracking_params_t params)
     {
+        std::lock_guard lock{*_currentTrackParamsMutex};
+
         _currentTrackParams = std::move(params);
+
+        return MccSimpleTrackingModelErrorCode::ERROR_OK;
     }
 
-    tracking_params_t getTrackingParams()
+    tracking_params_t getTrackingParams() const
     {
+        std::lock_guard lock{*_currentTrackParamsMutex};
+
         return _currentTrackParams;
     }
 
@@ -235,6 +249,7 @@ protected:
     std::unique_ptr<std::atomic_bool> _stopTracking{};
 
     tracking_params_t _currentTrackParams;
+    std::unique_ptr<std::mutex> _currentTrackParamsMutex;
 };
 
 }  // namespace mcc

mcc/mcc_traits.h

@@ -86,6 +86,9 @@ concept mcc_output_duration_range_c =
 template <typename T>
 concept mcc_is_callable = std::is_function_v<T> || (std::is_object_v<T> && requires(T) { &T::operator(); });
 
+template <typename T>
+concept mcc_callable_c = std::is_function_v<T> || (std::is_object_v<T> && requires(T) { &T::operator(); });
+
 
 // helper classes for callable signature deducing
 template <typename... Ts>