From 2478c1e8d2ddbc4ec122337e6c83647c2ac7ac8b Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Wed, 3 Sep 2025 18:28:52 +0300 Subject: [PATCH] remove guiding model now it are only slewing and tracking states --- CMakeLists.txt | 1 + asibfm700/CMakeLists.txt | 15 ++ asibfm700/asibfm700_common.h | 11 + asibfm700/asibfm700_servocontroller.cpp | 84 ++++++ asibfm700/asibfm700_servocontroller.h | 124 +++++++++ mcc/CMakeLists.txt | 8 +- mcc/mcc_generic_mount.h | 102 +++---- mcc/mcc_generics.h | 65 +++-- mcc/mcc_guiding_model.h | 302 --------------------- mcc/mcc_moving_model_common.h | 10 +- mcc/mcc_pzone.h | 189 ++++++++++++- mcc/mcc_slewing_model.h | 153 ++++------- mcc/mcc_telemetry.h | 54 ---- mcc/mcc_tracking_model.h | 337 +++++++++++++----------- 14 files changed, 757 insertions(+), 698 deletions(-) create mode 100644 asibfm700/CMakeLists.txt create mode 100644 asibfm700/asibfm700_common.h create mode 100644 asibfm700/asibfm700_servocontroller.cpp create mode 100644 asibfm700/asibfm700_servocontroller.h delete mode 100644 mcc/mcc_guiding_model.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c03234..7353c2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,3 +14,4 @@ set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) add_subdirectory(cxx) add_subdirectory(mcc) +add_subdirectory(asibfm700) diff --git a/asibfm700/CMakeLists.txt b/asibfm700/CMakeLists.txt new file mode 100644 index 0000000..ada1a34 --- /dev/null +++ b/asibfm700/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) + +# set(CMAKE_BUILD_TYPE Release) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake") + + +set(ASIBFM700_LIB_SRC asibfm700_common.h asibfm700_servocontroller.h asibfm700_servocontroller.cpp) + +set(ASIBFM700_LIB asibfm700mount) +add_library(${ASIBFM700_LIB} STATIC ${ASIBFM700_LIB_SRC}) +target_link_libraries(${ASIBFM700_LIB} PRIVATE mcc1) diff --git a/asibfm700/asibfm700_common.h b/asibfm700/asibfm700_common.h new file mode 100644 index 0000000..dcf35ca --- /dev/null +++ b/asibfm700/asibfm700_common.h @@ -0,0 +1,11 @@ +#pragma once + +/* AstroSib FORK MOUNT FM-700 CONTROL LIBRARY */ + + +/* COMMON LIBRARY DEFINITIONS */ + +namespace asibfm700 +{ + +} // namespace asibfm700 diff --git a/asibfm700/asibfm700_servocontroller.cpp b/asibfm700/asibfm700_servocontroller.cpp new file mode 100644 index 0000000..c94018f --- /dev/null +++ b/asibfm700/asibfm700_servocontroller.cpp @@ -0,0 +1,84 @@ +#include "asibfm700_servocontroller.h" + +namespace asibfm700 +{ + +AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {} + +AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController() +{ + _hardwareConfig = std::move(config); + + _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()); +} + + +constexpr std::string_view AsibFM700ServoController::hardwareName() const +{ + return "Sidereal-ServoControllerII"; +} + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop() +{ + return static_cast(Mount.stop()); +} + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit() +{ + return static_cast(Mount.init(&_hardwareConfig.devConfig)); +} + + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state) +{ + // time point from sidservo library is 'double' number represented UNIXTIME with + // microseconds/nanoseconds precision + double tp = std::chrono::duration(state.time_point.time_since_epoch()).count(); + + std::lock_guard lock{*_setStateMutex}; + + // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis + coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}}; + coordpair_t cpair{.X = state.Y, .Y = state.X}; + + // correctTo is asynchronous function!!! + // + // according to the Eddy's implementation of the LibSidServo library it is safe + // to pass the addresses of 'cvalpair' and 'cpair' automatic variables + auto err = static_cast(Mount.correctTo(&cvalpair, &cpair)); + + return err; +} + +AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state) +{ + using tp_t = decltype(hardware_state_t::time_point); + + mountdata_t mdata; + + error_t err = static_cast(Mount.getMountData(&mdata)); + if (!err) { + // 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{mdata.encXposition.t}; + state->time_point = tp_t{std::chrono::duration_cast(secs)}; + + // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis + state->X = mdata.encYposition.val; + state->Y = mdata.encXposition.val; + + state->speedX = mdata.encYspeed.val; + state->speedY = mdata.encXspeed.val; + } + + return err; +} + + +} // namespace asibfm700 diff --git a/asibfm700/asibfm700_servocontroller.h b/asibfm700/asibfm700_servocontroller.h new file mode 100644 index 0000000..4bc4ae6 --- /dev/null +++ b/asibfm700/asibfm700_servocontroller.h @@ -0,0 +1,124 @@ +#pragma once + +#include +#include + +#include "../LibSidServo/sidservo.h" + + +namespace asibfm700 +{ + +/* error codes enum definition */ + +enum class AsibFM700ServoControllerErrorCode : int { + // error codes from sidservo library + ERROR_OK = MCC_E_OK, + ERROR_FATAL = MCC_E_FATAL, + ERROR_BADFORMAT = MCC_E_BADFORMAT, + ERROR_ENCODERDEV = MCC_E_ENCODERDEV, + ERROR_MOUNTDEV = MCC_E_MOUNTDEV, + ERROR_FAILED = MCC_E_FAILED, + // my codes ... +}; + +// error category +struct AsibFM700HardwareErrorCategory : public std::error_category { + const char* name() const noexcept; + std::string message(int ec) const; + + static const AsibFM700HardwareErrorCategory& get(); +}; + + +static inline std::error_code make_error_code(AsibFM700ServoControllerErrorCode ec) +{ + return std::error_code(static_cast(ec), AsibFM700HardwareErrorCategory::get()); +} + +} // namespace asibfm700 + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + +namespace asibfm700 +{ + +class AsibFM700ServoController final +{ +public: + typedef std::error_code error_t; + + enum class hardware_moving_state_t : int { + HW_MOVE_STOPPED, + HW_MOVE_SLEWING, + HW_MOVE_ADJUSTING, + HW_MOVE_TRACKING, + HW_MOVE_GUIDING + }; + + struct hardware_state_t { + static constexpr mcc::MccCoordPairKind pair_kind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP; + mcc::MccTimePoint time_point; + + double X, Y, speedX, speedY; + + hardware_moving_state_t moving_state; + }; + + + struct hardware_config_t { + // the 'char*' fields from conf_t: + // wrap it to std::string + std::string MountDevPath; + std::string EncoderDevPath; + std::string EncoderXDevPath; + std::string EncoderYDevPath; + + conf_t devConfig; + hardware_configuration_t hwConfig; + }; + + /* constructors and destructor */ + + AsibFM700ServoController(); + + AsibFM700ServoController(hardware_config_t config); + + AsibFM700ServoController(const AsibFM700ServoController&) = delete; + AsibFM700ServoController& operator=(const AsibFM700ServoController&) = delete; + + AsibFM700ServoController(AsibFM700ServoController&&); + AsibFM700ServoController& operator=(AsibFM700ServoController&&); + + ~AsibFM700ServoController(); + + /* public methods */ + + constexpr std::string_view hardwareName() const; + + error_t hardwareSetState(hardware_state_t state); + error_t hardwareGetState(hardware_state_t* state); + + error_t hardwareStop(); + error_t hardwareInit(); + + void hardwareUpdateConfig(conf_t cfg); + void hardwareUpdateConfig(hardware_configuration_t cfg); + +private: + hardware_config_t _hardwareConfig; + + std::unique_ptr _setStateMutex; +}; + +} // namespace asibfm700 diff --git a/mcc/CMakeLists.txt b/mcc/CMakeLists.txt index 555d6b0..bdb739e 100644 --- a/mcc/CMakeLists.txt +++ b/mcc/CMakeLists.txt @@ -70,8 +70,7 @@ include_directories(${BSPLINES_INCLUDE_DIR}) set(MCC_LIBRARY_SRC1 mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h - mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_guiding_model.h - mcc_moving_model_common.h) + mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_moving_model_common.h) list(APPEND MCC_LIBRARY_SRC1 mcc_spdlog.h) @@ -79,6 +78,11 @@ set(MCC_LIBRARY1 mcc1) add_library(${MCC_LIBRARY1} INTERFACE ${MCC_LIBRARY_SRC1}) target_compile_features(${MCC_LIBRARY1} INTERFACE cxx_std_23) target_include_directories(${MCC_LIBRARY1} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR}) +target_include_directories(${MCC_LIBRARY1} INTERFACE + $ + $ +) + option(WITH_TESTS "Build tests" ON) diff --git a/mcc/mcc_generic_mount.h b/mcc/mcc_generic_mount.h index 640e4bb..665735c 100644 --- a/mcc/mcc_generic_mount.h +++ b/mcc/mcc_generic_mount.h @@ -37,14 +37,14 @@ template class MccGenericMount : public HardwareT, public TelemetryT, public PZoneContT, public SlewModelT, public TrackModelT, - public GuidingModelT, + // public GuidingModelT, public LoggerT { public: @@ -56,7 +56,7 @@ public: using LoggerT::logWarn; - using typename GuidingModelT::guiding_params_t; + // using typename GuidingModelT::guiding_params_t; using typename SlewModelT::slewing_params_t; using typename TrackModelT::tracking_params_t; @@ -65,14 +65,14 @@ public: PZoneContT pzone_cont, SlewModelT slew_model, TrackModelT track_model, - GuidingModelT guiding_model, + // GuidingModelT guiding_model, LoggerT logger = MccNullLogger{}) : HardwareT(std::move(hardware)), TelemetryT(std::move(telemetry)), PZoneContT(std::move(pzone_cont)), SlewModelT(std::move(slew_model)), TrackModelT(std::move(track_model)), - GuidingModelT(std::move(guiding_model)), + // GuidingModelT(std::move(guiding_model)), LoggerT(std::move(logger)) { } @@ -83,7 +83,7 @@ public: { logInfo("stop any movements ..."); - this->stopGuidingTarget(); + // this->stopGuidingTarget(); this->stopTracking(); this->stopSlewing(); @@ -119,15 +119,16 @@ template class MccGenericMountFSM : public fsm::MccFiniteStateMachine, - public MccGenericMount + // public MccGenericMount + public MccGenericMount { protected: - typedef MccGenericMount - base_gmount_t; + // typedef MccGenericMount + typedef MccGenericMount base_gmount_t; public: typedef typename base_gmount_t::error_t error_t; @@ -206,11 +207,11 @@ public: // to guiding state - struct MccGenericMountEventGuiding : MccGenericMountBaseEvent { - static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT"; + // struct MccGenericMountEventGuiding : MccGenericMountBaseEvent { + // static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT"; - MccGenericMountEventGuiding(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {} - }; + // MccGenericMountEventGuiding(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {} + // }; // to stoping state @@ -252,7 +253,7 @@ public: struct MccGenericMountStateError; struct MccGenericMountStateSlew; struct MccGenericMountStateTrack; - struct MccGenericMountStateGuiding; + // struct MccGenericMountStateGuiding; struct MccGenericMountStateStopping; struct MccGenericMountStateUninit : MccGenericMountBaseState { @@ -394,8 +395,7 @@ public: struct MccGenericMountStateTrack : MccGenericMountBaseState { using transition_t = fsm::fsm_transition_table_t, - std::pair, - std::pair>; + std::pair>; template EvT> void exit(EvT& event) @@ -421,34 +421,34 @@ public: }; - struct MccGenericMountStateGuiding : MccGenericMountBaseState { - using transition_t = - fsm::fsm_transition_table_t, - std::pair, - std::pair>; + // struct MccGenericMountStateGuiding : MccGenericMountBaseState { + // using transition_t = + // fsm::fsm_transition_table_t, + // std::pair, + // std::pair>; - template EvT> - void exit(EvT& event) - { - error_t err = event.mount()._stopGuidingMount(); - if (err) { - event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err}); - } + // template EvT> + // void exit(EvT& event) + // { + // error_t err = event.mount()._stopGuidingMount(); + // if (err) { + // event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err}); + // } - this->exitLog(event); - } + // this->exitLog(event); + // } - template EvT> - void enter(EvT& event) - { - this->enterLog(event); + // template EvT> + // void enter(EvT& event) + // { + // this->enterLog(event); - error_t err = event.mount()._startGuidingMount(); - if (err) { - event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err}); - } - } - }; + // error_t err = event.mount()._startGuidingMount(); + // if (err) { + // event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err}); + // } + // } + // }; /* CONSTRUCTORS AND DESTRUCTOR */ @@ -458,7 +458,7 @@ public: PZoneContT pzone_cont, SlewModelT slew_model, TrackModelT track_model, - GuidingModelT guiding_model, + // GuidingModelT guiding_model, LoggerT logger = MccNullLogger{}) : fsm::MccFiniteStateMachine(MccGenericMountStateUninit{}), base_gmount_t(std::move(hardware), @@ -466,7 +466,7 @@ public: std::move(pzone_cont), std::move(slew_model), std::move(track_model), - std::move(guiding_model), + // std::move(guiding_model), std::move(logger)) { } @@ -506,15 +506,15 @@ public: this->dispatchEvent(MccGenericMountEventIDLE{*this}); } - auto startGuidingTarget() - { - this->dispatchEvent(MccGenericMountEventGuiding{*this}); - } + // auto startGuidingTarget() + // { + // this->dispatchEvent(MccGenericMountEventGuiding{*this}); + // } - auto stopGuidingTarget() - { - this->dispatchEvent(MccGenericMountEventTrack{*this}); - } + // auto stopGuidingTarget() + // { + // this->dispatchEvent(MccGenericMountEventTrack{*this}); + // } protected: // wrappers diff --git a/mcc/mcc_generics.h b/mcc/mcc_generics.h index 557e218..4ed4854 100644 --- a/mcc/mcc_generics.h +++ b/mcc/mcc_generics.h @@ -64,18 +64,10 @@ static consteval bool mccIsAltAzMount(const MccMountType type) }; - -// enum MccCoordPairKind : size_t { -// COORDS_KIND_GENERIC, -// COORDS_KIND_RADEC_ICRS, -// COORDS_KIND_RADEC_APP, -// COORDS_KIND_HADEC_APP, -// COORDS_KIND_AZZD, -// COORDS_KIND_AZALT, -// COORDS_KIND_XY, -// COORDS_KIND_LATLON -// }; - +enum class MccProhibitedZonePolicy : int { + PZ_POLICY_STOP, // stop mount near the zone + PZ_POLICY_FLIP // flip mount, e.g., near the meridian, near HA-axis encoder limit switch +}; /* GENERIC LOGGER CLASS CONCEPT */ @@ -480,7 +472,7 @@ concept mcc_hardware_c = requires(T t, const T t_const) { requires mcc_angle_c; // moving speed along co-longitude coordinate requires mcc_angle_c; // moving speed along co-latitude coordinate - requires std::same_as; + requires std::same_as; }; // set hardware state: @@ -745,12 +737,12 @@ concept mcc_prohibited_zone_c = std::derived_from> && requires(const T t_const) { { t_const.name() } -> std::formattable; - requires requires(typename T::pzone_policy_t pp) { - []() { - static constexpr auto v1 = T::pzone_policy_t::STOP_POLICY; - static constexpr auto v2 = T::pzone_policy_t::FLIP_POLICY; - }(); - }; + // the 'T' class must contain static constexpr member of 'MccMountType' type + requires std::same_as; + []() { + static constexpr MccProhibitedZonePolicy val = T::pzPolicy; + return val; + }(); // to ensure 'pzPolicy' can be used in compile-time context }; @@ -867,22 +859,22 @@ concept mcc_tracking_model_c = requires(T t) { { t.getTrackingParams() } -> std::same_as; }; -template -concept mcc_guiding_model_c = requires(T t) { - requires mcc_error_c; +// template +// concept mcc_guiding_model_c = requires(T t) { +// requires mcc_error_c; - // a class of guiding process parameters - requires requires(typename T::guiding_params_t pars) { - // guide along both mount axis - requires std::convertible_to; - }; +// // a class of guiding process parameters +// requires requires(typename T::guiding_params_t pars) { +// // guide along both mount axis +// requires std::convertible_to; +// }; - { t.startGuidingTarget() } -> std::same_as; - { t.stopGuidingTarget() } -> std::same_as; +// { t.startGuidingTarget() } -> std::same_as; +// { t.stopGuidingTarget() } -> std::same_as; - { t.setGuidingParams(std::declval()) } -> std::same_as; - { t.getGuidingParams() } -> std::same_as; -}; +// { t.setGuidingParams(std::declval()) } -> std::same_as; +// { t.getGuidingParams() } -> std::same_as; +// }; /* GENERIC MOUNT CLASS CONCEPT */ @@ -897,7 +889,10 @@ template concept mcc_all_controls_c = mcc_position_controls_c && mcc_telemetry_c && mcc_pzone_container_c; - +// generic mount: +// 1) telemetry-related methods +// 2) prohibited zones related methods +// 3) slewing and tracking, stop and init mount methods template concept mcc_generic_mount_c = mcc_telemetry_c && mcc_pzone_container_c && requires(T t) { requires mcc_error_c; @@ -909,8 +904,8 @@ concept mcc_generic_mount_c = mcc_telemetry_c && mcc_pzone_container_c && { t.trackTarget() } -> std::same_as; - { t.startGuidingTarget() } -> std::same_as; - { t.stopGuidingTarget() } -> std::same_as; + // { t.startGuidingTarget() } -> std::same_as; + // { t.stopGuidingTarget() } -> std::same_as; // stop any movement { t.stopMount() } -> std::same_as; diff --git a/mcc/mcc_guiding_model.h b/mcc/mcc_guiding_model.h deleted file mode 100644 index f6b9ac6..0000000 --- a/mcc/mcc_guiding_model.h +++ /dev/null @@ -1,302 +0,0 @@ -#pragma once - -/* MOUNT CONTROL COMPONENTS LIBRARY */ - - -/* SIMPLE GUIDING MODEL IMPLEMENTATION */ - - -#include "mcc_defaults.h" -#include "mcc_moving_model_common.h" - -namespace mcc -{ - -enum class MccSimpleGuidingModelErrorCode : int { - ERROR_OK, - ERROR_CCTE, - ERROR_HW_GETSTATE, - ERROR_HW_SETSTATE, - ERROR_PCM_COMP, - ERROR_GET_TELEMETRY, - ERROR_DIST_TELEMETRY, - ERROR_DIFF_TELEMETRY, - ERROR_PZONE_CONTAINER_COMP, - ERROR_IN_PZONE, - ERROR_NEAR_PZONE, - ERROR_TIMEOUT, - ERROR_UNEXPECTED_AXIS_RATES, - ERROR_STOPPED -}; - -} // namespace mcc - - -namespace std -{ - -template <> -class is_error_code_enum : public true_type -{ -}; - -} // namespace std - - - -namespace mcc -{ - -class MccSimpleGuidingModel -{ -public: - typedef std::error_code error_t; - - typedef MccSimpleMovingModelParams guiding_params_t; - - - template - MccSimpleGuidingModel(CONTROLS_T* controls) - : _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex()) - { - _guidingFunc = [controls, this]() -> error_t { - typename CONTROLS_T::hardware_state_t hw_state; - - MccTelemetryData tdata; - MccEqtHrzCoords intsc_coords; - MccCelestialPoint target_in_future_pt; - - if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; - } else { - static_assert(false, "UNKNOW MOUNT TYPE!"); - } - - // double dist, dx, dy; - - auto t_err = controls->telemetryData(&tdata); - if (t_err) { - return mcc_deduce_error(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY); - } - - - bool no_intersects = false; - - // function to update the closest prohibited zone intersect point - auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state, - this]() -> error_t { - // compute intersection points with the prohibited zones - auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords); - if (pz_err) { - return mcc_deduce_error(pz_err, - MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - - if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - if (std::isfinite(intsc_coords.HA)) { - intsc_coords.X = intsc_coords.HA; - intsc_coords.Y = intsc_coords.DEC_APP; - } else { - no_intersects = true; - // intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min - // intsc_coords.Y = tdata.DEC_APP; - } - } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - if (std::isfinite(intsc_coords.AZ)) { - intsc_coords.X = intsc_coords.AZ; - intsc_coords.Y = intsc_coords.ZD; - } else { - no_intersects = true; - } - } else { - static_assert(false, "UNKNOW MOUNT TYPE!"); - } - - // MccPCMResult pcm_inv_res; - - // // endpoint of the mount moving - // auto pcm_err = controls->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state); - // if (pcm_err) { - // return mcc_deduce_error(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP); - // } - - // if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - // hw_state.speedX = _currentParams.trackSpeedX; - // hw_state.speedY = _currentParams.trackSpeedY; - // } - }; - - - auto target_point = [&, this](MccCelestialPoint* point) { - auto dt = std::chrono::duration{tdata.HA} + - _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds - - auto tp_dt = std::chrono::duration_cast( - _currentParams.timeShiftToTargetPoint); - - // point in +time_dist future - MccCelestialPoint pt{ - .pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, - .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize(), - .Y = tdata.DEC_APP}; - mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point); - - point->time_point = pt.time_point; - - // check for prohibited zone - if (std::isfinite(intsc_coords.HA)) { - bool through_pzone = - (intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone - through_pzone &= - (intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone - - if (through_pzone) { - pt.X = intsc_coords.HA; - } - } - - auto ret = controls->transformCoordinates(std::move(pt), point); - if (ret) { - return mcc_deduce_error(ret, MccSimpleGuidingModelErrorCode::ERROR_CCTE); - } else { - MccPCMResult pcm_inv_res; - - // endpoint of the mount moving - auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state); - if (pcm_err) { - return mcc_deduce_error(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP); - } - - mcc_tp2tp(tdata.time_point, hw_state.time_point); - } - - return MccSimpleGuidingModelErrorCode::ERROR_OK; - }; - - - auto pz_err = update_pzones_ipoint(); - if (pz_err) { - return mcc_deduce_error(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - - hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING; - - { - std::lock_guard lock{*_currentParamsMutex}; - - auto ccte_err = target_point(&target_in_future_pt); - if (ccte_err) { - return mcc_deduce_error(ccte_err, MccSimpleGuidingModelErrorCode::ERROR_CCTE); - } - - if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - hw_state.speedX = _currentParams.trackSpeedX; - hw_state.speedY = _currentParams.trackSpeedY; - } - } - - // move mount - auto hw_err = controls->hardwareSetState(hw_state); - if (hw_err) { - return mcc_deduce_error(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE); - } - - - std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp; - - while (*_stopGuiding) { - // wait for updated telemetry data - { - std::lock_guard lock{*_currentParamsMutex}; - - t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); - - if (t_err) { - return mcc_deduce_error(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY); - } - } - - if (*_stopGuiding) { - break; - } - - // control prohibited zones - if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) { - return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE; - } - if (pz_err) { - return mcc_deduce_error(pz_err, - MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - - if (*_stopGuiding) { - break; - } - - { - std::lock_guard lock{*_currentParamsMutex}; - auto now = std::chrono::steady_clock::now(); - - if ((now - last_corr_tp) < _currentParams.guidingMinInterval) { - continue; - } - - // update prohibited zones intersection point - if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) { - pz_err = update_pzones_ipoint(); - if (pz_err) { - return mcc_deduce_error( - pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - } - - // compute new target-in-future point - auto ccte_err = target_point(&target_in_future_pt); - if (ccte_err) { - return mcc_deduce_error(ccte_err, MccSimpleGuidingModelErrorCode::ERROR_CCTE); - } - } - - // send corrections - hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING; - hw_err = controls->hardwareSetState(hw_state); - if (hw_err) { - return mcc_deduce_error(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE); - } - } - - return MccSimpleGuidingModelErrorCode::ERROR_OK; - }; - } - - MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default; - MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default; - - MccSimpleGuidingModel(const MccSimpleGuidingModel&) = delete; - MccSimpleGuidingModel& operator=(const MccSimpleGuidingModel&) = delete; - - error_t startGuidingTarget() - { - *_stopGuiding = false; - - return _guidingFunc(); - } - - error_t stoptGuidingTarget() - { - *_stopGuiding = true; - - return MccSimpleGuidingModelErrorCode::ERROR_OK; - } - -protected: - std::function _guidingFunc{}; - std::unique_ptr _stopGuiding; - - guiding_params_t _currentParams{}; - std::unique_ptr _currentParamsMutex{}; -}; - -} // namespace mcc diff --git a/mcc/mcc_moving_model_common.h b/mcc/mcc_moving_model_common.h index df6f267..ad540ed 100644 --- a/mcc/mcc_moving_model_common.h +++ b/mcc/mcc_moving_model_common.h @@ -43,19 +43,21 @@ struct MccSimpleMovingModelParams { // slew process timeout std::chrono::seconds slewTimeout{3600}; - double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) - double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + double slewRateX{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + double slewRateY{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments - double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) - double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) // ******* tracking mode ******* double trackSpeedX{}; double trackSpeedY{}; + std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections + bool dualAxisTracking{true}; // mount must be of an equatorial type: false means guiding along only HA-axis // time shift into future to compute target position in future (UT1-scale time duration) std::chrono::duration timeShiftToTargetPoint{10.0}; diff --git a/mcc/mcc_pzone.h b/mcc/mcc_pzone.h index d44a64a..81cd677 100644 --- a/mcc/mcc_pzone.h +++ b/mcc/mcc_pzone.h @@ -12,7 +12,7 @@ namespace mcc { -enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM }; +enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP }; } // namespace mcc @@ -56,6 +56,8 @@ struct MccAltLimitPZCategory : public std::error_category { return "input argument os nullptr"; case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM: return "coordinate transformation error"; + case MccAltLimitPZErrorCode::ERROR_PCM_COMP: + return "PCM computation error"; default: return "UNKNOWN"; } @@ -85,6 +87,8 @@ protected: static constexpr auto pi2 = std::numbers::pi * 2.0; public: + static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_STOP; + typedef std::error_code error_t; MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine) @@ -551,4 +555,187 @@ protected: } }; + +/* co-longitude axis (HA or AZ) limit switch prohibited zone */ + +template +class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t +{ +public: + static_assert(AXIS_KIND == MccCoordKind::COORDS_KIND_AZ || AXIS_KIND == MccCoordKind::COORDS_KIND_HA, + "UNSUPPORTED AXIS TYPE!"); + + typedef std::error_code error_t; + + static constexpr MccCoordKind axisKind = AXIS_KIND; + + static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_FLIP; + + // + // min_limit_val and max_limit_val are hardware encoder angles in radians! + // + MccAxisLimitSwitchPZ(mcc_angle_c auto const& min_limit_val, + mcc_angle_c auto const& max_limit_val, + mcc_position_controls_c auto* controls) + : _minLimit(min_limit_val), _maxLimit(max_limit_val) + { + _transformCoordinates = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t { + if (to_pt == nullptr) { + return MccAltLimitPZErrorCode::ERROR_NULLPTR; + } + + auto err = controls->transformCoordinates(from_pt, to_pt); + if (!err) { + return MccAltLimitPZErrorCode::ERROR_OK; + } + + if (std::same_as) { + return err; + } else { + return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM; + } + }; + + _transformCoordinatesEqtHrzCoords = [controls](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t { + if (to_pt == nullptr) { + return MccAltLimitPZErrorCode::ERROR_NULLPTR; + } + + auto err = controls->transformCoordinates(from_pt, to_pt); + if (!err) { + return MccAltLimitPZErrorCode::ERROR_OK; + } + + if (std::same_as) { + return err; + } else { + return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM; + } + }; + + _computePCM = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t { + MccPCMResult inv_res; + auto err = controls->computeInversePCM(std::move(from_pt), &inv_res, to_pt); + if (err) { + return mcc_deduce_error(err, MccAltLimitPZErrorCode::ERROR_PCM_COMP); + } + + return MccAltLimitPZErrorCode::ERROR_OK; + }; + } + + + consteval std::string_view name() + { + return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE" + : axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE" + : "UKNOWN"; + } + + + template + error_t inPZone(InputT coords, bool* result) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + if (result == nullptr) { + return MccAltLimitPZErrorCode::ERROR_NULLPTR; + } + + if constexpr (mcc_eqt_hrz_coord_c) { + // assume here .X and are hardware encoder coordinate of corresponding axis + *result = (coords.X < _maxLimit) && (coords.X > _minLimit); + } else { // mcc_celestial_point_c + if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware + *result = (coords.X < _maxLimit) && (coords.X > _minLimit); + } else { // here one needs transform input coordinates to hardware encoder ones + MccCelestialPoint pt; + + auto ret = getHWCoords(std::move(coords), &pt); + if (ret) { + return ret; + } + + *result = (pt.X < _maxLimit) && (pt.X > _minLimit); + } + } + + return MccAltLimitPZErrorCode::ERROR_OK; + } + + template + error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + using res_t = std::remove_cvref_t; + using period_t = typename res_t::period; + + double time_ang; + + if (res_time == nullptr) { + return MccAltLimitPZErrorCode::ERROR_NULLPTR; + } + if constexpr (mcc_eqt_hrz_coord_c) { + // assume here .X and are hardware encoder coordinate of corresponding axis + if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) { + time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale + } else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) { + } + } else { // mcc_celestial_point_c + } + + std::chrono::nanoseconds ns{ + static_cast(time_ang * 43200.0 / std::numbers::pi * 1.0E9)}; + + period_t rat; + *res_time = res_t{static_cast(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)}; + } + + template + error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + } + + template + error_t intersectPZone(InputT coords, ResultT* point) + requires((mcc_eqt_hrz_coord_c || mcc_celestial_point_c) && + (mcc_eqt_hrz_coord_c || mcc_celestial_point_c)) + { + } + +protected: + double _minLimit, _maxLimit; + + std::function _transformCoordinates{}; + std::function _transformCoordinatesEqtHrzCoords{}; + + std::function _computePCM{}; + + error_t getHWCoords(MccCelestialPoint from_pt, MccCelestialPoint* to_pt) + { + error_t ret = MccAltLimitPZErrorCode::ERROR_OK; + + if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware + to_pt->X = from_pt.X; + to_pt->Y = from_pt.Y; + } else { // here one needs transform input coordinates to hardware encoder ones + if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) { + to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + + } else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) { + to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } + + mcc_tp2tp(from_pt.time_point, to_pt->time_point); + + ret = _transformCoordinates(std::move(from_pt), to_pt); + if (!ret) { + ret = _computePCM(*to_pt, to_pt); + } + } + + return ret; + } +}; + } // namespace mcc diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index 3458800..bff8daa 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -22,6 +22,7 @@ enum class MccSimpleSlewingModelErrorCode : int { ERROR_DIST_TELEMETRY, ERROR_DIFF_TELEMETRY, ERROR_PZONE_CONTAINER_COMP, + ERROR_TARGET_IN_PZONE, ERROR_IN_PZONE, ERROR_NEAR_PZONE, ERROR_TIMEOUT, @@ -58,42 +59,18 @@ public: typedef MccSimpleMovingModelParams slewing_params_t; - // struct slewing_params_t { - // bool slewAndStop{false}; // slew to target and stop mount - - // std::chrono::seconds telemetryTimeout{3}; - - // // minimal time to prohibited zone at current speed. if it is lesser then exit with error - // std::chrono::seconds minTimeToPZone{10}; - - // // target-mount coordinate difference to start adjusting of slewing (in radians) - // double adjustCoordDiff{10.0_degs}; - - // // coordinates difference to stop slewing (in radians) - // double slewToleranceRadius{5.0_arcsecs}; - - // // slew process timeout - // std::chrono::seconds slewTimeout{3600}; - - // double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) - // double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) - - // double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) - // double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) - // }; - - template - MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont) + template + MccSimpleSlewingModel(CONTROLS_T* controls) : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex) { - _slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t { + _slewingFunc = [controls, this]() -> error_t { // first, check target coordinates - typename TelemetryT::error_t t_err; + typename CONTROLS_T::error_t t_err; MccTelemetryData tdata; { std::lock_guard lock{*_currentParamsMutex}; - t_err = telemetry->telemetryData(&tdata); + t_err = controls->telemetryData(&tdata); if (t_err) { return mcc_deduce_error(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); @@ -101,13 +78,13 @@ public: } bool in_zone; - auto pz_err = pz_cont->inPZone(tdata.target, &in_zone); + auto pz_err = controls->inPZone(tdata.target, &in_zone); if (pz_err) { return mcc_deduce_error(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } if (in_zone) { - return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE; + return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE; } if (*_stopSlewing) { @@ -115,29 +92,23 @@ public: } MccCelestialPoint cpt; - mcc_tp2tp(tdata.time_point, cpt.time_point); + double min_time_to_pzone_in_secs; - if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; } else { static_assert(false, "UNKNOWN MOUNT TYPE!"); } - std::vector isct_pt(pz_cont->sizePZones, cpt); - pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt); - if (pz_err) { - return mcc_deduce_error(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - if (*_stopSlewing) { return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } - typename HardwareT::hardware_state_t hw_state; + typename CONTROLS_T::hardware_state_t hw_state; - auto hw_err = hardware->hardwareGetState(&hw_state); + auto hw_err = controls->hardwareGetState(&hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); } @@ -147,17 +118,20 @@ public: { std::lock_guard lock{*_currentParamsMutex}; - hw_state.speedX = _currentParams.slewXRate; - hw_state.speedY = _currentParams.slewYRate; + hw_state.speedX = _currentParams.slewRateX; + hw_state.speedY = _currentParams.slewRateY; + + min_time_to_pzone_in_secs = + std::chrono::duration_cast>(_currentParams.minTimeToPZone).count(); } - hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING; + hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_SLEWING; if (*_stopSlewing) { return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } // start slewing - hw_err = hardware->hardwareSetState(hw_state); + hw_err = controls->hardwareSetState(hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } @@ -165,8 +139,10 @@ public: std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp; mcc_tp2tp(hw_state.time_point, start_slewing_tp); - double dist, dx, dy, sinY, rate2, xrate; - std::chrono::duration dtx, dty; // seconds in double + // double dist, dx, dy, sinY, rate2, xrate; + // std::chrono::duration dtx, dty; // seconds in double + + double dist; bool adjust_mode = false; static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate; @@ -176,7 +152,7 @@ public: { std::lock_guard lock{*_currentParamsMutex}; - t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); if (t_err) { return mcc_deduce_error(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); @@ -187,42 +163,27 @@ public: return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } - // compute time to prohibited zones at current speed - for (auto const& pt : isct_pt) { - if (std::isfinite(pt.X) && std::isfinite(pt.Y)) { - if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { - // sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP); - dx = pt.X - tdata.HA; - dy = pt.Y - tdata.DEC_APP; - } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { - // sinY = sin(tdata.ZD); - dx = pt.X - tdata.AZ; - dy = pt.Y - tdata.ZD; - } - - // if (utils::isEqual(sinY, 0.0)) { - // dtx = decltype(dtx){std::numeric_limits::infinity()}; - // rate2 = std::numeric_limits::infinity(); - // } else { - // xrate = tdata.speedX * sinY; - // dtx = decltype(dtx){std::abs(dx / xrate)}; - // } - dtx = decltype(dtx){std::abs(dx / tdata.speedX)}; - dty = decltype(dty){std::abs(dy / tdata.speedY)}; - - { - std::lock_guard lock{*_currentParamsMutex}; - - if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) { - return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; - } - } - } - - if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; - } + // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead + // and check them for getting into the prohibited zones + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs; + cpt.X = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs; + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs; + cpt.X = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs; } + mcc_tp2tp(tdata.time_point, cpt.time_point); + + pz_err = controls->inPZone(cpt, &in_zone); + if (pz_err) { + return mcc_deduce_error(pz_err, + MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (in_zone) { + return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; + } + { @@ -233,12 +194,12 @@ public: } } - hw_err = hardware->hardwareGetState(&hw_state); + hw_err = controls->hardwareGetState(&hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); } - t_err = telemetry->targetToMountDist(&dist); + t_err = controls->targetToMountDist(&dist); if (t_err) { return mcc_deduce_error(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); } @@ -252,7 +213,7 @@ public: if (adjust_mode && !_currentParams.slewAndStop) { // do not allow mount speed fall below sideral - if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { // turn on sideral rate only if the current position point catches up with the target if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) { hw_state.X = (double)tdata.target.X; @@ -260,23 +221,23 @@ public: hw_state.speedX = slewing_params_t::sideralRate; - hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; + hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING; - hw_err = hardware->hardwareSetState(hw_state); + hw_err = controls->hardwareSetState(hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } } - } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { } else { static_assert(false, "UNKNOWN MOUNT TYPE!!"); } } if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle - if (hw_state.moving_type == - HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped + if (hw_state.moving_state == + CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped break; } } @@ -290,12 +251,12 @@ public: hw_state.X = (double)tdata.target.X; hw_state.Y = (double)tdata.target.Y; - hw_state.speedX = _currentParams.adjustXRate; - hw_state.speedY = _currentParams.adjustYRate; + hw_state.speedX = _currentParams.adjustRateX; + hw_state.speedY = _currentParams.adjustRateY; - hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING; + hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_ADJUSTING; - hw_err = hardware->hardwareSetState(hw_state); + hw_err = controls->hardwareSetState(hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index a7e91cc..2ea03c3 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -125,43 +125,6 @@ public: using pcm_t = std::remove_cvref_t; using hardware_t = std::remove_cvref_t; - - _toHardwareFunc = [ccte, pcm](const MccCelestialPoint& from_pt, MccCelestialPoint* to_pt) -> error_t { - if (to_pt == nullptr) { - return MccTelemetryErrorCode::ERROR_NULLPTR; - } - - if constexpr (mccIsEquatorialMount(pcm_t::mountType)) { - to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; - } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) { - to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; - } else { - static_assert(false, "UNKNOWN MOUNT TYPE!"); - } - - auto err = ccte->transformCoordinates(from_pt, to_pt); - if (err) { - return mcc_deduce_error(err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - } - - // compute hardware coordinates - // WARNING: It is assumed here that PCM corrections have small (arcseconds-arcminutes) values - // since ususaly there is no reverse transformation for "hardware-to-apparent" relation! - struct { - double dx, dy; - } pcm_res; - - auto pcm_err = pcm->compute(from_pt, &pcm_res); - if (pcm_err) { - return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP); - } - - to_pt->X -= pcm_res.dx; - to_pt->Y -= pcm_res.dy; - - return MccTelemetryErrorCode::ERROR_OK; - }; - _updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t { if (!only_hw) { // @@ -584,22 +547,6 @@ public: } - error_t transformToHardwareCoords(mcc_celestial_point_c auto pt, mcc_celestial_point_c auto* res) - { - MccCelestialPoint cpt, rcpt; - mcc_copy_celestial_point(pt, &cpt); - - rcpt.time_point = std::chrono::time_point_cast(res->time_point); - auto err = _toHardwareFunc(cpt, &rcpt); - if (err) { - return err; - } - - mcc_copy_celestial_point(rcpt, res); - - return MccTelemetryErrorCode::ERROR_OK; - } - error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy) { @@ -658,7 +605,6 @@ protected: std ::function _updateTargetFunc{}; std::function _updateFunc{}; std::function _setTargetFunc{}; - std::function _toHardwareFunc{}; std::unique_ptr _updateMutex; std::unique_ptr _updateCondVar; diff --git a/mcc/mcc_tracking_model.h b/mcc/mcc_tracking_model.h index eb62b9e..c79110f 100644 --- a/mcc/mcc_tracking_model.h +++ b/mcc/mcc_tracking_model.h @@ -3,11 +3,10 @@ /* MOUNT CONTROL COMPONENTS LIBRARY */ -/* SIMPLE TRACKING MODEL IMPLEMENTATION */ +/* SIMPLE Tracking MODEL IMPLEMENTATION */ #include "mcc_defaults.h" -#include "mcc_generics.h" #include "mcc_moving_model_common.h" namespace mcc @@ -15,14 +14,19 @@ namespace mcc enum class MccSimpleTrackingModelErrorCode : int { ERROR_OK, + ERROR_CCTE, ERROR_HW_GETSTATE, ERROR_HW_SETSTATE, ERROR_PCM_COMP, ERROR_GET_TELEMETRY, + ERROR_DIST_TELEMETRY, + ERROR_DIFF_TELEMETRY, ERROR_PZONE_CONTAINER_COMP, ERROR_IN_PZONE, ERROR_NEAR_PZONE, - ERROR_UNEXPECTED_AXIS_RATES + ERROR_TIMEOUT, + ERROR_UNEXPECTED_AXIS_RATES, + ERROR_STOPPED }; } // namespace mcc @@ -50,227 +54,254 @@ public: typedef MccSimpleMovingModelParams tracking_params_t; - // struct tracking_params_t { - // static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second - // double trackSpeedX{}; - // double trackSpeedY{}; - - // std::chrono::seconds telemetryTimeout{3}; - // // minimal time to prohibited zone. if it is lesser then exit with error - // std::chrono::seconds minTimeToPZone{10}; - // }; - - template - MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont) - : _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex) + template + MccSimpleTrackingModel(CONTROLS_T* controls) + : _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex()) { - *_stopTracking = false; - - // set default values - if constexpr (mccIsEquatorialMount(PcmT::mountType)) { - _currentTrackParams.trackSpeedX = tracking_params_t::sideralRate; // move along HA-axis with sideral rate - _currentTrackParams.trackSpeedY = 0.0; - - _currentTrackParams.telemetryTimeout = std::chrono::seconds(3); - _currentTrackParams.minTimeToPZone = std::chrono::seconds(10); - } - - _trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t { - typename HardwareT::hardware_state_t hw_state; - - MccEqtHrzCoords intsc_coords; + _trackingFunc = [controls, this]() -> error_t { + typename CONTROLS_T::hardware_state_t hw_state; MccTelemetryData tdata; - auto t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout); + MccEqtHrzCoords intsc_coords; + MccCelestialPoint target_in_future_pt; + + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + } else { + static_assert(false, "UNKNOW MOUNT TYPE!"); + } + + // double dist, dx, dy; + + auto t_err = controls->telemetryData(&tdata); if (t_err) { return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY); } - mcc_tp2tp(tdata.time_point, intsc_coords.time_point); + + bool no_intersects = false; + + // function to update the closest prohibited zone intersect point + auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state, + this]() -> error_t { + // compute intersection points with the prohibited zones + auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords); + if (pz_err) { + return mcc_deduce_error(pz_err, + MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + if (std::isfinite(intsc_coords.HA)) { + intsc_coords.X = intsc_coords.HA; + intsc_coords.Y = intsc_coords.DEC_APP; + } else { + no_intersects = true; + // intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min + // intsc_coords.Y = tdata.DEC_APP; + } + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + if (std::isfinite(intsc_coords.AZ)) { + intsc_coords.X = intsc_coords.AZ; + intsc_coords.Y = intsc_coords.ZD; + } else { + no_intersects = true; + } + } else { + static_assert(false, "UNKNOW MOUNT TYPE!"); + } + }; - std::vector> pz_timeto; // in seconds - std::chrono::duration min_time{0.0}; + auto target_point = [&, this](MccCelestialPoint* point) { + auto dt = std::chrono::duration{tdata.HA} + + _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds - // compute intersection points with the prohibited zones - auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords); + auto tp_dt = std::chrono::duration_cast( + _currentParams.timeShiftToTargetPoint); + + // point in +time_dist future + MccCelestialPoint pt{ + .pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, + .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize(), + .Y = tdata.DEC_APP}; + mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point); + + point->time_point = pt.time_point; + + // check for prohibited zone + if (std::isfinite(intsc_coords.HA)) { + bool through_pzone = + (intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone + through_pzone &= + (intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone + + if (through_pzone) { + pt.X = intsc_coords.HA; + } + } + + auto ret = controls->transformCoordinates(std::move(pt), point); + if (ret) { + return mcc_deduce_error(ret, MccSimpleTrackingModelErrorCode::ERROR_CCTE); + } else { + MccPCMResult pcm_inv_res; + + // endpoint of the mount moving + auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state); + if (pcm_err) { + return mcc_deduce_error(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP); + } + + mcc_tp2tp(tdata.time_point, hw_state.time_point); + } + + return MccSimpleTrackingModelErrorCode::ERROR_OK; + }; + + + auto pz_err = update_pzones_ipoint(); if (pz_err) { return mcc_deduce_error(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } - bool no_intersects = false; + hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking; - if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { - if (std::isfinite(intsc_coords.HA)) { - intsc_coords.X = intsc_coords.HA; - intsc_coords.Y = intsc_coords.DEC_APP; - } else { - no_intersects = true; - intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min - intsc_coords.Y = tdata.DEC_APP; + { + std::lock_guard lock{*_currentParamsMutex}; + + auto ccte_err = target_point(&target_in_future_pt); + if (ccte_err) { + return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE); } - } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { - if (std::isfinite(intsc_coords.AZ)) { - intsc_coords.X = intsc_coords.AZ; - intsc_coords.Y = intsc_coords.ZD; - } else { - no_intersects = true; + + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + hw_state.speedX = _currentParams.trackSpeedX; + hw_state.speedY = _currentParams.trackSpeedY; } - } else { - static_assert(false, "UNKNOW MOUNT TYPE!"); } - // compute position in future - auto hw_err = hardware->hardwareGetState(&hw_state); + // move mount + auto hw_err = controls->hardwareSetState(hw_state); if (hw_err) { - return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE); - } - - MccPCMResult pcm_inv_res; - - // endpoint of the mount moving - auto pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state); - if (pcm_err) { - return mcc_deduce_error(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP); + return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); } + std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp; - if constexpr (mccIsEquatorialMount(PcmT::mountType)) { - // just set sideral rate once - mcc_tp2tp(tdata.time_point, hw_state.time_point); + while (*_stopTracking) { + // wait for updated telemetry data { - std::lock_guard lock{*_currentTrackParamsMutex}; + std::lock_guard lock{*_currentParamsMutex}; - hw_state.speedX = _currentTrackParams.trackSpeedX; - hw_state.speedY = _currentTrackParams.trackSpeedY; - } - hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; + t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); - // start tracking - hw_err = hardware->hardwareSetState(std::move(hw_state)); - if (hw_err) { - return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); - } - - - while (!*_stopTracking) { - // control prohibited zones - pz_err = pz_cont->timeToPZone(tdata, &pz_timeto); - if (pz_err) { - return mcc_deduce_error(pz_err, - MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - - min_time = std::chrono::duration{0}; - for (size_t i = 0; i < pz_cont->sizePZones(); ++i) { - if (pz_timeto[i] < _currentTrackParams.minTimeToPZone) { - return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE; - } - if (pz_timeto[i] < min_time) { - min_time = pz_timeto[i]; - } - } - - t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout); if (t_err) { return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY); } + } - if (*_stopTracking) { - break; + if (*_stopTracking) { + break; + } + + // control prohibited zones + if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) { + return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE; + } + if (pz_err) { + return mcc_deduce_error(pz_err, + MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (*_stopTracking) { + break; + } + + { + std::lock_guard lock{*_currentParamsMutex}; + auto now = std::chrono::steady_clock::now(); + + if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) { + continue; } - if (no_intersects) { - if ((intsc_coords.HA - tdata.HA) < 10.0_mins) { // recompute target point - intsc_coords.X += 11.0_hours; - - hw_err = hardware->hardwareGetState(&hw_state); - if (hw_err) { - return mcc_deduce_error(hw_err, - MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE); - } - - pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state); - if (pcm_err) { - return mcc_deduce_error(pcm_err, - MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP); - } - - // just set sideral rate once - mcc_tp2tp(tdata.time_point, hw_state.time_point); - { - std::lock_guard lock{*_currentTrackParamsMutex}; - - hw_state.speedX = _currentTrackParams.trackSpeedX; - hw_state.speedY = _currentTrackParams.trackSpeedY; - } - hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; - - // start tracking - hw_err = hardware->hardwareSetState(std::move(hw_state)); - if (hw_err) { - return mcc_deduce_error(hw_err, - MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); - } + // update prohibited zones intersection point + if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) { + pz_err = update_pzones_ipoint(); + if (pz_err) { + return mcc_deduce_error( + pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } } + + // compute new target-in-future point + auto ccte_err = target_point(&target_in_future_pt); + if (ccte_err) { + return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE); + } } - return MccSimpleTrackingModelErrorCode::ERROR_OK; - - } else if constexpr (mccIsAltAzMount(PcmT::mountType)) { - static_assert(false, "NOT IMPLEMENTED!"); - } else { - static_assert(false, "UNKNOW MOUNT TYPE!"); + // send corrections + hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking; + hw_err = controls->hardwareSetState(hw_state); + if (hw_err) { + return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); + } } + + return MccSimpleTrackingModelErrorCode::ERROR_OK; }; } MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default; MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default; + MccSimpleTrackingModel(const MccSimpleTrackingModel&) = delete; + MccSimpleTrackingModel& operator=(const MccSimpleTrackingModel&) = delete; - error_t trackMount() + error_t trackTarget() { + *_stopTracking = false; + return _trackingFunc(); } - - error_t stopTracking() + error_t stoptTracking() { *_stopTracking = true; return MccSimpleTrackingModelErrorCode::ERROR_OK; } - error_t setTrackingParams(tracking_params_t params) { - std::lock_guard lock{*_currentTrackParamsMutex}; + std::lock_guard lock{*_currentParamsMutex}; - _currentTrackParams = std::move(params); + _currentParams = std::move(params); return MccSimpleTrackingModelErrorCode::ERROR_OK; } + tracking_params_t getTrackingParams() const { - std::lock_guard lock{*_currentTrackParamsMutex}; + std::lock_guard lock{*_currentParamsMutex}; - return _currentTrackParams; + return _currentParams; } + protected: std::function _trackingFunc{}; - std::unique_ptr _stopTracking{}; + std::unique_ptr _stopTracking; - tracking_params_t _currentTrackParams; - std::unique_ptr _currentTrackParamsMutex; + tracking_params_t _currentParams{}; + std::unique_ptr _currentParamsMutex{}; }; } // namespace mcc