...

mcc/mcc_slewing_model.h

@@ -87,15 +87,13 @@ public:
         : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
     {
         _slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
-            *_stopSlewing = false;
-
             // first, check target coordinates
             typename TelemetryT::error_t t_err;
             MccTelemetryData tdata;
 
             {
                 std::lock_guard lock{*_currentParamsMutex};
-                t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
+                t_err = telemetry->telemetryData(&tdata);
 
                 if (t_err) {
                     return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
@@ -332,6 +330,8 @@ public:
 
     error_t slewToTarget()
     {
+        *_stopSlewing = false;
+
         return _slewingFunc();
     }