Fix #887, commanded minor servo preset after offset (#894)

SRT/Servers/SRTMinorServo/src/SRTMinorServoBossCore.cpp

@@ -682,9 +682,11 @@ void SRTMinorServoBossCore::clearUserOffsets(std::string servo_name)
         for(const auto& [servo_name, servo] : m_current_servos)
         {
             servo->clearUserOffsets();
-            if(motion_status == MOTION_STATUS_CONFIGURED)
+            if(motion_status == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
             {
-                // We are not tracking but we need to update the servo position to load the offsets
+                // We get here in 2 different scenarios
+                // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+                // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
                 servo->preset(ACS::doubleSeq());
             }
         }
@@ -702,9 +704,11 @@ void SRTMinorServoBossCore::clearUserOffsets(std::string servo_name)
     {
         auto servo = m_current_servos.at(servo_name);
         servo->clearUserOffsets();
-        if(motion_status == MOTION_STATUS_CONFIGURED)
+        if(motion_status == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
         {
-            // We are not tracking but we need to update the servo position to load the offsets
+            // We get here in 2 different scenarios
+            // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+            // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
             servo->preset(ACS::doubleSeq());
         }
     }
@@ -764,9 +768,11 @@ void SRTMinorServoBossCore::setUserOffset(std::string servo_axis_name, double of
         }
         servo->setUserOffset(axis_name.c_str(), offset);
 
-        if(m_motion_status.load() == MOTION_STATUS_CONFIGURED)
+        if(m_motion_status.load() == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
         {
-            // We are not tracking but we need to update the servo position to load the offsets
+            // We get here in 2 different scenarios
+            // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+            // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
             servo->preset(ACS::doubleSeq());
         }
     }
@@ -835,9 +841,11 @@ void SRTMinorServoBossCore::clearSystemOffsets(std::string servo_name)
         for(const auto& [servo_name, servo] : m_current_servos)
         {
             servo->clearSystemOffsets();
-            if(motion_status == MOTION_STATUS_CONFIGURED)
+            if(motion_status == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
             {
-                // We are not tracking but we need to update the servo position to load the offsets
+                // We get here in 2 different scenarios
+                // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+                // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
                 servo->preset(ACS::doubleSeq());
             }
         }
@@ -855,9 +863,11 @@ void SRTMinorServoBossCore::clearSystemOffsets(std::string servo_name)
     {
         auto servo = m_current_servos.at(servo_name);
         servo->clearSystemOffsets();
-        if(motion_status == MOTION_STATUS_CONFIGURED)
+        if(motion_status == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
         {
-            // We are not tracking but we need to update the servo position to load the offsets
+            // We get here in 2 different scenarios
+            // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+            // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
             servo->preset(ACS::doubleSeq());
         }
     }
@@ -913,9 +923,11 @@ void SRTMinorServoBossCore::setSystemOffset(std::string servo_axis_name, double
         auto servo = m_current_servos.at(servo_name);
         servo->setSystemOffset(axis_name.c_str(), offset);
 
-        if(m_motion_status.load() == MOTION_STATUS_CONFIGURED)
+        if(m_motion_status.load() == MOTION_STATUS_CONFIGURED || m_tracking_servos.find(servo_name) == m_tracking_servos.end())
         {
-            // We are not tracking but we need to update the servo position to load the offsets
+            // We get here in 2 different scenarios
+            // 1) The system is configured and is not tracking the elevation, therefore we ALWAYS need to update the position of the servo we just set the offset
+            // 2) The servo we set the offset is not a program track servo, therefore we need to update its position manually
             servo->preset(ACS::doubleSeq());
         }
     }