diff --git a/mcc/CMakeLists.txt b/mcc/CMakeLists.txt index 806ee91..d0b0542 100644 --- a/mcc/CMakeLists.txt +++ b/mcc/CMakeLists.txt @@ -65,7 +65,7 @@ message(STATUS ${ERFA_INCLUDE_DIR}) add_subdirectory(bsplines) message(STATUS "BSPLINES_INCLUDE_DIR: " ${BSPLINES_INCLUDE_DIR}) - +include_directories(${BSPLINES_INCLUDE_DIR}) set(MCC_LIBRARY_SRC1 mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_telemetry.h diff --git a/mcc/bsplines/CMakeLists.txt b/mcc/bsplines/CMakeLists.txt index cb2e64e..224ebd2 100644 --- a/mcc/bsplines/CMakeLists.txt +++ b/mcc/bsplines/CMakeLists.txt @@ -27,5 +27,9 @@ FortranCInterface_HEADER(FortranCInterface.h ) FortranCInterface_VERIFY(CXX) -add_library(bsplines STATIC ${src_files} mcc_bsplines.h) set(BSPLINES_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR} PARENT_SCOPE) +include_directories(${BSPLINES_INCLUDE_DIR}) + +add_library(bsplines STATIC ${src_files} mcc_bsplines.h) + + diff --git a/mcc/mcc_generics.h b/mcc/mcc_generics.h index d2e49a9..9691cc1 100644 --- a/mcc/mcc_generics.h +++ b/mcc/mcc_generics.h @@ -442,6 +442,8 @@ struct mcc_telemetry_interface_t { return std::forward(self).waitForTelemetryData(data, timeout); } + + // set target coordinates template SelfT> RetT setPointingTarget(this SelfT&& self, mcc_celestial_point_c auto pt) { @@ -467,11 +469,16 @@ concept mcc_PCM_result_c = requires(T t) { template struct mcc_PCM_interface_t { + virtual ~mcc_PCM_interface_t() = default; + template SelfT> RetT computePCM(this SelfT&& self, mcc_celestial_point_c auto pt, ResT* result) { return std::forward(self).computePCM(std::move(pt), result); } + +protected: + mcc_PCM_interface_t() = default; }; template @@ -644,4 +651,22 @@ template concept mcc_pzone_container_c = std::derived_from>; +template +struct mcc_generic_mount_interface_t { + virtual ~mcc_generic_mount_interface_t() = default; + + RetT slew(mcc_celestial_point_c auto pt) {} + +protected: + mcc_generic_mount_interface_t() = default; +}; + +template +concept mcc_generic_mount_c = mcc_telemetry_c && requires(T t) { + requires mcc_error_c; + + { t.slewToTarget() } -> std::same_as; + { t.guidingTarget() } -> std::same_as; +}; + } // namespace mcc diff --git a/mcc/mcc_pcm.h b/mcc/mcc_pcm.h new file mode 100644 index 0000000..f75b811 --- /dev/null +++ b/mcc/mcc_pcm.h @@ -0,0 +1,278 @@ + +#pragma once + + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + +/* A REFERENCE "POINTING-CORRECTION-MODEL" CLASS IMPLEMENTATION */ + + +#include +#include "bsplines/mcc_bsplines.h" +#include "mcc_generics.h" + +namespace mcc +{ + +enum class MccDefaultPCMErrorCode : int { ERROR_OK, ERROR_INVALID_INPUTS_BISPLEV, ERROR_EXCEED_MAX_ITERS }; + +/* error category definition */ + +// error category +struct MccDefaultPCMCategory : public std::error_category { + MccDefaultPCMCategory() : std::error_category() {} + + const char* name() const noexcept + { + return "ADC_GENERIC_DEVICE"; + } + + std::string message(int ec) const + { + MccDefaultPCMErrorCode err = static_cast(ec); + + switch (err) { + case MccDefaultPCMErrorCode::ERROR_OK: + return "OK"; + case MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV: + return "invalid input arguments for bispev"; + case MccDefaultPCMErrorCode::ERROR_EXCEED_MAX_ITERS: + return "exceed maximum of iterations number"; + default: + return "UNKNOWN"; + } + } + + static const MccDefaultPCMCategory& get() + { + static const MccDefaultPCMCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccDefaultPCMErrorCode ec) +{ + return std::error_code(static_cast(ec), MccDefaultPCMCategory::get()); +} +} // namespace mcc + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + + +namespace mcc +{ + +namespace details +{ + +template +struct _pcm_result_t { + CT dx, dy; +}; + + +} // namespace details + +// type of PCM corrections (algorithm used): +// PCM_TYPE_GEOMETRY - "classic" geometry-based correction coefficients +// PCM_TYPE_GEOMETRY_BSPLINE - previous one and additional 2D B-spline corrections +// PCM_TYPE_BSPLINE - pure 2D B-spline corrections +enum class MccDefaultPCMType { PCM_TYPE_GEOMETRY, PCM_TYPE_GEOMETRY_BSPLINE, PCM_TYPE_BSPLINE }; + +template +class MccDefaultPCM : public mcc_PCM_interface_t> +{ +public: + static constexpr MccMountType mountType = MOUNT_TYPE; + + typedef std::error_code error_t; + typedef double coord_t; + + typedef details::_pcm_result_t pcm_result_t; + + // "classic" geometric PEC coefficients + struct pcm_geom_coeffs_t { + typedef double coeff_t; + + coeff_t zeroPointX; + coeff_t zeroPointY; + coeff_t collimationErr; // tube collimation error + coeff_t nonperpendErr; // X-Y axes nonperpendicularity + coeff_t misalignErr1; // misalignment of hour-angle/azimuth axis: left-right for equatorial, East-West for + // alt-azimuthal + coeff_t misalignErr2; // misalignment of hour-angle/azimuth axis: vertical for equatorial, North-South for + // alt-azimuthal + + coeff_t tubeFlexure; + coeff_t forkFlexure; + coeff_t DECaxisFlexure; // declination axis flexure + }; + + // B-splines related data structure (coefficients, knots ...) + struct pcm_bspline_t { + typedef double knot_t; + typedef double coeff_t; + + size_t bsplDegreeX = 3; + size_t bsplDegreeY = 3; + + std::vector knotsX{}; + std::vector knotsY{}; + + std::vector coeffsX{}; + std::vector coeffsY{}; + }; + + struct pcm_data_t { + MccDefaultPCMType type{MccDefaultPCMType::PCM_TYPE_GEOMETRY}; + double siteLatitude{0.0}; // in radians + pcm_geom_coeffs_t geomCoefficients{}; + pcm_bspline_t bspline{}; + }; + + // constructors + + MccDefaultPCM(pcm_data_t pdata) : _pecData(std::move(pdata)), _pecDataMutex(new std::mutex) {} + + MccDefaultPCM(MccDefaultPCM&& other) = default; + MccDefaultPCM& operator=(MccDefaultPCM&& other) = default; + + MccDefaultPCM(const MccDefaultPCM&) = delete; + MccDefaultPCM& operator=(const MccDefaultPCM&) = delete; + + virtual ~MccDefaultPCM() = default; + + void setData(pcm_data_t pdata) + { + std::lock_guard lock(*_pecDataMutex); + + _pecData = std::move(pdata); + } + + + pcm_data_t getData() const + { + std::lock_guard lock(*_pecDataMutex); + + return _pecData; + } + + void setType(MccDefaultPCMType type) + { + std::lock_guard lock(*_pecDataMutex); + + _pecData.type = type; + } + + MccDefaultPCMType getType() const + { + std::lock_guard lock(*_pecDataMutex); + + return _pecData.type; + } + + // The computed PEC quantities must be interpretated as: + // apparent_X = encoder_X + pcm_result_t.dx + // apparent_Y = encoder_Y + pcm_result_t.dy + // so, input x and y are assumed to be mount axis encoder coordinates + error_t compute(mcc_celestial_point_c auto pt, pcm_result_t& res) + { + std::lock_guard lock(*_pecDataMutex); + + if constexpr (mcc_is_equatorial_mount) { // equatorial + if (_pecData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY) { + const auto cosPhi = std::cos(_pecData.siteLatitude); + const auto sinPhi = std::sin(_pecData.siteLatitude); + const auto tanY = std::tan(pt.Y); + const auto sinX = std::sin(pt.X); + const auto cosX = std::cos(pt.X); + const auto cosY = std::cos(pt.Y); + + + if (utils::isEqual(cosY, 0.0)) { + res.dx = _pecData.geomCoefficients.zeroPointX; + } else { + res.dx = _pecData.geomCoefficients.zeroPointX + _pecData.geomCoefficients.collimationErr / cosY + + _pecData.geomCoefficients.nonperpendErr * tanY - + _pecData.geomCoefficients.misalignErr1 * cosX * tanY + + _pecData.geomCoefficients.misalignErr2 * sinX * tanY + + _pecData.geomCoefficients.tubeFlexure * cosPhi * sinX / cosY - + _pecData.geomCoefficients.DECaxisFlexure * (cosPhi * cosX + sinPhi * tanY); + } + + res.dy = _pecData.geomCoefficients.zeroPointY + _pecData.geomCoefficients.misalignErr1 * sinX + + _pecData.geomCoefficients.misalignErr2 * cosX + + _pecData.geomCoefficients.tubeFlexure * (cosPhi * cosX * std::sin(pt.Y) - sinPhi * cosY); + + if constexpr (mountType == MccMountType::FORK_TYPE) { + if (!utils::isEqual(cosX, 0.0)) { + res.dy += _pecData.geomCoefficients.forkFlexure / cosX; + } + } + } + + if (_pecData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE || + _pecData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) { + double spl_valX, spl_valY; + + int ret = bsplines::fitpack_eval_spl2d(_pecData.bspline.knotsX, _pecData.bspline.knotsY, + _pecData.bspline.coeffsX, pt.X, pt.Y, spl_valX, + _pecData.bspline.bsplDegreeX, _pecData.bspline.bsplDegreeY); + + if (ret) { + res.dx = std::numeric_limits::quiet_NaN(); + res.dy = std::numeric_limits::quiet_NaN(); + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + + ret = bsplines::fitpack_eval_spl2d(_pecData.bspline.knotsX, _pecData.bspline.knotsY, + _pecData.bspline.coeffsY, pt.X, pt.Y, spl_valY, + _pecData.bspline.bsplDegreeX, _pecData.bspline.bsplDegreeY); + + if (ret) { + res.dx = std::numeric_limits::quiet_NaN(); + res.dy = std::numeric_limits::quiet_NaN(); + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + + res.dx += spl_valX; + res.dy += spl_valY; + } + } else if constexpr (mcc_is_altaz_mount) { + static_assert(false, "NOT IMPLEMENTED!"); + } else { + static_assert(false, "UNSUPPORTED"); + } + + return MccDefaultPCMErrorCode::ERROR_OK; + } + + +private: + pcm_data_t _pecData; + + std::unique_ptr _pecDataMutex; +}; + + +typedef MccDefaultPCM MccMountDefaultAltAzPec; +typedef MccDefaultPCM MccMountDefaultForkPec; + +static_assert(mcc_PCM_c, ""); +static_assert(std::movable); + + +} // namespace mcc diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index 79cae35..0307648 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -94,20 +94,13 @@ inline std::error_code make_error_code(MccTelemetryErrorCode ec) } -/* TELEMETRY UPDATE POLICY */ -enum class MccTelemetryUpdatePolicy : int { TEMETRY_UPDATE_INNER, TEMETRY_UPDATE_EXTERNAL }; - - -template class MccTelemetry : public mcc_telemetry_interface_t { protected: static constexpr uint16_t internalUpdatingIntervalDiv = 5; public: - static constexpr MccTelemetryUpdatePolicy updatePolicy = UPDATE_POLICY; - static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100); static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5; @@ -115,13 +108,17 @@ public: MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) - : _isDataUpdated(false), + : _isDataUpdated(new std::atomic_bool()), _data(), - _internalUpdating(false), + _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; @@ -240,6 +237,8 @@ public: 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; @@ -375,12 +374,21 @@ public: } + MccTelemetry(MccTelemetry&&) = default; + MccTelemetry& operator=(MccTelemetry&&) = default; + + MccTelemetry(const MccTelemetry&) = delete; + MccTelemetry& operator=(const MccTelemetry&) = delete; + + virtual ~MccTelemetry() { - _internalUpdatingStopSource.request_stop(); + stopInternalTelemetryDataUpdating(); if (_internalUpdatingFuture.valid()) { - _internalUpdatingFuture.get(); + // try to exit correctly + auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1)); + // _internalUpdatingFuture.get(); } }; @@ -388,6 +396,8 @@ public: template DT telemetryDataUpdateInterval() const { + std::lock_guard lock{_currentUpdateIntervalMutex}; + return std::chrono::duration_cast
(_currentUpdateInterval); } @@ -400,6 +410,8 @@ public: { 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 { @@ -407,50 +419,57 @@ public: } } + // asynchronuosly periodicaly update telemetry data (internal synchronization) void startInternalTelemetryDataUpdating() { using intv_t = std::remove_cvref_t; - _internalUpdating = true; + *_internalUpdating = true; _internalUpdatingFuture = std::async( std::launch::async, - [this](std::stop_token stop_token) { - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_OK; - } - - _lastUpdateError = updateTelemetryData(defaultInternalUpdateTimeout); - if (_lastUpdateError) { - _internalUpdating = false; - return _lastUpdateError; - } - - auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; - - for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_OK; + [this](std::stop_token stop_token) -> error_t { + while (!stop_token.stop_requested()) { + _lastUpdateError = updateTelemetryData(defaultInternalUpdateTimeout); + if (_lastUpdateError) { + *_internalUpdating = false; + return _lastUpdateError; } - std::this_thread::sleep_for(sleep_td); - } + { + std::lock_guard lock{_currentUpdateIntervalMutex}; - if (stop_token.stop_requested()) { - return MccTelemetryErrorCode::ERROR_OK; - } + // compute it here because of possible changing _currentUpdateInterval + auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; - if constexpr (std::floating_point) { - std::this_thread::sleep_for(sleep_td); - } else { - auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv; + for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { + if (stop_token.stop_requested()) { + break; + } - if (rem.count()) { - std::this_thread::sleep_for(rem); - } else { - std::this_thread::sleep_for(sleep_td); + 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()); } @@ -459,13 +478,13 @@ public: void stopInternalTelemetryDataUpdating() { _internalUpdatingStopSource.request_stop(); - _internalUpdating = false; + *_internalUpdating = false; } bool isInternalTelemetryDataUpdating() const { - return _internalUpdating; + return *_internalUpdating; } @@ -475,24 +494,26 @@ public: std::stop_source stop_source; - _isDataUpdated = false; + *_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) { - auto ok = stop_source.stop_requested(); - return _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + if (status == std::future_status::ready) { + *_isDataUpdated = true; + _lastUpdateError = update_ft.get(); + } else { + stop_source.request_stop(); + _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; } - _isDataUpdated = true; - + // unblock waiting threads even in the case of timeout! _updateCondVar->notify_all(); - return _lastUpdateError = update_ft.get(); + return _lastUpdateError; } - // block the thread and wait for data to be ready (external synchronization) + // 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) { @@ -501,7 +522,7 @@ public: std::unique_lock ulock(*_updateMutex); - auto res = _updateCondVar->wait_for(ulock, timeout, [this]() { return _isDataUpdated; }); + auto res = _updateCondVar->wait_for(ulock, timeout, [this]() { return *_isDataUpdated; }); if (res == std::cv_status::timeout) { return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; } @@ -539,11 +560,12 @@ public: protected: - std::atomic_bool _isDataUpdated; + std::unique_ptr _isDataUpdated; MccTelemetryData _data; - std::atomic_bool _internalUpdating{false}; + std::unique_ptr _internalUpdating; std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)}; + std::unique_ptr _currentUpdateIntervalMutex; std::future _internalUpdatingFuture{}; std::stop_source _internalUpdatingStopSource{}; @@ -555,7 +577,9 @@ protected: std::unique_ptr _updateCondVar; error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; - std::jthread _timerThread; }; + +static_assert(mcc_telemetry_c, ""); + } // namespace mcc