443 lines
17 KiB
C++
443 lines
17 KiB
C++
#pragma once
|
|
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* A VERY SIMPLE SLEW MODEL GENERIC IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_mount_concepts.h"
|
|
|
|
#include "mcc_mount_telemetry.h"
|
|
#include "mcc_slew_guiding_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleSlewModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_UNSUPPORTED_COORD_PAIR,
|
|
ERROR_IN_PROHIBITED_ZONE,
|
|
ERROR_ASTROM_COMP,
|
|
ERROR_TELEMETRY_DATA,
|
|
ERROR_PEC_COMP,
|
|
ERROR_HARDWARE_SETPOS,
|
|
ERROR_HARDWARE_GETPOS,
|
|
ERROR_SLEW_STOPPED,
|
|
ERROR_SLEW_ADJ_MAXITER,
|
|
ERROR_SLEW_TIMEOUT
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleSlewModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
|
|
/* error category definition */
|
|
|
|
// error category
|
|
struct MccSimpleSlewModelCategory : public std::error_category {
|
|
MccSimpleSlewModelCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; }
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccSimpleSlewModelErrorCode err = static_cast<MccSimpleSlewModelErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccSimpleSlewModelErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccSimpleSlewModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
|
|
return "slew model: unsupported coordinate pair";
|
|
case MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE:
|
|
return "slew model: position is in prohibited zone";
|
|
case MccSimpleSlewModelErrorCode::ERROR_ASTROM_COMP:
|
|
return "slew model: cannot perform astrometrical computations";
|
|
case MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA:
|
|
return "slew model: cannot get telemetry data";
|
|
case MccSimpleSlewModelErrorCode::ERROR_PEC_COMP:
|
|
return "slew model: cannot compute PEC corrections";
|
|
case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS:
|
|
return "slew model: cannot set position";
|
|
case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS:
|
|
return "slew model: cannot get position";
|
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED:
|
|
return "slew model: stopped";
|
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER:
|
|
return "slew model: max number of adjusting iteration was exceeded";
|
|
case MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT:
|
|
return "slew model: timeout occured while slewing";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccSimpleSlewModelCategory& get()
|
|
{
|
|
static const MccSimpleSlewModelCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccSimpleSlewModelErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccSimpleSlewModelCategory::get());
|
|
}
|
|
|
|
|
|
/*
|
|
* It is very simple slew model!
|
|
* There are no any complex routes (bypass of prohibited),
|
|
* just a strait path from current point to target
|
|
*
|
|
*/
|
|
|
|
template <traits::mcc_logger_c LoggerT = MccNullLogger>
|
|
class MccSimpleSlewModel : public LoggerT
|
|
{
|
|
public:
|
|
using LoggerT::logDebug;
|
|
using LoggerT::logError;
|
|
using LoggerT::logInfo;
|
|
using LoggerT::logMessage;
|
|
using LoggerT::logWarn;
|
|
|
|
typedef std::error_code error_t;
|
|
|
|
static constexpr size_t defaultAdjustSuccessCycles = 5;
|
|
|
|
using slew_point_t = MccSlewAndGuidingPoint;
|
|
|
|
|
|
template <traits::mcc_mount_telemetry_c TELEMETRY_T,
|
|
traits::mcc_mount_hardware_c HARDWARE_T,
|
|
traits::mcc_tuple_c PZ_T, // std::tuple of prohibited zones
|
|
typename... LoggerCtorArgTs>
|
|
MccSimpleSlewModel(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 'MccSimpleSlewModel' 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 // std::tuple of prohibited zones
|
|
>
|
|
MccSimpleSlewModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
|
|
requires(std::same_as<LoggerT, MccNullLogger>)
|
|
{
|
|
init(telemetry, hardware, prohibited_zone);
|
|
}
|
|
|
|
MccSimpleSlewModel(MccSimpleSlewModel&& other)
|
|
: _stopRequested(other._stopRequested.load()), _slewFunc(std::move(other._slewFunc))
|
|
{
|
|
}
|
|
|
|
MccSimpleSlewModel& operator=(MccSimpleSlewModel&& other)
|
|
{
|
|
if (this == &other) {
|
|
return *this;
|
|
}
|
|
|
|
_stopRequested = other._stopRequested.load();
|
|
_slewFunc = std::move(_slewFunc);
|
|
|
|
return *this;
|
|
};
|
|
|
|
MccSimpleSlewModel(const MccSimpleSlewModel&) = delete;
|
|
MccSimpleSlewModel& operator=(const MccSimpleSlewModel&) = delete;
|
|
|
|
virtual ~MccSimpleSlewModel()
|
|
{
|
|
logDebug(std::format("Delete 'MccSimpleSlewModel' class instance ({})", (void*)this));
|
|
}
|
|
|
|
error_t slew(slew_point_t pars)
|
|
{
|
|
_stopRequested = false;
|
|
error_t res_err = _slewFunc(std::move(pars));
|
|
|
|
return res_err;
|
|
}
|
|
|
|
error_t stop()
|
|
{
|
|
_stopRequested = true;
|
|
return MccSimpleSlewModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
protected:
|
|
std::atomic_bool _stopRequested{false};
|
|
std::function<error_t(const slew_point_t&)> _slewFunc{};
|
|
|
|
void init(auto& telemetry, auto& hardware, auto& prohibited_zones)
|
|
{
|
|
// 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;
|
|
|
|
_slewFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](slew_point_t slew_point) {
|
|
_stopRequested = false;
|
|
|
|
typename hardware_t::axes_pos_t ax_pos;
|
|
|
|
error_t res_err;
|
|
// typename astrom_engine_t::error_t ast_err;
|
|
|
|
typename telemetry_t::error_t t_err;
|
|
typename telemetry_t::mount_telemetry_data_t t_data;
|
|
|
|
if (slew_point.withinToleranceCycleNumber == 0) {
|
|
slew_point.withinToleranceCycleNumber = MccSlewAndGuidingPoint::defaultWithinToleranceCycleNumber;
|
|
}
|
|
|
|
// first, compute encoder coordinates
|
|
ax_pos.time_point = astrom_engine_t::timePointNow();
|
|
t_err = p_telemetry->toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
|
|
if (!t_err) {
|
|
// SETUP TARGET SKY POINT
|
|
t_err = p_telemetry->setTarget(slew_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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
}
|
|
}
|
|
|
|
// start moving the mount (it is assumed this is asynchronous operation!!!)
|
|
ax_pos.xrate = slew_point.slewXRate;
|
|
ax_pos.yrate = slew_point.slewYRate;
|
|
ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_SLEWING;
|
|
typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);
|
|
|
|
if (hw_err) {
|
|
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
|
logError(
|
|
std::format("An hardware error occured: code = {} ({})", hw_err.value(), hw_err.message()));
|
|
return hw_err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
|
logError(std::format("An hardware error occured: code = {}", hw_err));
|
|
}
|
|
return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
|
}
|
|
}
|
|
|
|
|
|
// typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
|
|
|
|
auto adj_ax_pos = ax_pos; // to prevent possible effects in hardware 'setPos' method
|
|
adj_ax_pos.xrate = slew_point.adjustXRate;
|
|
adj_ax_pos.yrate = slew_point.adjustYRate;
|
|
adj_ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_ADJUSTING;
|
|
|
|
typename telemetry_t::mount_telemetry_data_t::coord_t adj_rad2 = slew_point.adjustCoordDiff *
|
|
slew_point.adjustCoordDiff,
|
|
tol_rad2 = slew_point.slewToleranceRadius *
|
|
slew_point.slewToleranceRadius;
|
|
|
|
std::array<bool, Nzones> in_zone_flag;
|
|
|
|
size_t i_adj_cycle = 0;
|
|
size_t i_in_tol_cycle = 0;
|
|
bool in_adj_mode = false;
|
|
|
|
// compute new hardware coordinate of target
|
|
// auto compute_new_coord = [](auto const& t_data, typename hardware_t::axes_pos_t& hw_pos) {
|
|
// // current celestial position of target is already computed for given time point
|
|
// // so one needs only correct apparent coordinates for PEC corrections
|
|
// hw_pos.time_point = t_data.time_point;
|
|
// if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
|
// hw_pos.x = t_data.tagHA - t_data.pecX;
|
|
// hw_pos.y = t_data.tagDEC - t_data.pecY;
|
|
// } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
|
|
// hw_pos.x = t_data.tagAZ - t_data.pecX;
|
|
// hw_pos.y = t_data.tagALT - t_data.pecY;
|
|
// } else {
|
|
// static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
|
// }
|
|
// };
|
|
|
|
auto cycle_func = [&](auto& t_data) mutable {
|
|
if (_stopRequested) {
|
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
|
|
}
|
|
|
|
// check for prohibited zones
|
|
if (mccCheckInZonePZTuple(t_data, *p_prohibited_zones, in_zone_flag)) {
|
|
return MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE;
|
|
};
|
|
|
|
|
|
// t_data was updated in caller!!!
|
|
auto coord_diff = p_telemetry->targetToMountDiff();
|
|
|
|
if (_stopRequested) {
|
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
|
|
}
|
|
|
|
if (coord_diff.r2 < adj_rad2) { // adjusting mode
|
|
in_adj_mode = true;
|
|
|
|
// compute_new_coord(t_data, adj_ax_pos);
|
|
|
|
adj_ax_pos.time_point = t_data.time_point;
|
|
adj_ax_pos.x += coord_diff.xdiff;
|
|
adj_ax_pos.y += coord_diff.ydiff;
|
|
|
|
hw_err = p_hardware->setPos(adj_ax_pos);
|
|
|
|
if (!hw_err) {
|
|
++i_adj_cycle;
|
|
if (coord_diff.r2 < tol_rad2) {
|
|
++i_in_tol_cycle;
|
|
|
|
if (i_in_tol_cycle == slew_point.withinToleranceCycleNumber) {
|
|
res_err = MccSimpleSlewModelErrorCode::ERROR_OK;
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (i_adj_cycle == slew_point.maxAdjustingCycleNumber) {
|
|
res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER;
|
|
return;
|
|
}
|
|
} else {
|
|
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
|
logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
|
|
hw_err.message()));
|
|
res_err = hw_err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
|
logError(std::format("An hardware error occured: code = {}", hw_err));
|
|
}
|
|
res_err = MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
} else { // continue to slewing
|
|
if (in_adj_mode) { // ?!!!!!!!!!!!!! slew again?!!!
|
|
logWarn(std::format(
|
|
"The slewing is in adjusting mode but computed target-to-mount coordinate difference "
|
|
"'{}' is greater than limit '{}' for adjusting mode!",
|
|
coord_diff.r2, adj_rad2));
|
|
|
|
in_adj_mode = false;
|
|
i_adj_cycle = 0;
|
|
i_in_tol_cycle = 0;
|
|
|
|
// compute_new_coord(t_data, ax_pos);
|
|
adj_ax_pos.time_point = t_data.time_point;
|
|
adj_ax_pos.x += coord_diff.xdiff;
|
|
adj_ax_pos.y += coord_diff.ydiff;
|
|
|
|
ax_pos.time_point = t_data.time_point;
|
|
ax_pos.x = adj_ax_pos.x;
|
|
ax_pos.y = adj_ax_pos.y;
|
|
// send command for slewing
|
|
typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);
|
|
|
|
|
|
if (hw_err) {
|
|
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
|
logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
|
|
hw_err.message()));
|
|
return hw_err;
|
|
} else {
|
|
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
|
logError(std::format("An hardware error occured: code = {}", hw_err));
|
|
}
|
|
return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
auto start_poll_tm = std::chrono::steady_clock::now();
|
|
|
|
// NOTE: TARGET COORDINATES WILL BE UPDATED FOR CURRENT TIME-POINT IN TELEMETRY-CLASS!!!
|
|
|
|
while (true) {
|
|
t_err = p_telemetry->waitForUpdatedData(t_data, slew_point.telemetryUpdateTimeout);
|
|
|
|
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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
|
}
|
|
}
|
|
|
|
cycle_func(t_data);
|
|
|
|
if (res_err) {
|
|
return res_err;
|
|
}
|
|
|
|
if ((std::chrono::steady_clock::now() - start_poll_tm) > slew_point.slewTimeout) {
|
|
logError("Waiting time for completion of slewing expired!");
|
|
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
|
|
}
|
|
}
|
|
|
|
|
|
return MccSimpleSlewModelErrorCode::ERROR_OK;
|
|
};
|
|
}
|
|
};
|
|
|
|
static_assert(std::movable<MccSimpleSlewModel<>>);
|
|
|
|
} // namespace mcc
|