Commit 653081a9 authored by Fabio Vitello's avatar Fabio Vitello
Browse files

#588 enable elevation tracking when servo is ready

parent ed85da4f

Medicina/Servers/MedicinaMinorServo/include/MSBossTracker.hpp

+1 −0
Original line number Diff line number Diff line
@@ -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

+1 −0
Original line number Diff line number Diff line
@@ -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

+12 −0
Original line number Diff line number Diff line
@@ -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

+7 −4
Original line number Diff line number Diff line
@@ -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();