...

include/mcc/mcc_concepts.h

@@ -803,7 +803,7 @@ struct mcc_telemetry_interface_t {
 
     // get entered target position
     template <std::derived_from<mcc_telemetry_interface_t> SelfT>
-    RetT setTarget(this SelfT&& self, mcc_skypoint_c auto* pt)
+    RetT getTarget(this SelfT&& self, mcc_skypoint_c auto* pt)
     {
         return std::forward<SelfT>(self).getTarget(pt);
     }
@@ -927,9 +927,14 @@ struct mcc_pzone_container_interface_t {
 
 
 template <typename T>
-concept mcc_pzone_container_c = std::derived_from<T, mcc_pzone_container_interface_t<typename T::error_t>> && requires {
+concept mcc_pzone_container_c =
+    std::derived_from<T, mcc_pzone_container_interface_t<typename T::error_t>> && requires(const T t_const) {
         // error type
         requires mcc_error_c<typename T::error_t>;
+
+        []<std::ranges::range R>(R const&) {
+            return requires { requires std::formattable<std::ranges::range_value_t<R>, char>; };
+        }(t_const.pzoneNames());
     };
 
 

include/mcc/mcc_movement_controls.h

@@ -0,0 +1,550 @@
+#pragma once
+
+
+
+/****************************************************************************************
+ *                                                                                      *
+ *                          MOUNT CONTROL COMPONENTS LIBRARY                            *
+ *                                                                                      *
+ *                                                                                      *
+ *             IMPLEMENTATION OF VERY SIMPLE MOUNT MOVEMENT CONTROLS CLASS              *
+ *                                                                                      *
+ ****************************************************************************************/
+
+#include <atomic>
+#include <fstream>
+
+#include "mcc/mcc_coordinate.h"
+#include "mcc_concepts.h"
+#include "mcc_error.h"
+
+namespace mcc::impl
+{
+
+namespace details
+{
+// just auxiliary class instance
+static MccNullLogger NullLogger{};
+}  // namespace details
+
+
+enum class MccSimpleMovementControlsErrorCode : int {
+    ERROR_OK,
+    ERROR_HW_GETSTATE,
+    ERROR_HW_SETSTATE,
+    ERROR_PCM_COMP,
+    ERROR_CCTE_COMP,
+    ERROR_GET_TELEMETRY,
+    ERROR_DIST_TELEMETRY,
+    ERROR_PZONE_CONTAINER_COMP,
+    ERROR_TARGET_IN_PZONE,
+    ERROR_NEAR_PZONE,
+    ERROR_TIMEOUT,
+    ERROR_ALREADY_SLEW,
+    ERROR_ALREADY_STOPPED,
+    ERROR_STOPPED
+};
+
+}  // namespace mcc::impl
+
+
+namespace std
+{
+
+template <>
+class is_error_code_enum<mcc::impl::MccSimpleMovementControlsErrorCode> : public true_type
+{
+};
+
+}  // namespace std
+
+
+
+namespace mcc::impl
+{
+
+// error category
+struct MccSimpleMovementControlsCategory : public std::error_category {
+    MccSimpleMovementControlsCategory() : std::error_category() {}
+
+    const char* name() const noexcept
+    {
+        return "SIMPLE-SLEWING-MODEL";
+    }
+
+    std::string message(int ec) const
+    {
+        MccSimpleMovementControlsErrorCode err = static_cast<MccSimpleMovementControlsErrorCode>(ec);
+
+        switch (err) {
+            case MccSimpleMovementControlsErrorCode::ERROR_OK:
+                return "OK";
+            case MccSimpleMovementControlsErrorCode::ERROR_HW_GETSTATE:
+                return "cannot get hardware state";
+            case MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE:
+                return "cannot set hardware state";
+            case MccSimpleMovementControlsErrorCode::ERROR_PCM_COMP:
+                return "PCM computation error";
+            case MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP:
+                return "celestial coordinate transformation error";
+            case MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY:
+                return "cannot get telemetry";
+            case MccSimpleMovementControlsErrorCode::ERROR_DIST_TELEMETRY:
+                return "cannot get target-to-mount-position distance";
+            case MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP:
+                return "pzone container computation error";
+            case MccSimpleMovementControlsErrorCode::ERROR_TARGET_IN_PZONE:
+                return "target is in prohibited zone";
+            case MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE:
+                return "near prohibited zone";
+            case MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT:
+                return "a timeout occured while slewing";
+            case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_SLEW:
+                return "already slewing";
+            case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED:
+                return "slewing is already stopped";
+            case MccSimpleMovementControlsErrorCode::ERROR_STOPPED:
+                return "slewing was stopped";
+            default:
+                return "UNKNOWN";
+        }
+    }
+
+    static const MccSimpleMovementControlsCategory& get()
+    {
+        static const MccSimpleMovementControlsCategory constInst;
+        return constInst;
+    }
+};
+
+
+inline std::error_code make_error_code(MccSimpleMovementControlsErrorCode ec)
+{
+    return std::error_code(static_cast<int>(ec), MccSimpleMovementControlsCategory::get());
+}
+
+
+
+struct MccSimpleMovementControlsParameters {
+    // *******    common for all modes    *******
+
+    // mean celestial rate
+    static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second
+
+    // timeout to telemetry updating
+    std::chrono::milliseconds telemetryTimeout{3000};
+
+    // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
+    std::chrono::seconds minTimeToPZone{10};
+
+    // time interval to update prohibited zones related quantities (e.g. intersection points)
+    std::chrono::milliseconds updatingPZoneInterval{5000};
+
+
+    // *******    slewing mode    *******
+
+    bool slewAndStop{false};  // slew to target and stop mount
+
+    // coordinates difference to stop slewing (in radians)
+    double slewToleranceRadius{5.0_arcsecs};
+
+    // telemetry request interval
+    std::chrono::milliseconds slewingTelemetryInterval{100};
+
+    // target-mount coordinate difference to start adjusting of slewing (in radians)
+    double adjustCoordDiff{slewToleranceRadius * 10.0};
+
+    // slew process timeout
+    std::chrono::seconds slewTimeout{3600};
+
+    double slewRateX{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
+    double slewRateY{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
+
+    std::chrono::milliseconds adjustCycleInterval{500};  // minimum time between two successive adjustments
+
+    double adjustRateX{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+    double adjustRateY{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+
+    // braking acceleration after execution of mount stopping command (in rads/s^2)
+    // it must be given as non-negative value!!!
+    double brakingAccelX{0.0};
+    double brakingAccelY{0.0};
+
+    // slewing trajectory file. if empty - just skip saving
+    std::string slewingPathFilename{};
+    // *******    tracking mode    *******
+
+    // telemetry request interval
+    std::chrono::milliseconds trackingTelemetryInterval{100};
+
+    double trackSpeedX{};
+    double trackSpeedY{};
+    std::chrono::milliseconds trackingCycleInterval{500};  // minimum time between two successive tracking corrections
+    bool dualAxisTracking{true};  // mount must be of an equatorial type: false means guiding along only HA-axis
+
+    // time shift into future to compute target position in future (UT1-scale time duration)
+    std::chrono::milliseconds timeShiftToTargetPoint{10000};
+    // maximal target-to-mount difference for tracking process (in arcsecs)
+    // it it is greater then the current mount coordinates are used as target one
+    double trackingMaxCoordDiff{20.0};
+
+    // tracking trajectory file. if empty - just skip saving
+    std::string trackingPathFilename{};
+};
+
+
+
+class MccSimpleMovementControls
+{
+protected:
+    // a simple class for movement path saving
+    class PathFile
+    {
+    public:
+        PathFile(const std::string& filename = "") : _filename(filename), _st() {}
+
+        void setFilename(const std::string& filename)
+        {
+            _filename = filename;
+        }
+
+        std::string getFilename() const
+        {
+            return _filename;
+        }
+
+        ~PathFile()
+        {
+            save();
+        }
+
+        friend PathFile& operator<<(PathFile& pf, auto&& v)
+        {
+            pf._st << std::forward<decltype(v)>(v);
+
+            return pf;
+        }
+
+        bool save()
+        {
+            std::fstream fst;
+
+            if (_filename.empty()) {
+                return false;
+            }
+
+            if (_st.str().empty()) {  // nothing to save
+                return true;
+            }
+
+            fst.open(_filename);
+
+            if (!fst.is_open()) {
+                return false;
+            }
+
+            fst << _st.str();
+            _st.str("");
+            _filename.clear();
+
+            return true;
+        }
+
+    private:
+        std::string _filename;
+        std::istringstream _st;
+    };
+
+
+public:
+    typedef MccError error_t;
+
+    typedef MccSimpleMovementControlsParameters movement_params_t;
+
+    template <mcc_hardware_c HARDWARE_T,
+              mcc_telemetry_c TELEMETRY_T,
+              mcc_pzone_container_c PZONE_CONT_T,
+              mcc_mount_status_c STATUS_T,
+              std::invocable<STATUS_T const&> CallbackFuncT = decltype([](STATUS_T const&) {}),
+              mcc_logger_c LOGGER_T = MccNullLogger>
+    MccSimpleMovementControls(
+        HARDWARE_T* hardware,
+        TELEMETRY_T* telemtry,
+        PZONE_CONT_T* pzone_cont,
+        CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {},
+        LOGGER_T& logger = details::NullLogger)
+    {
+        auto log_ptr = &logger;
+
+        auto send_to_hardware = [hardware, log_ptr](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
+            if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) {
+                log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(),
+                                              hw_state.XY.y().degrees()));
+            } else {  // user defined mcc_coord_pair_c
+                log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
+                                              MccAngle((double)hw_state.XY.x()).degrees(),
+                                              MccAngle((double)hw_state.XY.y()).degrees()));
+            }
+
+            auto hw_err = hardware->hardwareSetState(hw_state);
+            if (hw_err) {
+                mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE);
+            }
+
+            log_ptr->logDebug("    the 'hardwareSetState' method performed successfully!");
+
+
+            return MccSimpleMovementControlsErrorCode::ERROR_OK;
+        };
+
+
+        auto check_pzones = [pzone_cont, log_ptr](typename TELEMETRY_T::telemetry_data_t const& tdata,
+                                                  double min_time_to_pzone_in_secs, double braking_accelX,
+                                                  double braking_accelY) {
+            bool in_zone;
+            std::vector<bool> in_zone_vec;
+
+            // calculate the distances along the X and Y axes that the mount will travel at the current speed in a given
+            // time, taking into account the braking acceleration
+            double speedX = tdata.hwState.speedXY.x();
+            double speedY = tdata.hwState.speedXY.y();
+
+            // time to stop mount with given current speed and constant braking acceleration
+            double tx_stop = std::abs(speedX) / braking_accelX;
+            double ty_stop = std::abs(speedY) / braking_accelY;
+
+            double tx = min_time_to_pzone_in_secs;
+            double ty = min_time_to_pzone_in_secs;
+
+            if (std::isfinite(tx_stop) && (min_time_to_pzone_in_secs > tx_stop)) {
+                tx = tx_stop;
+            }
+            if (std::isfinite(ty_stop) && (min_time_to_pzone_in_secs > ty_stop)) {
+                ty = ty_stop;
+            }
+
+            // the distance:
+            // here, one must take into account the sign of the speed!!!
+            double dx = speedX * tx - std::copysign(braking_accelX, speedX) * tx * tx / 2.0;
+            double dy = speedY * ty - std::copysign(braking_accelY, speedY) * ty * ty / 2.0;
+
+            log_ptr->logTrace(
+                std::format("    the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
+                            min_time_to_pzone_in_secs, MccAngleFancyString(dx), MccAngleFancyString(dy)));
+
+
+            // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
+            // and check them for getting into the prohibited zones
+
+            std::conditional_t<mccIsEquatorialMount(HARDWARE_T::mountType), MccSkyHADEC_OBS,
+                               std::conditional_t<mccIsAltAzMount(HARDWARE_T::mountType), MccSkyAZZD, std::nullptr_t>>
+                mount_pos;
+
+            static_assert(!std::is_null_pointer_v<decltype(mount_pos)>, "UNKNOWn MOUNT TYPE");
+
+            mount_pos.setX(tdata.hwState.XY.x() + tdata.pcmCorrection.x + dx);
+            mount_pos.setY(tdata.hwState.XY.y() + tdata.pcmCorrection.y + dy);
+            mount_pos.setEpoch(tdata.hwState.XY.epoch());
+
+            log_ptr->logTrace(std::format("    mount:  speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
+                                          MccAngleFancyString(speedY)));
+
+            auto pz_err = pzone_cont->inPZone(mount_pos, &in_zone, &in_zone_vec);
+            if (pz_err) {
+                return mcc_deduced_err(pz_err, MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
+            }
+
+            if (in_zone) {
+                size_t i = 0;
+                for (; i < in_zone_vec.size(); ++i) {
+                    if (in_zone_vec[i]) {
+                        break;
+                    }
+                }
+
+                auto names = pzone_cont->pzoneNames();
+                auto it = names.begin();
+                std::ranges::advance(it, i);
+
+                log_ptr->logError(
+                    "target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
+                    *it);
+
+                MccSkyHADEC_OBS hadec;
+                MccSkyRADEC_OBS radec;
+                MccSkyAZZD azzd;
+                MccSkyAZALT azalt;
+                MccAngle lst;
+
+                auto ccte_err = tdata.mountPos.appSideralTime(&lst, true);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                ccte_err = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                log_ptr->logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
+                                              MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
+                                              MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
+                log_ptr->logError(std::format("    AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
+                                              MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));
+
+                log_ptr->logError(std::format("    hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
+                                              MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
+
+                return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
+            }
+
+
+            return MccSimpleMovementControlsErrorCode::ERROR_OK;
+        };
+
+
+        auto log_pos = [log_ptr, this](typename TELEMETRY_T::telemetry_data_t const& tdata) {
+            double x_mnt, y_mnt, x_tag, y_tag;
+            if constexpr (mccIsEquatorialMount(HARDWARE_T::mountType)) {
+                MccSkyHADEC_OBS hadec;
+                auto ccte_err = tdata.targetPos.toAtSameEpoch(hadec);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                x_tag = hadec.x();
+                y_tag = hadec.y();
+
+                log_ptr->logTrace(std::format("    current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true),
+                                              hadec.y().sexagesimal()));
+
+                ccte_err = tdata.mountPos.toAtSameEpoch(hadec);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                x_mnt = hadec.x();
+                y_mnt = hadec.y();
+
+
+                log_ptr->logTrace(std::format("    current mount:  HA = {}, DEC = {}", hadec.x().sexagesimal(true),
+                                              hadec.y().sexagesimal()));
+
+
+            } else if constexpr (mccIsAltAzMount(HARDWARE_T::mountType)) {
+                MccSkyAZZD azzd;
+
+                auto ccte_err = tdata.targetPos.toAtSameEpoch(azzd);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                x_tag = azzd.x();
+                y_tag = azzd.y();
+
+                log_ptr->logTrace(
+                    std::format("    target: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal()));
+
+
+                ccte_err = tdata.mountPos.toAtSameEpoch(azzd);
+                if (ccte_err) {
+                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                }
+
+                x_mnt = azzd.x();
+                y_mnt = azzd.y();
+
+                log_ptr->logTrace(
+                    std::format("    mount: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal()));
+            }
+
+            _pathFile << tdata.mountPos.epoch().UTC.time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
+                      << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " "
+                      << (int)tdata.hwState.movementState << "\n";
+        };
+
+        auto cb_sptr =
+            std::make_shared<std::function<void(STATUS_T const&)>>(std::forward<CallbackFuncT>(mode_switch_callback));
+
+
+        /*  stop moving function */
+
+        _stopMovingFunc = [hardware, cb_sptr, this]() {
+            typename HARDWARE_T::hardware_state_t hw_state;
+            hw_state.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPING;
+
+            *_stopMoving = true;
+
+            *_lastError = send_to_hardware(hw_state);
+
+            if (_lastError->load()) {
+                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
+            }
+        };
+    }
+
+
+    error_t slewToTarget(bool slew_and_stop = false)
+    {
+        _slewingFunc(slew_and_stop);
+
+        return *_lastError;
+    }
+
+    error_t trackTarget()
+    {
+        _trackingFunc();
+
+        return *_lastError;
+    }
+
+
+    error_t stopMount()
+    {
+        if (*_stopMoving) {
+            *_lastError = MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED;
+        } else {
+            _stopMovingFunc();
+        }
+
+        return *_lastError;
+    }
+
+    error_t setMovementParams(movement_params_t const& params)
+    {
+        std::lock_guard lock{*_currentParamsMutex};
+
+        _currentParams = params;
+
+        return MccSimpleMovementControlsErrorCode::ERROR_OK;
+    }
+
+    movement_params_t getMovementParams() const
+    {
+        std::lock_guard lock{*_currentParamsMutex};
+
+        return _currentParams;
+    }
+
+    error_t mountMovementLastError() const
+    {
+        return _lastError->load();
+    }
+
+protected:
+    std::function<void(bool)> _slewingFunc{};
+    std::function<void()> _trackingFunc{};
+    std::function<void()> _stopMovingFunc{};
+
+    std::unique_ptr<std::atomic_bool> _stopMoving{new std::atomic_bool{false}};
+
+    std::unique_ptr<std::mutex> _currentParamsMutex{new std::mutex{}};
+    movement_params_t _currentParams{};
+
+    std::unique_ptr<std::atomic<error_t>> _lastError{
+        new std::atomic<error_t>{MccSimpleMovementControlsErrorCode::ERROR_OK}};
+
+    PathFile _pathFile{};
+};
+
+}  // namespace mcc::impl

include/mcc/mcc_pzone_container.h

@@ -144,6 +144,8 @@ public:
             return MccPZoneContainerErrorCode::ERROR_OK;
         });
 
+        _names.push_back(std::format("{}", zone.pzoneName));
+
         return _inZoneFunc.size();
     }
 
@@ -152,9 +154,16 @@ public:
         _inZoneFunc.clear();
         _timeToZoneFunc.clear();
         _timeFromZoneFunc.clear();
+
+        _names.clear();
     }
 
 
+    std::vector<std::string> pzoneNames() const
+    {
+        return _names;
+    }
+
     error_t inPZone(mcc_skypoint_c auto const& coords, bool* at_least_one, std::ranges::output_range<bool> auto* result)
     {
         auto err = forEach(_inZoneFunc, coords, result);
@@ -188,9 +197,11 @@ public:
 protected:
     typedef std::chrono::nanoseconds duration_t;
 
-    std::vector<std::function<error_t(MccSkyPoint const& pt, bool*)>> _inZoneFunc;
+    std::vector<std::function<error_t(MccSkyPoint const& pt, bool*)>> _inZoneFunc{};
-    std::vector<std::function<error_t(MccSkyPoint const& pt, duration_t*)>> _timeToZoneFunc;
+    std::vector<std::function<error_t(MccSkyPoint const& pt, duration_t*)>> _timeToZoneFunc{};
-    std::vector<std::function<error_t(MccSkyPoint const& pt, duration_t*)>> _timeFromZoneFunc;
+    std::vector<std::function<error_t(MccSkyPoint const& pt, duration_t*)>> _timeFromZoneFunc{};
+
+    std::vector<std::string> _names{};
 
     error_t forEach(auto& func_cont, MccSkyPoint const& pt, auto* result)
     {
@@ -205,7 +216,7 @@ protected:
 
         res_elem_t res_elem;
 
-        size_t res_sz = std::ranges::size(*result);
+        // size_t res_sz = std::ranges::size(*result);
         auto it = result->begin();
 
         for (auto& func : func_cont) {