eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2025-07-21 23:49:12 +03:00
parent 743ade7916
commit 2faa3f0aca
7 changed files with 144 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
cxx/mcc_slew_model.h
View File

@@ -19,7 +19,8 @@ enum class MccSimpleSlewModelErrorCode : int {
ERROR_ASTROM_COMP,
ERROR_TELEMETRY_DATA,
ERROR_PEC_COMP,
ERROR_HARDWARE_SETPOS
ERROR_HARDWARE_SETPOS,
ERROR_SLEW_TIMEOUT
};
} // namespace mcc
@@ -105,12 +106,29 @@ public:
coord_t x{0.0};
coord_t y{0.0};
// if <= 0 then hardware must assume default rate
coord_t xrate{-1};
coord_t yrate{-1};
bool stop{false};
};
struct context_t {
// double eps{0.01};
// size_t maxIter{5};
slew_params_t::coord_t guidingRateX;
slew_params_t::coord_t guidingRateY;
slew_params_t::coord_t guidingRateEps;
size_t maxRateCycles{5};
std::chrono::seconds timeout{300};
};
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls)
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, context_t context)
{
// deduce controls types
using astrom_engine_t = decltype(mount_controls.astrometryEngine);
@@ -159,7 +177,8 @@ public:
};
_slewFunc = [p_mount_controls, check_zones](this auto&& self, const slew_params_t& slew_pars) {
_slewFunc = [p_mount_controls, context = std::move(context), check_zones](this auto&& self,
slew_params_t slew_pars) {
auto& astrom_engine = p_mount_controls->astrometryEngine;
auto& hardware = p_mount_controls->hardware;
auto& pec = p_mount_controls->PEC;
@@ -178,6 +197,9 @@ public:
// trivial case (the pair is interpretated as raw encoder coordinates)
ax_pos.x = slew_pars.x;
ax_pos.y = slew_pars.y;
ax_pos.xrate = slew_pars.xrate;
ax_pos.yrate = slew_pars.yrate;
} else if (slew_pars.coordPairKind ==
mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // catalog coordinates
jd_t jd;
@@ -191,15 +213,17 @@ public:
if (!ast_err) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
.x = ha,
.y = dec_app,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = ha;
slew_pars.y = dec_app;
res_err = self(std::move(slew_pars));
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT,
.x = az,
.y = alt,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_pars.x = az;
slew_pars.y = alt;
res_err = self(std::move(slew_pars));
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
@@ -218,10 +242,10 @@ public:
if (!ast_err) {
ast_err = astrom_engine->eqOrigins(jd, eo);
if (!ast_err) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
.x = lst - slew_pars.x + eo, // HA = LST - RA_APP + EO
.y = slew_pars.y,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = lst - slew_pars.x + eo; // HA = LST - RA_APP + EO
res_err = self(std::move(slew_pars));
}
}
}
@@ -231,12 +255,14 @@ public:
typename pec_t::pec_result_t pec_res;
pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, eps, 10);
// pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, context.eps, context.maxIter);
pec_err = pec->compute(slew_pars.x, slew_pars.y, pec_res);
if (!pec_err) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY,
.x = slew_pars.x - pec_res.dx,
.y = slew_pars.y - pec_res.dy,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_pars.x -= pec_res.dx;
slew_pars.y -= pec_res.dy;
res_err = self(std::move(slew_pars));
}
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
coord_t az, alt;
@@ -244,10 +270,11 @@ public:
ast_err = astrom_engine->hadec2azalt(slew_pars.x, slew_pars.y, az, alt);
if (!ast_err) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT,
.x = az,
.y = alt,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_pars.x = az;
slew_pars.y = alt;
res_err = self(std::move(slew_pars));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
@@ -259,22 +286,25 @@ public:
ast_err = astrom_engine->azalt2hadec(slew_pars.x, slew_pars.y, ha, dec);
if (!ast_err) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
.x = ha,
.y = dec,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = ha;
slew_pars.y = dec;
res_err = self(std::move(slew_pars));
}
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) { // compute encoder coordinates
coord_t eps = 1.0 / 3600.0 * std::numbers::pi / 180.0;
typename pec_t::pec_result_t pec_res;
pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, eps, 10);
// pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, context.eps, context.maxIter);
pec_err = pec->compute(slew_pars.x, slew_pars.y, pec_res);
if (!pec_err) {
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY,
.x = slew_pars.x - pec_res.dx,
.y = slew_pars.y - pec_res.dy,
.stop = slew_pars.stop});
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_pars.x -= pec_res.dx;
slew_pars.y -= pec_res.dy;
res_err = self(std::move(slew_pars));
}
} else {
@@ -284,10 +314,9 @@ public:
//
// WARNING: it is assumed that coordinates are in radians!
//
res_err = self({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT,
.x = slew_pars.x,
.y = std::numbers::pi / 2.0 - slew_pars.y,
.stop = slew_pars.stop});
slew_pars.y = std::numbers::pi / 2.0 - slew_pars.y;
res_err = self(std::move(slew_pars));
}
if (res_err) {
@@ -324,9 +353,15 @@ public:
}
}
//
// what is the condition for mount reaches given position?!!
//
size_t i_iter = 0;
context.guidingRateEps *= context.guidingRateEps;
typename telemetry_t::mount_telemetry_data_t::time_point_t prev_time_point{};
typename telemetry_t::mount_telemetry_data_t::coord_t xrate, yrate, mount_rate2;
auto start_poll_tm = std::chrono::high_resolution_clock::now();
while (true) {
// check prohibited zones
res_err = check_zones(std::make_index_sequence<Nzones>{});
@@ -347,6 +382,40 @@ public:
return MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
}
}
if (prev_time_point == t_data.time_point) {
continue;
}
if (slew_pars.stop) { // slew and stop, so mount moving rate must be 0 at the end
mount_rate2 = t_data.mntRateX * t_data.mntRateX + t_data.mntRateY * t_data.mntRateY;
if (utils::isEqual((double)mount_rate2, 0.0)) {
++i_iter;
} else {
i_iter = 0;
}
} else { // slew and guiding, so mount rate must be near guiding rate at the end
xrate = t_data.mntRateX - context.guidingRateX;
yrate = t_data.mntRateY - context.guidingRateY;
mount_rate2 = xrate * xrate + yrate * yrate;
if (mount_rate2 <= context.guidingRateEps) {
++i_iter;
} else {
i_iter = 0;
}
}
if (i_iter >= context.maxRateCycles) {
break;
}
prev_time_point = t_data.time_point;
if ((std::chrono::high_resolution_clock::now() - start_poll_tm) > context.timeout) {
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
}
}
@@ -354,9 +423,9 @@ public:
};
}
error_t slew(const slew_params_t& pars)
error_t slew(slew_params_t pars)
{
error_t res_err = _slewFunc(pars);
error_t res_err = _slewFunc(std::move(pars));
return res_err;
}