diff --git a/include/mcc/mcc_generic_movecontrols.h b/include/mcc/mcc_generic_movecontrols.h index b784fa9..22a3f0c 100644 --- a/include/mcc/mcc_generic_movecontrols.h +++ b/include/mcc/mcc_generic_movecontrols.h @@ -19,6 +19,7 @@ #include +#include #include "mcc_coordinate.h" #include "mcc_error.h" @@ -240,8 +241,6 @@ template class MccGenericMovementControls { - enum { STATE_SLEW, STATE_TRACK, STATE_IDLE } _currentState; - public: static constexpr MccGenericMovementControlsPolicy executePolicy = EXEC_POLICY; @@ -258,51 +257,45 @@ public: _stopFunc(std::forward(stop_func)) { if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - *_doStop = false; - *_doSlew = false; - *_doTrack = false; *_stopMovementRequest = false; - _currentState = STATE_IDLE; + *_fsmState = STATE_IDLE; + + // start thread of movements _fstFuture = std::async( - [&, this](std::stop_token stoken) { + [this](std::stop_token stoken) { + auto do_state = _fsmState->load(); + while (!stoken.stop_requested()) { - if (_doStop->load()) { - std::println("******* DO-STOP ({} {} {}) *******", _doStop->load(), _doSlew->load(), - _doTrack->load()); - *_stopMovementRequest = true; - // _currentState = STATE_IDLE; - _stopFunc(); - // *_doStop = false; - } - - if (_doSlew->load()) { - std::println("******* DO-SLEW ({} {} {}) *******", _doStop->load(), _doSlew->load(), - _doTrack->load()); - *_stopMovementRequest = false; - _currentState = STATE_SLEW; - _slewFunc(_slewAndStop->load()); - // _currentState = STATE_IDLE; - // *_doSlew = false; - continue; // to guarantee execute doStop if tracking is requested!!! - } - - if (_doTrack->load()) { - std::println("******* DO-TRACK ({} {} {}) *******", _doStop->load(), _doSlew->load(), - _doTrack->load()); - *_stopMovementRequest = false; - _currentState = STATE_TRACK; - _trackFunc(); - // _currentState = STATE_IDLE; - // *_doTrack = false; - } - - _currentState = STATE_IDLE; - + std::println("\n{:*^80}\n", " WAIT LOCK "); // wait here ... _wakeupRequest->wait(false, std::memory_order_relaxed); _wakeupRequest->clear(); + + std::println("\n{:*^80}\n", " UNLOCKED "); + + // if (stoken.stop_requested()) { + // break; + // } + + do_state = _fsmState->load(); + + 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()); @@ -318,24 +311,9 @@ public: virtual ~MccGenericMovementControls() { if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - // if (_slewFuncFuture.valid()) { - // auto status = _slewFuncFuture.wait_for(_waitTimeout->load()); - // } - - // if (_trackFuncFuture.valid()) { - // auto status = _trackFuncFuture.wait_for(_waitTimeout->load()); - // } - - // if (_stopFuncFuture.valid()) { - // auto status = _stopFuncFuture.wait_for(_waitTimeout->load()); - // } - - - *_doSlew = false; - *_doTrack = false; - // *_stopMovementRequest = false; - + *_fsmState = STATE_IDLE; _fstStopSource.request_stop(); + _wakeupRequest->test_and_set(); _wakeupRequest->notify_one(); @@ -352,34 +330,18 @@ public: // *_stopMovementRequest = false; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - // if (_slewFuncFuture.valid()) { // already slewing - // _slewFuncFuture = std::async( - // std::launch::async, - // [this](bool st) { - // auto err = _stopFunc(); // first, stop mount - // if (!err) { - // *_stopMovementRequest = false; + auto prev_state = _fsmState->load(); - // return _slewFunc(st); - // } else { - // return err; - // } - // }, - // slew_and_stop); - // } else { - // _slewFuncFuture = std::async(std::launch::async, _slewFunc, slew_and_stop); - // } + *_fsmState = STATE_SLEW; + if (prev_state & STATE_SLEW || prev_state & STATE_TRACK) { + *_fsmState |= STATE_STOP; - if (_currentState == STATE_SLEW || _currentState == STATE_TRACK) { // first, stop mount - *_doStop = true; *_stopMovementRequest = true; // to exit from slewing or tracking - } else { - *_doStop = false; } - *_doTrack = false; - *_doSlew = true; + *_slewAndStop = slew_and_stop; + _wakeupRequest->test_and_set(); _wakeupRequest->notify_one(); @@ -396,19 +358,16 @@ public: // *_stopMovementRequest = false; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - // if (!_trackFuncFuture.valid()) { - // _trackFuncFuture = std::async(std::launch::async, _trackFunc); - // } // already tracking, just ignore + auto prev_state = _fsmState->load(); + + if (!(prev_state & STATE_TRACK)) { + *_fsmState = STATE_TRACK; + + if (prev_state & STATE_SLEW) { + *_fsmState |= STATE_STOP; - if (_currentState != STATE_TRACK) { - if (_currentState == STATE_SLEW) { // first, stop mount - *_doStop = true; *_stopMovementRequest = true; // to exit from slewing - } else { - *_doStop = false; } - *_doSlew = false; - *_doTrack = true; _wakeupRequest->test_and_set(); _wakeupRequest->notify_one(); @@ -427,14 +386,7 @@ public: // *_stopMovementRequest = true; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { - // if future is valid then stop is already called - // if (!_stopFuncFuture.valid()) { - // _stopFuncFuture = std::async(std::launch::async, _stopFunc); - // } - - *_doStop = true; - *_doSlew = false; - *_doTrack = false; + *_fsmState = STATE_STOP; *_stopMovementRequest = true; // to exit from slewing or tracking @@ -475,22 +427,6 @@ protected: std::function _trackFunc{}; std::function _stopFunc{}; - std::conditional_t, - std::nullptr_t> - _slewFuncFuture{}; - - std::conditional_t, - std::nullptr_t> - _trackFuncFuture{}; - - std::conditional_t, - std::nullptr_t> - _stopFuncFuture{}; - - std::unique_ptr> _waitTimeout{ new std::atomic{defaultWaitTimeout}}; @@ -504,11 +440,12 @@ protected: std::stop_source _fstStopSource{}; std::unique_ptr _wakeupRequest{new std::atomic_flag}; - std::unique_ptr _doStop{new std::atomic_bool{false}}; - std::unique_ptr _doSlew{new std::atomic_bool{false}}; - std::unique_ptr _doTrack{new std::atomic_bool{false}}; std::unique_ptr _slewAndStop{new std::atomic_bool{false}}; + enum { STATE_IDLE = 0x00, STATE_SLEW = 0x01, STATE_TRACK = 0x02, STATE_STOP = 0x04 }; + + std::unique_ptr _fsmState{new std::atomic_int}; + // utility methods