From 961c72f17fcb60e032d07ca7ffb15b889fd923ba Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Thu, 24 Jul 2025 18:55:59 +0300 Subject: [PATCH] ... --- cxx/CMakeLists.txt | 4 +- cxx/asibfm700_hardware.cpp | 43 +++- cxx/asibfm700_hardware.h | 8 +- cxx/mcc_guiding_model.h | 26 +-- cxx/mcc_mount_coord.h | 10 + cxx/mcc_mount_telemetry.h | 4 +- cxx/mcc_mount_telemetry_astrom.h | 296 +++++++++++++++++++++++++ cxx/mcc_slew_model.h | 357 ++++++++++++++++--------------- 8 files changed, 562 insertions(+), 186 deletions(-) create mode 100644 cxx/mcc_mount_telemetry_astrom.h diff --git a/cxx/CMakeLists.txt b/cxx/CMakeLists.txt index 089835c..f2642a5 100644 --- a/cxx/CMakeLists.txt +++ b/cxx/CMakeLists.txt @@ -124,8 +124,8 @@ 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_guiding_model.h mcc_utils.h mcc_spdlog.h) + mcc_mount_pec.h mcc_mount_pz.h mcc_traits.h mcc_mount_telemetry_astrom.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_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 0b01ff8..bd9507f 100644 --- a/cxx/asibfm700_hardware.cpp +++ b/cxx/asibfm700_hardware.cpp @@ -62,12 +62,40 @@ const AsibFM700HardwareErrorCategory& AsibFM700HardwareErrorCategory::get() /* constructors and destructor */ -AsibFM700Hardware::AsibFM700Hardware(const hardware_config_t& conf) : _hardwareConfig(conf) +AsibFM700Hardware::AsibFM700Hardware(const hardware_config_t& conf) + : _hardwareConfig(conf), _sideralRate2(_hardwareConfig.hwConfig.eqrate) { _hardwareConfig.devConfig.MountDevPath = const_cast(_hardwareConfig.MountDevPath.c_str()); _hardwareConfig.devConfig.EncoderDevPath = const_cast(_hardwareConfig.EncoderDevPath.c_str()); _hardwareConfig.devConfig.EncoderXDevPath = const_cast(_hardwareConfig.EncoderXDevPath.c_str()); _hardwareConfig.devConfig.EncoderYDevPath = const_cast(_hardwareConfig.EncoderYDevPath.c_str()); + + _sideralRate2 *= _sideralRate2; + _sideralRateEps2 = 0.01; // 1% + + // start state polling + + _statePollingThread = std::jthread([this](std::stop_token stoken) { + mountdata_t data; + + while (true) { + if (stoken.stop_requested()) { + return; + } + + error_t err = static_cast(Mount.getMountData(&data)); + + + if (err == AsibFM700HardwareErrorCode::ERROR_OK) { + // are both motors stopped? + bool stop_motors = + (data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT); + if (stop_motors) { + _state = hw_state_t::HW_STATE_STOP; + } + } + } + }); } // AsibFM700Hardware::AsibFM700Hardware(AsibFM700Hardware&& other) @@ -102,6 +130,15 @@ AsibFM700Hardware::error_t AsibFM700Hardware::getState(AsibFM700Hardware::hw_sta state = hw_state_t::HW_STATE_STOP; return AsibFM700HardwareErrorCode::ERROR_OK; } + + // compute current speed + auto rate2 = data.encXspeed.val * data.encXspeed.val + data.encYspeed.val * data.encYspeed.val; + auto ratio2 = rate2 / _sideralRate2; + if (ratio2 <= _sideralRateEps2) { // tracking + state = hw_state_t::HW_STATE_TRACK; + } else { + state = hw_state_t::HW_STATE_SLEW; + } } return err; @@ -115,6 +152,10 @@ AsibFM700Hardware::error_t AsibFM700Hardware::setPos(AsibFM700Hardware::axes_pos // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis coordpair_t hw_pos{.X = pos.y, .Y = pos.x}; + if (!pos.flags.slewNguide) { + return static_cast(Mount.slewTo(&hw_pos, pos.flags)); + } + switch (pos.state) { case hw_state_t::HW_STATE_SLEW: // slew mount err = static_cast(Mount.slewTo(&hw_pos, pos.flags)); diff --git a/cxx/asibfm700_hardware.h b/cxx/asibfm700_hardware.h index f204473..e3f4c5f 100644 --- a/cxx/asibfm700_hardware.h +++ b/cxx/asibfm700_hardware.h @@ -4,6 +4,8 @@ /* HARDWARE WRAPPER IMPLEMENTATION */ +#include + #include "../LibSidServo/sidservo.h" #include "mcc_mount_concepts.h" @@ -116,7 +118,11 @@ public: private: hardware_config_t _hardwareConfig; - // static void moveInst(AsibFM700Hardware* from, AsibFM700Hardware* to); + std::jthread _statePollingThread; + hw_state_t _state; + + double _sideralRate2; // square of sideral rate + double _sideralRateEps2; }; static_assert(mcc::traits::mcc_mount_hardware_c, "AsibFM700Hardware!!!"); diff --git a/cxx/mcc_guiding_model.h b/cxx/mcc_guiding_model.h index e2ae22f..23b153d 100644 --- a/cxx/mcc_guiding_model.h +++ b/cxx/mcc_guiding_model.h @@ -144,15 +144,10 @@ public: std::chrono::duration predictedTrackResolution{0.1}; // 0.1 seconds }; - typedef MccCelestialPoint guiding_point_t; - - // struct guiding_point_t { - // typedef double coord_t; - - // mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; - - // coord_t x, y; - // }; + struct guiding_point_t : MccCelestialPoint { + coord_t corrThresh{(double)MccAngle("00:00:00.2"_dms)}; // correction threshold + coord_t correctionRange[2]{(double)MccAngle(0.5_arcsecs), (double)MccAngle(5.0_arcsecs)}; + }; template MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context, LoggerCtorArgTs&&... ctor_args) @@ -359,9 +354,16 @@ protected: } } - guiding_point.x += pec_res.dx; // app HA - guiding_point.y += pec_res.dy; // app DEC - guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + if constexpr (mccIsEquatorialMount(pec_t::mountType)) { // use of HA and DEC + guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(pec_t::mountType)) { // use of Az and Alt + guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT; + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); + } + + guiding_point.x += pec_res.dx; // app HA/Az + guiding_point.y += pec_res.dy; // app DEC/Alt res_err = self(std::move(guiding_point)); if (res_err) { diff --git a/cxx/mcc_mount_coord.h b/cxx/mcc_mount_coord.h index 2377331..ec253d0 100644 --- a/cxx/mcc_mount_coord.h +++ b/cxx/mcc_mount_coord.h @@ -13,6 +13,16 @@ constexpr double operator""_degs(long double val) // angle in degrees return val * std::numbers::pi / 180.0; } +constexpr double operator""_arcmins(long double val) // angle in arc minutes +{ + return val * std::numbers::pi / 180.0 / 60.0; +} + +constexpr double operator""_arcsecs(long double val) // angle in arc seconds +{ + return val * std::numbers::pi / 180.0 / 3600.0; +} + constexpr double operator""_dms(const char* s, size_t size) // as a string "DEGREES:MINUTES:SECONDS" { auto res = mcc::utils::parsAngleString(std::span{s, size}); diff --git a/cxx/mcc_mount_telemetry.h b/cxx/mcc_mount_telemetry.h index d24f15b..96c4f70 100644 --- a/cxx/mcc_mount_telemetry.h +++ b/cxx/mcc_mount_telemetry.h @@ -1,9 +1,9 @@ #pragma once -/* MOUNT CONTROL COMPONENTS LIBRARY */ +/* MOUNT CONTROL COMPONENTS LIBRARY */ -/* MOUNT TELEMETRY OBJECT POSSIBLE GENERIC IMPLEMENTATION */ +/* MOUNT TELEMETRY OBJECT POSSIBLE GENERIC IMPLEMENTATION */ #include diff --git a/cxx/mcc_mount_telemetry_astrom.h b/cxx/mcc_mount_telemetry_astrom.h new file mode 100644 index 0000000..c36962a --- /dev/null +++ b/cxx/mcc_mount_telemetry_astrom.h @@ -0,0 +1,296 @@ +#pragma once + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + + + +/* VARIUOS ASTROMETRIC TRANSFORMATIONS FOR TELEMETRY */ + + +#include "mcc_mount_concepts.h" + +namespace mcc +{ + +template +class MccMountTelemetryAstromTransform +{ + static typename ASTROM_ENGINE_T::coord_t dummyCoord{}; + +public: + // deduce mount type + static constexpr bool equatorialMount = mccIsEquatorialMount(PEC_T::mountType); + static constexpr bool altAzMount = mccIsAltAzMount(PEC_T::mountType); + + typedef ASTROM_ENGINE_T astrom_engine_t; + typedef PEC_T pec_t; + + typedef typename astrom_engine_t::coord_t coord_t; + + typedef std::error_code error_t; + + MccMountTelemetryAstromTransform(astrom_engine_t& astrom_engine, pec_t& pec) + : _astromEngine(astrom_engine), _pec(pec) + { + } + + virtual ~MccMountTelemetryAstromTransform() = default; + + template + error_t toApparent(CT coord, + astrom_engine_t::time_point_t time_point, + coord_t& X_app, + coord_t& Y_app, + coord_t& XX_app = dummyCoord) + { + typedef typename astrom_engine_t::jd_t jd_t; + jd_t jd; + + typedef typename astrom_engine_t::eo_t eo_t; + eo_t eo; + + typedef typename astrom_engine_t::sideral_time_t sideral_time_t; + sideral_time_t lst; + + typename astrom_engine_t::error_t ast_err; + typename pec_t::error_t pec_err; + + auto get_jd_lst_eo = [&time_point, this](jd_t& jd, sideral_time_t& lst, eo_t& eo) { + auto ast_err = _astromEngine.greg2jul(time_point, jd); + if (!ast_err) { + ast_err = _astromEngine.apparentSiderTime(jd, lst, true); + + if (!ast_err) { + ast_err = _astromEngine->eqOrigins(jd, eo); + } + } + + return ast_err; + }; + + if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's + typename pec_t::pec_result_t pec_res; + pec_err = _pec.compute(coord.x, coord.y, pec_res); + if (!pec_err) { + X_app = coord.x + pec_res.dx; + Y_app = coord.y + pec_res.dy; + } + + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { // from app RA-DEC + + coord_t HA; + + // logDebug("Input slew coordinates are apparent RA-DEC: convert it to apparent HA-DEC ..."); + + ast_err = get_jd_lst_eo(jd, lst, eo); + + if (!ast_err) { + HA = lst - coord.x + eo; // HA = LST - RA_APP + EO + + if constexpr (equatorialMount) { // compute HA (as XX_app) + X_app = coord.x; + Y_app = coord.y; + XX_app = HA; + } else if constexpr (altAzMount) { + ast_err = _astromEngine.hadec2azalt(HA, coord.y, X_app, Y_app); + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + } + + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { // from app HA-DEC + ast_err = get_jd_lst_eo(jd, lst, eo); + + if (!ast_err) { + if constexpr (equatorialMount) { // compute CIO RA (as XX_app) + X_app = coord.x; + Y_app = coord.y; + XX_app = lst - coord.x + eo; + } else if constexpr (altAzMount) { + ast_err = _astromEngine.hadec2azalt(coord.x, coord.y, X_app, Y_app); + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + } + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT + if constexpr (equatorialMount) { + ast_err = azalt2hadec(coord.x, coord.y, X_app, Y_app); // compute HA-DEC + if (!ast_err) { // compute CIO RA (as XX_app) + ast_err = toApparent(X_app, Y_app, X_app, Y_app, XX_app); + } + } else if (altAzMount) { + X_app = coord.x; + Y_app = coord.y; + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZZD) { // from app AZ-ZD + coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT; + coord.y = std::numbers::pi / 2.0 - coord.y; + ast_err = toApparent(std::move(coord), std::move(time_point), X_app, Y_app, XX_app); + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // from ICRS RA-DEC + coord_t az, alt; + eo_t eo; + + // for equatorial mount: + // X_app = RA_app, Y_app = DEC_app, XX_app = HA_app + // for alt-azimuthal mount: + // X_app = AZ, Y_app = ALT + + ast_err = _astromEngine.greg2jul(time_point, jd); + if (!ast_err) { + ast_err = _astromEngine.icrs2obs(coord.x, coord.y, jd, X_app, Y_app, XX_app, az, alt, eo); + + if (!ast_err) { + if constexpr (equatorialMount) { + // nothing to do + } else if (altAzMount) { + X_app = az; + Y_app = alt; + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + } + } + } else { + return std::make_error_code(std::errc::operation_canceled); + } + + if (pec_err) { + if constexpr (std::same_as) { + return pec_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + if (ast_err) { + if constexpr (std::same_as) { + return ast_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + return {}; + } + + + template + error_t toICRS(CT coord, astrom_engine_t::time_point_t time_point, coord_t& RA, coord_t& DEC) + { + typename astrom_engine_t::error_t ast_err; + typename pec_t::error_t pec_err; + + if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's + typename pec_t::pec_result_t pec_res; + pec_err = _pec.compute(coord.x, coord.y, pec_res); + if (!pec_err) { + coord.x += pec_res.dx; + coord.y += pec_res.dy; + + if constexpr (equatorialMount) { + coord.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (altAzMount) { + coord.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT; + } else { + static_assert(false, "UNSUPPORTED MOUNT TYPE!"); + } + + ast_err = toICRS(std::move(coord), std::move(time_point), RA, DEC); + } + + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { // from app RA-DEC + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { // from app HA-DEC + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZZD) { // from app AZ-ZD + coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT; + coord.y = std::numbers::pi / 2.0 - coord.y; + ast_err = toICRS(std::move(coord), std::move(time_point), RA, DEC); + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // from ICRS RA-DEC + RA = coord.x; + DEC = coord.y; + + return {}; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + + if (coord.coordPairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + typename astrom_engine_t::jd_t jd; + + ast_err = _astromEngine.greg2jul(astrom_engine_t::timePointNow(), jd); + if (!ast_err) { + ast_err = _astromEngine.obs2icrs(coord.coordPairKind, coord.x, coord.y, jd, RA, DEC); + } + } + + if (pec_err) { + if constexpr (std::same_as) { + return pec_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + if (ast_err) { + if constexpr (std::same_as) { + return ast_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + return {}; + } + + + template + error_t toHardware(CT coord, astrom_engine_t::time_point_t time_point, coord_t& X, coord_t& Y) + { + typename astrom_engine_t::error_t ast_err; + typename pec_t::error_t pec_err; + + if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_XY) { // from encoder's + X = coord.x; + Y = coord.y; + + return {}; + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { // from app RA-DEC + + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { // from app HA-DEC + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT) { // from app AZ-ALT + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZZD) { // from app AZ-ZD + coord.coordPairKind == MccCoordPairKind::COORDS_KIND_AZALT; + coord.y = std::numbers::pi / 2.0 - coord.y; + ast_err = toICRS(std::move(coord), std::move(time_point), X, Y); + } else if (coord.coordPairKind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // from ICRS RA-DEC + } else { + return std::make_error_code(std::errc::operation_canceled); + } + + if (pec_err) { + if constexpr (std::same_as) { + return pec_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + if (ast_err) { + if constexpr (std::same_as) { + return ast_err; + } else { + return std::make_error_code(std::errc::operation_canceled); + } + } + + return {}; + } + +protected: + astrom_engine_t& _astromEngine; + pec_t& _pec; +}; + +} // namespace mcc diff --git a/cxx/mcc_slew_model.h b/cxx/mcc_slew_model.h index f5a5aad..b5c5f6b 100644 --- a/cxx/mcc_slew_model.h +++ b/cxx/mcc_slew_model.h @@ -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 coordPollingInterval{0.1}; bool stopAfterSlew{false}; - std::chrono::seconds timeout{300}; + std::chrono::seconds timeout{3600}; }; + template - 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(std::forward(ctor_args)...) { logDebug(std::format("Create 'MccSimpleSlewModel' class instance ({})", (void*)this)); - init(mount_controls, std::move(context)); + init(mount_controls); } template - MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, context_t context) + MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls) requires(std::same_as) { - init(mount_controls, std::move(context)); + init(mount_controls); } virtual ~MccSimpleSlewModel() @@ -167,7 +158,7 @@ public: protected: std::function _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](std::index_sequence) { - // std::array 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) { - ret = tel_err; - } else { - ret = MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA; - } - } else { - ret = std::get(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) { + 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 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 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{}); - // 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; }