...

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