...

asibfm700/asibfm700_servocontroller.cpp

@@ -108,6 +108,38 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har
 {
     std::lock_guard lock{*_setStateMutex};
 
+    if (state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) {  // stop!
+        error_t err = static_cast<AsibFM700ServoControllerErrorCode>(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;
+    }
+
     // static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
     // static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};