From 6214b82a6cf67feefeb6deb67cd25c0ceffa9b3d Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Wed, 19 Nov 2025 12:02:43 +0300 Subject: [PATCH] ... --- asibfm700/asibfm700_configfile.h | 18 + mcc/mcc_defaults.h | 16 +- mcc/mcc_moving_model_common.h | 6 + mcc/mcc_telemetry.h | 32 +- mcc/mcc_telemetry.h.bad | 734 +++++++++++++++++++++++++++++++ mcc/mcc_telemetry.h.split | 618 -------------------------- 6 files changed, 797 insertions(+), 627 deletions(-) create mode 100644 mcc/mcc_telemetry.h.bad delete mode 100644 mcc/mcc_telemetry.h.split diff --git a/asibfm700/asibfm700_configfile.h b/asibfm700/asibfm700_configfile.h index c14ff78..26998bd 100644 --- a/asibfm700/asibfm700_configfile.h +++ b/asibfm700/asibfm700_configfile.h @@ -135,6 +135,10 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple( // coordinates difference in arcsecs to stop slewing simple_config_record_t{"slewToleranceRadius", 5.0, {"coordinates difference in arcsecs to stop slewing"}}, + simple_config_record_t{"slewingTelemetryInterval", + std::chrono::milliseconds(100), + {"telemetry request interval (in millisecs) in slewing mode"}}, + // target-mount coordinate difference in arcsecs to start adjusting of slewing simple_config_record_t{"adjustCoordDiff", 50.0, @@ -154,6 +158,12 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple( std::chrono::milliseconds(10000), {"a time shift into future to compute target position in future (UT1-scale time duration, millisecs)"}}, + + simple_config_record_t{"trackingTelemetryInterval", + std::chrono::milliseconds(100), + {"telemetry request interval (in millisecs) in tracking mode"}}, + + // minimum time in millisecs between two successive tracking corrections simple_config_record_t{"trackingCycleInterval", std::chrono::milliseconds(300), @@ -501,6 +511,10 @@ public: mcc::MccSimpleMovingModelParams pars; + auto get_value = [&pars, this](std::string_view name, VT& val) { + val = getValue(name).value_or(val); + }; + pars.telemetryTimeout = getValue("telemetryTimeout").value_or(pars.telemetryTimeout); @@ -513,6 +527,8 @@ public: getValue("slewToleranceRadius").value_or(pars.slewToleranceRadius) * arcsecs2rad; + get_value("slewingTelemetryInterval", pars.slewingTelemetryInterval); + pars.slewRateX = getValue("hwMaxRateHA").value_or(pars.slewRateX); pars.slewRateY = getValue("hwMaxRateDEC").value_or(pars.slewRateY); @@ -524,6 +540,8 @@ public: pars.slewTimeout = getValue("slewTimeout").value_or(pars.slewTimeout); + get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval); + pars.timeShiftToTargetPoint = getValue("timeShiftToTargetPoint") .value_or(pars.timeShiftToTargetPoint); diff --git a/mcc/mcc_defaults.h b/mcc/mcc_defaults.h index 37f7f7c..b3da5a3 100644 --- a/mcc/mcc_defaults.h +++ b/mcc/mcc_defaults.h @@ -1124,7 +1124,7 @@ public: // output format: , speedX, speedY, pcmX, pcmY, refCorr (in arcsecs), // RA-APP_mnt, DEC-APP_mnt, HA_mnt, AZ_mnt, ZD_mnt, ALT_mnt, X_mnt, Y_mnt, COO-PAIR_mnt, TIME-POINT_mnt, - // LST, SPEED_X_mnt, SPEED_Y_mnt, PCM_X, PCM_Y, REFCORR, + // LST, EO, SPEED_X_mnt, SPEED_Y_mnt, PCM_X, PCM_Y, REFCORR, // RA-ICRS_tag, DEC-ICRS_tag, RA-APP_tag, DEC-APP_tag, HA_tag, AZ_tag, ZD_tag, ALT_tag, X_tag, Y_tag, // COO-PAIR_tag, TIME-POINT_tag @@ -1134,10 +1134,15 @@ public: std::format_to(std::back_inserter(bytes), "{}", _delimiter); toSexagesimalHour(bytes, value.LST); + + std::format_to(std::back_inserter(bytes), "{}", _delimiter); + + toSexagesimalHour(bytes, value.EO); std::format_to(std::back_inserter(bytes), "{}", _delimiter); // '*3600.0' to express refraction correction in arcseconds! - toDegrees(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr * 3600.0); + // toDegrees(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr * 3600.0); + toSexagesimalDeg(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr); std::format_to(std::back_inserter(bytes), "{}", _delimiter); pt_ser.setFormat(_currentFormat); @@ -1158,7 +1163,7 @@ public: // valid format: , speedX, speedY, pcmX, pcmY, refCorr, auto els = splitToElements(std::forward(bytes)); - if (els.size() < 28) { + if (els.size() < 29) { // return std::make_error_code(std::errc::invalid_argument); return MccCoordinateConvErrorCode::ERROR_ARG_LEN; } @@ -1176,6 +1181,11 @@ public: return err; } + err = parseHourRepr(els[idx++], tdata.EO); + if (err) { + return err; + } + err = parseDegreeRepr(els[idx++], tdata.speedX); if (err) { return err; diff --git a/mcc/mcc_moving_model_common.h b/mcc/mcc_moving_model_common.h index c9f4d56..501d279 100644 --- a/mcc/mcc_moving_model_common.h +++ b/mcc/mcc_moving_model_common.h @@ -37,6 +37,9 @@ struct MccSimpleMovingModelParams { // 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}; @@ -59,6 +62,9 @@ struct MccSimpleMovingModelParams { // ******* 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 diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index d00cc14..e6d103f 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -391,6 +391,29 @@ public: return MccTelemetryErrorCode::ERROR_OK; }; + + + // update thread + // _updatingFuture = std::async( + // std::launch::async, + // [this](std::stop_token stoken) { + // while (!stoken.stop_requested()) { + // { + // std::unique_lock ulock{*_updateMutex}; + + // bool ok = _updateCondVar->wait(ulock, [&stoken, this]() -> bool { + // return _dataUpdatingRequested || stoken.stop_requested(); + // }); + // } + + // if (!stoken.stop_requested()) { + // std::lock_guard lock{*_timeoutMutex}; + + // _lastUpdateError = _updateFunc(stoken); + // } + // } + // }, + // _internalUpdatingStopSource.get_token()); } @@ -712,18 +735,15 @@ protected: std::chrono::nanoseconds _currentUpdateTimeout{defaultInternalUpdateTimeout}; std ::function _updateTargetFunc{}; - // std ::function _updateTargetFunc{}; std::function _updateFunc{}; std::function _setTargetFunc{}; std::unique_ptr _updateMutex; std::unique_ptr _updateCondVar; - // std::future _internalUpdatingLoopFuture{}; - // std::unique_ptr _internalUpdatingLoopMutex{new std::mutex()}; - // std::unique_ptr _internalUpdatingLoopCondVar{new std::condition_variable()}; - // std::unique_ptr _internalUpdatingLoopStop{new std::atomic_bool{false}}; - // std::unique_ptr _dataUpdatingRequested{new std::atomic_bool{false}}; + std::future _updatingFuture{}; + std::unique_ptr _dataUpdatingRequested{new std::atomic_bool{false}}; + std::unique_ptr _timeoutMutex{new std::timed_mutex()}; error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; }; diff --git a/mcc/mcc_telemetry.h.bad b/mcc/mcc_telemetry.h.bad new file mode 100644 index 0000000..d00cc14 --- /dev/null +++ b/mcc/mcc_telemetry.h.bad @@ -0,0 +1,734 @@ +#pragma once + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + + +/* IMPLEMENTATION OF TELEMETRY CLASS */ + + +#include +#include +#include +#include + +#include "mcc_defaults.h" + +namespace mcc +{ + +enum class MccTelemetryErrorCode : int { + ERROR_OK, + ERROR_NULLPTR, + ERROR_COORD_TRANSFORM, + ERROR_PCM_COMP, + ERROR_HARDWARE_GETPOS, + ERROR_UPDATE_STOPPED, + ERROR_DATA_TIMEOUT, + ERROR_UNSUPPORTED_COORD_PAIR +}; + +} // namespace mcc + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + +namespace mcc +{ + +/* error category definition */ + +// error category +struct MccTelemetryCategory : public std::error_category { + MccTelemetryCategory() : std::error_category() {} + + const char* name() const noexcept + { + return "MCC-TELEMETRY"; + } + + std::string message(int ec) const + { + MccTelemetryErrorCode err = static_cast(ec); + + switch (err) { + case MccTelemetryErrorCode::ERROR_OK: + return "OK"; + case MccTelemetryErrorCode::ERROR_NULLPTR: + return "nullptr input argument"; + case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM: + return "coordinate transformation error"; + case MccTelemetryErrorCode::ERROR_PCM_COMP: + return "PCM computation error"; + case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS: + return "cannot get hardware position"; + case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED: + return "telemetry update was stopped"; + case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT: + return "a timeout occured while waiting for new data"; + case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR: + return "unsupported coordinate pair"; + default: + return "UNKNOWN"; + } + } + + static const MccTelemetryCategory& get() + { + static const MccTelemetryCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccTelemetryErrorCode ec) +{ + return std::error_code(static_cast(ec), MccTelemetryCategory::get()); +} + + + +class MccTelemetry : public mcc_telemetry_interface_t +{ +protected: + static constexpr uint16_t internalUpdatingIntervalDiv = 5; + +public: + static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100); + static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5; + + typedef std::error_code error_t; + + + template + MccTelemetry(CONTROLS_T* controls) + : _isDataUpdated(new std::atomic_bool()), + _data(), + _userTarget(), + _internalUpdating(new std::atomic_bool), + _currentUpdateInterval(defaultUpdateInterval), + _currentUpdateIntervalMutex(new std::mutex), + _updateMutex(new std::mutex), + _updateCondVar(new std::condition_variable) + { + *_isDataUpdated = false; + *_internalUpdating = false; + + + // using ccte_t = std::remove_cvref_t; + using pcm_t = std::remove_cvref_t; + using hardware_t = std::remove_cvref_t; + + if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { + _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); + } + + _updateTargetFunc = [controls, this](std::stop_token stop_token) -> error_t { + MccPCMResult pcm_res; + + mcc_tp2tp(_data.time_point, _data.target.time_point); + + bool hw_coords = _data.target.pair_kind == MccCoordPairKind::COORDS_KIND_XY; + MccCelestialPoint hw_cp{.pair_kind = MccCoordPairKind::COORDS_KIND_XY}; + mcc_tp2tp(_data.time_point, hw_cp.time_point); + + if (hw_coords) { // compute corresponded apparent coordinates + hw_cp.X = _data.target.X; + hw_cp.Y = _data.target.Y; + + auto pcm_err = controls->computePCM(_data.target, &pcm_res, &_data.target); + if (pcm_err) { + return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); + } + + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + + if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!!!"); + } + } + + if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + _data.target.X = _data.target.RA_ICRS; + _data.target.Y = _data.target.DEC_ICRS; + } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + _data.target.X = _data.target.AZ; + _data.target.Y = _data.target.ZD; + } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) { + _data.target.X = _data.target.AZ; + _data.target.Y = _data.target.ALT; + } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { + _data.target.X = _data.target.HA; + _data.target.Y = _data.target.DEC_APP; + } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { + _data.target.X = _data.target.RA_APP; + _data.target.Y = _data.target.DEC_APP; + } else { + return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; + } + + auto ccte_err = controls->transformCoordinates(_data.target, &_data.target); + if (ccte_err) { + if (hw_coords) { // restore coordinates pair kind + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY; + } + + return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); + } + + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + + if (_data.target.pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + // fixed apparent coordinates (AZZD or HADEC) + // needs to compute ICRS + // (.X and .Y are already assigned above!) + + + // if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + // _data.target.X = _data.target.AZ; + // _data.target.Y = _data.target.ZD; + // } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) { + // _data.target.X = _data.target.AZ; + // _data.target.Y = _data.target.ALT; + // } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { + // _data.target.X = _data.target.HA; + // _data.target.Y = _data.target.DEC_APP; + // } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { + // _data.target.X = _data.target.RA_APP; + // _data.target.Y = _data.target.DEC_APP; + // } else { + // return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; + // } + + MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; + ccte_err = controls->transformCoordinates(_data.target, &pt); + if (ccte_err) { + if (hw_coords) { // restore coordinates pair kind + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY; + _data.target.X = hw_cp.X; + _data.target.Y = hw_cp.Y; + } + + return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); + } + _data.target.RA_ICRS = pt.X; + _data.target.DEC_ICRS = pt.Y; + } // from ICRS to apparent calculation is already performed above + + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + + // hardware coordinates + if (!hw_coords) { + auto pcm_err = controls->computeInversePCM(_data.target, &pcm_res, &_data.target); + if (pcm_err) { + return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); + } + } else { // restore coordinates pair kind + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY; + _data.target.X = hw_cp.X; + _data.target.Y = hw_cp.Y; + } + + return MccTelemetryErrorCode::ERROR_OK; + }; + + _updateFunc = [controls, this](std::stop_token stop_token) -> std::error_code { + // first, update mount quantities + typename hardware_t::hardware_state_t hw_pos; + auto hw_err = controls->hardwareGetState(&hw_pos); + if (hw_err) { + return mcc_deduce_error_code(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS); + } + + // if (stop_token.stop_requested()) { + // return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + // } + + double eo; + + _data.time_point = + std::chrono::time_point_cast(hw_pos.time_point); + + auto ccte_err = controls->timepointToJulday(_data.time_point, &_data.JD); + if (!ccte_err) { + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + + ccte_err = controls->juldayToAppSideral(_data.JD, &_data.LST, true); + if (!ccte_err) { + ccte_err = controls->equationOrigins(_data.JD, &eo); + _data.EO = eo; + } + } + + if (ccte_err) { + return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); + } + + // if (stop_token.stop_requested()) { + // return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + // } + + _data.X = (double)hw_pos.X; + _data.Y = (double)hw_pos.Y; + _data.speedX = (double)hw_pos.speedX; + _data.speedY = (double)hw_pos.speedY; + + // fill _data.pcmX, _data.pcmY and corresponded apparent coordinates + auto pcm_err = controls->computePCM(_data, &_data, &_data); + if (pcm_err) { + return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); + } + + // if (stop_token.stop_requested()) { + // return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + // } + + MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point}; + + if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { + _data.RA_APP = + MccAngle((double)_data.LST - (double)_data.HA - eo).normalize(); + // MccAngle((double)_data.LST - (double)_data.HA + eo).normalize(); + + _data.X = _data.HA; + _data.Y = _data.DEC_APP; + + _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + + ccte_err = controls->transformCoordinates(_data, &pt); + if (!ccte_err) { + _data.AZ = pt.X; + _data.ALT = pt.Y; + _data.ZD = std::numbers::pi / 2.0 - _data.ALT; + } + } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { + _data.ALT = std::numbers::pi / 2.0 - _data.ZD; + + _data.X = _data.AZ; + _data.Y = _data.ZD; + + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + + pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + ccte_err = controls->transformCoordinates(_data, &pt); + if (!ccte_err) { + _data.HA = pt.X; + _data.DEC_APP = pt.Y; + _data.RA_APP = + MccAngle((double)_data.LST - (double)_data.HA - eo).normalize(); + // MccAngle((double)_data.LST - (double)_data.HA + eo).normalize(); + } + + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); + } + + if (!ccte_err) { + // if (stop_token.stop_requested()) { + // return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + // } + + // to compute refraction coefficients + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + _data.X = _data.AZ; + _data.Y = _data.ZD; + + ccte_err = controls->refractionCorrection(_data, &_data.refCorr); + if (!ccte_err) { + // restore hardware encoders coordinates + _data.X = (double)hw_pos.X; + _data.Y = (double)hw_pos.Y; + + // update target (assuming target ICRS coordinates are already set) + + // auto ret = _updateTargetFunc(false, stop_token); + // update target according to its .pair_kind! + auto ret = _updateTargetFunc(stop_token); + if (ret) { + return ret; + } + } + } + + // restore according to the mount type + if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { + _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); + } + + + if (ccte_err) { + return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); + } + + return MccTelemetryErrorCode::ERROR_OK; + }; + } + + + MccTelemetry(MccTelemetry&&) = default; + MccTelemetry& operator=(MccTelemetry&&) = default; + + MccTelemetry(const MccTelemetry&) = delete; + MccTelemetry& operator=(const MccTelemetry&) = delete; + + + virtual ~MccTelemetry() + { + stopInternalTelemetryDataUpdating(); + + if (_internalUpdatingFuture.valid()) { + // try to exit correctly + // auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1)); + _internalUpdatingFuture.wait_for(std::chrono::seconds(1)); + // _internalUpdatingFuture.get(); + } + }; + + + template + DT telemetryDataUpdateInterval() const + { + std::lock_guard lock{*_currentUpdateIntervalMutex}; + + return std::chrono::duration_cast
(_currentUpdateInterval); + } + + std::chrono::milliseconds telemetryDataUpdateInterval() const + { + return telemetryDataUpdateInterval(); + } + + void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval) + { + using d_t = std::remove_cvref_t; + + std::lock_guard lock{*_currentUpdateIntervalMutex}; + + if constexpr (std::floating_point) { + _currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval; + } else { + _currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval; + } + } + + void setTelemetryUpdateTimeout(traits::mcc_time_duration_c auto const& timeout) + { + if (timeout.count() > 0) { + _currentUpdateTimeout = std::chrono::duration_cast(timeout); + } + } + + auto getTelemetryUpdateTimeout() const + { + return _currentUpdateTimeout; + } + + // asynchronuosly periodicaly update telemetry data (internal synchronization) + void startInternalTelemetryDataUpdating() + { + using intv_t = std::remove_cvref_t; + + _internalUpdatingStopSource = std::stop_source{}; // reset state + + *_internalUpdating = true; + + _internalUpdatingFuture = std::async( + std::launch::async, + [this](std::stop_token stop_token) -> error_t { + while (!stop_token.stop_requested()) { + // while (true) { + _lastUpdateError = updateTelemetryData(_currentUpdateTimeout); + if (_lastUpdateError) { + *_internalUpdating = false; + return _lastUpdateError; + } + + // auto nn = std::this_thread::get_id(); + + std::this_thread::sleep_for(_currentUpdateInterval); + + // { + // std::lock_guard lock{*_currentUpdateIntervalMutex}; + + // // compute it here because of possible changing _currentUpdateInterval + // auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; + + // for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { + // if (stop_token.stop_requested()) { + // break; + // } + + // std::this_thread::sleep_for(sleep_td); + // } + + // if (stop_token.stop_requested()) { + // break; + // } + + // if constexpr (std::floating_point) { + // std::this_thread::sleep_for(sleep_td); + // } else { + // auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv; + + // if (rem.count()) { + // std::this_thread::sleep_for(rem); + // } else { + // std::this_thread::sleep_for(sleep_td); + // } + // } + // } + } + + *_internalUpdating = false; + return MccTelemetryErrorCode::ERROR_OK; + }, + _internalUpdatingStopSource.get_token()); + } + + + void stopInternalTelemetryDataUpdating() + { + _internalUpdatingStopSource.request_stop(); + *_internalUpdating = false; + } + + + bool isInternalTelemetryDataUpdating() const + { + return *_internalUpdating; + } + + + error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout) + { + { + std::lock_guard thread_lock{*_updateMutex}; + + std::stop_source stop_source; + + *_isDataUpdated = false; + + // std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); + // // std::future update_ft = + // // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token()); + // auto status = update_ft.wait_for(timeout); + + // if (status == std::future_status::ready) { + // *_isDataUpdated = true; + // _lastUpdateError = update_ft.get(); + // } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get + // result + // _lastUpdateError = update_ft.get(); + // if (!_lastUpdateError) { + // *_isDataUpdated = true; + // } + // } else { // timeout + // stop_source.request_stop(); + // _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + // } + + _lastUpdateError = _updateFunc(_internalUpdatingStopSource.get_token()); + *_isDataUpdated = true; + } + + // unblock waiting threads even in the case of timeout! + _updateCondVar->notify_all(); + + // *_isDataUpdated = false; + + return _lastUpdateError; + } + + // block the thread and wait for data to be ready (internal synchronization) + error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout) + { + if (tdata == nullptr) { + return MccTelemetryErrorCode::ERROR_NULLPTR; + } + + std::unique_lock ulock(*_updateMutex); + + auto res = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; }); + if (!res) { + return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + } + + // std::lock_guard thread_lock{*_updateMutex}; + + if (!_lastUpdateError) { + mcc_copy_telemetry_data(_data, tdata); + } + + return _lastUpdateError; + } + + // just get current data + error_t telemetryData(mcc_telemetry_data_c auto* tdata) + { + if (tdata == nullptr) { + return MccTelemetryErrorCode::ERROR_NULLPTR; + } + + std::lock_guard thread_lock{*_updateMutex}; + + mcc_copy_telemetry_data(_data, tdata); + + return MccTelemetryErrorCode::ERROR_OK; + } + + error_t lastUpdateError() const + { + return _lastUpdateError; + } + + error_t setPointingTarget(mcc_celestial_point_c auto pt) + { + /* + * If apparent coordinates are specified (e.g. AZZD), + * they are assumed to be fixed in time (i.e. pt.time_point will be ignored), + * and other coordinates will be calculated from them + */ + + std::lock_guard lock{*_updateMutex}; + + mcc_copy_celestial_point(pt, &_userTarget); + + + _data.target.pair_kind = pt.pair_kind; + if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) { + _data.target.AZ = pt.X; + _data.target.ALT = pt.Y; + } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + _data.target.AZ = pt.X; + _data.target.ZD = pt.Y; + } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { + _data.target.HA = pt.X; + _data.target.DEC_APP = pt.Y; + } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { + _data.target.RA_APP = pt.X; + _data.target.DEC_APP = pt.Y; + } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + _data.target.RA_ICRS = pt.X; + _data.target.DEC_ICRS = pt.Y; + } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { + _data.target.X = pt.X; + _data.target.Y = pt.Y; + } else { + return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; + } + + return _updateTargetFunc({}); + + // return _setTargetFunc(pt); + } + + + + error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy) + { + std::lock_guard lock{*_updateMutex}; + + if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + *dx = (double)_data.target.AZ - (double)_data.AZ; + *dy = (double)_data.target.ALT - (double)_data.ALT; + } else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP || + pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP || + pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + *dx = (double)_data.target.HA - (double)_data.HA; + *dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP; + } else { + return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; + } + + return MccTelemetryErrorCode::ERROR_OK; + } + + + error_t targetToMountDist(mcc_angle_c auto* dist) + { + if (dist == nullptr) { + return MccTelemetryErrorCode::ERROR_NULLPTR; + } + + std::lock_guard lock{*_updateMutex}; + + double dHA = _data.HA - _data.target.HA; + double cosDHA = cos(dHA); + + double cosT = cos(_data.target.DEC_APP); + double sinT = sin(_data.target.DEC_APP); + double cosM = cos(_data.DEC_APP); + double sinM = sin(_data.DEC_APP); + + double term1 = cosT * sin(dHA); + double term2 = cosM * sinT - sinM * cosT * cosDHA; + + *dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA))); + + return MccTelemetryErrorCode::ERROR_OK; + } + +protected: + std::unique_ptr _isDataUpdated; + MccTelemetryData _data; + + MccCelestialPoint _userTarget{}; + + std::unique_ptr _internalUpdating; + std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)}; + std::unique_ptr _currentUpdateIntervalMutex; + std::future _internalUpdatingFuture{}; + std::stop_source _internalUpdatingStopSource{}; + + std::chrono::nanoseconds _currentUpdateTimeout{defaultInternalUpdateTimeout}; + + std ::function _updateTargetFunc{}; + // std ::function _updateTargetFunc{}; + std::function _updateFunc{}; + std::function _setTargetFunc{}; + + std::unique_ptr _updateMutex; + std::unique_ptr _updateCondVar; + + // std::future _internalUpdatingLoopFuture{}; + // std::unique_ptr _internalUpdatingLoopMutex{new std::mutex()}; + // std::unique_ptr _internalUpdatingLoopCondVar{new std::condition_variable()}; + // std::unique_ptr _internalUpdatingLoopStop{new std::atomic_bool{false}}; + // std::unique_ptr _dataUpdatingRequested{new std::atomic_bool{false}}; + + error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; +}; + + +static_assert(mcc_telemetry_c, ""); + +} // namespace mcc diff --git a/mcc/mcc_telemetry.h.split b/mcc/mcc_telemetry.h.split deleted file mode 100644 index 2ea03c3..0000000 --- a/mcc/mcc_telemetry.h.split +++ /dev/null @@ -1,618 +0,0 @@ -#pragma once - -/* MOUNT CONTROL COMPONENTS LIBRARY */ - - -/* IMPLEMENTATION OF TELEMETRY CLASS */ - - -#include -#include -#include -#include - -#include "mcc_defaults.h" - -namespace mcc -{ - -enum MccTelemetryErrorCode : int { - ERROR_OK, - ERROR_NULLPTR, - ERROR_COORD_TRANSFORM, - ERROR_PCM_COMP, - ERROR_HARDWARE_GETPOS, - ERROR_UPDATE_STOPPED, - ERROR_DATA_TIMEOUT, - ERROR_UNSUPPORTED_COORD_PAIR -}; - -} // namespace mcc - -namespace std -{ - -template <> -class is_error_code_enum : public true_type -{ -}; - -} // namespace std - - -namespace mcc -{ - -/* error category definition */ - -// error category -struct MccTelemetryCategory : public std::error_category { - MccTelemetryCategory() : std::error_category() {} - - const char* name() const noexcept - { - return "ALTITUDE-LIMIT-PZ"; - } - - std::string message(int ec) const - { - MccTelemetryErrorCode err = static_cast(ec); - - switch (err) { - case MccTelemetryErrorCode::ERROR_OK: - return "OK"; - case MccTelemetryErrorCode::ERROR_NULLPTR: - return "nullptr input argument"; - case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM: - return "coordinate transformation error"; - case MccTelemetryErrorCode::ERROR_PCM_COMP: - return "PCM computation error"; - case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS: - return "cannot get hardware position"; - case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED: - return "telemetry update was stopped"; - case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT: - return "a timeout occured while waiting for new data"; - case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR: - return "unsupported coordinate pair"; - default: - return "UNKNOWN"; - } - } - - static const MccTelemetryCategory& get() - { - static const MccTelemetryCategory constInst; - return constInst; - } -}; - - -inline std::error_code make_error_code(MccTelemetryErrorCode ec) -{ - return std::error_code(static_cast(ec), MccTelemetryCategory::get()); -} - - - -class MccTelemetry : public mcc_telemetry_interface_t -{ -protected: - static constexpr uint16_t internalUpdatingIntervalDiv = 5; - -public: - static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100); - static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5; - - typedef std::error_code error_t; - - - MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) - : _isDataUpdated(new std::atomic_bool()), - _data(), - _internalUpdating(new std::atomic_bool), - _currentUpdateInterval(defaultUpdateInterval), - _currentUpdateIntervalMutex(new std::mutex), - _updateMutex(new std::mutex), - _updateCondVar(new std::condition_variable) - { - *_isDataUpdated = false; - *_internalUpdating = false; - - _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; - - // using ccte_t = std::remove_cvref_t; - using pcm_t = std::remove_cvref_t; - using hardware_t = std::remove_cvref_t; - - _updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t { - if (!only_hw) { - // - // compute apparent coordinates - // ICRS coordinates of the taget must be already set - // - _data.target.time_point = - std::chrono::time_point_cast( - _data.time_point); - - _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; - _data.target.X = _data.target.RA_ICRS; - _data.target.Y = _data.target.DEC_ICRS; - - // update apparent cordinates - auto ccte_err = ccte->transformCoordinates(_data.target, &_data.target); - if (ccte_err) { - return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - } - } - - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - - typename pcm_t::error_t pcm_err; - - MccPCMResult pcm_res; - - // MccCelestialPoint pt; - // pt.time_point = - // std::chrono::time_point_cast(_data.target.time_point); - - pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data); - - // if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - // pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data); - - // pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - // pt.X = _data.target.HA; - // pt.Y = _data.target.DEC_APP; - // pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res); - // if (!pcm_err) { - // _data.target.X = _data.target.HA - pcm_res.pcmX; - // _data.target.Y = _data.target.DEC_APP - pcm_res.pcmY; - // } - // } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { - // pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; - // pt.X = _data.target.AZ; - // pt.Y = _data.target.ALT; - // pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res); - // if (!pcm_err) { - // _data.target.X = _data.target.AZ - pcm_res.pcmX; - // _data.target.Y = _data.target.ALT - pcm_res.pcmY; - // } - // } else { - // static_assert(false, "UNKNOWN MOUNT TYPE!"); - // } - - if (pcm_err) { - return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); - } - - - return MccTelemetryErrorCode::ERROR_OK; - }; - - _updateFunc = [ccte, pcm, hardware, this](std::stop_token stop_token) { - // first, update mount quantities - typename hardware_t::hardware_state_t hw_pos; - auto hw_err = hardware->hardwareGetState(&hw_pos); - if (hw_err) { - return mcc_deduce_error(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS); - } - - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - double eo; - - _data.time_point = - std::chrono::time_point_cast(hw_pos.time_point); - - auto ccte_err = ccte->timepointToJulday(_data.time_point, &_data.JD); - if (!ccte_err) { - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - ccte_err = ccte->juldayToAppSideral(_data.JD, &_data.LST, true); - if (!ccte_err) { - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - ccte_err = ccte->equationOrigins(_data.JD, &eo); - } - } - - if (ccte_err) { - return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - } - - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - _data.X = (double)hw_pos.X; - _data.Y = (double)hw_pos.Y; - _data.speedX = (double)hw_pos.speedX; - _data.speedY = (double)hw_pos.speedY; - - // fill _data.pcmX, _data.pcmY and corresponded apparent coordinates - auto pcm_err = pcm->computePCM(_data, &_data, &_data); - if (pcm_err) { - return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); - } - - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point}; - - if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - _data.RA_APP = (double)_data.LST - (double)_data.HA + eo; - - _data.X = _data.HA; - _data.Y = _data.DEC_APP; - - _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - - ccte_err = ccte->transformCoordinates(_data, &pt); - if (!ccte_err) { - _data.AZ = pt.X; - _data.ALT = pt.Y; - _data.ZD = std::numbers::pi / 2.0 - _data.ALT; - } - } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { - _data.ALT = std::numbers::pi / 2.0 - _data.ZD; - - _data.X = _data.AZ; - _data.Y = _data.ZD; - - _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; - - pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - ccte_err = ccte->transformCoordinates(_data, &pt); - if (!ccte) { - _data.HA = pt.X; - _data.DEC_APP = pt.Y; - _data.RA_APP = (double)_data.LST - (double)_data.HA + eo; - } - - } else { - static_assert(false, "UNKNOWN MOUNT TYPE!"); - } - - if (!ccte_err) { - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; - } - - _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; - _data.X = _data.AZ; - _data.Y = _data.ZD; - - ccte_err = ccte->refractionCorrection(_data, &_data.refCorr); - if (!ccte_err) { - // restore hardware encoders coordinates - _data.X = (double)hw_pos.X; - _data.Y = (double)hw_pos.Y; - - // update target (assuming target ICRS coordinates are already set) - - auto ret = _updateTargetFunc(false, stop_token); - if (ret) { - return ret; - } - } - } - - if (ccte_err) { - return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - } - - if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { - _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; - } else { - static_assert(false, "UNKNOWN MOUNT TYPE!"); - } - - - return MccTelemetryErrorCode::ERROR_OK; - }; - - - _setTargetFunc = [ccte, this](MccCelestialPoint const& pt) { - // in the case of apparent input coordinates - // one must ensure the same time points - - _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; - _data.target.time_point = - std::chrono::time_point_cast(pt.time_point); - - auto ret = ccte->transformCoordinates(pt, &_data.target); - - if (!ret) { - if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { - _data.target.RA_ICRS = _data.target.X; - _data.target.DEC_ICRS = _data.target.Y; - - // update apparent coordinates - ret = _updateTargetFunc(false, {}); - } else { // apparent coordinates were computed above - // compute ICRS coordinates - MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; - ret = ccte->transformCoordinates(pt, &cpt); - - _data.target.RA_ICRS = cpt.X; - _data.target.DEC_ICRS = cpt.Y; - - // compute only hardware coordinates - ret = _updateTargetFunc(true, {}); - } - } - - return mcc_deduce_error(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - }; - } - - - MccTelemetry(MccTelemetry&&) = default; - MccTelemetry& operator=(MccTelemetry&&) = default; - - MccTelemetry(const MccTelemetry&) = delete; - MccTelemetry& operator=(const MccTelemetry&) = delete; - - - virtual ~MccTelemetry() - { - stopInternalTelemetryDataUpdating(); - - if (_internalUpdatingFuture.valid()) { - // try to exit correctly - // auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1)); - _internalUpdatingFuture.wait_for(std::chrono::seconds(1)); - // _internalUpdatingFuture.get(); - } - }; - - - template - DT telemetryDataUpdateInterval() const - { - std::lock_guard lock{_currentUpdateIntervalMutex}; - - return std::chrono::duration_cast
(_currentUpdateInterval); - } - - std::chrono::milliseconds telemetryDataUpdateInterval() const - { - return telemetryDataUpdateInterval(); - } - - void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval) - { - using d_t = std::remove_cvref_t; - - std::lock_guard lock{_currentUpdateIntervalMutex}; - - if constexpr (std::floating_point) { - _currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval; - } else { - _currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval; - } - } - - // asynchronuosly periodicaly update telemetry data (internal synchronization) - void startInternalTelemetryDataUpdating() - { - using intv_t = std::remove_cvref_t; - - *_internalUpdating = true; - - _internalUpdatingFuture = std::async( - std::launch::async, - [this](std::stop_token stop_token) -> error_t { - while (!stop_token.stop_requested()) { - _lastUpdateError = updateTelemetryData(defaultInternalUpdateTimeout); - if (_lastUpdateError) { - *_internalUpdating = false; - return _lastUpdateError; - } - - { - std::lock_guard lock{_currentUpdateIntervalMutex}; - - // compute it here because of possible changing _currentUpdateInterval - auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; - - for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { - if (stop_token.stop_requested()) { - break; - } - - std::this_thread::sleep_for(sleep_td); - } - - if (stop_token.stop_requested()) { - break; - } - - if constexpr (std::floating_point) { - std::this_thread::sleep_for(sleep_td); - } else { - auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv; - - if (rem.count()) { - std::this_thread::sleep_for(rem); - } else { - std::this_thread::sleep_for(sleep_td); - } - } - } - } - - *_internalUpdating = false; - return MccTelemetryErrorCode::ERROR_OK; - }, - _internalUpdatingStopSource.get_token()); - } - - - void stopInternalTelemetryDataUpdating() - { - _internalUpdatingStopSource.request_stop(); - *_internalUpdating = false; - } - - - bool isInternalTelemetryDataUpdating() const - { - return *_internalUpdating; - } - - - error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout) - { - std::lock_guard thread_lock{*_updateMutex}; - - std::stop_source stop_source; - - *_isDataUpdated = false; - - std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); - auto status = update_ft.wait_for(timeout); - - if (status == std::future_status::ready) { - *_isDataUpdated = true; - _lastUpdateError = update_ft.get(); - } else { - stop_source.request_stop(); - _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; - } - - // unblock waiting threads even in the case of timeout! - _updateCondVar->notify_all(); - - return _lastUpdateError; - } - - // block the thread and wait for data to be ready (internal synchronization) - error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout) - { - if (tdata == nullptr) { - return MccTelemetryErrorCode::ERROR_NULLPTR; - } - - std::unique_lock ulock(*_updateMutex); - - auto res = _updateCondVar->wait_for(ulock, timeout, [this]() { return *_isDataUpdated; }); - if (res == std::cv_status::timeout) { - return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; - } - - std::lock_guard thread_lock{*_updateMutex}; - - if (!_lastUpdateError) { - mcc_copy_telemetry_data(_data, tdata); - } - - return _lastUpdateError; - } - - // just get current data - error_t telemetryData(mcc_telemetry_data_c auto* tdata) - { - if (tdata == nullptr) { - return MccTelemetryErrorCode::ERROR_NULLPTR; - } - - std::lock_guard thread_lock{*_updateMutex}; - - mcc_copy_telemetry_data(_data, tdata); - - return MccTelemetryErrorCode::ERROR_OK; - } - - - error_t setPointingTarget(mcc_celestial_point_c auto pt) - { - std::lock_guard lock{*_updateMutex}; - - return _setTargetFunc(pt); - } - - - - error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy) - { - std::lock_guard lock{*_updateMutex}; - - if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { - *dx = (double)_data.target.AZ - (double)_data.AZ; - *dy = (double)_data.target.ALT - (double)_data.ALT; - } else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP || - pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP || - pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { - *dx = (double)_data.target.HA - (double)_data.HA; - *dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP; - } else { - return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR; - } - - return MccTelemetryErrorCode::ERROR_OK; - } - - - error_t targetToMountDist(mcc_angle_c auto* dist) - { - if (dist == nullptr) { - return MccTelemetryErrorCode::ERROR_NULLPTR; - } - - std::lock_guard lock{*_updateMutex}; - - double dHA = _data.HA - _data.target.HA; - double cosDHA = cos(dHA); - - double cosT = cos(_data.target.DEC_APP); - double sinT = sin(_data.target.DEC_APP); - double cosM = cos(_data.DEC_APP); - double sinM = sin(_data.DEC_APP); - - double term1 = cosT * sin(dHA); - double term2 = cosM * sinT - sinM * cosT * cosDHA; - - *dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA))); - - return MccTelemetryErrorCode::ERROR_OK; - } - -protected: - std::unique_ptr _isDataUpdated; - MccTelemetryData _data; - - std::unique_ptr _internalUpdating; - std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)}; - std::unique_ptr _currentUpdateIntervalMutex; - std::future _internalUpdatingFuture{}; - std::stop_source _internalUpdatingStopSource{}; - - std ::function _updateTargetFunc{}; - std::function _updateFunc{}; - std::function _setTargetFunc{}; - - std::unique_ptr _updateMutex; - std::unique_ptr _updateCondVar; - - error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; -}; - - -static_assert(mcc_telemetry_c, ""); - -} // namespace mcc