309 lines
9.8 KiB
C++
309 lines
9.8 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* IMPLEMENTATION OF TELEMETRY CLASS */
|
|
|
|
|
|
#include "mcc_defaults.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
static constexpr double mcc_sideral_to_UT1_ratio = 1.002737909350795; // sideral/UT1
|
|
|
|
|
|
enum MccTelemetryErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_NULLPTR,
|
|
ERROR_COORD_TRANSFORM,
|
|
ERROR_PCM_COMP,
|
|
ERROR_HARDWARE_GETPOS
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccTelemetryErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
/* error category definition */
|
|
|
|
// error category
|
|
struct MccTelemetryCategory : public std::error_category {
|
|
MccTelemetryCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept
|
|
{
|
|
return "ALTITUDE-LIMIT-PZ";
|
|
}
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccTelemetryErrorCode err = static_cast<MccTelemetryErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccTelemetryErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccTelemetryErrorCode::ERROR_NULLPTR:
|
|
return "nullptr input argument";
|
|
case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM:
|
|
return "coordinate transformation error";
|
|
case MccTelemetryErrorCode::ERROR_PCM_COMP:
|
|
return "PCM computation error";
|
|
case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS:
|
|
return "cannot get hardware position";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccTelemetryCategory& get()
|
|
{
|
|
static const MccTelemetryCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccTelemetryErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccTelemetryCategory::get());
|
|
}
|
|
|
|
|
|
/* TELEMETRY UPDATE POLICY */
|
|
|
|
enum class MccTelemetryUpdatePolicy : int { TEMETRY_UPDATE_INNER, TEMETRY_UPDATE_EXTERNAL };
|
|
|
|
|
|
template <MccTelemetryUpdatePolicy UPDATE_POLICY = MccTelemetryUpdatePolicy::TEMETRY_UPDATE_INNER>
|
|
class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
|
|
{
|
|
public:
|
|
static constexpr MccTelemetryUpdatePolicy updatePolicy = UPDATE_POLICY;
|
|
|
|
typedef std::error_code error_t;
|
|
|
|
|
|
MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) : _data()
|
|
{
|
|
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
|
|
|
using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
|
|
using pcm_t = std::remove_cvref_t<decltype(*pcm)>;
|
|
using hardware_t = std::remove_cvref_t<decltype(*hardware)>;
|
|
|
|
|
|
_updateTargetFunc = [ccte, this]() {
|
|
//
|
|
// ICRS coordinates of the taget must be already set
|
|
//
|
|
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
|
_data.target.X = _data.target.RA_ICRS;
|
|
_data.target.Y = _data.target.DEC_ICRS;
|
|
|
|
// update apparent cordinates
|
|
auto ret = ccte->transformCoordinates(_data.target, &_data.target);
|
|
|
|
return mcc_deduce_error<error_t>(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
};
|
|
|
|
_updateFunc = [ccte, pcm, hardware, this](MccTelemetryData* data) {
|
|
// first, update mount quantities
|
|
typename hardware_t::axes_pos_t hw_pos;
|
|
auto hw_err = hardware->getPos(&hw_pos);
|
|
if (hw_err) {
|
|
return mcc_deduce_error(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
|
|
}
|
|
|
|
double eo;
|
|
|
|
data->time_point =
|
|
std::chrono::time_point_cast<typename decltype(data->time_point)::duration>(hw_pos.time_point);
|
|
|
|
auto ccte_err = ccte->timepointToJulday(data->time_point, &data->JD);
|
|
if (!ccte_err) {
|
|
ccte_err = ccte->juldayToAppSideral(data->JD, &data->LST, true);
|
|
if (!ccte_err) {
|
|
ccte_err = ccte->equationOrigins(data->JD, &eo);
|
|
}
|
|
}
|
|
|
|
if (ccte_err) {
|
|
return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
}
|
|
|
|
data->speedX = (double)hw_pos.speedX;
|
|
data->speedY = (double)hw_pos.speedY;
|
|
|
|
struct {
|
|
double dx, dy;
|
|
} pcm_res;
|
|
|
|
auto pcm_err = pcm->computePCM(data, &pcm_res);
|
|
if (pcm_err) {
|
|
return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
|
|
data->pcmX = pcm_res.dx;
|
|
data->pcmY = pcm_res.dy;
|
|
|
|
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = data->time_point};
|
|
|
|
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
|
data->HA = (double)hw_pos.X + pcm_res.dx;
|
|
data->DEC_APP = (double)hw_pos.Y + pcm_res.dy;
|
|
data->RA_APP = (double)data->LST - (double)data->HA + eo;
|
|
|
|
data->X = data->HA;
|
|
data->Y = data->DEC_APP;
|
|
|
|
data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
|
|
ccte_err = ccte->transformCoordinates(*data, &pt);
|
|
if (!ccte_err) {
|
|
data->AZ = pt.X;
|
|
data->ALT = pt.Y;
|
|
data->ZD = std::numbers::pi / 2.0 - data->ALT;
|
|
}
|
|
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
|
data->AZ = (double)hw_pos.X + pcm_res.dx;
|
|
data->ALT = (double)hw_pos.Y + pcm_res.dy;
|
|
data->ZD = std::numbers::pi / 2.0 - data->ALT;
|
|
|
|
data->X = data->AZ;
|
|
data->Y = data->ALT;
|
|
|
|
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
|
|
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
ccte_err = ccte->transformCoordinates(*data, &pt);
|
|
if (!ccte) {
|
|
data->HA = pt.X;
|
|
data->DEC_APP = pt.Y;
|
|
data->RA_APP = (double)data->LST - (double)data->HA + eo;
|
|
}
|
|
|
|
} else {
|
|
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
}
|
|
|
|
if (!ccte_err) {
|
|
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
|
data->X = data->AZ;
|
|
data->Y = data->ZD;
|
|
|
|
ccte_err = ccte->refractionCorrection(data, &data->refCorr);
|
|
if (!ccte_err) {
|
|
// hardware encoders coordinates
|
|
data->X = (double)hw_pos.X;
|
|
data->Y = (double)hw_pos.Y;
|
|
|
|
// update target (assuming target ICRS coordinates are already set)
|
|
|
|
data->target.time_point =
|
|
std::chrono::time_point_cast<typename decltype(data->target.time_point)::duration>(
|
|
data->time_point);
|
|
|
|
data->target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
|
data->target.X = data->target.RA_ICRS;
|
|
data->target.Y = data->target.DEC_ICRS;
|
|
|
|
ccte_err = ccte->transformCoordinates(data->target, &data->target);
|
|
}
|
|
}
|
|
|
|
if (ccte_err) {
|
|
return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
}
|
|
|
|
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
|
data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
|
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
} else {
|
|
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
}
|
|
|
|
|
|
return MccTelemetryErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
_setTargetFunc = [ccte, this](MccCelestialPoint const& pt) {
|
|
// in the case of apparent input coordinates
|
|
// one must ensure the same time points
|
|
|
|
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
|
_data.target.time_point =
|
|
std::chrono::time_point_cast<typename decltype(_data.target.time_point)::duration>(pt.time_point);
|
|
|
|
auto ret = ccte->transformCoordinates(pt, &_data.target);
|
|
|
|
if (!ret) {
|
|
if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
|
_data.target.RA_ICRS = _data.target.X;
|
|
_data.target.DEC_ICRS = _data.target.Y;
|
|
|
|
// update apparent coordinates
|
|
ret = _updateTargetFunc();
|
|
} else { // apparent coordinates were computed above
|
|
// compute ICRS coordinates
|
|
MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
|
|
ret = ccte->transformCoordinates(pt, &cpt);
|
|
|
|
_data.target.RA_ICRS = cpt.X;
|
|
_data.target.DEC_ICRS = cpt.Y;
|
|
}
|
|
}
|
|
|
|
return mcc_deduce_error<error_t>(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
};
|
|
}
|
|
|
|
|
|
virtual ~MccTelemetry() = default;
|
|
|
|
error_t telemetryData(mcc_telemetry_data_c auto* tdata)
|
|
{
|
|
if (tdata == nullptr) {
|
|
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
error_t ret = _updateFunc(&_data);
|
|
|
|
if (!ret) {
|
|
mcc_copy_telemetry_data(_data, tdata);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
error_t setPointingTarget(mcc_celestial_point_c auto pt)
|
|
{
|
|
// return
|
|
}
|
|
|
|
protected:
|
|
MccTelemetryData _data;
|
|
|
|
std ::function<error_t()> _updateTargetFunc{};
|
|
std::function<error_t(MccTelemetryData*)> _updateFunc{};
|
|
std::function<error_t(MccCelestialPoint const&)> _setTargetFunc{};
|
|
};
|
|
|
|
} // namespace mcc
|