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-23 19:44:05 +03:00
parent 14f3bb7a83
commit fd67d04ca2
8 changed files with 423 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

230
cxx/mcc_slew_model.h
View File

@@ -21,6 +21,7 @@ enum class MccSimpleSlewModelErrorCode : int {
ERROR_TELEMETRY_DATA,
ERROR_PEC_COMP,
ERROR_HARDWARE_SETPOS,
ERROR_HARDWARE_GETPOS,
ERROR_SLEW_TIMEOUT
};
@@ -59,6 +60,8 @@ struct MccSimpleSlewModelCategory : public std::error_category {
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:
@@ -69,6 +72,8 @@ struct MccSimpleSlewModelCategory : public std::error_category {
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";
default:
return "UNKNOWN";
}
@@ -106,31 +111,26 @@ public:
typedef std::error_code error_t;
struct slew_params_t {
typedef mcc::MccAngle coord_t;
typedef MccCelestialPoint slew_point_t;
mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
// struct slew_params_t {
// typedef mcc::MccAngle coord_t;
coord_t x{0.0};
coord_t y{0.0};
// mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
// if <= 0 then hardware must assume default rate
coord_t xrate{-1};
coord_t yrate{-1};
// coord_t x{0.0};
// coord_t y{0.0};
bool stop{false};
};
// // 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};
bool stopAfterSlew{false};
std::chrono::seconds timeout{300};
};
@@ -156,7 +156,7 @@ public:
logDebug(std::format("Delete 'MccSimpleSlewModel' class instance ({})", (void*)this));
}
error_t slew(slew_params_t pars)
error_t slew(slew_point_t pars)
{
error_t res_err = _slewFunc(std::move(pars));
@@ -164,7 +164,7 @@ public:
}
protected:
std::function<error_t(const slew_params_t&)> _slewFunc{};
std::function<error_t(const slew_point_t&)> _slewFunc{};
void init(auto& mount_controls, context_t context)
{
@@ -215,7 +215,7 @@ protected:
};
_slewFunc = [p_mount_controls, context = std::move(context), check_zones](this auto&& self,
slew_params_t slew_pars) {
slew_point_t slew_point) {
auto& astrom_engine = p_mount_controls->astrometryEngine;
auto& hardware = p_mount_controls->hardware;
auto& pec = p_mount_controls->PEC;
@@ -234,14 +234,14 @@ protected:
typename telemetry_t::mount_telemetry_data_t t_data;
if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
// 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;
ax_pos.x = slew_point.x;
ax_pos.y = slew_point.y;
// ax_pos.xrate = slew_point.xrate;
// ax_pos.yrate = slew_point.yrate;
} else if (slew_pars.coordPairKind ==
} else if (slew_point.coordPairKind ==
mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // catalog coordinates
jd_t jd;
coord_t ra_app, dec_app, ha, az, alt;
@@ -249,37 +249,38 @@ protected:
logDebug("Input slew coordinates are ICRS RA-DEC: convert it to apparent ...");
ast_err = astrom_engine->greg2jul(std::chrono::system_clock::now(), jd);
ast_err = astrom_engine->greg2jul(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
ast_err = astrom_engine->icrs2obs(slew_pars.x, slew_pars.y, jd, ra_app, dec_app, ha, az, alt, eo);
ast_err = astrom_engine->icrs2obs(slew_point.x, slew_point.y, jd, ra_app, dec_app, ha, az, alt, eo);
if (!ast_err) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = ha;
slew_pars.y = dec_app;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = ha;
slew_point.y = dec_app;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_pars.x = az;
slew_pars.y = alt;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_point.x = az;
slew_point.y = alt;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
}
}
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) { // apparent
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) { // apparent
jd_t jd;
typename astrom_engine_t::eo_t eo;
logDebug("Input slew coordinates are apparent RA-DEC: convert it to apparent HA-DEC ...");
ast_err = astrom_engine->greg2jul(std::chrono::system_clock::now(), jd);
ast_err = astrom_engine->greg2jul(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
typename astrom_engine_t::sideral_time_t lst;
ast_err = astrom_engine->apparentSiderTime(jd, lst, true);
@@ -287,14 +288,14 @@ protected:
if (!ast_err) {
ast_err = astrom_engine->eqOrigins(jd, eo);
if (!ast_err) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = lst - slew_pars.x + eo; // HA = LST - RA_APP + EO
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = lst - slew_point.x + eo; // HA = LST - RA_APP + EO
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
}
}
}
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) { // apparent
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) { // apparent
if constexpr (mccIsEquatorialMount(pec_t::mountType)) { // compute encoder coordinates
logDebug("Input slew coordinates are apparent HA-DEC: convert it to hardware encoder ones ...");
@@ -302,46 +303,46 @@ protected:
typename pec_t::pec_result_t pec_res;
// 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);
// pec_err = pec->reverseCompute(slew_point.x, slew_point.y, pec_res, context.eps, context.maxIter);
pec_err = pec->compute(slew_point.x, slew_point.y, pec_res);
if (!pec_err) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_pars.x -= pec_res.dx;
slew_pars.y -= pec_res.dy;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_point.x -= pec_res.dx;
slew_point.y -= pec_res.dy;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
}
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
coord_t az, alt;
logDebug("Input slew coordinates are apparent HA-DEC: convert it to AZ-ALT ...");
ast_err = astrom_engine->hadec2azalt(slew_pars.x, slew_pars.y, az, alt);
ast_err = astrom_engine->hadec2azalt(slew_point.x, slew_point.y, az, alt);
if (!ast_err) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_pars.x = az;
slew_pars.y = alt;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_point.x = az;
slew_point.y = alt;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
coord_t ha, dec;
logDebug("Input slew coordinates are AZ-ALT: convert it to HA-DEC ...");
ast_err = astrom_engine->azalt2hadec(slew_pars.x, slew_pars.y, ha, dec);
ast_err = astrom_engine->azalt2hadec(slew_point.x, slew_point.y, ha, dec);
if (!ast_err) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_pars.x = ha;
slew_pars.y = dec;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = ha;
slew_point.y = dec;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
}
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) { // compute encoder coordinates
coord_t eps = 1.0 / 3600.0 * std::numbers::pi / 180.0;
@@ -350,28 +351,28 @@ protected:
typename pec_t::pec_result_t pec_res;
// 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);
// pec_err = pec->reverseCompute(slew_point.x, slew_point.y, pec_res, context.eps, context.maxIter);
pec_err = pec->compute(slew_point.x, slew_point.y, pec_res);
if (!pec_err) {
slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_pars.x -= pec_res.dx;
slew_pars.y -= pec_res.dy;
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
slew_point.x -= pec_res.dx;
slew_point.y -= pec_res.dy;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
} else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
//
// WARNING: it is assumed that coordinates are in radians!
//
logDebug("Input slew coordinates are AZ-ZD: convert it to AZ-ALT ...");
slew_pars.y = std::numbers::pi / 2.0 - slew_pars.y;
slew_point.y = std::numbers::pi / 2.0 - slew_point.y;
res_err = self(std::move(slew_pars));
res_err = self(std::move(slew_point));
} else {
return MccSimpleSlewModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
}
@@ -424,12 +425,13 @@ protected:
size_t i_iter = 0;
context.guidingRateEps *= context.guidingRateEps;
// 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;
typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
// 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();
auto start_poll_tm = std::chrono::steady_clock::now();
while (true) {
// check prohibited zones
@@ -441,53 +443,75 @@ protected:
}
// it is assumed here that telemetry data is in actual state!
t_err = telemetry.data(t_data);
if (t_err) {
hardware.stop();
// t_err = telemetry.data(t_data);
// if (t_err) {
// hardware.stop();
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;
// 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;
// }
// }
err = hardware->getPos(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(t_err)>) {
logError(std::format("An telemetry error occured: code = {}", t_err));
if constexpr (traits::mcc_formattable<decltype(err)>) {
logError(std::format("An hardware error occured: code = {}", err));
}
return MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS;
}
}
if (prev_time_point == t_data.time_point) {
// if (prev_time_point == t_data.time_point) {
if (prev_time_point == ax_pos.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;
if (context.stopAfterSlew) { // slew and stop, so mount moving rate must be 0 at the end
if (ax_pos.state == hardware_t::hw_state_t::HW_STATE_STOP) {
break;
}
} 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;
// mount_rate2 = t_data.mntRateX * t_data.mntRateX + t_data.mntRateY * t_data.mntRateY;
if (mount_rate2 <= context.guidingRateEps) {
++i_iter;
} else {
i_iter = 0;
// if (utils::isEqual((double)mount_rate2, 0.0)) {
// ++i_iter;
// } else {
// i_iter = 0;
// }
} else { // slew and guiding, so mount rate must be near tracking rate at the end
if (ax_pos.state == hardware_t::hw_state_t::HW_STATE_TRACK) {
break;
}
// 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;
}
// 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) {
if ((std::chrono::steady_clock::now() - start_poll_tm) > context.timeout) {
logError("Waiting time for completion of slewing expired!");
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
}