#588 enable elevation tracking when servo is ready

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;
 

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

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 {

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();