249 lines
8.6 KiB
C++
249 lines
8.6 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
/* PROHIBITED ZONE IMPLEMENTATION */
|
|
|
|
#include <chrono>
|
|
#include <string_view>
|
|
|
|
#include "mcc_mount_concepts.h"
|
|
|
|
#include "mcc_mount_coord.h"
|
|
#include "mcc_traits.h"
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
static constexpr double mcc_sideral_to_UT1_ratio = 1.002737909350795; // sideral/UT1
|
|
|
|
/* SOME SIMPLE PROHIBITED ZONE IMPLEMENTATIONS */
|
|
|
|
|
|
// minimal or maximal altitude prohibited zones
|
|
|
|
enum class MccAltLimitKind { MIN_ALT_LIMIT, MAX_ALT_LIMIT };
|
|
|
|
template <MccAltLimitKind KIND = MccAltLimitKind::MIN_ALT_LIMIT>
|
|
class MccAltLimitPZ
|
|
// class MccAltLimitPZ : public MccProhibitedZone
|
|
{
|
|
static constexpr auto pi2 = std::numbers::pi * 2.0;
|
|
|
|
public:
|
|
static constexpr MccCoordPairKind preferedCoordKind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
|
|
static constexpr MccAltLimitKind altLimitKind = KIND;
|
|
|
|
// floating-point time duration (seconds)
|
|
typedef std::chrono::duration<double> duration_t;
|
|
|
|
typedef MccAngle coord_t;
|
|
typedef std::chrono::system_clock::time_point time_point_t;
|
|
|
|
static constexpr duration_t infiniteDuration{std::numeric_limits<double>::infinity()};
|
|
static constexpr duration_t zeroDuration{0.0};
|
|
|
|
//
|
|
// TODO: add context (e.g. TT-TAI, UT1-UTC, geo location and so on)!!!
|
|
MccAltLimitPZ(const MccAngle& alt_limit, const MccAngle& lat)
|
|
// : MccProhibitedZone(KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE"
|
|
// : KIND == MccAltLimitKind::MAX_ALT_LIMIT ? "MAXALT-ZONE"
|
|
// : "ALTLIMIT-UNKNOWN"),
|
|
: _altLimit(alt_limit), _latitude(lat), _abs_lat(std::abs(_latitude)), _lat_lim(pi2 - _abs_lat)
|
|
{
|
|
_lat_lim = pi2 - _abs_lat;
|
|
_altLimit.normalize<MccAngle::NORM_KIND_90_90>();
|
|
}
|
|
|
|
|
|
consteval std::string_view name() const
|
|
{
|
|
return KIND == MccAltLimitKind::MIN_ALT_LIMIT ? "MINALT-ZONE"
|
|
: KIND == MccAltLimitKind::MAX_ALT_LIMIT ? "MAXALT-ZONE"
|
|
: "ALTLIMIT-UNKNOWN";
|
|
}
|
|
|
|
// check if current mount coordinates are within the zone
|
|
bool inZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
{
|
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
|
return telemetry_data.mntALT <= _altLimit;
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
|
return telemetry_data.mntALT >= _altLimit;
|
|
}
|
|
}
|
|
|
|
|
|
// returns a time to reach the zone
|
|
duration_t timeTo(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
{
|
|
if (inZone(telemetry_data)) {
|
|
return zeroDuration;
|
|
}
|
|
|
|
if (!doesObjectReachZone(telemetry_data)) {
|
|
return infiniteDuration;
|
|
}
|
|
|
|
if constexpr (KIND ==
|
|
MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one after upper culmination
|
|
return compute(telemetry_data, false);
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one before upper
|
|
// culmination
|
|
return compute(telemetry_data, true);
|
|
}
|
|
}
|
|
|
|
// returns a time to exit from the zone
|
|
duration_t timeFrom(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
{
|
|
if (!inZone(telemetry_data)) {
|
|
return zeroDuration;
|
|
}
|
|
|
|
if (!doesObjectExitFromZone(telemetry_data)) {
|
|
return infiniteDuration;
|
|
}
|
|
|
|
if (!doesObjectReachZone(telemetry_data)) {
|
|
return zeroDuration;
|
|
}
|
|
|
|
if constexpr (KIND ==
|
|
MccAltLimitKind::MIN_ALT_LIMIT) { // the closest time point is one before upper culmination
|
|
return compute(telemetry_data, true);
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) { // the closest time point is one after upper
|
|
// culmination
|
|
return compute(telemetry_data, false);
|
|
}
|
|
}
|
|
|
|
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
|
// bool inZone(const XT& x, const YT& y, traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
bool inZone(const XT& x, const YT& y)
|
|
{
|
|
static constexpr MccCoordPairKind coord_kind = traits::mcc_type_pair_hash<XT, YT>();
|
|
|
|
if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZALT) { // trivial case
|
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
|
return y <= _altLimit;
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
|
return y >= _altLimit;
|
|
}
|
|
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZZD) { // trivial case
|
|
// return inZone(x, MccAngleALT(std::numbers::pi / 2 - (double)y), utc);
|
|
return inZone(x, MccAngleALT(std::numbers::pi / 2 - (double)y));
|
|
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
|
// implementation ...
|
|
return false;
|
|
} else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
|
// implementation ...
|
|
return false;
|
|
} else {
|
|
throw std::system_error(std::make_error_code(std::errc::operation_not_supported));
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
|
duration_t timeTo(const XT& x,
|
|
const YT& y,
|
|
traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
{
|
|
return duration_t{std::numeric_limits<double>::infinity()};
|
|
}
|
|
|
|
|
|
template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
|
|
duration_t timeFrom(const XT& x,
|
|
const YT& y,
|
|
traits::mcc_systime_c auto const& utc = std::chrono::system_clock::now())
|
|
{
|
|
return duration_t{0.0};
|
|
}
|
|
|
|
private:
|
|
MccAngle _altLimit, _latitude, _abs_lat, _lat_lim;
|
|
|
|
bool doesObjectReachZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
{
|
|
// check for limit conditions
|
|
auto dd = std::abs(telemetry_data.mntDEC);
|
|
|
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
|
dd += _altLimit;
|
|
|
|
if (dd > _lat_lim) { // never fall below altitude limit
|
|
return false;
|
|
}
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
|
if ((dd < (_abs_lat - _altLimit)) || (dd > (_abs_lat + _altLimit))) { // never rise above altitude limit
|
|
return false;
|
|
}
|
|
} else {
|
|
static_assert(false, "UNKNOWN ALTITUDE LIMIT TYPE!");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool doesObjectExitFromZone(traits::mcc_mount_telemetry_data_c auto const& telemetry_data)
|
|
{
|
|
// check for limit conditions
|
|
auto dd = std::abs(telemetry_data.mntDEC);
|
|
|
|
if constexpr (KIND == MccAltLimitKind::MIN_ALT_LIMIT) {
|
|
dd -= _altLimit;
|
|
|
|
if (-dd <= -_lat_lim) { // always below altitude limit
|
|
return false;
|
|
}
|
|
} else if constexpr (KIND == MccAltLimitKind::MAX_ALT_LIMIT) {
|
|
if ((dd >= (_abs_lat - _altLimit)) || (dd <= (_abs_lat + _altLimit))) { // always above altitude limit
|
|
return false;
|
|
}
|
|
} else {
|
|
static_assert(false, "UNKNOWN ALTITUDE LIMIT TYPE!");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
duration_t compute(traits::mcc_mount_telemetry_data_c auto const& telemetry_data, bool before_upper_culm)
|
|
{
|
|
double cos_ha = (std::sin(_altLimit) - std::sin(telemetry_data.mntDEC) * std::sin(_latitude)) /
|
|
std::cos(telemetry_data.mntDEC) / std::cos(_latitude);
|
|
|
|
if (cos_ha > 1.0) { // should not be!
|
|
return infiniteDuration;
|
|
}
|
|
|
|
double ha;
|
|
if (before_upper_culm) {
|
|
ha = -std::acos(cos_ha); // HA before upper culmination
|
|
} else {
|
|
ha = std::acos(cos_ha); // HA after upper culmination!!
|
|
}
|
|
|
|
MccAngle time_ang = ha - telemetry_data.mntHA; // in sideral time scale
|
|
|
|
if (time_ang < 0.0) { // next day
|
|
time_ang += pi2;
|
|
}
|
|
|
|
time_ang /= mcc_sideral_to_UT1_ratio; // to UT1 time scale
|
|
|
|
return duration_t{time_ang.seconds()};
|
|
}
|
|
};
|
|
|
|
typedef MccAltLimitPZ<MccAltLimitKind::MIN_ALT_LIMIT> MccMinAltPZ;
|
|
typedef MccAltLimitPZ<MccAltLimitKind::MAX_ALT_LIMIT> MccMaxAltPZ;
|
|
|
|
} // namespace mcc
|