...

mcc/mcc_slewing_model.h

@@ -27,6 +27,8 @@ enum class MccSimpleSlewingModelErrorCode : int {
     ERROR_NEAR_PZONE,
     ERROR_TIMEOUT,
     ERROR_UNEXPECTED_AXIS_RATES,
+    ERROR_ALREADY_SLEW,
+    ERROR_ALREADY_STOPPED,
     ERROR_STOPPED
 };
 
@@ -63,6 +65,8 @@ public:
     MccSimpleSlewingModel(CONTROLS_T* controls)
         : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
     {
+        *_stopSlewing = true;
+
         _slewingFunc = [controls, this]() -> error_t {
             // first, check target coordinates
             typename CONTROLS_T::error_t t_err;
@@ -80,10 +84,12 @@ public:
             bool in_zone;
             auto pz_err = controls->inPZone(tdata.target, &in_zone);
             if (pz_err) {
+                *_stopSlewing = true;
                 return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
             }
 
             if (in_zone) {
+                *_stopSlewing = true;
                 return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
             }
 
@@ -110,6 +116,7 @@ public:
 
             auto hw_err = controls->hardwareGetState(&hw_state);
             if (hw_err) {
+                *_stopSlewing = true;
                 return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
             }
 
@@ -133,6 +140,7 @@ public:
             // start slewing
             hw_err = controls->hardwareSetState(hw_state);
             if (hw_err) {
+                *_stopSlewing = true;
                 return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
             }
 
@@ -155,6 +163,7 @@ public:
                     t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
 
                     if (t_err) {
+                        *_stopSlewing = true;
                         return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                     }
                 }
@@ -176,11 +185,13 @@ public:
 
                 pz_err = controls->inPZone(cpt, &in_zone);
                 if (pz_err) {
+                    *_stopSlewing = true;
                     return mcc_deduce_error<error_t>(pz_err,
                                                      MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                 }
 
                 if (in_zone) {
+                    *_stopSlewing = true;
                     return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
                 }
 
@@ -196,11 +207,13 @@ public:
 
                 hw_err = controls->hardwareGetState(&hw_state);
                 if (hw_err) {
+                    *_stopSlewing = true;
                     return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
                 }
 
                 t_err = controls->targetToMountDist(&dist);
                 if (t_err) {
+                    *_stopSlewing = true;
                     return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
                 }
 
@@ -225,6 +238,7 @@ public:
 
                                 hw_err = controls->hardwareSetState(hw_state);
                                 if (hw_err) {
+                                    *_stopSlewing = true;
                                     return mcc_deduce_error<error_t>(hw_err,
                                                                      MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                                 }
@@ -238,6 +252,7 @@ public:
                     if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
                         if (hw_state.moving_state ==
                             CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) {  // mount was stopped
+                            *_stopSlewing = true;
                             break;
                         }
                     }
@@ -258,6 +273,7 @@ public:
 
                         hw_err = controls->hardwareSetState(hw_state);
                         if (hw_err) {
+                            *_stopSlewing = true;
                             return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                         }
 
@@ -291,6 +307,10 @@ public:
 
     error_t slewToTarget()
     {
+        if (!(*_stopSlewing)) {
+            return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_SLEW;
+        }
+
         *_stopSlewing = false;
 
         return _slewingFunc();
@@ -299,6 +319,10 @@ public:
 
     error_t stopSlewing()
     {
+        if (*_stopSlewing) {
+            return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED;
+        }
+
         *_stopSlewing = true;
 
         return MccSimpleSlewingModelErrorCode::ERROR_OK;