diff --git a/asibfm700/asibfm700_common.h b/asibfm700/asibfm700_common.h index dcf35ca..a563e11 100644 --- a/asibfm700/asibfm700_common.h +++ b/asibfm700/asibfm700_common.h @@ -5,6 +5,8 @@ /* COMMON LIBRARY DEFINITIONS */ +#include "asibfm700_servocontroller.h" + namespace asibfm700 { diff --git a/mcc/mcc_pzone.h b/mcc/mcc_pzone.h index 81cd677..9321e5a 100644 --- a/mcc/mcc_pzone.h +++ b/mcc/mcc_pzone.h @@ -662,6 +662,7 @@ public: return MccAltLimitPZErrorCode::ERROR_OK; } + // time to reach maximal limit template error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time) requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) @@ -681,6 +682,18 @@ public: } else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) { } } else { // mcc_celestial_point_c + if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { + time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } else { + MccCelestialPoint pt; + + auto ret = getHWCoords(std::move(coords), &pt); + if (ret) { + return ret; + } + + time_ang = (_maxLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } } std::chrono::nanoseconds ns{ @@ -688,19 +701,60 @@ public: period_t rat; *res_time = res_t{static_cast(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)}; + + return MccAltLimitPZErrorCode::ERROR_OK; } + // time to reach minimal limit template error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time) requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) { + using res_t = std::remove_cvref_t; + using period_t = typename res_t::period; + + double time_ang; + + if (res_time == nullptr) { + return MccAltLimitPZErrorCode::ERROR_NULLPTR; + } + if constexpr (mcc_eqt_hrz_coord_c) { + // assume here .X and are hardware encoder coordinate of corresponding axis + if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) { + time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) { + } + } else { // mcc_celestial_point_c + if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { + time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } else { + MccCelestialPoint pt; + + auto ret = getHWCoords(std::move(coords), &pt); + if (ret) { + return ret; + } + + time_ang = (_minLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } + } + + std::chrono::nanoseconds ns{ + static_cast(time_ang * 43200.0 / std::numbers::pi * 1.0E9)}; + + period_t rat; + *res_time = res_t{static_cast(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)}; + + return MccAltLimitPZErrorCode::ERROR_OK; } + template error_t intersectPZone(InputT coords, ResultT* point) requires((mcc_eqt_hrz_coord_c || mcc_celestial_point_c) && (mcc_eqt_hrz_coord_c || mcc_celestial_point_c)) { + return MccAltLimitPZErrorCode::ERROR_OK; } protected: diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index 2ea03c3..f3a0baa 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -107,7 +107,8 @@ public: typedef std::error_code error_t; - MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) + template + MccTelemetry(CONTROLS_T* controls) : _isDataUpdated(new std::atomic_bool()), _data(), _internalUpdating(new std::atomic_bool), @@ -122,10 +123,10 @@ public: _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; + 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 { + _updateTargetFunc = [controls, this](bool only_hw, std::stop_token stop_token) -> error_t { if (!only_hw) { // // compute apparent coordinates @@ -140,7 +141,7 @@ public: _data.target.Y = _data.target.DEC_ICRS; // update apparent cordinates - auto ccte_err = ccte->transformCoordinates(_data.target, &_data.target); + auto ccte_err = controls->transformCoordinates(_data.target, &_data.target); if (ccte_err) { return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); } @@ -159,7 +160,7 @@ public: // pt.time_point = // std::chrono::time_point_cast(_data.target.time_point); - pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data); + pcm_err = controls->computeInversePCM(_data, &pcm_res, &_data); // if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { // pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data); @@ -193,10 +194,10 @@ public: return MccTelemetryErrorCode::ERROR_OK; }; - _updateFunc = [ccte, pcm, hardware, this](std::stop_token stop_token) { + _updateFunc = [controls, 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); + auto hw_err = controls->hardwareGetState(&hw_pos); if (hw_err) { return mcc_deduce_error(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS); } @@ -210,19 +211,19 @@ public: _data.time_point = std::chrono::time_point_cast(hw_pos.time_point); - auto ccte_err = ccte->timepointToJulday(_data.time_point, &_data.JD); + 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 = ccte->juldayToAppSideral(_data.JD, &_data.LST, true); + ccte_err = controls->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); + ccte_err = controls->equationOrigins(_data.JD, &eo); } } @@ -240,7 +241,7 @@ public: _data.speedY = (double)hw_pos.speedY; // fill _data.pcmX, _data.pcmY and corresponded apparent coordinates - auto pcm_err = pcm->computePCM(_data, &_data, &_data); + auto pcm_err = controls->computePCM(_data, &_data, &_data); if (pcm_err) { return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); } @@ -259,7 +260,7 @@ public: _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - ccte_err = ccte->transformCoordinates(_data, &pt); + ccte_err = controls->transformCoordinates(_data, &pt); if (!ccte_err) { _data.AZ = pt.X; _data.ALT = pt.Y; @@ -274,8 +275,8 @@ public: _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - ccte_err = ccte->transformCoordinates(_data, &pt); - if (!ccte) { + ccte_err = controls->transformCoordinates(_data, &pt); + if (!ccte_err) { _data.HA = pt.X; _data.DEC_APP = pt.Y; _data.RA_APP = (double)_data.LST - (double)_data.HA + eo; @@ -294,7 +295,7 @@ public: _data.X = _data.AZ; _data.Y = _data.ZD; - ccte_err = ccte->refractionCorrection(_data, &_data.refCorr); + ccte_err = controls->refractionCorrection(_data, &_data.refCorr); if (!ccte_err) { // restore hardware encoders coordinates _data.X = (double)hw_pos.X; @@ -326,7 +327,7 @@ public: }; - _setTargetFunc = [ccte, this](MccCelestialPoint const& pt) { + _setTargetFunc = [controls, this](MccCelestialPoint const& pt) { // in the case of apparent input coordinates // one must ensure the same time points @@ -334,7 +335,7 @@ public: _data.target.time_point = std::chrono::time_point_cast(pt.time_point); - auto ret = ccte->transformCoordinates(pt, &_data.target); + auto ret = controls->transformCoordinates(pt, &_data.target); if (!ret) { if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { @@ -346,7 +347,7 @@ public: } else { // apparent coordinates were computed above // compute ICRS coordinates MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; - ret = ccte->transformCoordinates(pt, &cpt); + ret = controls->transformCoordinates(pt, &cpt); _data.target.RA_ICRS = cpt.X; _data.target.DEC_ICRS = cpt.Y; diff --git a/mcc/mcc_telemetry.h.split b/mcc/mcc_telemetry.h.split new file mode 100644 index 0000000..2ea03c3 --- /dev/null +++ b/mcc/mcc_telemetry.h.split @@ -0,0 +1,618 @@ +#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