#pragma once /* MOUNT CONTROL COMPONENTS LIBRARY */ /* IMPLEMENTATION OF SOME SIMPLE PROHIBITED ZONES */ #include "mcc_defaults.h" #include "mcc_generics.h" namespace mcc { enum class MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP }; } // namespace mcc namespace std { template <> class is_error_code_enum : public true_type { }; } // namespace std namespace mcc { /* MINIMAL OR MAXIMAL ALTITUDE PROHIBITED ZONES */ /* error category definition */ // error category struct MccAltLimitPZCategory : public std::error_category { MccAltLimitPZCategory() : std::error_category() {} const char* name() const noexcept { return "ALTITUDE-LIMIT-PZ"; } std::string message(int ec) const { MccAltLimitPZErrorCode err = static_cast(ec); switch (err) { case MccAltLimitPZErrorCode::ERROR_OK: return "OK"; case MccAltLimitPZErrorCode::ERROR_NULLPTR: return "input argument os nullptr"; case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM: return "coordinate transformation error"; case MccAltLimitPZErrorCode::ERROR_PCM_COMP: return "PCM computation error"; default: return "UNKNOWN"; } } static const MccAltLimitPZCategory& get() { static const MccAltLimitPZCategory constInst; return constInst; } }; inline std::error_code make_error_code(MccAltLimitPZErrorCode ec) { return std::error_code(static_cast(ec), MccAltLimitPZCategory::get()); } enum class MccAltLimitKind { MIN_ALT_LIMIT, MAX_ALT_LIMIT }; template class MccAltLimitPZ : public mcc_pzone_interface_t { protected: static constexpr auto pi2 = std::numbers::pi * 2.0; public: static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_STOP; typedef std::error_code error_t; MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine) : _altLimit(MccAngle(alt_limit).normalize()), _cosALim(cos(_altLimit)), _sinAlim(sin(_altLimit)), _cosLat(cos(latitude)), _sinLat(sin(latitude)), _absLat(abs(latitude)), _latLim(MccAltLimitPZ::pi2 - _altLimit) { _transformCoordinates = [ccte_engine](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t { if (to_pt == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } auto err = ccte_engine->transformCoordinates(from_pt, to_pt); if (!err) { return MccAltLimitPZErrorCode::ERROR_OK; } if (std::same_as) { return err; } else { return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM; } }; _transformCoordinatesEqtHrzCoords = [ccte_engine](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t { if (to_pt == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } auto err = ccte_engine->transformCoordinates(from_pt, to_pt); if (!err) { return MccAltLimitPZErrorCode::ERROR_OK; } if (std::same_as) { return err; } else { return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM; } }; } MccAltLimitPZ(MccAltLimitPZ&&) = default; MccAltLimitPZ(const MccAltLimitPZ&) = default; consteval std::string_view name() const { return KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE" : KIND == MccAltLimitKind::MAX_ALT_LIMIT ? "MAXALT-ZONE" : "ALTLIMIT-UNKNOWN"; } template error_t inPZone(InputT coords, bool* result) requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) { double alt; if (result == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } error_t ret = MccAltLimitPZErrorCode::ERROR_OK; if constexpr (mcc_eqt_hrz_coord_c) { alt = coords.ALT; } else { MccCelestialPoint to_pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = coords.time_point}; ret = getCoord(coords, &to_pt); if (ret) { return ret; } alt = to_pt.Y; } if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { *result = alt <= _altLimit; } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { *result = alt >= _altLimit; } return ret; } template error_t timeToPZone(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; if (res_time == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } double ha, dec; error_t ret = MccAltLimitPZErrorCode::ERROR_OK; bool inzone; ret = inPZone(coords, &inzone); if (ret) { return ret; } if (inzone) { *res_time = res_t{0}; return ret; } if constexpr (mcc_eqt_hrz_coord_c) { ha = coords.HA; dec = coords.DEC_APP; } else { MccCelestialPoint to_pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, .time_point = coords.time_point}; ret = getCoord(coords, &to_pt); if (ret) { return ret; } ha = to_pt.X; dec = to_pt.Y; } if (!doesObjectReachZone(dec)) { *res_time = mcc_infinite_duration_v; return ret; } if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination compute(ha, dec, false, res_time); } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper // culmination compute(ha, dec, true, res_time); } return ret; } 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; if (res_time == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } double ha, dec; error_t ret = MccAltLimitPZErrorCode::ERROR_OK; bool inzone; ret = inPZone(coords, &inzone); if (ret) { return ret; } if (!inzone) { *res_time = res_t{0}; return ret; } if constexpr (mcc_eqt_hrz_coord_c) { ha = coords.HA; dec = coords.DEC_APP; } else { MccCelestialPoint to_pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, .time_point = coords.time_point}; ret = getCoord(coords, &to_pt); if (ret) { return ret; } ha = to_pt.X; dec = to_pt.Y; } if (!doesObjectExitFromZone(dec)) { *res_time = mcc_infinite_duration_v; return ret; } if (!doesObjectReachZone(dec)) { *res_time = res_t{0}; return ret; } if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one before upper culmination compute(ha, dec, true, res_time); } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one after upper // culmination compute(ha, dec, false, res_time); } return ret; } // template // error_t intersectPZone(InputT coords, mcc_celestial_point_c auto* point) // requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) // { // // double ha, dec, az; // double dec, az; // if (point == nullptr) { // return MccAltLimitPZErrorCode::ERROR_NULLPTR; // } // error_t ret = MccAltLimitPZErrorCode::ERROR_OK; // if constexpr (mcc_eqt_hrz_coord_c) { // // ha = coords.HA; // dec = coords.DEC_APP; // } else { // MccCelestialPoint to_pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP}; // mcc_tp2tp(coords.time_point, to_pt.time_point); // ret = getCoord(coords, &to_pt); // if (ret) { // return ret; // } // // ha = to_pt.X; // dec = to_pt.Y; // } // double sinDec = sin(dec), cosDec = cos(dec); // auto cos_ha = (_sinAlim - sinDec * _sinLat) / cosDec / _cosLat; // if (cos_ha > 1.0) { // no intersection // // point->pair_kind = MccCoordPairKind::COORDS_KIND_GENERIC; // point->X = std::numeric_limits::quiet_NaN(); // point->Y = std::numeric_limits::quiet_NaN(); // return ret; // } // // WARNNIG: THE EXPRESSION ASSUMES THAT AZIMUTH IS COUNTED FROM THE SOUTH THROUGH THE WEST!!! // double cosA = (-sinDec * _cosLat + cosDec * _sinLat * cos_ha) / _cosALim; // if constexpr (KIND == // MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination // az = std::acos(cosA); // } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper // // culmination // az = -std::acos(cosA); // } // MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .X = az, .Y = _altLimit}; // mcc_tp2tp(coords.time_point, pt.time_point); // MccCelestialPoint to_pt{.pair_kind = point->pair_kind}; // mcc_tp2tp(point->time_point, to_pt.time_point); // ret = _transformCoordinates(pt, &to_pt); // if (!ret) { // point->X = MccAngle(to_pt.X).normalize(); // point->Y = MccAngle(to_pt.Y).normalize(); // } // return ret; // } 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)) { double dec, az; if (point == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } error_t ret = MccAltLimitPZErrorCode::ERROR_OK; if constexpr (mcc_eqt_hrz_coord_c) { // ha = coords.HA; dec = coords.DEC_APP; } else { MccCelestialPoint to_pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP}; mcc_tp2tp(coords.time_point, to_pt.time_point); ret = getCoord(coords, &to_pt); if (ret) { return ret; } // ha = to_pt.X; dec = to_pt.Y; } double sinDec = sin(dec), cosDec = cos(dec); auto cos_ha = (_sinAlim - sinDec * _sinLat) / cosDec / _cosLat; if (cos_ha > 1.0) { // no intersection (outputs are all NaN) // point->pair_kind = MccCoordPairKind::COORDS_KIND_GENERIC; point->X = std::numeric_limits::quiet_NaN(); point->Y = std::numeric_limits::quiet_NaN(); if constexpr (mcc_eqt_hrz_coord_c) { point->HA = std::numeric_limits::quiet_NaN(); point->RA_APP = std::numeric_limits::quiet_NaN(); point->DEC_APP = std::numeric_limits::quiet_NaN(); point->AZ = std::numeric_limits::quiet_NaN(); point->ZD = std::numeric_limits::quiet_NaN(); point->ALT = std::numeric_limits::quiet_NaN(); } return ret; } // WARNNIG: THE EXPRESSION ASSUMES THAT AZIMUTH IS COUNTED FROM THE SOUTH THROUGH THE WEST!!! double cosA = (-sinDec * _cosLat + cosDec * _sinLat * cos_ha) / _cosALim; if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination az = std::acos(cosA); } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper // culmination az = -std::acos(cosA); } MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .X = az, .Y = _altLimit}; mcc_tp2tp(coords.time_point, pt.time_point); if constexpr (mcc_eqt_hrz_coord_c) { MccEqtHrzCoords to_pt; mcc_tp2tp(point->time_point, to_pt.time_point); ret = _transformCoordinates(pt, &to_pt); if (!ret) { mcc_copy_eqt_hrz_coord(to_pt, point); } } else { MccCelestialPoint to_pt{.pair_kind = point->pair_kind}; mcc_tp2tp(point->time_point, to_pt.time_point); ret = _transformCoordinates(pt, &to_pt); if (!ret) { point->X = to_pt.X; point->Y = to_pt.Y; } } return ret; } protected: double _altLimit, _cosALim, _sinAlim; double _cosLat, _sinLat, _absLat, _latLim; std::function _transformCoordinates{}; std::function _transformCoordinatesEqtHrzCoords{}; error_t getCoord(mcc_celestial_point_c auto const& from_pt, MccCelestialPoint* to_pt) { MccCelestialPoint pt{ .pair_kind = from_pt.pair_kind, .time_point = from_pt.time_point, .X = from_pt.X, .Y = from_pt.Y}; return _transformCoordinates(pt, to_pt); } bool doesObjectReachZone(const double& dec_app) { // check for limit conditions auto dd = std::abs(dec_app); if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { dd += _altLimit; if (dd > _latLim) { // never fall below altitude limit return false; } } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_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!"); } return true; } bool doesObjectExitFromZone(const double& dec_app) { // check for limit conditions auto dd = std::abs(dec_app); if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) { dd -= _altLimit; if (-dd <= -_latLim) { // always below altitude limit return false; } } else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { if ((dd >= (_absLat - _altLimit)) || (dd <= (_absLat + _altLimit))) { // always above altitude limit return false; } } else { static_assert(false, "UNKNOWN ALTITUDE LIMIT TYPE!"); } return true; } void compute(const double& ha_app, const double& dec_app, bool before_upper_culm, traits::mcc_time_duration_c auto* result) { using res_t = std::remove_cvref_t; using period_t = typename res_t::period; double cos_ha = (_sinAlim - std::sin(dec_app) * _sinLat) / std::cos(dec_app) / _cosLat; if (cos_ha > 1.0) { // should not be! *result = mcc_infinite_duration_v; return; } double ha; // WARNING: what about south hemisphere?!!! if (before_upper_culm) { ha = -std::acos(cos_ha); // HA before upper culmination } else { ha = std::acos(cos_ha); // HA after upper culmination!! } auto time_ang = ha - ha_app; // in sideral time scale if (time_ang < 0.0) { // next day time_ang += MccAltLimitPZ::pi2; } time_ang /= mcc_sideral_to_UT1_ratio; // to UT1 time scale std::chrono::nanoseconds ns{ static_cast(time_ang * 43200.0 / std::numbers::pi * 1.0E9)}; period_t rat; *result = res_t{static_cast(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)}; } }; /* co-longitude axis (HA or AZ) limit switch prohibited zone */ template class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t { public: static_assert(AXIS_KIND == MccCoordKind::COORDS_KIND_AZ || AXIS_KIND == MccCoordKind::COORDS_KIND_HA, "UNSUPPORTED AXIS TYPE!"); typedef std::error_code error_t; static constexpr MccCoordKind axisKind = AXIS_KIND; static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_FLIP; // // min_limit_val and max_limit_val are hardware encoder angles in radians! // MccAxisLimitSwitchPZ(mcc_angle_c auto const& min_limit_val, mcc_angle_c auto const& max_limit_val, mcc_position_controls_c auto* controls) : _minLimit(min_limit_val), _maxLimit(max_limit_val) { _transformCoordinates = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t { if (to_pt == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } auto err = controls->transformCoordinates(from_pt, to_pt); if (!err) { return MccAltLimitPZErrorCode::ERROR_OK; } return mcc_deduce_error_code(err, MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM); }; _transformCoordinatesEqtHrzCoords = [controls](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t { if (to_pt == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } auto err = controls->transformCoordinates(from_pt, to_pt); if (!err) { return MccAltLimitPZErrorCode::ERROR_OK; } if (std::same_as) { return err; } else { return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM; } }; _computePCM = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t { MccPCMResult inv_res; auto err = controls->computeInversePCM(std::move(from_pt), &inv_res, to_pt); if (err) { return mcc_deduce_error_code(err, MccAltLimitPZErrorCode::ERROR_PCM_COMP); } return MccAltLimitPZErrorCode::ERROR_OK; }; } consteval std::string_view name() const { return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE" : axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE" : "UKNOWN"; } template error_t inPZone(InputT coords, bool* result) requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) { if (result == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } if constexpr (mcc_eqt_hrz_coord_c) { // assume here .X and are hardware encoder coordinate of corresponding axis *result = (coords.X > _maxLimit) || (coords.X < _minLimit); // *result = (coords.X < _maxLimit) && (coords.X > _minLimit); } else { // mcc_celestial_point_c if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware // *result = (coords.X < _maxLimit) && (coords.X > _minLimit); *result = (coords.X > _maxLimit) || (coords.X < _minLimit); } else { // here one needs transform input coordinates to hardware encoder ones MccCelestialPoint pt; auto ret = getHWCoords(std::move(coords), &pt); if (ret) { return ret; } *result = (pt.X > _maxLimit) || (pt.X < _minLimit); // *result = (pt.X < _maxLimit) && (pt.X > _minLimit); } } 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) { 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 = (_maxLimit - 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 = (_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{ 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; } // 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)) { if (point == nullptr) { return MccAltLimitPZErrorCode::ERROR_NULLPTR; } point->X = _minLimit; // if constexpr (mcc_eqt_hrz_coord_c) { // point->X = _minLimit; // } else { // mcc_celestial_point_c // point->X = _minLimit; // } return MccAltLimitPZErrorCode::ERROR_OK; } protected: double _minLimit, _maxLimit; std::function _transformCoordinates{}; std::function _transformCoordinatesEqtHrzCoords{}; std::function _computePCM{}; error_t getHWCoords(MccCelestialPoint from_pt, MccCelestialPoint* to_pt) { error_t ret = MccAltLimitPZErrorCode::ERROR_OK; if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware to_pt->X = from_pt.X; to_pt->Y = from_pt.Y; } else { // here one needs transform input coordinates to hardware encoder ones if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) { to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; } else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) { to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; } mcc_tp2tp(from_pt.time_point, to_pt->time_point); ret = _transformCoordinates(std::move(from_pt), to_pt); if (!ret) { ret = _computePCM(*to_pt, to_pt); } } return ret; } }; } // namespace mcc