...

mcc/mcc_tracking_model.h

@@ -26,6 +26,8 @@ enum class MccSimpleTrackingModelErrorCode : int {
     ERROR_NEAR_PZONE,
     ERROR_TIMEOUT,
     ERROR_UNEXPECTED_AXIS_RATES,
+    ERROR_ALREADY_TRACK,
+    ERROR_ALREADY_STOPPED,
     ERROR_STOPPED
 };
 
@@ -59,6 +61,8 @@ public:
     MccSimpleTrackingModel(CONTROLS_T* controls)
         : _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
     {
+        *_stopTracking = true;
+
         _trackingFunc = [controls, this]() -> error_t {
             typename CONTROLS_T::hardware_state_t hw_state;
 
@@ -78,6 +82,7 @@ public:
 
             auto t_err = controls->telemetryData(&tdata);
             if (t_err) {
+                *_stopTracking = true;
                 return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
             }
 
@@ -165,6 +170,7 @@ public:
 
             auto pz_err = update_pzones_ipoint();
             if (pz_err) {
+                *_stopTracking = true;
                 return mcc_deduce_error<error_t>(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
             }
 
@@ -175,6 +181,7 @@ public:
 
                 auto ccte_err = target_point(&target_in_future_pt);
                 if (ccte_err) {
+                    *_stopTracking = true;
                     return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
                 }
 
@@ -187,6 +194,7 @@ public:
             // move mount
             auto hw_err = controls->hardwareSetState(hw_state);
             if (hw_err) {
+                *_stopTracking = true;
                 return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
             }
 
@@ -201,6 +209,7 @@ public:
                     t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
 
                     if (t_err) {
+                        *_stopTracking = true;
                         return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
                     }
                 }
@@ -211,9 +220,11 @@ public:
 
                 // control prohibited zones
                 if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
+                    *_stopTracking = true;
                     return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
                 }
                 if (pz_err) {
+                    *_stopTracking = true;
                     return mcc_deduce_error<error_t>(pz_err,
                                                      MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                 }
@@ -234,6 +245,7 @@ public:
                     if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
                         pz_err = update_pzones_ipoint();
                         if (pz_err) {
+                            *_stopTracking = true;
                             return mcc_deduce_error<error_t>(
                                 pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                         }
@@ -242,6 +254,7 @@ public:
                     // compute new target-in-future point
                     auto ccte_err = target_point(&target_in_future_pt);
                     if (ccte_err) {
+                        *_stopTracking = true;
                         return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
                     }
                 }
@@ -250,6 +263,7 @@ public:
                 hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking;
                 hw_err = controls->hardwareSetState(hw_state);
                 if (hw_err) {
+                    *_stopTracking = true;
                     return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
                 }
             }
@@ -266,13 +280,21 @@ public:
 
     error_t trackTarget()
     {
+        if (!(*_stopTracking)) {  // already tracking
+            return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_TRACK;
+        }
+
         *_stopTracking = false;
 
         return _trackingFunc();
     }
 
-    error_t stoptTracking()
+    error_t stopTracking()
     {
+        if (*_stopTracking) {  // already stopped
+            return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_STOPPED;
+        }
+
         *_stopTracking = true;
 
         return MccSimpleTrackingModelErrorCode::ERROR_OK;