...

CMakeLists.txt

@@ -214,6 +214,7 @@ set(MCC_SRC
     include/mcc/mcc_pzone_container.h
     include/mcc/mcc_pcm.h
     include/mcc/mcc_telemetry.h
+    include/mcc/mcc_movement_controls.h
     include/mcc/mcc_serialization_common.h
     include/mcc/mcc_deserializer.h
     include/mcc/mcc_serializer.h
@@ -225,7 +226,12 @@ if(USE_SPDLOG)
 endif()
 
 if(USE_ERFA)
-    list(APPEND MCC_SRC include/mcc/mcc_ccte_iers.h include/mcc/mcc_ccte_iers_default.h include/mcc/mcc_ccte_erfa.h)
+    list(
+        APPEND MCC_SRC
+        include/mcc/mcc_ccte_iers.h
+        include/mcc/mcc_ccte_iers_default.h
+        include/mcc/mcc_ccte_erfa.h
+    )
 endif()
 
 if(USE_ASIO)
@@ -239,14 +245,10 @@ endif()
 
 if(USE_BSPLINE_PCM)
     # fitpack by P. Dierckx
-    list(
+    list(APPEND MCC_SRC include/mcc/mcc_bsplines.h)
-        APPEND MCC_SRC
-        include/mcc/mcc_bsplines.h
-    )
     add_subdirectory(fitpack)
 endif()
 
-
 add_library(${PROJECT_NAME} INTERFACE ${MCC_SRC})
 target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_23)
 target_include_directories(

include/mcc/mcc_angle.h

@@ -16,7 +16,6 @@
 #include "mcc_traits.h"
 #include "mcc_utils.h"
 
-
 /*                          HELPERS TO REPRESENT ANGLE VALUE                            */
 
 constexpr double operator""_rads(long double val)  // angle in radians (no conversion)

include/mcc/mcc_concepts.h

@@ -743,6 +743,14 @@ concept mcc_telemetry_data_c = requires(T t) {
     // target celestial point (position on sky where mount must be slewed)
     requires mcc_skypoint_c<decltype(t.targetPos)>;
 
+    // computed target encoder coordinates
+    requires(mcc_coord_pair_c<decltype(t.targetXY)> &&
+             (decltype(t.targetXY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_GENERIC ||
+              decltype(t.targetXY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_XY));
+
+    // corrections to transform observed celestial coordinates of target to hardware encoder ones
+    requires mcc_pcm_result_c<decltype(t.pcmReverseCorrection)>;
+
     // mount current celestial position
     requires mcc_skypoint_c<decltype(t.mountPos)>;
 

include/mcc/mcc_movement_controls.h

@@ -11,6 +11,22 @@
  *                                                                                      *
  ****************************************************************************************/
 
+
+
+/*
+    The user needs to accept some assumptions implemented in this class:
+
+        1) It is assumed that the implementation of the HARDWARE_T class must return movement state
+           as "HW_MOVE_STOPPED" to indicate that underlying hardware is at required position
+        2) In the case of "slew_and_stop == false" the slewing cycle waits for "HW_MOVE_STOPPED" to switch
+           to state "tracking", but the implementation of the HARDWARE_T class may return movement state
+           as "HW_MOVE_GUIDING" to indicate that underlying hardware is near required position, i.e., its
+           inner condition "hardware is at position" is true. The latter is usefull to avoid full hardware stop
+           and allows to switch to state "tracking" as soon as possible
+
+ */
+
+
 #include <atomic>
 #include <fstream>
 
@@ -261,6 +277,19 @@ public:
 
     typedef MccSimpleMovementControlsParameters movement_params_t;
 
+    template <mcc_hardware_c HARDWARE_T,
+              mcc_generic_mount_c MOUNT_T,  // MOUNT_T is mcc_telemetry_c, mcc_pzone_container_c and mcc_logger_c
+                                            // (see mcc_concepts.h)
+              mcc_mount_status_c STATUS_T,
+              std::invocable<STATUS_T const&> CallbackFuncT = decltype([](STATUS_T const&) {})>
+    MccSimpleMovementControls(
+        HARDWARE_T* hardware,
+        MOUNT_T* mount,
+        CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {})
+        : MccSimpleMovementControls(hardware, mount, mount, mode_switch_callback, *mount)
+    {
+    }
+
     template <mcc_hardware_c HARDWARE_T,
               mcc_telemetry_c TELEMETRY_T,
               mcc_pzone_container_c PZONE_CONT_T,
@@ -269,21 +298,19 @@ public:
               mcc_logger_c LOGGER_T = MccNullLogger>
     MccSimpleMovementControls(
         HARDWARE_T* hardware,
-        TELEMETRY_T* telemtry,
+        TELEMETRY_T* telemetry,
         PZONE_CONT_T* pzone_cont,
         CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {},
-        LOGGER_T& logger = details::NullLogger)
+        LOGGER_T* logger = &details::NullLogger)
     {
-        auto log_ptr = &logger;
+        auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
-
-        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(),
+                logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(),
-                                              hw_state.XY.y().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",
+                logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
-                                              MccAngle((double)hw_state.XY.x()).degrees(),
+                                             MccAngle((double)hw_state.XY.x()).degrees(),
-                                              MccAngle((double)hw_state.XY.y()).degrees()));
+                                             MccAngle((double)hw_state.XY.y()).degrees()));
             }
 
             auto hw_err = hardware->hardwareSetState(hw_state);
@@ -291,16 +318,16 @@ public:
                 mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE);
             }
 
-            log_ptr->logDebug("    the 'hardwareSetState' method performed successfully!");
+            logger->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,
+        auto check_pzones = [pzone_cont, logger](typename TELEMETRY_T::telemetry_data_t const& tdata,
-                                                  double min_time_to_pzone_in_secs, double braking_accelX,
+                                                 double min_time_to_pzone_in_secs, double braking_accelX,
-                                                  double braking_accelY) {
+                                                 double braking_accelY) -> error_t {
             bool in_zone;
             std::vector<bool> in_zone_vec;
 
@@ -328,7 +355,7 @@ public:
             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(
+            logger->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)));
 
@@ -346,8 +373,8 @@ public:
             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),
+            logger->logTrace(std::format("    mount:  speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
-                                          MccAngleFancyString(speedY)));
+                                         MccAngleFancyString(speedY)));
 
             auto pz_err = pzone_cont->inPZone(mount_pos, &in_zone, &in_zone_vec);
             if (pz_err) {
@@ -366,7 +393,7 @@ public:
                 auto it = names.begin();
                 std::ranges::advance(it, i);
 
-                log_ptr->logError(
+                logger->logError(
                     "target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
                     *it);
 
@@ -386,14 +413,14 @@ public:
                     return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
                 }
 
-                log_ptr->logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
+                logger->logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
-                                              MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
+                                             MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
-                                              MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
+                                             MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
-                log_ptr->logError(std::format("    AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
+                logger->logError(std::format("    AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
-                                              MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));
+                                             MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));
 
-                log_ptr->logError(std::format("    hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
+                logger->logError(std::format("    hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
-                                              MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
+                                             MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
 
                 return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
             }
@@ -403,61 +430,43 @@ public:
         };
 
 
-        auto log_pos = [log_ptr, this](typename TELEMETRY_T::telemetry_data_t const& tdata) {
+        auto log_pos = [logger, 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();
+            std::conditional_t<mccIsEquatorialMount(HARDWARE_T::mountType), MccSkyHADEC_OBS,
-                y_tag = hadec.y();
+                               std::conditional_t<mccIsAltAzMount(HARDWARE_T::mountType), MccSkyAZZD, std::nullptr_t>>
+                coord_pair;
 
-                log_ptr->logTrace(std::format("    current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true),
+            static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!");
-                                              hadec.y().sexagesimal()));
 
-                ccte_err = tdata.mountPos.toAtSameEpoch(hadec);
+            const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "HA"
-                if (ccte_err) {
+                                           : mccIsAltAzMount(HARDWARE_T::mountType)    ? "AZ"
-                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+                                                                                       : "GEN_X";
-                }
+            const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "DEC"
+                                           : mccIsAltAzMount(HARDWARE_T::mountType)    ? "ZD"
+                                                                                       : "GEN_Y";
+            ;
 
-                x_mnt = hadec.x();
+            auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair);
-                y_mnt = hadec.y();
+            if (ccte_err) {
-
+                return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
-
-                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()));
             }
 
+
+            x_tag = coord_pair.x();
+            y_tag = coord_pair.y();
+
+            logger->logTrace(std::format("    current target: {} = {}, {} = {}", x_str,
+                                         coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal()));
+
+            ccte_err = tdata.mountPos.toAtSameEpoch(coord_pair);
+            if (ccte_err) {
+                return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
+            }
+
+            logger->logTrace(std::format("    current mount: {} = {}, {} = {}", x_str, coord_pair.x().sexagesimal(true),
+                                         y_str, coord_pair.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";
@@ -481,6 +490,229 @@ public:
                 (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
             }
         };
+
+
+        /*    slewing function    */
+
+        _slewingFunc = [log_pos, send_to_hardware, check_pzones, cb_sptr, hardware, telemetry, logger,
+                        this](bool slew_and_stop) {
+            double braking_accelX, braking_accelY;
+            double min_time_to_pzone_in_secs;
+
+
+            {
+                // std::lock_guard lock{*_currentParamsMutex};
+                if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
+                    braking_accelX = std::numeric_limits<double>::min();
+                } else {
+                    braking_accelX = std::abs(_currentParams.brakingAccelX);
+                }
+
+                if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
+                    braking_accelY = std::numeric_limits<double>::min();
+                } else {
+                    braking_accelY = std::abs(_currentParams.brakingAccelY);
+                }
+
+                min_time_to_pzone_in_secs =
+                    std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
+
+                if (!_currentParams.slewingPathFilename.empty()) {  // open slewing trajectory file
+                    _pathFile.setFilename(_currentParams.slewingPathFilename);
+                } else {
+                    logger->logError("Slewing path filename is empty! Do not save it!");
+                }
+            }
+
+            logger->logInfo(
+                std::format("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK"));
+            logger->logInfo(std::format("    slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
+            if (!slew_and_stop) {
+                logger->logInfo(std::format("    slewing tolerance radius: {} arcsecs",
+                                            MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
+            }
+            logger->logInfo(std::format("    braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
+                                        MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
+            logger->logInfo(std::format("    braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
+                                        MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
+            logger->logInfo(std::format("    min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
+
+
+            _pathFile << "# \n";
+            _pathFile << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
+            _pathFile << "# Config:\n";
+            _pathFile << "#     slewing tolerance radius: " << MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
+                      << " arcsecs\n";
+            _pathFile << "#     slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
+            _pathFile << "# \n";
+            _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
+            _pathFile << "#     <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
+                         "<dY_{target-mount}> <moving state>\n";
+
+
+            typename TELEMETRY_T::telemetry_data_t tdata;
+            typename HARDWARE_T::hardware_state_t hw_state;
+
+
+            auto t_err = telemetry->telemetryData(&tdata);
+            if (t_err) {
+                *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
+                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
+
+                _pathFile.save();
+
+                return;
+            }
+
+
+            *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
+            if (_lastError->load()) {
+                _pathFile.save();
+
+                return;
+            }
+
+            hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
+            hw_state.XY.setX(tdata.targetXY.x());
+            hw_state.XY.setY(tdata.targetXY.y());
+
+            // start slewing ...
+
+            *_lastError = send_to_hardware(hw_state);
+            if (_lastError->load()) {
+                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
+
+                _pathFile.save();
+
+                return;
+            }
+
+
+            (*cb_sptr)(STATUS_T::MOUNT_STATUS_SLEWING);
+
+            auto start_point = tdata.hwState.XY.epoch().UTC();  // needed for trajectory file
+            auto last_hw_time = start_point;
+
+            while (!*_stopMoving) {
+                t_err = telemetry->telemetryData(&tdata);
+                if (t_err) {
+                    *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
+                    (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
+
+                    return;
+                }
+
+                log_pos(tdata);
+
+                if (*_stopMoving) {
+                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
+                    break;
+                }
+
+                *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
+                if (_lastError->load()) {
+                    break;
+                }
+
+                {
+                    std::lock_guard lock{*_currentParamsMutex};
+
+                    if ((std::chrono::steady_clock::now() - start_point) > _currentParams.slewTimeout) {
+                        logger->logError("slewing process timeout!");
+                        *_lastError = MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT;
+                        break;
+                    }
+                }
+
+
+                if (slew_and_stop) {  // just wait until the mount stops
+                    if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
+                        logger->logInfo("mount movement state is STOPPED! Exit from slewing process!");
+                        break;
+                    }
+                } else {
+                    if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
+                        logger->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
+                        continue;
+                    }
+
+                    last_hw_time = tdata.hwState.XY.epoch().UTC();
+
+                    auto dist = utils::distanceOnSphere(tdata.targetPos.x(), tdata.targetPos.y(), tdata.mountPos.x(),
+                                                        tdata.mountPos.y());
+
+                    logger->logTrace(std::format("    target-to-mount distance: {} (dx = {}, dy = {})",
+                                                 MccAngleFancyString(std::get<2>(dist)), std::get<0>(dist),
+                                                 std::get<1>(dist)));
+
+
+                    // stop slewing and exit from the cycle?
+                    if (dist <= _currentParams.slewToleranceRadius) {
+                        if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED ||
+                            tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_GUIDING) {
+                            logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");
+
+                            break;
+                        } else {
+                            logger->logDebug(
+                                "target-to-mount distance is lesser than slew acceptable radius but hardware is still "
+                                "not ready!");
+                        }
+                    }
+
+
+                    if (*_stopMoving) {
+                        *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
+                        break;
+                    }
+
+
+                    // resend new position since target coordinates are changed in time
+                    hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
+                    hw_state.XY.setX(tdata.targetXY.x());
+                    hw_state.XY.setY(tdata.targetXY.y());
+
+                    *_lastError = send_to_hardware(hw_state);
+                    if (_lastError->load()) {
+                        break;
+                    }
+                }
+            }
+
+            *_stopMoving = true;
+
+            logger->logInfo("Slewing finished");
+            auto err = _lastError->load();
+            logger->logInfo(std::format("    exit code: {} {} {}", err.value(), err.category().name(), err.message()));
+
+            _pathFile.save();
+
+            // get final position
+
+            if (!err) {
+                t_err = telemetry->telemetryData(&tdata);
+                if (t_err) {
+                    *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
+                    (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
+
+                    return;
+                }
+
+                auto dist = utils::distanceOnSphere(tdata.targetPos.x(), tdata.targetPos.y(), tdata.mountPos.x(),
+                                                    tdata.mountPos.y());
+
+                log_pos(tdata);
+
+                logger->logDebug(
+                    std::format("    target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist))));
+
+                if (!slew_and_stop) {  // start tracking
+                    _trackingFunc();
+                } else {
+                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
+                }
+            }
+        };
     }
 
 

include/mcc/mcc_pcm.h

@@ -318,7 +318,8 @@ public:
         ret = _compResult(x, y, res, true);
         if (!ret) {
             if constexpr (mcc_coord_pair_c<T>) {
-                *hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY};
+                *hw_pt =
+                    MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY, obs_skycoord.epoch()};
             }
         }
 
@@ -394,11 +395,10 @@ private:
         bspline = inverse ? &_pcmData.inverseBspline : &_pcmData.bspline;
 #endif
 
-        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY
 #ifdef USE_BSPLINE_PCM
-            || _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE
+        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY ||
+            _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
 #endif
-        ) {
             const auto tanY = std::tan(y);
             const auto sinX = std::sin(x);
             const auto cosX = std::cos(x);
@@ -422,7 +422,9 @@ private:
                     res->pcmY += geom_coeffs->forkFlexure / cosX;
                 }
             }
+#ifdef USE_BSPLINE_PCM
         }
+#endif
 
 
 #ifdef USE_BSPLINE_PCM

include/mcc/mcc_telemetry.h

@@ -115,6 +115,12 @@ public:
     struct telemetry_data_t {
         MccSkyPoint targetPos{};
 
+        MccGenXY targetXY{};
+
+        struct {
+            double pcmX{}, pcmY{};
+        } pcmReverseCorrection{};
+
         MccSkyPoint mountPos{};
 
         typename HARDWARE_T::hardware_state_t hwState{};
@@ -229,14 +235,44 @@ public:
                                                     mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                                             }
 
+                                            _tdataPtr->pcmReverseCorrection.pcmX = -pcm_corr.pcmX;
+                                            _tdataPtr->pcmReverseCorrection.pcmY = -pcm_corr.pcmY;
                                         } else {
                                             _lastUpdateError =
                                                 mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
                                         }
-                                    } else {  // observed, apparent or ICRS
-                                        _tdataPtr->targetPos = _enteredTargetPos;
-                                    }
 
+                                    } else {  // observed, apparent or ICRS
+                                        // _tdataPtr->targetPos = _enteredTargetPos;
+                                        using pcm_t = std::remove_pointer_t<decltype(pcm_ptr)>;
+
+                                        std::conditional_t<mcc_is_equatorial_mount<pcm_t::mountType>, MccSkyHADEC_OBS,
+                                                           std::conditional_t<mcc_is_altaz_mount<pcm_t::mountType>,
+                                                                              MccSkyAZZD, std::nullptr_t>>
+                                            cp;
+
+                                        static_assert(!std::is_null_pointer_v<decltype(cp)>, "UNKNOW MOUNT TYPE!");
+
+                                        // calculate target celestial coordinates (of the same type as .mountPos) at the
+                                        // epoch of the current mount position
+                                        cp.setEpoch(_tdataPtr->mountPos.epoch());
+                                        auto ccte_err = _enteredTargetPos.to(cp);
+                                        if (ccte_err) {
+                                            _lastUpdateError =
+                                                mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
+                                        } else {
+                                            _tdataPtr->targetPos.from(cp);
+
+                                            // calculate reverse PCM corrections for the current target position and
+                                            // its encoder XY
+                                            auto pcm_err = pcm_ptr->pcmReverseCorrection(
+                                                _tdataPtr->targetPos, &_tdataPtr->pcmReverseCorrection,
+                                                &_tdataPtr->targetXY);
+                                            if (pcm_err) {
+                                                return mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
+                                            }
+                                        }
+                                    }
                                 } else {
                                     _lastUpdateError = mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                                 }
@@ -321,6 +357,7 @@ public:
         return _lastUpdateError;
     }
 
+
     //
     // Set a timeout for the telemetry receiving process
     //

include/mcc/mcc_utils.h

@@ -376,6 +376,33 @@ std::pair<double, double> parseAnglePair(R&& str, bool hms1 = false, bool hms2 =
 }
 
 
+// calculate distance between two point on sphere:
+//     the function returns a std::tuple with
+//         0th element: difference along co-longitude axis
+//         1st element: difference along co-latitude axis
+//         2nd element: distance
+
+std::tuple<double, double, double> distanceOnSphere(double co_lon1, double co_lat1, double co_lon2, double co_lat2)
+{
+    std::tuple<double, double, double> res{};
+
+    std::get<0>(res) = co_lon1 - co_lon2;
+    std::get<1>(res) = co_lat1 - co_lat2;
+
+    double cosDco_lon = cos(std::get<0>(res));
+    double cos1 = cos(co_lat1);
+    double sin1 = sin(co_lat1);
+    double cos2 = cos(co_lat2);
+    double sin2 = sin(co_lat2);
+
+    double term1 = cos1 * sin(std::get<0>(res));
+    double term2 = cos2 * sin1 - sin2 * cos1 * cosDco_lon;
+
+    std::get<2>(res) = atan2(sqrt(term1 * term1 + term2 * term2), sin2 * sin1 + cos2 * cos1 * cosDco_lon);
+
+    return res;
+}
+
 template <traits::mcc_input_char_range R>
 static constexpr size_t FNV1aHash(const R& r)
 {