From 33002f1711994dd3c7970f393c529e9a99b31a31 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Thu, 21 Aug 2025 03:47:53 +0300 Subject: [PATCH] ... --- mcc/CMakeLists.txt | 4 +- mcc/mcc_defaults.h | 6 + mcc/mcc_generics.h | 26 ++- mcc/mcc_pzone.h | 22 ++- mcc/mcc_pzone_container.h | 384 ++++++++++++++++++++++++++++++++++++++ mcc/mcc_telemetry.h | 8 +- mcc/mcc_traits.h | 22 +++ mcc/tests/ccte_test.cpp | 16 ++ 8 files changed, 469 insertions(+), 19 deletions(-) create mode 100644 mcc/mcc_pzone_container.h diff --git a/mcc/CMakeLists.txt b/mcc/CMakeLists.txt index 1fc21d0..f0e6869 100644 --- a/mcc/CMakeLists.txt +++ b/mcc/CMakeLists.txt @@ -64,7 +64,9 @@ set(ERFA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/erfa_lib) message(STATUS ${ERFA_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_pzone.h mcc_telemetry.h) +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 + mcc_pzone.h mcc_pzone_container.h) set(MCC_LIBRARY1 mcc1) add_library(${MCC_LIBRARY1} INTERFACE ${MCC_LIBRARY_SRC1}) target_compile_features(${MCC_LIBRARY1} INTERFACE cxx_std_23) diff --git a/mcc/mcc_defaults.h b/mcc/mcc_defaults.h index caeef69..74428ac 100644 --- a/mcc/mcc_defaults.h +++ b/mcc/mcc_defaults.h @@ -18,6 +18,12 @@ namespace mcc typedef std::chrono::system_clock::time_point MccTimePoint; +template +static constexpr DT mcc_infinite_duration_v = + std::floating_point ? DT{std::numeric_limits::infinity()} + : DT{std::numeric_limits::max()}; + + /* DEFAULT JULIAN DAY CLASS */ struct MccJulianDay { diff --git a/mcc/mcc_generics.h b/mcc/mcc_generics.h index 7ee0ada..400fcbd 100644 --- a/mcc/mcc_generics.h +++ b/mcc/mcc_generics.h @@ -142,6 +142,8 @@ static constexpr ErrT mcc_deduce_error(const DErrT& err, const ErrT& default_err } } + + /* ATMOSPHERIC REFRACTION MODEL CLASS CONCEPT */ template @@ -179,7 +181,8 @@ static constexpr void mcc_copy_celestial_point(mcc_celestial_point_c auto const& } to_pt->pair_kind = from_pt.pair_kind; - to_pt->time_point = std::chrono::time_point_casttime_point)::duration>(from_pt); + to_pt->time_point = + std::chrono::time_point_casttime_point)::duration>(from_pt.time_point); to_pt->X = (double)from_pt.X; to_pt->Y = (double)from_pt.Y; } @@ -214,7 +217,8 @@ static constexpr void mcc_copy_eqt_hrz_coord(mcc_eqt_hrz_coord_c auto const& fro } to_pt->pair_kind = from_pt.pair_kind; - to_pt->time_point = std::chrono::time_point_casttime_point)::duration>(from_pt); + to_pt->time_point = + std::chrono::time_point_casttime_point)::duration>(from_pt.time_point); to_pt->X = (double)from_pt.X; to_pt->Y = (double)from_pt.Y; @@ -321,7 +325,8 @@ static constexpr void mcc_copy_pointing_target_coord(mcc_pointing_target_coord_c } to_pt->pair_kind = from_pt.pair_kind; - to_pt->time_point = std::chrono::time_point_casttime_point)::duration>(from_pt); + to_pt->time_point = + std::chrono::time_point_casttime_point)::duration>(from_pt.time_point); to_pt->X = (double)from_pt.X; to_pt->Y = (double)from_pt.Y; @@ -374,7 +379,8 @@ static constexpr void mcc_copy_telemetry_data(mcc_telemetry_data_c auto const& f } to_pt->pair_kind = from_pt.pair_kind; - to_pt->time_point = std::chrono::time_point_casttime_point)::duration>(from_pt); + to_pt->time_point = + std::chrono::time_point_casttime_point)::duration>(from_pt.time_point); to_pt->X = (double)from_pt.X; to_pt->Y = (double)from_pt.Y; @@ -571,10 +577,10 @@ struct mcc_pzone_container_interface_t { template SelfT, typename InputT> - RetT inPZone(this SelfT&& self, InputT coords, std::ranges::output_range auto* result) + RetT inPZone(this SelfT&& self, InputT coords, bool* common_result, std::ranges::output_range auto* result) requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) { - return std::forward(self).InPZone(std::move(coords), result); + return std::forward(self).InPZone(std::move(coords), common_result, result); } @@ -592,6 +598,14 @@ struct mcc_pzone_container_interface_t { return std::forward(self).timeFromPZone(std::move(coords), res_time); } + + template SelfT, typename InputT, mcc_celestial_point_c CPT> + RetT intersectPZone(this SelfT&& self, InputT coords, std::ranges::output_range auto* result) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + return std::forward(self).intersectPZone(std::move(coords), result); + } + protected: mcc_pzone_container_interface_t() = default; }; diff --git a/mcc/mcc_pzone.h b/mcc/mcc_pzone.h index 25ab488..1c94b40 100644 --- a/mcc/mcc_pzone.h +++ b/mcc/mcc_pzone.h @@ -84,11 +84,6 @@ class MccAltLimitPZ : public mcc_pzone_interface_t protected: static constexpr auto pi2 = std::numbers::pi * 2.0; - template - static constexpr T infiniteDuration = - std::integral ? T{std::numeric_limits::max()} - : T{std::numeric_limits::infinity()}; - public: typedef std::error_code error_t; @@ -115,6 +110,10 @@ public: }; } + + MccAltLimitPZ(MccAltLimitPZ&&) = default; + MccAltLimitPZ(const MccAltLimitPZ&) = default; + consteval std::string_view name() const { return KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE" @@ -189,7 +188,7 @@ public: } if (!doesObjectReachZone(dec)) { - *res_time = infiniteDuration; + *res_time = mcc_infinite_duration_v; return ret; } @@ -241,7 +240,7 @@ public: } if (!doesObjectExitFromZone(dec)) { - *res_time = infiniteDuration; + *res_time = mcc_infinite_duration_v; return ret; } @@ -343,9 +342,13 @@ protected: return false; } } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { - if ((dd < (_absLat - _altLimit)) || (dd > (_absLat + _altLimit))) { // never rise above altitude limit + auto z = std::numbers::pi / 2.0 - _altLimit; + if ((dd < (_absLat - z)) || (dd > (_absLat + z))) { // never rise above altitude limit return false; } + // if ((dd < (_absLat - _altLimit)) || (dd > (_absLat + _altLimit))) { // never rise above altitude limit + // return false; + // } } else { static_assert(false, "UNKNOWN ALTITUDE LIMIT TYPE!"); } @@ -388,7 +391,8 @@ protected: double cos_ha = (_sinAlim - std::sin(dec_app) * _sinLat) / std::cos(dec_app) / _cosLat; if (cos_ha > 1.0) { // should not be! - *result = infiniteDuration; + *result = mcc_infinite_duration_v; + return; } double ha; diff --git a/mcc/mcc_pzone_container.h b/mcc/mcc_pzone_container.h new file mode 100644 index 0000000..c023a60 --- /dev/null +++ b/mcc/mcc_pzone_container.h @@ -0,0 +1,384 @@ +#pragma once + +#pragma once + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + + +/* IMPLEMENTATION OF PROHIBITED ZONES CONTAINER */ + +#include "mcc_defaults.h" + +namespace mcc +{ + +enum class MccPZoneContainerErrorCode : int { + ERROR_OK, + ERROR_NULLPTR, + ERROR_INZONE_FUNC, + ERROR_TIMETO_FUNC, + ERROR_TIMEFROM_FUNC, + ERROR_INTERSECT_FUNC +}; + +} // namespace mcc + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + + + +} // namespace std + + +namespace mcc +{ + + +/* error category definition */ + +// error category +struct MccPZoneContainerCategory : public std::error_category { + MccPZoneContainerCategory() : std::error_category() {} + + const char* name() const noexcept + { + return "ALTITUDE-LIMIT-PZ"; + } + + std::string message(int ec) const + { + MccPZoneContainerErrorCode err = static_cast(ec); + + switch (err) { + case MccPZoneContainerErrorCode::ERROR_OK: + return "OK"; + case MccPZoneContainerErrorCode::ERROR_NULLPTR: + return "nullptr argument"; + case MccPZoneContainerErrorCode::ERROR_INZONE_FUNC: + return "inPZone method error"; + case MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC: + return "timeToPZone method error"; + case MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC: + return "timeFromPZone method error"; + case MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC: + return "intersectPZone method error"; + default: + return "UNKNOWN"; + } + } + + static const MccPZoneContainerCategory& get() + { + static const MccPZoneContainerCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccPZoneContainerErrorCode ec) +{ + return std::error_code(static_cast(ec), MccPZoneContainerCategory::get()); +} + + + +template +class MccPZoneContainer : public mcc_pzone_container_interface_t +{ +public: + typedef std::error_code error_t; + + typedef DurT duration_t; + + MccPZoneContainer() = default; + + MccPZoneContainer(MccPZoneContainer&&) = default; + MccPZoneContainer(const MccPZoneContainer&) = default; + + MccPZoneContainer& operator=(MccPZoneContainer&&) = default; + MccPZoneContainer& operator=(const MccPZoneContainer&) = default; + + + virtual ~MccPZoneContainer() = default; + + size_t addPZone(mcc_prohibited_zone_c auto zone) + { + auto sptr = std::make_shared(std::move(zone)); + + _inZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, bool* res) { + auto ret = sptr->inPZone(pt, res); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INZONE_FUNC)); + }); + + _inZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, bool* res) { + auto ret = sptr->inPZone(pt, res); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INZONE_FUNC)); + }); + + + _timeToZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) { + auto ret = sptr->timeToPZone(pt, res_time); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC)); + }); + + _timeToZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) { + auto ret = sptr->timeToPZone(pt, res_time); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMETO_FUNC)); + }); + + _timeFromZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, duration_t* res_time) { + auto ret = sptr->timeFromPZone(pt, res_time); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC)); + }); + + _timeFromZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, duration_t* res_time) { + auto ret = sptr->timeFromPZone(pt, res_time); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_TIMEFROM_FUNC)); + }); + + _intersectZoneFuncCPT.emplace_back([sptr](const MccCelestialPoint& pt, MccCelestialPoint* res_pt) { + auto ret = sptr->intersectPZone(pt, res_pt); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); + }); + + _intersectZoneFuncEHC.emplace_back([sptr](const MccEqtHrzCoords& pt, MccCelestialPoint* res_pt) { + auto ret = sptr->intersectPZone(pt, res_pt); + + return mcc_deduce_error(ret, mcc::make_error_code(MccPZoneContainerErrorCode::ERROR_INTERSECT_FUNC)); + }); + + return _inZoneFuncCPT.size(); + } + + + void clearPZones() + { + _inZoneFuncCPT.clear(); + _inZoneFuncEHC.clear(); + + _timeToZoneFuncCPT.clear(); + _timeToZoneFuncEHC.clear(); + + _timeFromZoneFuncCPT.clear(); + _timeFromZoneFuncEHC.clear(); + + _intersectZoneFuncCPT.clear(); + _intersectZoneFuncEHC.clear(); + } + + + size_t sizePZones() const + { + return _inZoneFuncCPT.size(); + } + + + template + error_t inPZone(InputT coords, bool* common_result, std::ranges::output_range auto* result = nullptr) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + if (common_result == nullptr) { + return MccPZoneContainerErrorCode::ERROR_NULLPTR; + } + + *common_result = false; + + auto apply_func = [&](auto& func, auto& pt_arg, size_t i) { + bool res; + error_t ret = func(pt_arg, &res); + if (!ret) { + *common_result |= res; + if (result) { + if (traits::mcc_range_size(*result) == i) { + std::back_inserter(*result) = res; + } else { + auto ptr = result->begin(); + std::ranges::advance(ptr, i); + *ptr = res; + } + } + } + }; + + return forEach(coords, apply_func, _inZoneFuncCPT, _inZoneFuncEHC); + } + + + // template + // error_t timeToPZone(InputT coords, std::ranges::output_range
auto* res_time) + // requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + template + error_t timeToPZone(InputT coords, R* res_time) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) && traits::mcc_output_duration_range_c + { + if (res_time == nullptr) { + return MccPZoneContainerErrorCode::ERROR_NULLPTR; + } + + using DT = std::ranges::range_value_t; + + duration_t res; + + auto apply_func = [&](auto& func, auto& pt_arg, size_t i) { + error_t ret = func(pt_arg, &res); + + DT val; + if (res == mcc_infinite_duration_v) { + val = mcc_infinite_duration_v
; + } else { + val = std::chrono::duration_cast
(res); + } + + if (!ret) { + if (traits::mcc_range_size(*res_time) == i) { + std::back_inserter(*res_time) = val; + } else { + auto ptr = res_time->begin(); + std::ranges::advance(ptr, i); + *ptr = val; + } + } + + return ret; + }; + + return forEach(coords, apply_func, _timeToZoneFuncCPT, _timeToZoneFuncEHC); + } + + + // template + // error_t timeFromPZone(InputT coords, std::ranges::output_range
auto* res_time) + // requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + template + error_t timeFromPZone(InputT coords, R* res_time) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) && traits::mcc_output_duration_range_c + { + if (res_time == nullptr) { + return MccPZoneContainerErrorCode::ERROR_NULLPTR; + } + + using DT = std::ranges::range_value_t; + + duration_t res; + + auto apply_func = [&](auto& func, auto& pt_arg, size_t i) { + error_t ret = func(pt_arg, &res); + if (!ret) { + if (traits::mcc_range_size(*res_time) == i) { + std::back_inserter(*res_time) = std::chrono::duration_cast
(res); + } else { + auto ptr = res_time->begin(); + std::ranges::advance(ptr, i); + *ptr = std::chrono::duration_cast
(res); + } + } + + return ret; + }; + + return forEach(coords, apply_func, _timeFromZoneFuncCPT, _timeFromZoneFuncEHC); + } + + // template + // error_t intersectPZone(InputT coords, std::ranges::output_range auto* result) + // requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + template + error_t intersectPZone(InputT coords, R* result) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) && + std::ranges::output_range> && + mcc_celestial_point_c> + { + if (result == nullptr) { + return MccPZoneContainerErrorCode::ERROR_NULLPTR; + } + + using CPT = std::ranges::range_value_t; + + MccCelestialPoint pt; + + auto apply_func = [&](auto& func, auto& pt_arg, size_t i) { + error_t ret = func(pt_arg, &pt); + if (!ret) { + if (traits::mcc_range_size(*result) == i) { + std::back_inserter(*result) = CPT(); + } + + auto ptr = result->begin(); + std::ranges::advance(ptr, i); + mcc_copy_celestial_point(pt, &(*ptr)); + } + + return ret; + }; + + return forEach(coords, apply_func, _intersectZoneFuncCPT, _intersectZoneFuncEHC); + } + + +protected: + std::vector> _inZoneFuncCPT; + std::vector> _inZoneFuncEHC; + + std::vector> _timeToZoneFuncCPT; + std::vector> _timeToZoneFuncEHC; + + std::vector> _timeFromZoneFuncCPT; + std::vector> _timeFromZoneFuncEHC; + + std::vector> _intersectZoneFuncCPT; + std::vector> _intersectZoneFuncEHC; + + error_t forEach(auto const& coords, auto& apply_func, auto& containerCPT, auto& containerEHC) + { + using coords_t = std::remove_cvref_t; + + error_t ret = MccPZoneContainerErrorCode::ERROR_OK; + + size_t i = 0; + + if constexpr (mcc_eqt_hrz_coord_c) { + MccEqtHrzCoords pt; + mcc_copy_eqt_hrz_coord(coords, &pt); + + for (auto& func : containerEHC) { + ret = apply_func(func, pt, i); + if (ret) { + break; + } + + ++i; + } + + } else { + MccCelestialPoint pt; + mcc_copy_celestial_point(coords, &pt); + + for (auto& func : containerCPT) { + ret = apply_func(func, pt, i); + if (ret) { + break; + } + + ++i; + } + } + + return ret; + } +}; + +} // namespace mcc diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index 756c768..def087e 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -387,13 +387,15 @@ public: return MccTelemetryErrorCode::ERROR_NULLPTR; } - std::unique_lock thread_lock(*_updateMutex); + std::unique_lock ulock(*_updateMutex); - auto res = _updateCondVar->wait_for(thread_lock, timeout, [this]() { return _updated; }); + auto res = _updateCondVar->wait_for(ulock, timeout, [this]() { return _updated; }); 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); } @@ -424,7 +426,7 @@ public: { std::lock_guard lock{*_updateMutex}; - return _setTargetFunc(); + return _setTargetFunc(pt); } diff --git a/mcc/mcc_traits.h b/mcc/mcc_traits.h index 183bcf7..fb583ae 100644 --- a/mcc/mcc_traits.h +++ b/mcc/mcc_traits.h @@ -7,6 +7,18 @@ namespace mcc::traits { + +template +static constexpr size_t mcc_range_size(const R& r) +{ + if constexpr (std::ranges::sized_range) { + return r.size(); + } else { + return std::ranges::distance(r.begin(), r.end()); + } +} + + template concept mcc_char_view = std::ranges::view && std::same_as, char>; @@ -59,6 +71,16 @@ concept mcc_systime_c = requires { }; +template +concept mcc_output_duration_range_c = + std::ranges::output_range> && mcc_time_duration_c>; +// concept mcc_output_duration_range_c = std::ranges::range && requires(R r) { +// [](std::type_identity>) { + +// }(std::ranges::range_value_t()); + +// }; + /* a callable concept and its signature traits */ template diff --git a/mcc/tests/ccte_test.cpp b/mcc/tests/ccte_test.cpp index b126af5..a8db752 100644 --- a/mcc/tests/ccte_test.cpp +++ b/mcc/tests/ccte_test.cpp @@ -2,6 +2,7 @@ #include "../mcc_ccte_erfa.h" #include "../mcc_pzone.h" +#include "../mcc_pzone_container.h" using namespace mcc::ccte::erfa; @@ -77,5 +78,20 @@ int main() std::cout << "INTERSEC AZ = " << mcc::MccAngle(icp.X).sexagesimal() << "\n"; std::cout << "INTERSEC ALT = " << mcc::MccAngle(icp.Y).sexagesimal() << "\n"; + + mcc::MccAltLimitPZ altmax(80.0_degs, state.lat, &erfa); + + mcc::MccPZoneContainer> pzcont; + + pzcont.addPZone(altmin); + pzcont.addPZone(altmax); + + // std::vector>> vm; + std::vector vm; + ret = pzcont.timeToPZone(eqhrz, &vm); + std::cout << "ret = " << ret.message() << "\n"; + std::cout << "TIME TO ZONE 1: " << vm[0] << "\n"; + std::cout << "TIME TO ZONE 2: " << vm[1] << "\n"; + return 0; }