diff --git a/include/mcc/mcc_generic_movecontrols.h b/include/mcc/mcc_generic_movecontrols.h index 68b16df..b95e4b8 100644 --- a/include/mcc/mcc_generic_movecontrols.h +++ b/include/mcc/mcc_generic_movecontrols.h @@ -262,48 +262,50 @@ public: _stopFunc(std::forward(stop_func)) { if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - *_stopMovementRequest = false; + // *_stopMovementRequest = false; - *_fsmState = STATE_IDLE; + // *_fsmState = STATE_IDLE; - // start thread of movements - _fstFuture = std::async( - [this](std::stop_token stoken) { - auto do_state = _fsmState->load(); + // // start thread of movements + // _fstFuture = std::async( + // [this](std::stop_token stoken) { + // auto do_state = _fsmState->load(); - while (!stoken.stop_requested()) { - std::println("\n{:*^80}\n", " WAIT LOCK "); - // wait here ... - _wakeupRequest->wait(false, std::memory_order_relaxed); + // while (!stoken.stop_requested()) { + // std::println("\n{:*^80}\n", " WAIT LOCK "); + // // wait here ... + // _wakeupRequest->wait(false, std::memory_order_relaxed); - _wakeupRequest->clear(); + // _wakeupRequest->clear(); - std::println("\n{:*^80}\n", " UNLOCKED "); + // std::println("\n{:*^80}\n", " UNLOCKED "); - // if (stoken.stop_requested()) { - // break; - // } + // // if (stoken.stop_requested()) { + // // break; + // // } - do_state = _fsmState->load(); + // do_state = _fsmState->load(); - if (do_state & STATE_STOP) { - // if (_fsmState->load() & STATE_STOP) { - *_stopMovementRequest = true; - _stopFunc(); - } + // if (do_state & STATE_STOP) { + // // if (_fsmState->load() & STATE_STOP) { + // *_stopMovementRequest = true; + // _stopFunc(); + // } - if (do_state & STATE_SLEW) { - // if (_fsmState->load() & STATE_SLEW) { - *_stopMovementRequest = false; - _slewFunc(_slewAndStop->load()); - } else if (do_state & STATE_TRACK) { - // } else if (_fsmState->load() & STATE_TRACK) { - *_stopMovementRequest = false; - _trackFunc(); - } - } - }, - _fstStopSource.get_token()); + // if (do_state & STATE_SLEW) { + // // if (_fsmState->load() & STATE_SLEW) { + // *_stopMovementRequest = false; + // _slewFunc(_slewAndStop->load()); + // } else if (do_state & STATE_TRACK) { + // // } else if (_fsmState->load() & STATE_TRACK) { + // *_stopMovementRequest = false; + // _trackFunc(); + // } + // } + // }, + // _fstStopSource.get_token()); + + startAsyncMovementCycle(); } } @@ -316,20 +318,108 @@ public: virtual ~MccGenericMovementControls() { if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - *_fsmState = STATE_IDLE; - _fstStopSource.request_stop(); + // *_fsmState = STATE_IDLE; + // _fstStopSource.request_stop(); - _wakeupRequest->test_and_set(); - _wakeupRequest->notify_one(); + // _wakeupRequest->test_and_set(); + // _wakeupRequest->notify_one(); - std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // std::this_thread::sleep_for(std::chrono::milliseconds(200)); - if (_fstFuture.valid()) { - auto status = _fstFuture.wait_for(_waitTimeout->load()); - } + // if (_fstFuture.valid()) { + // auto status = _fstFuture.wait_for(_waitTimeout->load()); + // } + + stopAsyncMovementCycle(); } } + + template + void stopAsyncMovementCycle(this SelfT&& self) + requires(EXEC_POLICY == MccGenericMovementControlsPolicy::POLICY_ASYNC) + { + self->_fsmState = STATE_IDLE; + self._fstStopSource.request_stop(); + + self._wakeupRequest->test_and_set(); + self._wakeupRequest->notify_all(); + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + if (self._fstFuture.valid()) { + auto status = self._fstFuture.wait_for(self._waitTimeout->load()); + } + } + + template + void startAsyncMovementCycle(this SelfT&& self) + requires(EXEC_POLICY == MccGenericMovementControlsPolicy::POLICY_ASYNC) + { + using self_t = std::decay_t; + + if (self._fstFuture.valid()) { + self.stopAsyncMovementCycle(); + } + + self->_stopMovementRequest = false; + + self._fstStopSource = std::stop_source{}; + + self->_fsmState = STATE_IDLE; + + // start thread of movements + self._fstFuture = std::async( + [&self](std::stop_token stoken) { + auto do_state = self._fsmState->load(); + + std::string log_str; + while (!stoken.stop_requested()) { + log_str = std::format("\n{:*^80}\n", " WAIT LOCK "); + if constexpr (mcc_generic_mount_c) { + self.logTrace(log_str); + } else { + std::println(log_str); + } + + // wait here ... + self._wakeupRequest->wait(false, std::memory_order_relaxed); + + self._wakeupRequest->clear(); + + log_str = std::format("\n{:*^80}\n", " UNLOCKED "); + if constexpr (mcc_generic_mount_c) { + self.logTrace(log_str); + } else { + std::println(log_str); + } + + // if (stoken.stop_requested()) { + // break; + // } + + do_state = self._fsmState->load(); + + if (do_state & STATE_STOP) { + // if (_fsmState->load() & STATE_STOP) { + self->_stopMovementRequest = true; + self._stopFunc(); + } + + if (do_state & STATE_SLEW) { + // if (_fsmState->load() & STATE_SLEW) { + self->_stopMovementRequest = false; + self._slewFunc(self._slewAndStop->load()); + } else if (do_state & STATE_TRACK) { + // } else if (_fsmState->load() & STATE_TRACK) { + self->_stopMovementRequest = false; + self._trackFunc(); + } + } + }, + self._fstStopSource.get_token()); + } + error_t slewToTarget(bool slew_and_stop) { // *_stopMovementRequest = false; diff --git a/tests/mcc_telemetry_test.cpp b/tests/mcc_telemetry_test.cpp index f8516e1..f572fb8 100644 --- a/tests/mcc_telemetry_test.cpp +++ b/tests/mcc_telemetry_test.cpp @@ -14,6 +14,9 @@ static std::uniform_real_distribution y_distrib(1000, 7000); static std::uniform_real_distribution xs_distrib(100, 700); static std::uniform_real_distribution ys_distrib(100, 700); +static std::uniform_int_distribution err_distrib(0, 255); + + struct hw_t { static constexpr mcc::MccMountType hwMountType{mcc::MccMountType::ALTAZ_TYPE}; static constexpr std::string_view hardwareName{"HW-TEST"}; @@ -53,13 +56,19 @@ struct hw_t { *state = hardware_state_t{.XY{x_distrib(gen), y_distrib(gen)}, .speedXY{xs_distrib(gen), ys_distrib(gen)}}; - return 0; + // return 0; + return err_distrib(gen) < 5 ? 1 : 0; } error_t hardwareInit() { return 0; } + + error_t hardwareShutdown() + { + return 0; + } }; template <>