From 26011e7630fc0694333e99a0e6a9dd01cf65212f Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Mon, 16 Feb 2026 18:38:59 +0300 Subject: [PATCH] ... --- CMakeLists.txt | 14 +- include/mcc/mcc_angle.h | 1 - include/mcc/mcc_concepts.h | 8 + include/mcc/mcc_movement_controls.h | 378 ++++++++++++++++++++++------ include/mcc/mcc_pcm.h | 10 +- include/mcc/mcc_telemetry.h | 43 +++- include/mcc/mcc_utils.h | 27 ++ 7 files changed, 394 insertions(+), 87 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aafdb56..5dee43d 100644 --- a/CMakeLists.txt +++ b/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( - APPEND MCC_SRC - include/mcc/mcc_bsplines.h - ) + list(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( diff --git a/include/mcc/mcc_angle.h b/include/mcc/mcc_angle.h index 7309080..068dd65 100644 --- a/include/mcc/mcc_angle.h +++ b/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) diff --git a/include/mcc/mcc_concepts.h b/include/mcc/mcc_concepts.h index 5501589..aee7ad4 100644 --- a/include/mcc/mcc_concepts.h +++ b/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; + // computed target encoder coordinates + requires(mcc_coord_pair_c && + (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; + // mount current celestial position requires mcc_skypoint_c; diff --git a/include/mcc/mcc_movement_controls.h b/include/mcc/mcc_movement_controls.h index 18aa311..2b4c44c 100644 --- a/include/mcc/mcc_movement_controls.h +++ b/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 #include @@ -261,6 +277,19 @@ public: typedef MccSimpleMovementControlsParameters movement_params_t; + template 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 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, log_ptr](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t { + auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t { if constexpr (std::derived_from>) { - log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(), - hw_state.XY.y().degrees())); + logger->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())); + logger->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); @@ -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, - double min_time_to_pzone_in_secs, double braking_accelX, - double braking_accelY) { + 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 braking_accelY) -> error_t { bool in_zone; std::vector 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), - MccAngleFancyString(speedY))); + logger->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) { @@ -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: {}, {}, {}, {}", - 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())); + logger->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))); + logger->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())); + logger->logError(std::format(" hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.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(); - y_tag = hadec.y(); + std::conditional_t> + coord_pair; - log_ptr->logTrace(std::format(" current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true), - hadec.y().sexagesimal())); + static_assert(!std::is_null_pointer_v, "UNKNOWN MOUNT TYPE!"); - ccte_err = tdata.mountPos.toAtSameEpoch(hadec); - if (ccte_err) { - return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - } + const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "HA" + : mccIsAltAzMount(HARDWARE_T::mountType) ? "AZ" + : "GEN_X"; + const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "DEC" + : mccIsAltAzMount(HARDWARE_T::mountType) ? "ZD" + : "GEN_Y"; + ; - 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())); + auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); } + + 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::min(); + } else { + braking_accelX = std::abs(_currentParams.brakingAccelX); + } + + if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) { + braking_accelY = std::numeric_limits::min(); + } else { + braking_accelY = std::abs(_currentParams.brakingAccelY); + } + + min_time_to_pzone_in_secs = + std::chrono::duration_cast>(_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 << "# " + " \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; + } + } + }; } diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index f6d0afe..51bfca7 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -318,7 +318,8 @@ public: ret = _compResult(x, y, res, true); if (!ret) { if constexpr (mcc_coord_pair_c) { - *hw_pt = MccCoordPair{x + res->pcmX, y + res->pcmY}; + *hw_pt = + MccCoordPair{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 diff --git a/include/mcc/mcc_telemetry.h b/include/mcc/mcc_telemetry.h index 333fcab..0e5d526 100644 --- a/include/mcc/mcc_telemetry.h +++ b/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; + + std::conditional_t, MccSkyHADEC_OBS, + std::conditional_t, + MccSkyAZZD, std::nullptr_t>> + cp; + + static_assert(!std::is_null_pointer_v, "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 // diff --git a/include/mcc/mcc_utils.h b/include/mcc/mcc_utils.h index 7b0da5c..51bab6d 100644 --- a/include/mcc/mcc_utils.h +++ b/include/mcc/mcc_utils.h @@ -376,6 +376,33 @@ std::pair 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 distanceOnSphere(double co_lon1, double co_lat1, double co_lon2, double co_lat2) +{ + std::tuple 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 static constexpr size_t FNV1aHash(const R& r) {