From 653081a9fd2b613868750232a33a20c2cfd86591 Mon Sep 17 00:00:00 2001
From: Fabio Vitello <fvitello@users.noreply.github.com>
Date: Thu, 21 Oct 2021 17:02:20 +0200
Subject: [PATCH] #588 enable elevation tracking when servo is ready

---
 .../MedicinaMinorServo/include/MSBossTracker.hpp     |  1 +
 .../MedicinaMinorServo/include/SetupThread.hpp       |  1 +
 .../Servers/MedicinaMinorServo/src/MSBossTracker.cpp | 12 ++++++++++++
 .../MedicinaMinorServo/src/MinorServoBossImpl.cpp    | 11 +++++++----
 4 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp b/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp
index dc6bd731c..7c66a5da9 100644
--- a/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp
@@ -31,6 +31,7 @@
 
 // Sleep Time in 100ns
 #define TRACKER_SLEEP_TIME 1000000 // 100 mseconds
+#define READY_MAX_WAIT 3000000000 // 5 minutes
 
 using namespace std;
 
diff --git a/Medicina/Servers/MedicinaMinorServo/include/SetupThread.hpp b/Medicina/Servers/MedicinaMinorServo/include/SetupThread.hpp
index 81a40e215..a4f79bb37 100644
--- a/Medicina/Servers/MedicinaMinorServo/include/SetupThread.hpp
+++ b/Medicina/Servers/MedicinaMinorServo/include/SetupThread.hpp
@@ -17,6 +17,7 @@ class SetupThreadParameters
         virtual ~SetupThreadParameters();
         MedMinorServoControl_sp m_control;
         MedMinorServoStatus *m_status;
+
 };
 
 class SetupThread : public ACS::Thread
diff --git a/Medicina/Servers/MedicinaMinorServo/src/MSBossTracker.cpp b/Medicina/Servers/MedicinaMinorServo/src/MSBossTracker.cpp
index db6d6393a..b8d033d4f 100644
--- a/Medicina/Servers/MedicinaMinorServo/src/MSBossTracker.cpp
+++ b/Medicina/Servers/MedicinaMinorServo/src/MSBossTracker.cpp
@@ -56,6 +56,18 @@ void MSBossTracker::runLoop()
     double elevation = 45.0;
     double azimuth;
     MedMinorServoPosition offset_position, correct_position;
+
+    IRA::CTimer timer;
+    bool tracking = false;
+    while(!(m_status->ready))
+    {
+        if(timer.elapsed() > READY_MAX_WAIT)
+        {
+            throw ServoTimeoutError("Timeout wating for Minor Servo to became ready");
+        }
+
+    }
+
     if(m_status->elevation_tracking)
     {
         try {
diff --git a/Medicina/Servers/MedicinaMinorServo/src/MinorServoBossImpl.cpp b/Medicina/Servers/MedicinaMinorServo/src/MinorServoBossImpl.cpp
index 2ecce1a28..98353f0d0 100644
--- a/Medicina/Servers/MedicinaMinorServo/src/MinorServoBossImpl.cpp
+++ b/Medicina/Servers/MedicinaMinorServo/src/MinorServoBossImpl.cpp
@@ -404,6 +404,9 @@ throw (MinorServoErrors::SetupErrorExImpl)
         CUSTOM_LOG(LM_FULL_INFO, "MinorServo::MinorServoBossImpl::setupImpl",
                    (LM_DEBUG, "Started setup positioning thread"));
         
+        if(isElevationTrackingEn())
+            turnTrackingOn();
+
     }catch(const ServoTimeoutError& ste){
         THROW_EX(MinorServoErrors, SetupErrorEx, ste.what(), false);
     }catch(const ServoConnectionError& sce){
@@ -840,12 +843,12 @@ throw (MinorServoErrors::MinorServoErrorsEx)
     if(!(m_control))
         THROW_MINORSERVO_EX(CommunicationErrorEx, 
                  "Minor Servo Server is not connected", false);
-    if(isStarting())
-        THROW_MINORSERVO_EX(TrackingErrorEx, "turnTrackingOn: the system is starting.", true);
+    //if(isStarting())
+    //    THROW_MINORSERVO_EX(TrackingErrorEx, "turnTrackingOn: the system is starting.", true);
     if(isParking())
         THROW_MINORSERVO_EX(TrackingErrorEx, "turnTrackingOn: the system is parking.", true);
-    if(!isReady())
-        THROW_MINORSERVO_EX(TrackingErrorEx, "turnTrackingOn: the system is not ready.", true);
+    //if(!isReady())
+    //    THROW_MINORSERVO_EX(TrackingErrorEx, "turnTrackingOn: the system is not ready.", true);
 
     if(m_tracking_thread_ptr != NULL) {
         m_tracking_thread_ptr->suspend();
-- 
GitLab