Updates for SRTMinorServos (d51e7785) · Commits · INAF / DISCOS

SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl

+6 −0

Original line number	Diff line number	Diff line
		@@ -178,6 +178,12 @@ module MinorServo
		*/
		void clearSystemOffsets() raises (MinorServoErrors::MinorServoErrorsEx);

		/**
		* This method resets the Leonardo offsets to the sum of the DISCOS user and system offsets.
		* @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted
		*/
		void reloadOffsets() raises (MinorServoErrors::MinorServoErrorsEx);

		/**
		* This method returns in the two parameters passed as reference, the names and the units of measure of the axes of the servo, respectively
		* @param axes_names the sequence of strings containing the names of the virtual axes of the servo

SRT/Servers/SRTMinorServo/include/SRTMinorServoBossCore.h

+6 −0

Original line number	Diff line number	Diff line
		@@ -382,6 +382,12 @@ private:
		*/
		std::atomic<Management::TBoolean> m_tracking;

		/**
		* This boolean will be set to true every time the socket connects.
		* When true it will trigger a procedure that will check if the minor servos offsets need to be reloaded because of a discrepancy between the DISCOS offsets (user + system) and the Leonardo offsets.
		*/
		bool m_reload_servo_offsets;

		/**
		* Configuration of the socket object.
		*/

SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h

+7 −7

Original line number	Diff line number	Diff line
		@@ -158,6 +158,12 @@ public:
		*/
		void clearSystemOffsets();

		/**
		* Reload the user and the system offsets to the minor servo when the Leonardo offsets do not correspond to the sum of the DISCOS user and system offsets.
		* @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted.
		*/
		void reloadOffsets();

		/**
		* Returns the name and the unit of each virtual axes of the servo system, as reference arguments.
		* @param axes_names_out a string sequence object containing the names of the virtual axes of the servo system.
		@@ -291,13 +297,6 @@ protected:
		static std::vector<double> getMotionConstant(SRTBaseMinorServoImpl& object, const std::string& constant);

		private:
		/**
		* Resets both the user and the system offsets to 0. This is needed since the Leonardo servo system only tracks a single set of offsets.
		* If we lose track of the offsets inside DISCOS (when restarting this component, for example), we need to reset the offsets in order to track them again.
		* @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted.
		*/
		void resetOffsets();

		/**
		* Static function used to retrieve a table from the CDB DataBlock directory. Used inside the initialization list.
		* @param object the instance of this class, used inside the function logic.
		@@ -513,6 +512,7 @@ protected:
		ACS::doubleSeq* getSystemOffsets() { return SRTBaseMinorServoImpl::getSystemOffsets(); }\
		void setSystemOffset(const char* axis_name, CORBA::Double offset) { SRTBaseMinorServoImpl::setSystemOffset(axis_name, offset); }\
		void clearSystemOffsets() { SRTBaseMinorServoImpl::clearSystemOffsets(); }\
		void reloadOffsets() { SRTBaseMinorServoImpl::reloadOffsets(); }\
		void getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out) { SRTBaseMinorServoImpl::getAxesInfo(axes_names_out, axes_units_out); }\
		ACS::doubleSeq* getAxesPositions(ACS::Time acs_time) { return SRTBaseMinorServoImpl::getAxesPositions(acs_time); }\
		long getTravelTime(const ACS::doubleSeq& start, const ACS::doubleSeq& dest) { return SRTBaseMinorServoImpl::getTravelTime(start, dest); }\

SRT/Servers/SRTMinorServo/include/SRTMinorServoStatusThread.h

+5 −0

Original line number	Diff line number	Diff line
		@@ -67,6 +67,11 @@ private:
		*/
		SRTMinorServoBossCore& m_core;

		/**
		* The current status of the finite-state machine.
		*/
		unsigned int m_status;

		/**
		* The sleeping time of the thread.
		* The thread should be cycling at a constant rate, therefore the inner sleeping time is always updated taking into account this and the thread execution time.

SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp

+29 −33

Original line number	Diff line number	Diff line
		@@ -129,10 +129,6 @@ bool SRTBaseMinorServoImpl::status()
		{
		m_socket.sendCommand(SRTMinorServoCommandLibrary::status(m_servo_name), m_status);

		// We need to call this here since we might just have established the connection, therefore we might not know how the Leonardo offsets are split between user and system
		// This might eventually throw MinorServoErrorsEx
		resetOffsets();

		ACSErr::Completion_var comp;
		ACS::doubleSeq current_point = *virtual_positions()->get_sync(comp.out());

		@@ -491,6 +487,35 @@ void SRTBaseMinorServoImpl::clearSystemOffsets()
		}
		}

		void SRTBaseMinorServoImpl::reloadOffsets()
		{
		AUTO_TRACE(m_servo_name + "::reloadOffsets()");

		// Sum the user and system DISCOS offsets to check whether they correspond to the Leonardo offsets
		std::vector<double> DISCOS_offsets(m_virtual_axes, 0.0);
		std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), DISCOS_offsets.begin(), std::plus<double>());

		// Read the Leonardo offsets
		ACSErr::Completion_var comp;
		ACS::doubleSeq sequence = *virtual_offsets()->get_sync(comp.out());
		std::vector<double> LEONARDO_offsets(sequence.get_buffer(), sequence.get_buffer() + sequence.length());

		// Check if the offsets correspond or not
		if(!std::equal(DISCOS_offsets.begin(), DISCOS_offsets.end(), LEONARDO_offsets.begin()))
		{
		// Offsets do not correspond, should reset them by sending a offset command
		if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, DISCOS_offsets)).checkOutput())
		{
		_EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::reloadOffsets()").c_str());
		ex.setReason("Received NAK in response to an OFFSET command.");
		ex.log(LM_DEBUG);
		throw ex.getMinorServoErrorsEx();
		}

		ACS_LOG(LM_FULL_INFO, m_servo_name + "::reloadOffsets()", (LM_INFO, "Offsets discrepancy, reload"));
		}
		}

		void SRTBaseMinorServoImpl::getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out)
		{
		AUTO_TRACE("SRTBaseMinorServoImpl::getAxesInfo()");
		@@ -703,35 +728,6 @@ std::vector<double> SRTBaseMinorServoImpl::getMotionConstant(SRTBaseMinorServoIm
		}

		/////////////////// PRIVATE methods
		void SRTBaseMinorServoImpl::resetOffsets()
		{
		AUTO_TRACE(m_servo_name + "::resetOffsets()");

		// Sum the user and system DISCOS offsets to check whether they correspond to the Leonardo offsets
		std::vector<double> DISCOS_offsets(m_virtual_axes, 0.0);
		std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), DISCOS_offsets.begin(), std::plus<double>());

		// Read the Leonardo offsets
		ACSErr::Completion_var comp;
		ACS::doubleSeq sequence = *virtual_offsets()->get_sync(comp.out());
		std::vector<double> LEONARDO_offsets(sequence.get_buffer(), sequence.get_buffer() + sequence.length());

		// Check if the offsets correspond or not
		if(!std::equal(DISCOS_offsets.begin(), DISCOS_offsets.end(), LEONARDO_offsets.begin()))
		{
		// Offsets do not correspond, should reset them by sending a offset command
		if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, DISCOS_offsets)).checkOutput())
		{
		_EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::resetOffsets()").c_str());
		ex.setReason("Received NAK in response to an OFFSET command.");
		ex.log(LM_DEBUG);
		throw ex.getMinorServoErrorsEx();
		}

		ACS_LOG(LM_FULL_INFO, m_servo_name + "::resetOffsets()", (LM_INFO, "Offsets discrepancy, reset"));
		}
		}

		std::vector<std::string> SRTBaseMinorServoImpl::getPropertiesTable(SRTBaseMinorServoImpl& object, const std::string& properties_name)
		{
		AUTO_STATIC_TRACE(object.m_servo_name + "::getPropertiesTable()");