From a57427833c258b585f3725e3ff2bd614e5a89d9a Mon Sep 17 00:00:00 2001
From: Giuseppe Carboni <giuseppecarboni89@live.com>
Date: Mon, 28 Apr 2025 13:47:05 +0200
Subject: [PATCH] Fix #931, SRT MS axes positions are now comprehend the offset
(#932)
* Fix #931, SRT MS axes positions are now plain
The axes positions ending up in the fits file now are comprehensive of their user and system offsets, like it used to be in the old implementation.
* Fix #931, updated interfaces as well
---
SRT/CDB/alma/MINORSERVO/GFR/GFR.xml | 2 +-
SRT/CDB/alma/MINORSERVO/M3R/M3R.xml | 2 +-
SRT/CDB/alma/MINORSERVO/SRP/SRP.xml | 2 +-
.../CDB/alma/MINORSERVO/GFR/GFR.xml | 2 +-
.../CDB/alma/MINORSERVO/M3R/M3R.xml | 2 +-
.../CDB/alma/MINORSERVO/SRP/SRP.xml | 2 +-
.../idl/SRTMinorServo.idl | 3 +-
SRT/Outdated/CDB/alma/MINORSERVO/PFP/PFP.xml | 2 +-
.../config/CDB/schemas/SRTMinorServo.xsd | 2 +-
.../SRTMinorServo/include/SRTMinorServoImpl.h | 18 ++++----
.../src/SRTBaseMinorServoImpl.cpp | 43 +++++++++++--------
.../src/SRTProgramTrackMinorServoImpl.cpp | 16 ++++---
12 files changed, 54 insertions(+), 42 deletions(-)
diff --git a/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml b/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml
index f34f6a21c..4e5b13294 100644
--- a/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml
+++ b/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml
@@ -27,7 +27,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml b/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml
index 8dfa3aa1c..15d5e2db4 100644
--- a/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml
+++ b/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml
@@ -27,7 +27,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml b/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml
index d783896b4..53e1c5438 100644
--- a/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml
+++ b/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml
@@ -28,7 +28,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml b/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml
index f34f6a21c..4e5b13294 100644
--- a/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml
+++ b/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml
@@ -27,7 +27,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml b/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml
index 8dfa3aa1c..15d5e2db4 100644
--- a/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml
+++ b/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml
@@ -27,7 +27,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml b/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml
index d783896b4..53e1c5438 100644
--- a/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml
+++ b/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml
@@ -28,7 +28,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl
index 6cd48ffbe..1d6018ae4 100644
--- a/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl
+++ b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl
@@ -31,9 +31,8 @@ module MinorServo
/**
* This property returns the latest virtual positions commanded with a preset command.
- * Offsets are not taken into account since they are added by the LDO system.
*/
- readonly attribute ACS::ROdoubleSeq commanded_virtual_positions;
+ readonly attribute ACS::ROdoubleSeq commanded_plain_virtual_positions;
/**
* This property indicates whether the servo is in use in the current configuration
diff --git a/SRT/Outdated/CDB/alma/MINORSERVO/PFP/PFP.xml b/SRT/Outdated/CDB/alma/MINORSERVO/PFP/PFP.xml
index 5503ee2fd..067ee1b18 100644
--- a/SRT/Outdated/CDB/alma/MINORSERVO/PFP/PFP.xml
+++ b/SRT/Outdated/CDB/alma/MINORSERVO/PFP/PFP.xml
@@ -28,7 +28,7 @@
<virtual_offsets description="Sequence, sum of user and system virtual axis offsets" />
<virtual_user_offsets description="Sequence, virtual user axes offsets" />
<virtual_system_offsets description="Sequence, virtual system axes offsets" />
- <commanded_virtual_positions description="Sequence, latest commanded (via preset) virtual positions" />
+ <commanded_plain_virtual_positions description="Sequence, latest commanded plain virtual positions" />
<in_use description="Boolean indicating whether the servo is in use in the current configuration" />
<current_setup description="The name of the current configuration" />
<error_code description="The type of error encountered" />
diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd
index 34ec43182..6ac34e217 100644
--- a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd
+++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd
@@ -51,7 +51,7 @@
<xs:sequence>
<xs:element name="virtual_user_offsets" type="baci:ROdoubleSeq" />
<xs:element name="virtual_system_offsets" type="baci:ROdoubleSeq" />
- <xs:element name="commanded_virtual_positions" type="baci:ROdoubleSeq" />
+ <xs:element name="commanded_plain_virtual_positions" type="baci:ROdoubleSeq" />
<xs:element name="in_use" type="mng:BooleanType" />
<xs:element name="current_setup" type="baci:ROstring" />
<xs:element name="error_code" type="ms:SRTMinorServoErrorType" />
diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h
index a664462c0..4304d05fb 100644
--- a/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h
+++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h
@@ -283,10 +283,10 @@ public:
virtual ACS::ROdoubleSeq_ptr virtual_system_offsets();
/**
- * Returns a reference to the commanded_virtual_positions property implementation of the IDL interface.
- * @return pointer to the read-only double sequence property commanded_virtual_positions.
+ * Returns a reference to the commanded_plain_virtual_positions property implementation of the IDL interface.
+ * @return pointer to the read-only double sequence property commanded_plain_virtual_positions.
*/
- virtual ACS::ROdoubleSeq_ptr commanded_virtual_positions();
+ virtual ACS::ROdoubleSeq_ptr commanded_plain_virtual_positions();
/**
* Returns a reference to the in_use property implementation of the IDL interface.
@@ -388,12 +388,12 @@ protected:
std::vector<double> m_system_offsets;
/**
- * Latest commanded virtual positions. It only takes into account the preset command.
+ * Latest commanded virtual positions.
*/
- std::vector<double> m_commanded_virtual_positions;
+ std::vector<double> m_commanded_plain_virtual_positions;
/**
- * Queue of positions assumed by the servo system in time.
+ * Queue of positions assumed by the servo system in time, they comprehend the LDO offsets.
*/
SRTMinorServoPositionsQueue m_positions_queue;
@@ -503,9 +503,9 @@ private:
baci::SmartPropertyPointer<baci::ROdoubleSeq> m_virtual_system_offsets_ptr;
/**
- * Pointer to the commanded_virtual_positions property.
+ * Pointer to the commanded_plain_virtual_positions property.
*/
- baci::SmartPropertyPointer<baci::ROdoubleSeq> m_commanded_virtual_positions_ptr;
+ baci::SmartPropertyPointer<baci::ROdoubleSeq> m_commanded_plain_virtual_positions_ptr;
/**
* Pointer to the in_use property.
@@ -584,7 +584,7 @@ protected:
virtual ACS::ROdoubleSeq_ptr virtual_offsets() { return SRTBaseMinorServoImpl::virtual_offsets(); }\
virtual ACS::ROdoubleSeq_ptr virtual_user_offsets() { return SRTBaseMinorServoImpl::virtual_user_offsets(); }\
virtual ACS::ROdoubleSeq_ptr virtual_system_offsets() { return SRTBaseMinorServoImpl::virtual_system_offsets(); }\
- virtual ACS::ROdoubleSeq_ptr commanded_virtual_positions() { return SRTBaseMinorServoImpl::commanded_virtual_positions(); }\
+ virtual ACS::ROdoubleSeq_ptr commanded_plain_virtual_positions() { return SRTBaseMinorServoImpl::commanded_plain_virtual_positions(); }\
virtual Management::ROTBoolean_ptr in_use() { return SRTBaseMinorServoImpl::in_use(); }\
virtual ACS::ROstring_ptr current_setup() { return SRTBaseMinorServoImpl::current_setup(); }\
virtual ROSRTMinorServoError_ptr error_code() { return SRTBaseMinorServoImpl::error_code(); }
diff --git a/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp
index ce3eca5c2..2e384ca0c 100644
--- a/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp
+++ b/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp
@@ -20,7 +20,7 @@ SRTBaseMinorServoImpl::SRTBaseMinorServoImpl(const ACE_CString& component_name,
m_error_code(ERROR_NO_ERROR),
m_user_offsets(m_virtual_axes, 0.0),
m_system_offsets(m_virtual_axes, 0.0),
- m_commanded_virtual_positions(m_virtual_axes, 0.0),
+ m_commanded_plain_virtual_positions(m_virtual_axes, 0.0),
m_positions_queue(5 * 60 * int(1 / getCDBValue<double>(container_services, "status_thread_period", "/MINORSERVO/Boss")), m_virtual_axes),
m_min(SRTBaseMinorServoImpl::getMotionConstant(*this, "min_range")),
m_max(SRTBaseMinorServoImpl::getMotionConstant(*this, "max_range")),
@@ -43,7 +43,7 @@ SRTBaseMinorServoImpl::SRTBaseMinorServoImpl(const ACE_CString& component_name,
m_virtual_offsets_ptr(this),
m_virtual_user_offsets_ptr(this),
m_virtual_system_offsets_ptr(this),
- m_commanded_virtual_positions_ptr(this),
+ m_commanded_plain_virtual_positions_ptr(this),
m_in_use_ptr(this),
m_current_setup_ptr(this),
m_error_code_ptr(this),
@@ -89,8 +89,8 @@ void SRTBaseMinorServoImpl::initialize()
new MSGenericDevIO<ACS::doubleSeq, std::vector<double>>(m_user_offsets), true);
m_virtual_system_offsets_ptr = new baci::ROdoubleSeq((m_component_name + ":virtual_system_offsets").c_str(), getComponent(),
new MSGenericDevIO<ACS::doubleSeq, std::vector<double>>(m_system_offsets), true);
- m_commanded_virtual_positions_ptr = new baci::ROdoubleSeq((m_component_name + ":commanded_virtual_positions").c_str(), getComponent(),
- new MSGenericDevIO<ACS::doubleSeq, std::vector<double>>(m_commanded_virtual_positions), true);
+ m_commanded_plain_virtual_positions_ptr = new baci::ROdoubleSeq((m_component_name + ":commanded_plain_virtual_positions").c_str(), getComponent(),
+ new MSGenericDevIO<ACS::doubleSeq, std::vector<double>>(m_commanded_plain_virtual_positions), true);
m_in_use_ptr = new ROEnumImpl<ACS_ENUM_T(Management::TBoolean), POA_Management::ROTBoolean>((m_component_name + ":in_use").c_str(), getComponent(),
new MSGenericDevIO<Management::TBoolean, std::atomic<Management::TBoolean>>(m_in_use), true);
m_current_setup_ptr = new baci::ROstring((m_component_name + ":current_setup").c_str(), getComponent(),
@@ -142,7 +142,7 @@ bool SRTBaseMinorServoImpl::status()
{
m_socket.sendCommand(SRTMinorServoCommandLibrary::status(m_servo_name), m_status);
- ACS::doubleSeq current_point = m_status.getVirtualPositions();
+ ACS::doubleSeq current_point = m_status.getPlainVirtualPositions();
// Calculate the current speed of the axes
try
@@ -215,8 +215,8 @@ void SRTBaseMinorServoImpl::preset(const ACS::doubleSeq& virtual_coords)
if(virtual_coordinates.length() == 0)
{
// It means we want to command the latest coordinates again, to apply the offset in the LDO servo system
- virtual_coordinates.length(m_commanded_virtual_positions.size());
- std::copy(m_commanded_virtual_positions.begin(), m_commanded_virtual_positions.end(), virtual_coordinates.begin());
+ virtual_coordinates.length(m_commanded_plain_virtual_positions.size());
+ std::copy(m_commanded_plain_virtual_positions.begin(), m_commanded_plain_virtual_positions.end(), virtual_coordinates.begin());
}
if(virtual_coordinates.length() != m_virtual_axes)
@@ -251,7 +251,7 @@ void SRTBaseMinorServoImpl::preset(const ACS::doubleSeq& virtual_coords)
throw ex.getMinorServoErrorsEx();
}
- std::copy(coordinates.begin(), coordinates.end(), m_commanded_virtual_positions.begin());
+ std::copy(coordinates.begin(), coordinates.end(), m_commanded_plain_virtual_positions.begin());
}
bool SRTBaseMinorServoImpl::setup(const char* configuration_name, CORBA::Boolean as_off)
@@ -353,7 +353,7 @@ bool SRTBaseMinorServoImpl::setup(const char* configuration_name, CORBA::Boolean
// The positions tables inside the Leonardo minor servo systems are calculated with an elevation of 45 degrees.
// We need to be sure the values are correct otherwise there will be a discrepancy.
ACS::doubleSeq commanded_coordinates = *calcCoordinates(45);
- std::copy(commanded_coordinates.begin(), commanded_coordinates.end(), m_commanded_virtual_positions.begin());
+ std::copy(commanded_coordinates.begin(), commanded_coordinates.end(), m_commanded_plain_virtual_positions.begin());
return true;
}
else
@@ -641,10 +641,24 @@ ACS::TimeInterval SRTBaseMinorServoImpl::getTravelTime(const ACS::doubleSeq& _s_
ACS::doubleSeq s_p(_s_p);
- // No starting coordinates, it means we have to start from the current position taking into account the current speed
+ if(d_p.length() != m_virtual_axes)
+ {
+ _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "getTravelTime()").c_str());
+ ex.setReason("Wrong number of axes for destination_position.");
+ ex.log(LM_DEBUG);
+ throw ex.getMinorServoErrorsEx();
+ }
+
+ // No starting coordinates, it means we have to start from the current position taking into account the current speed.
if(_s_p.length() == 0)
{
s_p = *getAxesPositions(0);
+
+ // Subtract the offsets since the destination coordinates are pure virtual
+ for(size_t i = 0; i < m_virtual_axes; i++)
+ {
+ s_p[i] -= m_user_offsets[i] + m_system_offsets[i];
+ }
}
else if(_s_p.length() != m_virtual_axes)
{
@@ -653,13 +667,6 @@ ACS::TimeInterval SRTBaseMinorServoImpl::getTravelTime(const ACS::doubleSeq& _s_
ex.log(LM_DEBUG);
throw ex.getMinorServoErrorsEx();
}
- if(d_p.length() != m_virtual_axes)
- {
- _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "getTravelTime()").c_str());
- ex.setReason("Wrong number of axes for destination_position.");
- ex.log(LM_DEBUG);
- throw ex.getMinorServoErrorsEx();
- }
// d = delta/distance
std::vector<double> d(m_virtual_axes);
@@ -891,7 +898,7 @@ GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_positi
GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_offsets_ptr, virtual_offsets);
GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_user_offsets_ptr, virtual_user_offsets);
GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_system_offsets_ptr, virtual_system_offsets);
-GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_commanded_virtual_positions_ptr, commanded_virtual_positions);
+GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_commanded_plain_virtual_positions_ptr, commanded_plain_virtual_positions);
GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTBaseMinorServoImpl, m_in_use_ptr, in_use);
GET_PROPERTY_REFERENCE(ACS::ROstring, SRTBaseMinorServoImpl, m_current_setup_ptr, current_setup);
GET_PROPERTY_REFERENCE(ROSRTMinorServoError, SRTBaseMinorServoImpl, m_error_code_ptr, error_code);
diff --git a/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp
index 925881a89..ec4bfdada 100644
--- a/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp
+++ b/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp
@@ -55,7 +55,7 @@ bool SRTProgramTrackMinorServoImpl::status()
{
bool status = SRTBaseMinorServoImpl::status();
- ACS::doubleSeq virtual_positions = m_status.getVirtualPositions();
+ ACS::doubleSeq virtual_positions = m_status.getPlainVirtualPositions();
std::vector<double> commanded_positions;
SRTMinorServoOperativeMode operative_mode = m_status.getOperativeMode();
@@ -69,6 +69,7 @@ bool SRTProgramTrackMinorServoImpl::status()
// The tracking timestamp is interpolated instead
std::pair<ACS::Time, std::vector<double>> tracking_point = m_tracking_queue.get(last_timestamp);
commanded_positions = tracking_point.second;
+ m_commanded_plain_virtual_positions = commanded_positions;
m_remaining_trajectory_points.store(m_tracking_queue.getRemainingPoints(last_timestamp));
@@ -92,7 +93,11 @@ bool SRTProgramTrackMinorServoImpl::status()
}
else if(operative_mode == OPERATIVE_MODE_SETUP || operative_mode == OPERATIVE_MODE_PRESET)
{
- commanded_positions = m_commanded_virtual_positions;
+ commanded_positions = m_commanded_plain_virtual_positions;
+ for(size_t i = 0; i < m_virtual_axes; i++)
+ {
+ commanded_positions[i] += m_user_offsets[i] + m_system_offsets[i];
+ }
}
else
{
@@ -149,11 +154,12 @@ void SRTProgramTrackMinorServoImpl::programTrack(CORBA::Long trajectory_id, CORB
std::vector<double> coordinates(virtual_coordinates.get_buffer(), virtual_coordinates.get_buffer() + virtual_coordinates.length());
ACS::doubleSeq offsets = m_status.getVirtualOffsets();
+ std::vector<double> coordinates_and_offsets = coordinates;
for(size_t i = 0; i < m_virtual_axes; i++)
{
- double coordinate = coordinates[i] + offsets[i];
- if(coordinate < m_min[i] || coordinate > m_max[i])
+ coordinates_and_offsets[i] += offsets[i];
+ if(coordinates_and_offsets[i] < m_min[i] || coordinates_and_offsets[i] > m_max[i])
{
_EXCPT(MinorServoErrors::TrackingErrorExImpl, ex, (m_servo_name + "::programTrack()").c_str());
ex.addData("Reason", "Resulting position out of range, check the offsets!");
@@ -178,7 +184,7 @@ void SRTProgramTrackMinorServoImpl::programTrack(CORBA::Long trajectory_id, CORB
// Clear the tracking queue to avoid interpolation between 2 different trajectories
m_tracking_queue.clear();
}
- m_tracking_queue.put(point_time, coordinates);
+ m_tracking_queue.put(point_time, coordinates_and_offsets);
}
--
GitLab