From 5f802ff57e6f0125ab00875b3031cf05b5f248d8 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Fri, 12 Sep 2025 12:53:05 +0300 Subject: [PATCH] ... --- asibfm700/asibfm700_servocontroller.cpp | 63 +++++- asibfm700/asibfm700_servocontroller.h | 12 +- mcc/mcc_generic_mount.h | 252 ++++++++++++++++++++++-- mcc/mcc_generics.h | 23 +-- 4 files changed, 319 insertions(+), 31 deletions(-) diff --git a/asibfm700/asibfm700_servocontroller.cpp b/asibfm700/asibfm700_servocontroller.cpp index c94018f..6c193a3 100644 --- a/asibfm700/asibfm700_servocontroller.cpp +++ b/asibfm700/asibfm700_servocontroller.cpp @@ -23,7 +23,35 @@ constexpr std::string_view AsibFM700ServoController::hardwareName() const AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop() { - return static_cast(Mount.stop()); + error_t err = static_cast(Mount.stop()); + if (err) { + return err; + } + + hardware_state_t hw_state; + + auto start_tp = std::chrono::steady_clock::now(); + + // poll hardware till stopped-state detected ... + while (true) { + err = hardwareGetState(&hw_state); + if (err) { + return err; + } + + if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { + break; + } + + if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) { + err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT; + break; + } + + std::this_thread::sleep_for(_hardwareConfig.pollingInterval); + } + + return err; } AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit() @@ -55,6 +83,10 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state) { + if (state == nullptr) { + return AsibFM700ServoControllerErrorCode::ERROR_NULLPTR; + } + using tp_t = decltype(hardware_state_t::time_point); mountdata_t mdata; @@ -75,6 +107,35 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(har state->speedX = mdata.encYspeed.val; state->speedY = mdata.encXspeed.val; + + state->stateX = mdata.Xstate; + state->stateY = mdata.Ystate; + + if (mdata.Xstate == AXIS_STOPPED) { + if (mdata.Ystate == AXIS_STOPPED) { + state->moving_state = hardware_moving_state_t::HW_MOVE_STOPPED; + } else if (mdata.Ystate == AXIS_SLEWING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING; + } else if (mdata.Ystate == AXIS_POINTING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING; + } else if (mdata.Ystate == AXIS_GUIDING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING; + } else if (mdata.Ystate == AXIS_ERROR) { + state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR; + } else { + state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN; + } + } else if (mdata.Xstate == AXIS_SLEWING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING; + } else if (mdata.Xstate == AXIS_POINTING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING; + } else if (mdata.Xstate == AXIS_GUIDING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING; + } else if (mdata.Xstate == AXIS_ERROR) { + state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR; + } else { + state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN; + } } return err; diff --git a/asibfm700/asibfm700_servocontroller.h b/asibfm700/asibfm700_servocontroller.h index 098ed48..fdf881f 100644 --- a/asibfm700/asibfm700_servocontroller.h +++ b/asibfm700/asibfm700_servocontroller.h @@ -20,6 +20,8 @@ enum class AsibFM700ServoControllerErrorCode : int { ERROR_MOUNTDEV = MCC_E_MOUNTDEV, ERROR_FAILED = MCC_E_FAILED, // my codes ... + ERROR_POLLING_TIMEOUT, + ERROR_NULLPTR }; // error category @@ -59,11 +61,13 @@ public: typedef std::error_code error_t; enum class hardware_moving_state_t : int { - HW_MOVE_STOPPED, + HW_MOVE_ERROR = -1, + HW_MOVE_STOPPED = 0, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, - HW_MOVE_GUIDING + HW_MOVE_GUIDING, + HW_MOVE_UNKNOWN }; struct hardware_state_t { @@ -71,6 +75,7 @@ public: mcc::MccTimePoint time_point; double X, Y, speedX, speedY; + axis_status_t stateX, stateY; // Eddy's LibSidServo axis state hardware_moving_state_t moving_state; }; @@ -86,6 +91,9 @@ public: conf_t devConfig; hardware_configuration_t hwConfig; + + std::chrono::milliseconds pollingInterval{300}; // hardware polling interval + std::chrono::milliseconds pollingTimeout{30000}; // hardware polling timeout }; /* constructors and destructor */ diff --git a/mcc/mcc_generic_mount.h b/mcc/mcc_generic_mount.h index f0db699..d7dba25 100644 --- a/mcc/mcc_generic_mount.h +++ b/mcc/mcc_generic_mount.h @@ -65,14 +65,12 @@ public: PZoneContT pzone_cont, SlewModelT slew_model, TrackModelT track_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)), LoggerT(std::move(logger)) { } @@ -81,9 +79,8 @@ public: error_t stopMount() { - logInfo("stop any movements ..."); + logInfo("Stop any movements ..."); - // this->stopGuidingTarget(); this->stopTracking(); this->stopSlewing(); @@ -92,26 +89,237 @@ public: return mcc_deduce_error(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); } - // poll hardware till stopped-state detected ... - - logInfo("mount was stopped"); + logInfo("Stop command was sent"); return MccGenericMountErrorCode::ERROR_OK; } error_t initMount() { + logInfo("Start mount initialization ..."); + auto hw_err = this->hardwareInit(); if (hw_err) { return mcc_deduce_error(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); } + logInfo("Mount initialization was performed"); + return MccGenericMountErrorCode::ERROR_OK; } }; -/* GENERIC FINITE-STATE-MOCHINE MOUNT REFERENCE IMPLEMENTATION */ + +/* GENERIC FINITE-STATE-MACHINE MOUNT REFERENCE IMPLEMENTATION */ + + +template +struct MccGenericFsmMountBaseEvent { + // static constexpr std::string_view ID{""}; + virtual ~MccGenericFsmMountBaseEvent() = default; + + MOUNT_T& mount() const + { + return _mount; + } + +protected: + MOUNT_T& _mount; + + MccGenericFsmMountBaseEvent(MOUNT_T& mount) : _mount(mount) {} +}; + + +template +struct MccGenericFsmMountInitEvent; + +template +struct MccGenericFsmMountIdleEvent; + +template +struct MccGenericFsmMountStopEvent; + +template +struct MccGenericFsmMountErrorEvent; + +template +struct MccGenericFsmMountSlewEvent; + +template +struct MccGenericFsmMountTrackEvent; + +template +struct MccGenericFsmMountBaseState { + // static constexpr std::string_view ID{""}; + virtual ~MccGenericFsmMountBaseState() = default; + +protected: + MccGenericFsmMountBaseState() = default; + + template + void exitLog(this auto&& self, EvT& event) + { + using self_t = std::remove_cvref_t; + + if constexpr (mcc_generic_log_mount_c && + std::derived_from>) { + event.mount().logDebug(std::format("Exit from '{}' state due to '{}' event ...", self_t::ID, EvT::ID)); + } + } + + template + void enterLog(this auto&& self, EvT& event) + { + using self_t = std::remove_cvref_t; + + if constexpr (mcc_generic_log_mount_c && + std::derived_from>) { + event.mount().logDebug(std::format("Enter to '{}' state due to '{}' event ...", self_t::ID, EvT::ID)); + } + } +}; + +template +struct MccGenericFsmMountStartState; + +template +struct MccGenericFsmMountIdleState; + +template +struct MccGenericFsmMountStopState; + +template +struct MccGenericFsmMountErrorState; + +template +struct MccGenericFsmMountSlewState; + +template +struct MccGenericFsmMountTrackState; + +template +struct MccGenericFsmMountInitState { + static constexpr std::string_view ID{"GENERIC-MOUNT-INIT-STATE"}; + + using transition_t = fsm::fsm_transition_table_t< + std::pair, MccGenericFsmMountIdleState>, + std::pair, MccGenericFsmMountErrorState>, + std::pair, MccGenericFsmMountInitState>>; + + + void exit(MccGenericFsmMountInitEvent& event) + { + if constexpr (mcc_generic_log_mount_c) { + event.mount().logWarn("It seems a re-entering to the initializing state was asked! Ignore the event!"); + } + } + + void enter(MccGenericFsmMountInitEvent& event) + { + if constexpr (mcc_generic_log_mount_c) { + event.mount().logWarn( + "It seems a re-entering to the initializing state was asked! Ignore the event and wait for the mount " + "to initialize!"); + } + } + + void exit(fsm::traits::fsm_event_c auto& event) + { + exitLog(event); + } + + void enter(fsm::traits::fsm_event_c auto& event) + { + enterLog(event); + } +}; + + + +template , + fsm::traits::fsm_event_c INIT_EVENT_T = MccGenericFsmMountInitEvent, + fsm::traits::fsm_event_c IDLE_EVENT_T = MccGenericFsmMountIdleEvent, + fsm::traits::fsm_event_c STOP_EVENT_T = MccGenericFsmMountStopEvent, + fsm::traits::fsm_event_c ERROR_EVENT_T = MccGenericFsmMountErrorEvent, + fsm::traits::fsm_event_c SLEW_EVENT_T = MccGenericFsmMountSlewEvent, + fsm::traits::fsm_event_c TRACK_EVENT_T = MccGenericFsmMountTrackEvent> +class MccGenericFsmMount : public MOUNT_T, protected fsm::MccFiniteStateMachine +{ +public: + using typename MOUNT_T::error_t; + + // reimplementation of base-class methods to adapt it to FSM-behavior + + auto initMount() + { + this->dispatchEvent(INIT_EVENT_T{*this}); + } + + auto stopMount() + { + this->dispatchEvent(STOP_EVENT_T{*this}); + } + + auto slewToTarget() + { + this->dispatchEvent(SLEW_EVENT_T{*this}); + } + + auto stopSlewing() + { + this->dispatchEvent(STOP_EVENT_T{*this}); + } + + auto trackTarget() + { + this->dispatchEvent(TRACK_EVENT_T{*this}); + } + + + auto stopTracking() + { + this->dispatchEvent(STOP_EVENT_T{*this}); + } + + +protected: + // wrappers + + auto _initMount() + { + return MOUNT_T::initMount(); + } + + + auto _stopMount() + { + return MOUNT_T::stopMount(); + } + + auto _slewToTarget() + { + return MOUNT_T::slewToTarget(); + } + + + auto _trackTarget() + { + return MOUNT_T::trackTarget(); + } + + + auto _startGuidingTarget() + { + return MOUNT_T::startGuidingTarget(); + } + + auto _stopGuidingTarget() + { + return MOUNT_T::stopGuidingTarget(); + } +}; template base_gmount_t; public: - typedef typename base_gmount_t::error_t error_t; + using typename base_gmount_t::error_t; /* EVENTS AND STATES DEFINITION */ @@ -270,7 +478,19 @@ public: using transition_t = fsm::fsm_transition_table_t, - std::pair>; + std::pair, + std::pair>; + + void exit(MccGenericMountEventInit& event) + { + event.mount().logWarn("It seems re-entering to initializing state was asked! Just ignore this event!"); + } + + void enter(MccGenericMountEventInit& event) + { + event.mount().logWarn( + "It seems re-entering to initializing state was asked! Ignore it and wait for mount to initialize!"); + } template EvT> void exit(EvT& event) @@ -300,7 +520,8 @@ public: std::pair, std::pair, std::pair, - std::pair>; + std::pair, + std::pair>; template EvT> void exit(EvT& event) @@ -331,7 +552,8 @@ public: void enter(MccGenericMountEventStop& event) { - event.mount().logWarn("It seems re-entering to stopping state was asked! Wait for mount stopping!"); + event.mount().logWarn( + "It seems re-entering to stopping state was asked! Ignore it and wait for mount to stop!"); } template EvT> @@ -436,7 +658,9 @@ public: template EvT> void exit(EvT& event) { - error_t err = event.mount()._stopTrackingMount(); + // error_t err = event.mount()._stopTrackingMount(); + + error_t err = static_cast(&event.mount())->stopTracking(); if (err) { event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err}); } @@ -474,7 +698,6 @@ public: PZoneContT pzone_cont, SlewModelT slew_model, TrackModelT track_model, - // GuidingModelT guiding_model, LoggerT logger = MccNullLogger{}) : fsm::MccFiniteStateMachine(MccGenericMountStateUninit{}), base_gmount_t(std::move(hardware), @@ -482,7 +705,6 @@ public: std::move(pzone_cont), std::move(slew_model), std::move(track_model), - // std::move(guiding_model), std::move(logger)) { } diff --git a/mcc/mcc_generics.h b/mcc/mcc_generics.h index e0b21ac..69fa60f 100644 --- a/mcc/mcc_generics.h +++ b/mcc/mcc_generics.h @@ -424,13 +424,13 @@ concept mcc_hardware_c = requires(T t, const T t_const) { // }(); // to ensure 'mountType' can be used in compile-time context - // a type that defines at least HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING + // a type that defines at least HW_MOVE_ERROR, HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING // and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a // possible tunning of hardware hardwareSetState-related commands and detect stop-state // // e.g. an implementations can be as follows: - // enum class hardware_moving_state_t: int {HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, - // HW_MOVE_TRACKING, HW_MOVE_GUIDING} + // enum class hardware_moving_state_t: int {HW_MOVE_ERROR = -1, HW_MOVE_STOPPED = 0, HW_MOVE_SLEWING, + // HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING} // // struct hardware_moving_state_t { // uint16_t HW_MOVE_STOPPED = 0; @@ -438,6 +438,7 @@ concept mcc_hardware_c = requires(T t, const T t_const) { // uint16_t HW_MOVE_ADJUSTING = 222; // uint16_t HW_MOVE_TRACKING = 333; // uint16_t HW_MOVE_GUIDING = 444; + // uint16_t HW_MOVE_ERROR = 555; // } requires requires(typename T::hardware_moving_state_t type) { []() { @@ -457,6 +458,9 @@ concept mcc_hardware_c = requires(T t, const T t_const) { // hardware was asked for guiding // (small corrections to align actual mount position with target celestial point) static constexpr auto v4 = T::hardware_moving_state_t::HW_MOVE_GUIDING; + + // to detect possible hardware error + static constexpr auto v5 = T::hardware_moving_state_t::HW_MOVE_ERROR; }(); }; @@ -491,15 +495,6 @@ concept mcc_hardware_c = requires(T t, const T t_const) { -/* AUXILIARY COORDINATE-TRANSFORMATON CLASS CONCEPT */ - -// a concept of class that consist of the full set of coordinate transformation mount control components -// (celestial coordinate transformation engine, mount hardware encoders readings and pointing correction model) -// the set of methods of this class is enough to transform coordinates from ICRS to hardware and back -template -concept mcc_coord_trfm_controls_c = mcc_ccte_c && mcc_hardware_c && mcc_PCM_c; - - /* MOUNT TELEMETRY DATA CLASS CONCEPT */ template @@ -881,7 +876,9 @@ concept mcc_tracking_model_c = requires(T t) { /* GENERIC MOUNT CLASS CONCEPT */ -// a class containing mount position related controls +// a concept of class that consist of the full set of coordinate transformation mount control components +// (celestial coordinate transformation engine, mount hardware encoders readings and pointing correction model) +// the set of methods of this class is enough to transform coordinates from ICRS to hardware and back template concept mcc_position_controls_c = mcc_ccte_c && mcc_hardware_c && mcc_PCM_c;