792 lines
26 KiB
C++
792 lines
26 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* IMPLEMENTATION OF SOME SIMPLE PROHIBITED ZONES */
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_generics.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
|
|
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP };
|
|
|
|
} // namespace mcc
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccAltLimitPZErrorCode> : 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<MccAltLimitPZErrorCode>(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<int>(ec), MccAltLimitPZCategory::get());
|
|
}
|
|
|
|
|
|
|
|
enum class MccAltLimitKind { MIN_ALT_LIMIT, MAX_ALT_LIMIT };
|
|
|
|
template <MccAltLimitKind KIND = MccAltLimitKind::MIN_ALT_LIMIT>
|
|
class MccAltLimitPZ : public mcc_pzone_interface_t<std::error_code>
|
|
{
|
|
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<MccAngle::NORM_KIND_90_90>()),
|
|
_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<decltype(err), error_t>) {
|
|
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<decltype(err), error_t>) {
|
|
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 <typename InputT>
|
|
error_t inPZone(InputT coords, bool* result)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
double alt;
|
|
|
|
if (result == nullptr) {
|
|
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
error_t ret = MccAltLimitPZErrorCode::ERROR_OK;
|
|
|
|
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
|
|
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 <typename InputT>
|
|
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
using res_t = std::remove_cvref_t<decltype(*res_time)>;
|
|
|
|
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<InputT>) {
|
|
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<res_t>;
|
|
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 <typename InputT>
|
|
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
using res_t = std::remove_cvref_t<decltype(*res_time)>;
|
|
|
|
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<InputT>) {
|
|
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<res_t>;
|
|
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 <typename InputT>
|
|
// error_t intersectPZone(InputT coords, mcc_celestial_point_c auto* point)
|
|
// requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
// {
|
|
// // 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<InputT>) {
|
|
// // 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<double>::quiet_NaN();
|
|
// point->Y = std::numeric_limits<double>::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<MccAngle::NORM_KIND_0_360>();
|
|
// point->Y = MccAngle(to_pt.Y).normalize<MccAngle::NORM_KIND_90_90>();
|
|
// }
|
|
|
|
// return ret;
|
|
// }
|
|
|
|
template <typename InputT, typename ResultT>
|
|
error_t intersectPZone(InputT coords, ResultT* point)
|
|
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
|
|
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
|
|
{
|
|
double dec, az;
|
|
|
|
if (point == nullptr) {
|
|
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
error_t ret = MccAltLimitPZErrorCode::ERROR_OK;
|
|
|
|
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
|
|
// 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<double>::quiet_NaN();
|
|
point->Y = std::numeric_limits<double>::quiet_NaN();
|
|
|
|
if constexpr (mcc_eqt_hrz_coord_c<ResultT>) {
|
|
point->HA = std::numeric_limits<double>::quiet_NaN();
|
|
point->RA_APP = std::numeric_limits<double>::quiet_NaN();
|
|
point->DEC_APP = std::numeric_limits<double>::quiet_NaN();
|
|
point->AZ = std::numeric_limits<double>::quiet_NaN();
|
|
point->ZD = std::numeric_limits<double>::quiet_NaN();
|
|
point->ALT = std::numeric_limits<double>::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<ResultT>) {
|
|
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<error_t(MccCelestialPoint, MccCelestialPoint*)> _transformCoordinates{};
|
|
std::function<error_t(MccCelestialPoint, MccEqtHrzCoords*)> _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<decltype(*result)>;
|
|
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<res_t>;
|
|
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<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
|
|
|
|
period_t rat;
|
|
*result = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
|
|
}
|
|
};
|
|
|
|
|
|
/* co-longitude axis (HA or AZ) limit switch prohibited zone */
|
|
|
|
template <MccCoordKind AXIS_KIND>
|
|
class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t<std::error_code>
|
|
{
|
|
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<decltype(err), error_t>) {
|
|
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()
|
|
{
|
|
return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE"
|
|
: axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE"
|
|
: "UKNOWN";
|
|
}
|
|
|
|
|
|
template <typename InputT>
|
|
error_t inPZone(InputT coords, bool* result)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
if (result == nullptr) {
|
|
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
|
|
// assume here .X and are hardware encoder coordinate of corresponding axis
|
|
*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);
|
|
} 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);
|
|
}
|
|
}
|
|
|
|
return MccAltLimitPZErrorCode::ERROR_OK;
|
|
}
|
|
|
|
// time to reach maximal limit
|
|
template <typename InputT>
|
|
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
using res_t = std::remove_cvref_t<decltype(*res_time)>;
|
|
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<InputT>) {
|
|
// 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<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
|
|
|
|
period_t rat;
|
|
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
|
|
|
|
return MccAltLimitPZErrorCode::ERROR_OK;
|
|
}
|
|
|
|
// time to reach minimal limit
|
|
template <typename InputT>
|
|
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
|
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
|
{
|
|
using res_t = std::remove_cvref_t<decltype(*res_time)>;
|
|
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<InputT>) {
|
|
// 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<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
|
|
|
|
period_t rat;
|
|
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
|
|
|
|
return MccAltLimitPZErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
template <typename InputT, typename ResultT>
|
|
error_t intersectPZone(InputT coords, ResultT* point)
|
|
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
|
|
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
|
|
{
|
|
return MccAltLimitPZErrorCode::ERROR_OK;
|
|
}
|
|
|
|
protected:
|
|
double _minLimit, _maxLimit;
|
|
|
|
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _transformCoordinates{};
|
|
std::function<error_t(MccCelestialPoint, MccEqtHrzCoords*)> _transformCoordinatesEqtHrzCoords{};
|
|
|
|
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _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
|