diff --git a/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp b/Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp index dc6bd731c3174b9c65bb30d1ee6c6852b351d5e8..7c66a5da9276fe393371daba0eb3b7441b33a92e 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 81a40e2153c17796536ca048ba3c9c8ad1c49a24..a4f79bb373ee5b8511b414c56dc495c774185b0e 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 db6d6393aa5fdc4ed30e40d6373037d3050b8557..b8d033d4f21e4f33722e3726241da64a75d28a41 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 2ecce1a280e53ab07e07139443433ed9f71014f3..98353f0d0b54a61f1d86ac243c5ffd5ab844a41e 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();