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-24 18:55:59 +03:00
parent 7e1709727a
commit 961c72f17f
8 changed files with 562 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

357
cxx/mcc_slew_model.h
View File

@@ -112,44 +112,35 @@ public:
typedef std::error_code error_t;
typedef MccCelestialPoint slew_point_t;
struct slew_point_t : MccCelestialPoint {
// target-mount coordinate difference to start adjusting slewing (in radians)
coord_t adjustCoordDiff{(double)MccAngle{10.0_degs}};
// struct slew_params_t {
// typedef mcc::MccAngle coord_t;
// coordinates difference to stop slewing (in radians)
coord_t slewPrecision{(double)MccAngle{5.0_arcsecs}};
// mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
// 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 {
// coordinates polling interval in seconds
std::chrono::duration<double> coordPollingInterval{0.1};
bool stopAfterSlew{false};
std::chrono::seconds timeout{300};
std::chrono::seconds timeout{3600};
};
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T, typename... LoggerCtorArgTs>
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, context_t context, LoggerCtorArgTs&&... ctor_args)
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, 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(mount_controls, std::move(context));
init(mount_controls);
}
template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, context_t context)
MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls)
requires(std::same_as<LoggerT, MccNullLogger>)
{
init(mount_controls, std::move(context));
init(mount_controls);
}
virtual ~MccSimpleSlewModel()
@@ -167,7 +158,7 @@ public:
protected:
std::function<error_t(const slew_point_t&)> _slewFunc{};
void init(auto& mount_controls, context_t context)
void init(auto& mount_controls)
{
// deduce controls types
using astrom_engine_t = decltype(mount_controls.astrometryEngine);
@@ -180,43 +171,8 @@ protected:
const auto p_mount_controls = &mount_controls;
// prohibited zones related lambdas
auto check_zones = [p_mount_controls]<size_t... Is>(std::index_sequence<Is...>) {
// std::array<std::error_code, sizeof...(Is)> result{};
error_t ret;
(
[&ret]() {
if constexpr (Is > 0) {
if (ret) {
return;
}
}
typename telemetry_t::mount_telemetry_data_t tdata;
auto tel_err = p_mount_controls->telemetry.data(tdata);
if (tel_err) {
if constexpr (std::same_as<decltype(tel_err), error_t>) {
ret = tel_err;
} else {
ret = MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
}
} else {
ret = std::get<Is>(p_mount_controls->prohibitedZones).inZone(tdata)
? MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE
: MccSimpleSlewModelErrorCode::ERROR_OK;
}
}(),
...);
return ret;
};
_slewFunc = [p_mount_controls, context = std::move(context), check_zones](this auto&& self,
slew_point_t slew_point) {
_slewFunc = [p_mount_controls](this auto&& self, 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,137 +190,164 @@ protected:
typename telemetry_t::error_t t_err;
typename telemetry_t::mount_telemetry_data_t t_data;
coord_t ra_icrs, dec_icrs;
if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
// trivial case (the pair is interpretated as raw encoder coordinates)
ax_pos.x = slew_point.x;
ax_pos.y = slew_point.y;
// ax_pos.xrate = slew_point.xrate;
// ax_pos.yrate = slew_point.yrate;
// the pair is interpretated as raw encoder coordinates
if (slew_point.stopAfterSlew) {
ax_pos.x = slew_point.x;
ax_pos.y = slew_point.y;
} else { // very strange but should be processed! forward to compute ICRS RA AND DEC
typename pec_t::pec_result_t pec_res;
pec_err = pec->compute(slew_point.x, slew_point.y, pec_res);
if (!pec_err) {
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_point));
}
}
} 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;
typename astrom_engine_t::eo_t eo;
if (slew_point.stopAfterSlew) {
jd_t jd;
coord_t ra_app, dec_app, ha, az, alt;
typename astrom_engine_t::eo_t eo;
logDebug("Input slew coordinates are ICRS RA-DEC: convert it to apparent ...");
logDebug("Input slew coordinates are ICRS RA-DEC: convert it to apparent ...");
ast_err = astrom_engine->greg2jul(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
ast_err = astrom_engine->icrs2obs(slew_point.x, slew_point.y, jd, ra_app, dec_app, ha, az, alt, eo);
ast_err = astrom_engine->greg2jul(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = ha;
slew_point.y = dec_app;
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_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = ha;
slew_point.y = dec_app;
res_err = self(std::move(slew_point));
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_point.x = az;
slew_point.y = alt;
res_err = self(std::move(slew_point));
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
}
}
} else { // OK, here one should stop with coordinates converting
ra_icrs = slew_point.x;
dec_icrs = slew_point.y;
}
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) { // apparent
if (slew_point.stopAfterSlew) {
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(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
typename astrom_engine_t::sideral_time_t lst;
ast_err = astrom_engine->apparentSiderTime(jd, lst, true);
if (!ast_err) {
ast_err = astrom_engine->eqOrigins(jd, eo);
if (!ast_err) {
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_point));
}
}
}
}
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) { // apparent
if (slew_point.stopAfterSlew) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) { // compute encoder coordinates
logDebug("Input slew coordinates are apparent HA-DEC: convert it to hardware encoder ones ...");
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_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_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_point));
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
}
} 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_point.x, slew_point.y, az, alt);
if (!ast_err) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_point.x = az;
slew_point.y = alt;
res_err = self(std::move(slew_point));
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
}
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
if (slew_point.stopAfterSlew) {
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
coord_t ha, dec;
} 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 AZ-ALT: convert it to HA-DEC ...");
logDebug("Input slew coordinates are apparent RA-DEC: convert it to apparent HA-DEC ...");
ast_err = astrom_engine->azalt2hadec(slew_point.x, slew_point.y, ha, dec);
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);
if (!ast_err) {
ast_err = astrom_engine->eqOrigins(jd, eo);
if (!ast_err) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = lst - slew_point.x + eo; // HA = LST - RA_APP + EO
slew_point.x = ha;
slew_point.y = dec;
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;
logDebug("Input slew coordinates are AZ-ALT: convert it to hardware encoder ones ...");
typename pec_t::pec_result_t 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_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_point));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
}
} 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 ...");
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_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_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_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_point.x, slew_point.y, az, alt);
if (!ast_err) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
slew_point.x = az;
slew_point.y = alt;
res_err = self(std::move(slew_point));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
} 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_point.x, slew_point.y, ha, dec);
if (!ast_err) {
slew_point.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
slew_point.x = ha;
slew_point.y = dec;
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;
logDebug("Input slew coordinates are AZ-ALT: convert it to hardware encoder ones ...");
typename pec_t::pec_result_t 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_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_point));
}
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
} else if (slew_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
//
// WARNING: it is assumed that coordinates are in radians!
@@ -408,6 +391,33 @@ protected:
}
}
// compute ICRS RA and DEC if needed
if (!slew_point.stopAfterSlew) {
if (slew_point.coordPairKind != mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
jd_t jd;
ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd);
if (!ast_err) {
ast_err = astrom_engine.obs2icrs(slew_point.coordPairKind, slew_point.x, slew_point.y, jd,
ra_icrs, dec_icrs);
}
if (ast_err) {
if constexpr (std::same_as<decltype(ast_err), error_t>) {
logError(
std::format("An error occured while performing astrometry computations: code = {} ({})",
ast_err.value(), ast_err.message()));
return ast_err;
} else {
if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
logError(std::format(
"An error occured while performing astrometry computations: code = {}", ast_err));
}
return MccSimpleSlewModelErrorCode::ERROR_ASTROM_COMP;
}
}
}
}
// move mount (it is assumed this is asynchronous operation!!!)
typename hardware_t::error_t err = hardware->setPos(ax_pos);
@@ -426,11 +436,11 @@ protected:
size_t i_iter = 0;
// context.guidingRateEps *= context.guidingRateEps;
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;
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
adjRad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff;
std::array<bool, Nzones> in_zone_flag;
auto start_poll_tm = std::chrono::steady_clock::now();
@@ -438,10 +448,6 @@ protected:
while (true) {
// check prohibited zones
// res_err = check_zones(std::make_index_sequence<Nzones>{});
// if (res_err) {
// return res_err;
// }
t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);
@@ -474,12 +480,27 @@ protected:
}
}
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
xr = slew_point.x - t_data.mntHA;
yr = slew_point.y - t_data.mntDEC;
} else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
xr = slew_point.x - t_data.mntAZ;
yr = slew_point.y - t_data.mntALT;
} else {
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
}
coord_diff2 = xr * xr + yr * yr;
if (coord_diff2 < adjRad2) { // switch to adjusting mode
}
// if (prev_time_point == t_data.time_point) {
if (prev_time_point == ax_pos.time_point) {
continue;
}
if (context.stopAfterSlew) { // slew and stop, so mount moving rate must be 0 at the end
if (slew_point.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;
}
@@ -511,7 +532,7 @@ protected:
prev_time_point = t_data.time_point;
if ((std::chrono::steady_clock::now() - start_poll_tm) > context.timeout) {
if ((std::chrono::steady_clock::now() - start_poll_tm) > slew_point.timeout) {
logError("Waiting time for completion of slewing expired!");
return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
}