From fd67d04ca216fc8537ec38781216745882f752db Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Wed, 23 Jul 2025 19:44:05 +0300 Subject: [PATCH] ... --- cxx/CMakeLists.txt | 2 +- cxx/asibfm700_hardware.cpp | 24 ++-- cxx/asibfm700_hardware.h | 7 ++ cxx/mcc_guiding_model.h | 181 ++++++++++++++++++++++++++--- cxx/mcc_mount_astro_erfa.h | 5 + cxx/mcc_mount_concepts.h | 117 +++++++++++++++---- cxx/mcc_mount_coord.h | 20 ++-- cxx/mcc_slew_model.h | 230 ++++++++++++++++++++----------------- 8 files changed, 423 insertions(+), 163 deletions(-) diff --git a/cxx/CMakeLists.txt b/cxx/CMakeLists.txt index acc18f0..089835c 100644 --- a/cxx/CMakeLists.txt +++ b/cxx/CMakeLists.txt @@ -125,7 +125,7 @@ add_library(${CNTR_PROTO_LIB} STATIC ${CNTR_PROTO_LIB_SRC}) set(MCC_LIBRARY_SRC mcc_mount_concepts.h mcc_fsm_mount.h mcc_mount_coord.h mcc_mount_events_states.h mcc_finite_state_machine.h mcc_mount_pec.h mcc_mount_pz.h mcc_traits.h mcc_mount_telemetry.h mcc_mount_config.h mcc_mount_astro_erfa.h mcc_astrom_iers.h mcc_astrom_iers_default.h - mcc_slew_model.h mcc_utils.h mcc_spdlog.h) + mcc_slew_model.h mcc_guiding_model.h mcc_utils.h mcc_spdlog.h) set(MCC_LIBRARY mcc) add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC}) target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23) diff --git a/cxx/asibfm700_hardware.cpp b/cxx/asibfm700_hardware.cpp index 047573a..c9311f9 100644 --- a/cxx/asibfm700_hardware.cpp +++ b/cxx/asibfm700_hardware.cpp @@ -108,19 +108,29 @@ AsibFM700Hardware::error_t AsibFM700Hardware::getPos(AsibFM700Hardware::axes_pos auto err = static_cast(Mount.getMountData(&data)); if (err == AsibFM700HardwareErrorCode::ERROR_OK) { - // time point from sidservo library is 'struct timeval', i.e., seconds and - // microseconds ellapsed from UNIX time epoch + // time point from sidservo library is 'double' number represented UNIXTIME with + // microseconds/nanoseconds precision (must be equal for encXposition and encYposition) using secs_t = std::chrono::duration; - secs_t secs = secs_t{1.0 * data.encposition.msrtime.tv_sec + 1.0E-6 * data.encposition.msrtime.tv_usec}; + secs_t secs = secs_t{data.encXposition.t}; pos.time_point = time_point_t{std::chrono::duration_cast(secs)}; - // according to hardware configuration (encoders pins (Eddy said)) X is DEC-axis and Y is HA-axis - pos.x = data.encposition.Y; - pos.y = data.encposition.X; + // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis + pos.x = data.encYposition.val; + pos.y = data.encXposition.val; - // ERROR: how can I get rates?!!!!!!!!!! + pos.xrate = data.encYspeed.val; + pos.yrate = data.encXspeed.val; + + // mount state + + // are both motors stopped? + bool stop_motors = (data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT); + if (stop_motors) { + pos.state = hw_state_t::HW_STATE_STOP; + return AsibFM700HardwareErrorCode::ERROR_OK; + } } return err; diff --git a/cxx/asibfm700_hardware.h b/cxx/asibfm700_hardware.h index 7f934a7..a992b54 100644 --- a/cxx/asibfm700_hardware.h +++ b/cxx/asibfm700_hardware.h @@ -61,15 +61,22 @@ namespace asibfm700 class AsibFM700Hardware final { public: + static constexpr uint8_t XMOTOR_STOP_BIT = 0b00000001; // if 0th bit set then motor is stopped + static constexpr uint8_t YMOTOR_STOP_BIT = 0b00010000; // if 4th bit set then motor is stopped + // definitions from concept typedef std::error_code error_t; typedef mcc::MccAngle coord_t; typedef std::chrono::system_clock::time_point time_point_t; // UTC time + enum class hw_state_t : int { HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK }; + struct axes_pos_t { time_point_t time_point; coord_t x, y; coord_t xrate, yrate; + + hw_state_t state; }; diff --git a/cxx/mcc_guiding_model.h b/cxx/mcc_guiding_model.h index 784080b..761d0c7 100644 --- a/cxx/mcc_guiding_model.h +++ b/cxx/mcc_guiding_model.h @@ -19,7 +19,10 @@ enum class MccSimpleGuidingModelErrorCode : int { ERROR_ASTROM_COMP, ERROR_TELEMETRY_DATA, ERROR_PEC_COMP, + ERROR_HARDWARE_SETPOS, ERROR_INVALID_CONTEXT_PARAM, + ERROR_INVALID_THRESH, + ERROR_INVALID_CORR_RANGE, }; } // namespace mcc @@ -56,12 +59,22 @@ struct MccSimpleGuidingModelCategory : public std::error_category { 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"; default: return "UNKNOWN"; } @@ -123,34 +136,38 @@ public: typedef std::error_code error_t; struct guiding_context_t { - double corrThresh{MccAngle("00:00:00.2"_dms)}; // correction threshold + double corrThresh{MccAngle("00:00:00.2"_dms)}; // correction threshold + double correctionRange[2]{MccAngle("00:00:00.5"_dms), MccAngle("00:00:05"_dms)}; + std::chrono::duration predictedTrackDuration{10.0}; // 10 seconds std::chrono::duration predictedTrackResolution{0.1}; // 0.1 seconds }; - struct guiding_point_t { - typedef double coord_t; + typedef MccCelestialPoint guiding_point_t; - mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; + // struct guiding_point_t { + // typedef double coord_t; - coord_t x, y; - }; + // mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; + + // coord_t x, y; + // }; template - MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, LoggerCtorArgTs&&... ctor_args) + MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context, LoggerCtorArgTs&&... ctor_args) requires(!std::same_as) : LoggerT(std::forward(ctor_args)...) { logDebug(std::format("Create 'MccSimpleGuidingModel' class instance ({})", (void*)this)); - init(mount_controls); + init(mount_controls, std::move(context)); } template - MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls) + MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context) requires(std::same_as) { - init(mount_controls); + init(mount_controls, std::move(context)); } virtual ~MccSimpleGuidingModel() @@ -159,7 +176,20 @@ public: } - error_t guiding(guiding_point_t guiding_point) {} + 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; + } protected: std::function _guidingFunc{}; @@ -182,10 +212,63 @@ protected: return MccSimpleGuidingModelErrorCode::ERROR_INVALID_CONTEXT_PARAM; } + auto resi_thresh2 = context.corrThresh * context.corrThresh; + + if (utils::isEqual(resi_thresh2, 0.0)) { + return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH; + } + const auto p_mount_controls = &mount_controls; - _guidingFunc = [p_mount_controls, context = std::move(context), predicted_Npoints]( + auto check_zones = [p_mount_controls, this]() { + return [this](std::index_sequence) { + 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) { + ret = tel_err; + } else { + ret = MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA; + } + } else { + ret = std::get(p_mount_controls->prohibitedZones).inZone(tdata) + ? MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE + : MccSimpleGuidingModelErrorCode::ERROR_OK; + if (ret) { + auto log_str = std::format("given coordinates are in prohibited zone '{}'", + std::get(p_mount_controls->prohibitedZones).name()); + logError(log_str); + } + } + }(), + ...); + + + return ret; + }(std::make_index_sequence{}); + }; + + + _guidingFunc = [p_mount_controls, context = std::move(context), predicted_Npoints, this]( this auto&& self, guiding_point_t guiding_point) { + if (context.correctionRange[0] >= context.correctionRange[1]) { + return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH; + } + + auto low_corr_limit = context.correctionRange[0] * context.correctionRange[0]; + auto high_corr_limit = context.correctionRange[1] * context.correctionRange[1]; + auto& astrom_engine = p_mount_controls->astrometryEngine; auto& hardware = p_mount_controls->hardware; auto& pec = p_mount_controls->PEC; @@ -196,8 +279,6 @@ protected: jd_t jd; - typename hardware_t::axes_pos_t ax_pos; - error_t res_err; typename astrom_engine_t::error_t ast_err; typename pec_t::error_t pec_err; @@ -297,7 +378,7 @@ protected: } if (guiding_point.coordPairKind != mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { - 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.obs2icrs(guiding_point.coordPairKind, guiding_point.x, guiding_point.y, jd, ra_icrs, dec_icrs); @@ -321,11 +402,33 @@ protected: coord_t ha, ra_app, dec_app, az, alt, eo; + coord_t xr, yr, coord_diff; + typename hardware_t::axes_pos_t ax_pos; while (true) { // check prohibited zones ... - ast_err = astrom_engine.greg2jul(std::chrono::system_clock::now(), jd); - ast_err = astrom_engine.icrs2obs(ra_icrs, dec_icrs, jd, ra_app, dec_app, ha, az, alt, eo); + if ((res_err = check_zones())) { + return res_err; + } + + ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd); + if (!ast_err) { + ast_err = astrom_engine.icrs2obs(ra_icrs, dec_icrs, jd, ra_app, dec_app, ha, az, alt, eo); + } + if (ast_err) { + if constexpr (std::same_as) { + 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) { + logError(std::format("An error occured while performing astrometry computations: code = {}", + ast_err)); + } + return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP; + } + } t_err = telemetry.data(t_data); if (t_err) { @@ -342,6 +445,50 @@ protected: } // compare t_data with computed coordinates ... + if (_doCorrection) { + if constexpr (mccIsEquatorialMount(pec_t::mountType)) { + xr = t_data.mntHA - ha; + yr = t_data.mntDEC - dec_app; + } else if constexpr (mccIsAltAzMount(pec_t::mountType)) { + xr = t_data.mntAZ - az; + yr = t_data.mntALT - alt; + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + + coord_diff = xr * xr + yr * yr; + + if (coord_diff < low_corr_limit) { + continue; + } + + if (coord_diff > high_corr_limit) { + logWarn(std::format( + "guiding model: the 'mount-target' difference exceeds the limit (diff = {}; lim = {})", + (double)coord_diff, (double)high_corr_limit)); + continue; + } + + // do correction + ax_pos.state = hardware_t::hw_state_t::HW_STATE_TRACK; + ax_pos.x = t_data.mntPosX; + ax_pos.y = t_data.mntPosY; + + // asynchronous operation! + auto err = hardware.setPos(std::move(ax_pos)); + if (err) { + if constexpr (std::same_as) { + logError( + std::format("An hardware error occured: code = {} ({})", err.value(), err.message())); + return err; + } else { + if constexpr (traits::mcc_formattable) { + logError(std::format("An hardware error occured: code = {}", err)); + } + return MccSimpleGuidingModelErrorCode::ERROR_HARDWARE_SETPOS; + } + } + } } return MccSimpleGuidingModelErrorCode::ERROR_OK; diff --git a/cxx/mcc_mount_astro_erfa.h b/cxx/mcc_mount_astro_erfa.h index d638438..018c3ea 100644 --- a/cxx/mcc_mount_astro_erfa.h +++ b/cxx/mcc_mount_astro_erfa.h @@ -251,6 +251,11 @@ public: /* time-related methods */ + static time_point_t timePointNow() + { + return time_point_t::clock::now(); + } + // templated generic version template error_t greg2jul(TpT time_point, juldate_t& juldate) diff --git a/cxx/mcc_mount_concepts.h b/cxx/mcc_mount_concepts.h index 55ed9dc..8763f89 100644 --- a/cxx/mcc_mount_concepts.h +++ b/cxx/mcc_mount_concepts.h @@ -72,6 +72,15 @@ struct MccNullLogger { void logInfo(const std::string&) {}; }; + +struct MccCelestialPoint { + typedef double coord_t; + + MccCoordPairKind coordPairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; + coord_t x{0.0}, y{0.0}; +}; + + } // namespace mcc @@ -98,6 +107,7 @@ concept mcc_logger_c = requires(T t, const T t_const) { { t.logInfo(std::declval()) }; }; + /* ASTROMETRY-RELATED COMPUTATION ENGINE */ template @@ -165,6 +175,9 @@ concept mcc_astrom_engine_c = requires(T t, const T t_const) { /* time-related methods */ + // this static method must return a current time point + { T::timePointNow() } -> std::same_as; + // Gregorian Calendar time point to Julian Date: greg2jul(time_point, jd) { t.greg2jul(std::declval(), std::declval()) @@ -202,20 +215,48 @@ concept mcc_mount_hardware_c = !std::copyable && std::movable && requires( { t_const.id() } -> mcc_formattable; + // a type that defines at least HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK + // compile-time constants + // e.g. an implementations can be as follows: + // enum class HW_STATE: int {HW_STATE_STOP, HW_STATE_SLEW, HW_STATE_TRACK} + // + // struct HW_STATE { + // uint8_t HW_STATE_STOP = 100; + // uint8_t HW_STATE_SLEW = 200; + // uint8_t HW_STATE_TRACK = 300; + // } + requires requires(typename T::hw_state_t state) { + []() { + // hardware is in stop state (no any moving) + static constexpr auto v1 = T::hw_state_t::HW_STATE_STOP; + // hardware is in slew state (move to given celestial point) + + static constexpr auto v2 = T::hw_state_t::HW_STATE_SLEW; + + // hardware is in track state (track given celestial point) + static constexpr auto v3 = T::hw_state_t::HW_STATE_TRACK; + }(); + }; + + // a class that contains at least time of measurement, coordinates for x,y axes and its moving rates requires requires(typename T::axes_pos_t pos) { - requires std::same_as; + requires std::same_as; // time point of measurement - requires std::same_as; - requires std::same_as; + requires std::same_as; // co-longitude coordinate + requires std::same_as; // co-latitude coordinate requires std::same_as; requires std::same_as; + + requires std::same_as; // hardware state }; { t.setPos(std::declval()) } -> std::same_as; { t.getPos(std::declval()) } -> std::same_as; + { t_const.getState(std::declval()) } -> std::same_as; + { t.stop() } -> std::same_as; { t.init() } -> std::same_as; }; @@ -302,11 +343,11 @@ concept mcc_mount_telemetry_c = requires(T t, const T t_const) { }; +/* A CONCEPT FOR CLASS TO REPRESENT CELESTIAL POINT */ -/* SLEW PARAMETERS */ template -concept mcc_slew_params_c = std::movable && requires(T t) { +concept mcc_celestial_point_c = requires(T t) { // input coordinates pair type (e.g. IRCS RA,DEC, Az,Alt and so on) requires std::same_as; @@ -317,37 +358,49 @@ concept mcc_slew_params_c = std::movable && requires(T t) { // co-latitude (e.g. DEC or ZD) requires std::same_as; - - // stop after slewing - requires std::convertible_to; }; +// /* SLEW PARAMETERS */ + +// template +// concept mcc_slew_params_c = std::movable && requires(T t) { +// // input coordinates pair type (e.g. IRCS RA,DEC, Az,Alt and so on) +// requires std::same_as; + +// typename T::coord_t; + +// // co-longitude (e.g. RA or Az) +// requires std::same_as; + +// // co-latitude (e.g. DEC or ZD) +// requires std::same_as; + +// // stop after slewing +// requires std::convertible_to; +// }; + + /* GENERIC SLEW AND GUIDING MODEL */ template concept mcc_slew_model_c = requires(T t) { requires mcc_error_c; - requires mcc_slew_params_c; + // requires mcc_slew_params_c; + requires mcc_celestial_point_c; - { t.slew(std::declval()) } -> std::same_as; + // { t.slew(std::declval()) } -> std::same_as; + { t.slew(std::declval()) } -> std::same_as; }; -// template -// concept mcc_slew_model_c = mcc_mount_telemetry_c && requires(T t) { -// requires mcc_error_c; -// requires mcc_slew_params_c; -// { -// t.slew(std::declval(), std::declval()) -// } -> std::same_as; -// }; +template +concept mcc_guiding_model_c = requires(T t) { + requires mcc_error_c; + requires mcc_celestial_point_c; -template -concept mcc_guiding_model_c = mcc_mount_telemetry_c && requires(T t) { - typename T::error_t; - - { t.guiding(std::declval()) } -> std::same_as; + // start process of guiding + { t.guiding(std::declval()) } -> std::same_as; }; @@ -438,7 +491,8 @@ concept mcc_mount_controls_c = std::move_constructible && std::movable && requires mcc_slew_model_c; // requires mcc_slew_model_c; - requires mcc_guiding_model_c; + requires mcc_guiding_model_c; + // requires mcc_guiding_model_c; // a std::tuple of prohibited zones []... Ts>(std::tuple) { @@ -463,7 +517,8 @@ concept mcc_mount_c = requires(T t) { requires mcc_mount_hardware_c; // requires mcc_slew_model_c; requires mcc_slew_model_c; - requires mcc_guiding_model_c; + // requires mcc_guiding_model_c; + requires mcc_guiding_model_c; requires std::same_as; @@ -481,3 +536,15 @@ template concept mcc_fsm_log_mount_c = std::derived_from && mcc_log_mount_c; } // namespace mcc::traits + + + +/* CHECK LIBRARY-WIDE CLASS DECLARATIONS FOR ITS CONCEPTS SATISFACTION */ + +namespace mcc +{ + +static_assert(traits::mcc_logger_c, "MccNullLogger INVALID DECLARATION!"); +static_assert(traits::mcc_celestial_point_c, "MccCelestialPoint INVALID DECLARATION!"); + +} // namespace mcc diff --git a/cxx/mcc_mount_coord.h b/cxx/mcc_mount_coord.h index 0f01f11..2377331 100644 --- a/cxx/mcc_mount_coord.h +++ b/cxx/mcc_mount_coord.h @@ -98,18 +98,18 @@ public: // by default angle is in radians - MccAngle(const double& val, const MccRadianTag = MccRadianTag{}) : _angleInRads(val) {} + constexpr MccAngle(const double& val, const MccRadianTag = MccRadianTag{}) : _angleInRads(val) {} // construct angle in degrees, e.g.: // auto ang = MccAngle{180.0, mcc_degrees}; - MccAngle(const double& val, const MccDegreeTag) : _angleInRads(val * utils::deg2radCoeff) {} + constexpr MccAngle(const double& val, const MccDegreeTag) : _angleInRads(val * utils::deg2radCoeff) {} // constuct angle from sexagesimal representation or floating-point number of degrees, e.g.: // auto ang = MccAngle{"-12:34:56.789"}; // from degrees:minutes:seconds // auto ang = MccAngle{"123.574698"}; // from degrees - MccAngle(traits::mcc_input_char_range auto const& val) + constexpr MccAngle(traits::mcc_input_char_range auto const& val) { auto res = utils::parsAngleString(val); if (res.has_value()) { @@ -122,7 +122,7 @@ public: // construct angle from sexagesimal representation or floating-point number of degrees, e.g.: // auto ang = MccAngle{"01:23:45.6789", mcc_hms}; // from hours:minutes:seconds // auto ang = MccAngle{"123.574698"}; // from degrees - MccAngle(traits::mcc_input_char_range auto const& val, const MccHMSTag) + constexpr MccAngle(traits::mcc_input_char_range auto const& val, const MccHMSTag) { auto res = utils::parsAngleString(val, true); if (res.has_value()) { @@ -135,7 +135,7 @@ public: virtual ~MccAngle() = default; template T> - auto& operator=(this T&& self, const T& other) + constexpr auto& operator=(this T&& self, const T& other) { std::forward(self)._angleInRads = other._angleInRads; @@ -143,7 +143,7 @@ public: } template T> - auto& operator=(this T&& self, T&& other) + constexpr auto& operator=(this T&& self, T&& other) { std::forward(self)._angleInRads = std::move(other._angleInRads); @@ -151,7 +151,7 @@ public: } template - auto& operator=(this auto&& self, const T& val) + constexpr auto& operator=(this auto&& self, const T& val) requires std::is_arithmetic_v { std::forward(self)._angleInRads = val; @@ -209,19 +209,19 @@ public: // return _angleInRads; // } - operator double() const + constexpr operator double() const { return _angleInRads; } template - T degrees() const + constexpr T degrees() const { return _angleInRads * 180.0 / std::numbers::pi; } - double degrees() const + constexpr double degrees() const { return degrees(); } diff --git a/cxx/mcc_slew_model.h b/cxx/mcc_slew_model.h index 1671a0b..9d08ba8 100644 --- a/cxx/mcc_slew_model.h +++ b/cxx/mcc_slew_model.h @@ -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 _slewFunc{}; + std::function _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) { - logError( - std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message())); - return t_err; + // if constexpr (std::same_as) { + // logError( + // std::format("An telemetry error occured: code = {} ({})", t_err.value(), + // t_err.message())); + // return t_err; + // } else { + // if constexpr (traits::mcc_formattable) { + // 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) { + logError(std::format("An hardware error occured: code = {} ({})", err.value(), err.message())); + return err; } else { - if constexpr (traits::mcc_formattable) { - logError(std::format("An telemetry error occured: code = {}", t_err)); + if constexpr (traits::mcc_formattable) { + 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; }