diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index def087e..c4f4818 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -7,6 +7,7 @@ #include +#include #include #include @@ -24,6 +25,7 @@ enum MccTelemetryErrorCode : int { ERROR_COORD_TRANSFORM, ERROR_PCM_COMP, ERROR_HARDWARE_GETPOS, + ERROR_UPDATE_STOPPED, ERROR_DATA_TIMEOUT }; @@ -69,6 +71,8 @@ struct MccTelemetryCategory : public std::error_category { 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"; default: @@ -114,7 +118,7 @@ public: using hardware_t = std::remove_cvref_t; - _updateTargetFunc = [ccte, pcm, this](bool only_hw) -> error_t { + _updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t { if (!only_hw) { // // compute apparent coordinates @@ -135,6 +139,11 @@ public: } } + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + + // 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! @@ -179,7 +188,7 @@ public: return MccTelemetryErrorCode::ERROR_OK; }; - _updateFunc = [ccte, pcm, hardware, this]() { + _updateFunc = [ccte, pcm, hardware, this](std::stop_token stop_token) { // first, update mount quantities typename hardware_t::axes_pos_t hw_pos; auto hw_err = hardware->getPos(&hw_pos); @@ -187,6 +196,10 @@ public: 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 = @@ -194,8 +207,16 @@ public: 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); } } @@ -204,6 +225,10 @@ public: return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); } + if (stop_token.stop_requested()) { + return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED; + } + _data.speedX = (double)hw_pos.speedX; _data.speedY = (double)hw_pos.speedY; @@ -219,6 +244,10 @@ public: _data.pcmX = pcm_res.dx; _data.pcmY = pcm_res.dy; + 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)) { @@ -260,6 +289,10 @@ public: } 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; @@ -272,7 +305,7 @@ public: // update target (assuming target ICRS coordinates are already set) - auto ret = _updateTargetFunc(false); + auto ret = _updateTargetFunc(false, stop_token); if (ret) { return ret; } @@ -314,7 +347,7 @@ public: _data.target.DEC_ICRS = _data.target.Y; // update apparent coordinates - ret = _updateTargetFunc(false); + ret = _updateTargetFunc(false, {}); } else { // apparent coordinates were computed above // compute ICRS coordinates MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; @@ -324,7 +357,7 @@ public: _data.target.DEC_ICRS = cpt.Y; // compute only hardware coordinates - ret = _updateTargetFunc(true); + ret = _updateTargetFunc(true, {}); } } @@ -335,49 +368,76 @@ public: virtual ~MccTelemetry() = default; + template + DT telemetryDataUpdateInterval() const + { + return std::chrono::duration_cast
(_currentUpdateInterval); + } + + std::chrono::milliseconds telemetryDataUpdateInterval() const + { + return telemetryDataUpdateInterval(); + } + error_t updateTelemetryData(traits::mcc_time_duration_c auto const& period) { - using d_t = typename std::remove_cvref_t::rep; + // using d_t = typename std::remove_cvref_t::rep; - bool is_zero; + // 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 (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; + // } + + std::lock_guard thread_lock{*_updateMutex}; + + std::stop_source stop_source; + + std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); + auto status = update_ft.wait_for(period); + + if (status != std::future_status::ready) { + auto ok = stop_source.stop_requested(); + return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; } - if (is_zero) { // just update once - return _updateFunc(); - } else { - // try to update once - auto ret = _updateFunc(); - if (ret) { - return ret; - } + _updateCondVar->notify_all(); - _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; - } + return update_ft.get(); } // block the thread and wait for data to be ready (external synchronization) @@ -412,13 +472,17 @@ public: std::lock_guard thread_lock{*_updateMutex}; - error_t ret = _updateFunc(); + // error_t ret = _updateFunc(); - if (!ret) { - mcc_copy_telemetry_data(_data, tdata); - } + // if (!ret) { + // mcc_copy_telemetry_data(_data, tdata); + // } - return ret; + // return ret; + + mcc_copy_telemetry_data(_data, tdata); + + return MccTelemetryErrorCode::ERROR_OK; } @@ -434,8 +498,10 @@ protected: std::atomic_bool _updated; MccTelemetryData _data; - std ::function _updateTargetFunc{}; - std::function _updateFunc{}; + std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)}; + + std ::function _updateTargetFunc{}; + std::function _updateFunc{}; std::function _setTargetFunc{}; std::unique_ptr _updateMutex;