From 889d4ff3b2c7c07a547e854c540accbe08a8e50c Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Wed, 18 Feb 2026 16:22:58 +0300 Subject: [PATCH] ... --- include/mcc/mcc_concepts.h | 10 +- include/mcc/mcc_generic_mount.h | 2 +- include/mcc/mcc_movement_controls.h | 178 ++++++++++++++++++++++++++-- include/mcc/mcc_pcm.h | 8 +- include/mcc/mcc_serializer.h | 2 +- include/mcc/mcc_telemetry.h | 17 +-- tests/mcc_telemetry_test.cpp | 2 +- 7 files changed, 189 insertions(+), 30 deletions(-) diff --git a/include/mcc/mcc_concepts.h b/include/mcc/mcc_concepts.h index 24a4974..ba4df47 100644 --- a/include/mcc/mcc_concepts.h +++ b/include/mcc/mcc_concepts.h @@ -560,9 +560,11 @@ concept mcc_pcm_c = std::derived_from; // the 'T' class must contain static constexpr member of 'MccMountType' type - requires std::same_as; + requires std::same_as; + // requires std::same_as; []() { - [[maybe_unused]] static constexpr MccMountType val = T::mountType; + // [[maybe_unused]] static constexpr MccMountType val = T::mountType; + [[maybe_unused]] static constexpr MccMountType val = T::pcmMountType; }(); // to ensure 'mountType' can be used in compile-time context // static const variable with name of PCM @@ -664,9 +666,9 @@ concept mcc_hardware_c = requires(T t) { // the 'T' class must contain static constexpr member of 'MccMountType' type - requires std::same_as; + requires std::same_as; []() { - [[maybe_unused]] static constexpr MccMountType val = T::mountType; + [[maybe_unused]] static constexpr MccMountType val = T::hwMountType; }(); // to ensure 'mountType' can be used in compile-time context diff --git a/include/mcc/mcc_generic_mount.h b/include/mcc/mcc_generic_mount.h index 3351945..6afe8b9 100644 --- a/include/mcc/mcc_generic_mount.h +++ b/include/mcc/mcc_generic_mount.h @@ -258,7 +258,7 @@ protected: std::string formatError(error_t const& err, std::string_view prefix = "") const { - return std::format("{}{} (category: {}, code: {})", prefix, err.message(), err.value(), err.category().name()); + return std::format("{}{} (category: {}, code: {})", prefix, err.message(), err.category().name(), err.value()); } }; diff --git a/include/mcc/mcc_movement_controls.h b/include/mcc/mcc_movement_controls.h index 0dfa5fd..044e3f5 100644 --- a/include/mcc/mcc_movement_controls.h +++ b/include/mcc/mcc_movement_controls.h @@ -29,6 +29,7 @@ #include #include +#include #include "mcc/mcc_coordinate.h" #include "mcc_concepts.h" @@ -370,8 +371,8 @@ public: // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead // and check them for getting into the prohibited zones - std::conditional_t> + std::conditional_t> mount_pos; static_assert(!std::is_null_pointer_v, "UNKNOWn MOUNT TYPE"); @@ -440,18 +441,18 @@ public: auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata) -> error_t { double x_mnt, y_mnt, x_tag, y_tag; - std::conditional_t> + std::conditional_t> coord_pair; static_assert(!std::is_null_pointer_v, "UNKNOWN MOUNT TYPE!"); - 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"; + const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "HA" + : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ" + : "GEN_X"; + const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC" + : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ZD" + : "GEN_Y"; ; auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair); @@ -529,7 +530,7 @@ public: 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->logWarn("Slewing path filename is empty! Do not save it!"); } } @@ -602,7 +603,7 @@ public: auto start_point = std::chrono::steady_clock::now(); auto last_hw_time = tdata.hwState.XY.epoch().UTC(); - mcc_deduced_coord_pair_t tag_cp, mnt_cp; + mcc_deduced_coord_pair_t tag_cp, mnt_cp; while (!*_stopMoving) { t_err = telemetry->telemetryData(&tdata); @@ -695,6 +696,9 @@ public: if (_lastError->load()) { break; } + + // sleep here + std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval); } } @@ -720,11 +724,15 @@ public: auto ccte_err = tdata.targetPos.to(tag_cp); if (ccte_err) { *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); + return; } ccte_err = tdata.mountPos.to(mnt_cp); if (ccte_err) { *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); + return; } @@ -742,6 +750,152 @@ public: } } }; + + + _trackingFunc = [telemetry, cb_sptr, logger, this]() { + 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.trackingPathFilename.empty()) { // open slewing trajectory file + _pathFile.setFilename(_currentParams.trackingPathFilename); + } else { + logger->logWarn("Tracking path filename is empty! Do not save it!"); + } + } + + logger->logInfo("Start tracking"); + 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 << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\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_TRACKING; + + auto last_hw_time = tdata.hwState.XY.epoch().UTC(); + + mcc_deduced_coord_pair_t tag_cp, mnt_cp; + + + while (!_stopMoving->load()) { + 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; + } + + + 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 ccte_err = tdata.targetPos.to(tag_cp); + if (ccte_err) { + *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + ccte_err = tdata.mountPos.to(mnt_cp); + if (ccte_err) { + *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); + + logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})", + MccAngleFancyString(std::get<2>(dist)), std::get<0>(dist), + std::get<1>(dist))); + + // resend new position since target coordinates are changed in time + hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING; + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); + + *_lastError = send_to_hardware(hw_state); + if (_lastError->load()) { + break; + } + + if (*_stopMoving) { + *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED; + break; + } + + // sleep here + std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval); + } + + *_stopMoving = true; + + logger->logInfo("Tracking finished"); + auto err = _lastError->load(); + logger->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); + + _pathFile.save(); + }; } virtual ~MccSimpleMovementControls() = default; diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index b20345b..e2e0cc4 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -37,7 +37,7 @@ struct MccDefaultPCMCategory : public std::error_category { const char* name() const noexcept { - return "ADC_GENERIC_DEVICE"; + return "MCC-DEFAULT-PCM-ERROR-CATEGORY"; } std::string message(int ec) const @@ -112,7 +112,8 @@ template class MccDefaultPCM : public mcc_pcm_interface_t { public: - static constexpr MccMountType mountType = MOUNT_TYPE; + // static constexpr MccMountType mountType = MOUNT_TYPE; + static constexpr MccMountType pcmMountType = MOUNT_TYPE; #ifdef USE_BSPLINE_PCM static constexpr std::string_view pcmName{"MCC-GEOMETRY-BSPLINES-PCM"}; @@ -417,7 +418,8 @@ private: res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX + geom_coeffs->tubeFlexure * (_cosPhi * cosX * std::sin(y) - _sinPhi * cosY); - if constexpr (mountType == MccMountType::FORK_TYPE) { + // if constexpr (mountType == MccMountType::FORK_TYPE) { + if constexpr (pcmMountType == MccMountType::FORK_TYPE) { if (!utils::isEqual(cosX, 0.0)) { res->pcmY += geom_coeffs->forkFlexure / cosX; } diff --git a/include/mcc/mcc_serializer.h b/include/mcc/mcc_serializer.h index 73132d4..d148522 100644 --- a/include/mcc/mcc_serializer.h +++ b/include/mcc/mcc_serializer.h @@ -501,7 +501,7 @@ struct MccSerializer : MccSerializerBase { // quantities in degree representation MccSerializerBase::angleFormatFromCoordPairType(pars_d); + MccSerializerBase::CO_LAT>(pars_d); MccSerializer ang_sr; diff --git a/include/mcc/mcc_telemetry.h b/include/mcc/mcc_telemetry.h index 5a1d3a7..6684b82 100644 --- a/include/mcc/mcc_telemetry.h +++ b/include/mcc/mcc_telemetry.h @@ -113,17 +113,17 @@ public: struct telemetry_data_t { - MccSkyPoint targetPos{}; + MccSkyPoint targetPos{}; // celestial coordinates - MccGenXY targetXY{}; + MccGenXY targetXY{}; // encoder coordinates struct { double pcmX{}, pcmY{}; } pcmReverseCorrection{}; - MccSkyPoint mountPos{}; + MccSkyPoint mountPos{}; // celestial coordinates - typename HARDWARE_T::hardware_state_t hwState{}; + typename HARDWARE_T::hardware_state_t hwState{}; // here encoder coordinates struct { double pcmX{}, pcmY{}; @@ -216,8 +216,8 @@ public: mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE); } else { // compute PCM corrections and observed (corrected for PCM) mount coordinates - auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &_tdataPtr->pcmCorrection, - &_tdataPtr->mountPos); + auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &(_tdataPtr->pcmCorrection), + &(_tdataPtr->mountPos)); if (!pcm_err) { // set target coordinates @@ -246,8 +246,9 @@ public: // _tdataPtr->targetPos = _enteredTargetPos; using pcm_t = std::remove_pointer_t; - std::conditional_t, MccSkyHADEC_OBS, - std::conditional_t, + std::conditional_t, + MccSkyHADEC_OBS, + std::conditional_t, MccSkyAZZD, std::nullptr_t>> cp; diff --git a/tests/mcc_telemetry_test.cpp b/tests/mcc_telemetry_test.cpp index 5211c9c..f8516e1 100644 --- a/tests/mcc_telemetry_test.cpp +++ b/tests/mcc_telemetry_test.cpp @@ -15,7 +15,7 @@ static std::uniform_real_distribution xs_distrib(100, 700); static std::uniform_real_distribution ys_distrib(100, 700); struct hw_t { - static constexpr mcc::MccMountType mountType{mcc::MccMountType::ALTAZ_TYPE}; + static constexpr mcc::MccMountType hwMountType{mcc::MccMountType::ALTAZ_TYPE}; static constexpr std::string_view hardwareName{"HW-TEST"}; typedef int error_t;