diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index ce7377c..756c768 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -6,6 +6,10 @@ /* IMPLEMENTATION OF TELEMETRY CLASS */ +#include +#include +#include + #include "mcc_defaults.h" namespace mcc @@ -19,7 +23,8 @@ enum MccTelemetryErrorCode : int { ERROR_NULLPTR, ERROR_COORD_TRANSFORM, ERROR_PCM_COMP, - ERROR_HARDWARE_GETPOS + ERROR_HARDWARE_GETPOS, + ERROR_DATA_TIMEOUT }; } // namespace mcc @@ -64,6 +69,8 @@ struct MccTelemetryCategory : public std::error_category { return "PCM computation error"; case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS: return "cannot get hardware position"; + case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT: + return "a timeout occured while waiting for new data"; default: return "UNKNOWN"; } @@ -97,7 +104,8 @@ public: typedef std::error_code error_t; - MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) : _data() + MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) + : _updated(false), _data(), _updateMutex(new std::mutex), _updateCondVar(new std::condition_variable) { _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; @@ -106,21 +114,72 @@ public: using hardware_t = std::remove_cvref_t; - _updateTargetFunc = [ccte, this]() { - // - // ICRS coordinates of the taget must be already set - // - _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; - _data.target.X = _data.target.RA_ICRS; - _data.target.Y = _data.target.DEC_ICRS; + _updateTargetFunc = [ccte, pcm, this](bool only_hw) -> 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); - // update apparent cordinates - auto ret = ccte->transformCoordinates(_data.target, &_data.target); + _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; + _data.target.X = _data.target.RA_ICRS; + _data.target.Y = _data.target.DEC_ICRS; - return mcc_deduce_error(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); + // 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); + } + } + + // compute hardware coordinates + // WARNING: It is assumed here that PCM corrections have small (arcseconds-arcminutes) values + // since ususaly there is no reverse transformation for "hardware-to-apparent" relation! + typename pcm_t::error_t pcm_err; + + struct { + double dx, dy; + } pcm_res; + + + MccCelestialPoint pt; + pt.time_point = + std::chrono::time_point_cast(_data.target.time_point); + + if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { + pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + pt.X = _data.target.HA; + pt.Y = _data.target.DEC_APP; + pcm_err = pcm->compute(std::move(pt), &pcm_res); + if (!pcm_err) { + _data.target.X = _data.target.HA - pcm_res.dx; + _data.target.Y = _data.target.DEC_APP - pcm_res.dy; + } + } 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->compute(std::move(pt), &pcm_res); + if (!pcm_err) { + _data.target.X = _data.target.AZ - pcm_res.dx; + _data.target.Y = _data.target.ALT - pcm_res.dy; + } + } 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](MccTelemetryData* data) { + _updateFunc = [ccte, pcm, hardware, this]() { // first, update mount quantities typename hardware_t::axes_pos_t hw_pos; auto hw_err = hardware->getPos(&hw_pos); @@ -130,14 +189,14 @@ public: double eo; - data->time_point = - std::chrono::time_point_casttime_point)::duration>(hw_pos.time_point); + _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 = ccte->timepointToJulday(_data.time_point, &_data.JD); if (!ccte_err) { - ccte_err = ccte->juldayToAppSideral(data->JD, &data->LST, true); + ccte_err = ccte->juldayToAppSideral(_data.JD, &_data.LST, true); if (!ccte_err) { - ccte_err = ccte->equationOrigins(data->JD, &eo); + ccte_err = ccte->equationOrigins(_data.JD, &eo); } } @@ -145,55 +204,55 @@ public: return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); } - data->speedX = (double)hw_pos.speedX; - data->speedY = (double)hw_pos.speedY; + _data.speedX = (double)hw_pos.speedX; + _data.speedY = (double)hw_pos.speedY; struct { double dx, dy; } pcm_res; - auto pcm_err = pcm->computePCM(data, &pcm_res); + auto pcm_err = pcm->computePCM(_data, &pcm_res); if (pcm_err) { return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); } - data->pcmX = pcm_res.dx; - data->pcmY = pcm_res.dy; + _data.pcmX = pcm_res.dx; + _data.pcmY = pcm_res.dy; - MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = data->time_point}; + MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point}; if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - data->HA = (double)hw_pos.X + pcm_res.dx; - data->DEC_APP = (double)hw_pos.Y + pcm_res.dy; - data->RA_APP = (double)data->LST - (double)data->HA + eo; + _data.HA = (double)hw_pos.X + pcm_res.dx; + _data.DEC_APP = (double)hw_pos.Y + pcm_res.dy; + _data.RA_APP = (double)_data.LST - (double)_data.HA + eo; - data->X = data->HA; - data->Y = data->DEC_APP; + _data.X = _data.HA; + _data.Y = _data.DEC_APP; - data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - ccte_err = ccte->transformCoordinates(*data, &pt); + 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; + _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->AZ = (double)hw_pos.X + pcm_res.dx; - data->ALT = (double)hw_pos.Y + pcm_res.dy; - data->ZD = std::numbers::pi / 2.0 - data->ALT; + _data.AZ = (double)hw_pos.X + pcm_res.dx; + _data.ALT = (double)hw_pos.Y + pcm_res.dy; + _data.ZD = std::numbers::pi / 2.0 - _data.ALT; - data->X = data->AZ; - data->Y = data->ALT; + _data.X = _data.AZ; + _data.Y = _data.ALT; - data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - ccte_err = ccte->transformCoordinates(*data, &pt); + 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; + _data.HA = pt.X; + _data.DEC_APP = pt.Y; + _data.RA_APP = (double)_data.LST - (double)_data.HA + eo; } } else { @@ -201,27 +260,22 @@ public: } if (!ccte_err) { - data->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; - data->X = data->AZ; - data->Y = data->ZD; + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + _data.X = _data.AZ; + _data.Y = _data.ZD; - ccte_err = ccte->refractionCorrection(data, &data->refCorr); + ccte_err = ccte->refractionCorrection(_data, &_data.refCorr); if (!ccte_err) { - // hardware encoders coordinates - data->X = (double)hw_pos.X; - data->Y = (double)hw_pos.Y; + // 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) - data->target.time_point = - std::chrono::time_point_casttarget.time_point)::duration>( - 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; - - ccte_err = ccte->transformCoordinates(data->target, &data->target); + auto ret = _updateTargetFunc(false); + if (ret) { + return ret; + } } } @@ -230,14 +284,16 @@ public: } if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { - data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; + _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; } else { static_assert(false, "UNKNOWN MOUNT TYPE!"); } + _updated = true; + return MccTelemetryErrorCode::ERROR_OK; }; @@ -258,7 +314,7 @@ public: _data.target.DEC_ICRS = _data.target.Y; // update apparent coordinates - ret = _updateTargetFunc(); + ret = _updateTargetFunc(false); } else { // apparent coordinates were computed above // compute ICRS coordinates MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; @@ -266,6 +322,9 @@ public: _data.target.RA_ICRS = cpt.X; _data.target.DEC_ICRS = cpt.Y; + + // compute only hardware coordinates + ret = _updateTargetFunc(true); } } @@ -276,13 +335,82 @@ public: virtual ~MccTelemetry() = default; + error_t updateTelemetryData(traits::mcc_time_duration_c auto const& period) + { + using d_t = typename std::remove_cvref_t::rep; + + bool is_zero; + + if (std::floating_point) { + is_zero = utils::isEqual(period.count(), d_t::zero()); + } else { + is_zero = period.count() == d_t::zero(); + } + + if (is_zero) { // just update once + return _updateFunc(); + } else { + // try to update once + auto ret = _updateFunc(); + if (ret) { + return ret; + } + + _lastUpdateError = MccTelemetryErrorCode::ERROR_OK; + + _timerThread = [period, this](std::stop_token st) { + while (!st.stop_requested()) { + { + std::lock_guard thread_lock{*_updateMutex}; + + _lastUpdateError = _updateFunc(); + _updateCondVar->notify_all(); + if (_lastUpdateError) { + return; + } + } + + std::this_thread::sleep_for(period); + } + }; + + _timerThread.detach(); + + return MccTelemetryErrorCode::ERROR_OK; + } + } + + // block the thread and wait for data to be ready (external 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 thread_lock(*_updateMutex); + + auto res = _updateCondVar->wait_for(thread_lock, timeout, [this]() { return _updated; }); + if (res == std::cv_status::timeout) { + return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + } + + if (!_lastUpdateError) { + mcc_copy_telemetry_data(_data, tdata); + } + + return _lastUpdateError; + } + + // update and get data as soon as possible error_t telemetryData(mcc_telemetry_data_c auto* tdata) { if (tdata == nullptr) { return MccTelemetryErrorCode::ERROR_NULLPTR; } - error_t ret = _updateFunc(&_data); + std::lock_guard thread_lock{*_updateMutex}; + + error_t ret = _updateFunc(); if (!ret) { mcc_copy_telemetry_data(_data, tdata); @@ -294,15 +422,25 @@ public: error_t setPointingTarget(mcc_celestial_point_c auto pt) { - // return + std::lock_guard lock{*_updateMutex}; + + return _setTargetFunc(); } + protected: + std::atomic_bool _updated; MccTelemetryData _data; - std ::function _updateTargetFunc{}; - std::function _updateFunc{}; - std::function _setTargetFunc{}; + std ::function _updateTargetFunc{}; + std::function _updateFunc{}; + std::function _setTargetFunc{}; + + std::unique_ptr _updateMutex; + std::unique_ptr _updateCondVar; + + error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; + std::jthread _timerThread; }; } // namespace mcc