360 lines
13 KiB
C++
360 lines
13 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* A VERY SIMPLE GUIDING MODEL GENERIC IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_mount_concepts.h"
|
|
#include "mcc_mount_telemetry.h"
|
|
#include "mcc_slew_guiding_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleGuidingModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_UNSUPPORTED_COORD_PAIR,
|
|
ERROR_IN_PROHIBITED_ZONE,
|
|
ERROR_ASTROM_COMP,
|
|
ERROR_TELEMETRY_DATA,
|
|
ERROR_PEC_COMP,
|
|
ERROR_HARDWARE_SETPOS,
|
|
ERROR_INVALID_CONTEXT_PARAM,
|
|
ERROR_INVALID_THRESH,
|
|
ERROR_INVALID_CORR_RANGE,
|
|
ERROR_GUIDING_STOPPED
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleGuidingModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
/* error category definition */
|
|
|
|
// error category
|
|
struct MccSimpleGuidingModelCategory : public std::error_category {
|
|
MccSimpleGuidingModelCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; }
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccSimpleGuidingModelErrorCode err = static_cast<MccSimpleGuidingModelErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccSimpleGuidingModelErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
|
|
return "slew model: unsupported coordinate pair";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP:
|
|
return "guiding model: cannot perform astrometrical computations";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA:
|
|
return "guiding model: cannot get telemetry data";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_PEC_COMP:
|
|
return "guiding model: cannot compute PEC corrections";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_HARDWARE_SETPOS:
|
|
return "guiding model: cannot set position";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CONTEXT_PARAM:
|
|
return "guiding model: invalid context parameter";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH:
|
|
return "guiding model: invalid guiding residual threshold";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CORR_RANGE:
|
|
return "guiding model: invalid guiding correction range";
|
|
case MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED:
|
|
return "guiding model: stopped";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccSimpleGuidingModelCategory& get()
|
|
{
|
|
static const MccSimpleGuidingModelCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccSimpleGuidingModelErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccSimpleGuidingModelCategory::get());
|
|
}
|
|
|
|
|
|
|
|
/* */
|
|
|
|
class MccCelestialPointTrack final
|
|
{
|
|
public:
|
|
template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_time_duration_c DT>
|
|
MccCelestialPointTrack(ASTROM_ENGINE_T& astrom_engine,
|
|
typename ASTROM_ENGINE_T::juldate_t start,
|
|
DT step,
|
|
size_t Npoints)
|
|
{
|
|
const auto p_astrom_engine = &astrom_engine;
|
|
|
|
_compFunc = []() {
|
|
|
|
};
|
|
}
|
|
|
|
private:
|
|
std::function<size_t()> _compFunc;
|
|
};
|
|
|
|
/* */
|
|
|
|
|
|
template <traits::mcc_logger_c LoggerT = MccNullLogger>
|
|
class MccSimpleGuidingModel : public LoggerT
|
|
{
|
|
template <traits::mcc_mount_controls_c T>
|
|
using coord_t = typename decltype(T::astrometryEngine)::coord_t;
|
|
|
|
public:
|
|
using LoggerT::logDebug;
|
|
using LoggerT::logError;
|
|
using LoggerT::logInfo;
|
|
using LoggerT::logMessage;
|
|
using LoggerT::logWarn;
|
|
|
|
typedef std::error_code error_t;
|
|
|
|
|
|
using guiding_point_t = MccSlewAndGuidingPoint;
|
|
|
|
template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
|
traits::mcc_tuple_c PZ_T,
|
|
typename... LoggerCtorArgTs>
|
|
MccSimpleGuidingModel(TELEMETRY_T& telemetry,
|
|
HARDWARE_T& hardware,
|
|
PZ_T& prohibited_zone,
|
|
LoggerCtorArgTs&&... ctor_args)
|
|
requires(!std::same_as<LoggerT, MccNullLogger>)
|
|
: LoggerT(std::forward<LoggerCtorArgTs>(ctor_args)...)
|
|
{
|
|
logDebug(std::format("Create 'MccSimpleGuidingModel' class instance ({})", (void*)this));
|
|
|
|
init(telemetry, hardware, prohibited_zone);
|
|
}
|
|
|
|
template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
|
traits::mcc_tuple_c PZ_T>
|
|
MccSimpleGuidingModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
|
|
requires(std::same_as<LoggerT, MccNullLogger>)
|
|
{
|
|
init(telemetry, hardware, prohibited_zone);
|
|
}
|
|
|
|
|
|
MccSimpleGuidingModel(MccSimpleGuidingModel&& other)
|
|
: _guidingFunc(std::move(other._guidingFunc)),
|
|
_doCorrection(other._doCorrection.load()),
|
|
_stopRequested(other._stopRequested.load())
|
|
{
|
|
}
|
|
|
|
MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&& other)
|
|
{
|
|
if (this == &other) {
|
|
return *this;
|
|
}
|
|
|
|
_guidingFunc = std::move(other._guidingFunc);
|
|
_doCorrection = other._doCorrection.load();
|
|
_stopRequested = other._stopRequested.load();
|
|
|
|
return *this;
|
|
}
|
|
|
|
virtual ~MccSimpleGuidingModel()
|
|
{
|
|
logDebug(std::format("Delete 'MccSimpleGuidingModel' class instance ({})", (void*)this));
|
|
}
|
|
|
|
|
|
error_t guiding(guiding_point_t guiding_point) { return _guidingFunc(std::move(guiding_point)); }
|
|
|
|
error_t stopGuiding(bool off) { _doCorrection = off; }
|
|
|
|
bool inGuiding() { return _doCorrection; }
|
|
|
|
error_t stop() { return MccSimpleGuidingModelErrorCode::ERROR_OK; }
|
|
|
|
protected:
|
|
std::function<error_t(guiding_point_t)> _guidingFunc{};
|
|
std::atomic_bool _doCorrection{true};
|
|
std::atomic_bool _stopRequested{false};
|
|
|
|
|
|
|
|
error_t init(auto& telemetry, auto& hardware, auto& prohibited_zones)
|
|
{
|
|
// deduce controls types
|
|
// deduce controls types
|
|
using hardware_t = decltype(hardware);
|
|
using telemetry_t = decltype(telemetry);
|
|
static_assert(traits::mcc_mount_default_telemetry_c<telemetry_t>,
|
|
"TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry'!");
|
|
|
|
using astrom_engine_t = typename telemetry_t::astrom_engine_t;
|
|
|
|
static constexpr size_t Nzones = std::tuple_size_v<decltype(prohibited_zones)>;
|
|
|
|
const auto p_telemetry = &telemetry;
|
|
const auto p_hardware = &hardware;
|
|
const auto p_prohibited_zones = &prohibited_zones;
|
|
|
|
_guidingFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](guiding_point_t guiding_point) {
|
|
_stopRequested = false;
|
|
|
|
if (guiding_point.correctionRange[0] >= guiding_point.correctionRange[1]) {
|
|
return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
|
|
}
|
|
|
|
auto low_corr_limit = guiding_point.correctionRange[0] * guiding_point.correctionRange[0];
|
|
auto high_corr_limit = guiding_point.correctionRange[1] * guiding_point.correctionRange[1];
|
|
|
|
// using coord_t = typename astrom_engine_t::coord_t;
|
|
// using jd_t = typename astrom_engine_t::juldate_t;
|
|
// jd_t jd;
|
|
|
|
|
|
error_t res_err;
|
|
typename astrom_engine_t::error_t ast_err;
|
|
// typename pec_t::error_t pec_err;
|
|
|
|
typename telemetry_t::error_t t_err;
|
|
typename telemetry_t::mount_telemetry_data_t t_data;
|
|
|
|
|
|
typename hardware_t::axes_pos_t ax_pos;
|
|
|
|
ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_GUIDING;
|
|
|
|
t_err = p_telemetry->setTarget(guiding_point);
|
|
if (t_err) {
|
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
|
logError(std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
|
return t_err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
|
logError(std::format("An telemetry error occured: code = {}", t_err));
|
|
}
|
|
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
}
|
|
}
|
|
|
|
std::array<bool, Nzones> in_zone_flag;
|
|
|
|
while (true) {
|
|
if (_stopRequested) {
|
|
// return MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED;
|
|
// interpetate stoping as 'no error' exit
|
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
// suspend the thread here until telemetry data is updated
|
|
t_err = p_telemetry->waitForUpdatedData(t_data, guiding_point.telemetryUpdateTimeout);
|
|
|
|
ax_pos.x = t_data.mntPosX;
|
|
ax_pos.y = t_data.mntPosY;
|
|
|
|
// check prohibited zones ...
|
|
if (mccCheckInZonePZTuple(t_data, *p_prohibited_zones, in_zone_flag)) {
|
|
return MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE;
|
|
};
|
|
|
|
|
|
if (t_err) {
|
|
std::string err_str = "An error occured while waiting for updated telemetry";
|
|
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
|
std::format_to(std::back_inserter(err_str), ": code = {} ({})", t_err.value(), t_err.message());
|
|
logError(err_str);
|
|
return t_err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
|
std::format_to(std::back_inserter(err_str), ": code = {}", t_err.value());
|
|
}
|
|
logError(err_str);
|
|
return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
}
|
|
}
|
|
|
|
if (_stopRequested) {
|
|
// interpetate stoping as 'no error' exit
|
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
// compare t_data with computed coordinates ...
|
|
if (_doCorrection) {
|
|
auto coord_diff = p_telemetry->targetToMountDiff();
|
|
|
|
if (coord_diff.r2 < low_corr_limit) {
|
|
logWarn(
|
|
std::format("guiding model: the 'mount-target' square of difference is less than the limit "
|
|
"(diff = {}; lim = {}). Do not perfrom the corrections!",
|
|
(double)coord_diff.r2, (double)high_corr_limit));
|
|
continue;
|
|
}
|
|
|
|
if (coord_diff.r2 > high_corr_limit) {
|
|
logWarn(
|
|
std::format("guiding model: the 'mount-target' square of difference exceeds the limit "
|
|
"(diff = {}; lim = {})",
|
|
(double)coord_diff.r2, (double)high_corr_limit));
|
|
continue;
|
|
}
|
|
|
|
// do correction
|
|
ax_pos.time_point = t_data.time_point;
|
|
ax_pos.x += coord_diff.xdiff;
|
|
if (guiding_point.dualAxisGuiding) {
|
|
ax_pos.y += coord_diff.ydiff;
|
|
}
|
|
|
|
// asynchronous operation!
|
|
auto err = p_hardware->setPos(ax_pos);
|
|
if (err) {
|
|
if constexpr (std::same_as<decltype(err), error_t>) {
|
|
logError(
|
|
std::format("An hardware error occured: code = {} ({})", err.value(), err.message()));
|
|
return err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(err)>) {
|
|
logError(std::format("An hardware error occured: code = {}", err));
|
|
}
|
|
return MccSimpleGuidingModelErrorCode::ERROR_HARDWARE_SETPOS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
};
|
|
}
|
|
};
|
|
|
|
// static_assert(std::movable<MccSimpleGuidingModel<>>);
|
|
|
|
} // namespace mcc
|