Updated sinqMotor to fix bug in forcedPoll regarding watchdog

Fixed a bug which caused the axes not to move in parallel, but after
each other.

src/detectorTowerAngleAxis.cpp

@@ -34,17 +34,16 @@ static void deferredMovementCollectorLoop(void *drvPvt) {
     detectorTowerAngleAxis *axis = (detectorTowerAngleAxis *)drvPvt;
     while (1) {
         if (axis->receivedTarget_) {
-            // Wait for 100 ms and then start the movement with the information
+            // Wait for the time span defined in deferredMovementWait_ and then
-            // available
+            // start the movement with the information available
-            axis->startingDeferredMovement_ = true;
             epicsThreadSleep(axis->deferredMovementWait_);
             axis->startCombinedMove();
 
             // After the movement command has been send, reset the flag
             axis->receivedTarget_ = false;
         }
-        // Limit this loop to an idle frequency of 1 kHz
+        // Limit this loop to an idle frequency of 10 kHz
-        epicsThreadSleep(0.001);
+        epicsThreadSleep(0.01);
     }
 }
 
@@ -76,11 +75,10 @@ detectorTowerAngleAxis::detectorTowerAngleAxis(detectorTowerController *pC,
     }
 
     // Initialize all member variables
-    // Assumed to be not ready by default, this is overwritten in the next poll
     error_ = 0;
+    waitingForStart_ = false;
     receivedTarget_ = false;
-    startingDeferredMovement_ = false;
+    deferredMovementWait_ = 0.5; // seconds
-    deferredMovementWait_ = 0.1; // seconds
 
     // Will be populated in the init() method
     beamRadius_ = 0.0;
@@ -199,7 +197,7 @@ asynStatus detectorTowerAngleAxis::doMove(double position, int relative,
     // started to the defined target position.
     setTargetPosition(position * motorRecResolution);
     receivedTarget_ = true;
-
+    waitingForStart_ = true;
     return asynSuccess;
 }
 
@@ -237,7 +235,6 @@ asynStatus detectorTowerAngleAxis::startCombinedMove() {
         setAxisParamChecked(this, motorStatusProblem, true);
 
         callParamCallbacks();
-        startingDeferredMovement_ = false;
         return asynError;
     }
 
@@ -293,9 +290,9 @@ asynStatus detectorTowerAngleAxis::stop(double acceleration) {
     }
 
     // Reset the deferred movement flags
-    startingDeferredMovement_ = false;
+    receivedTarget_ = false;
-    deferredMovementWait_ = false;
+    waitingForStart_ = false;
-
+    liftAxis()->waitingForStart_ = false;
     return status;
 }
 
@@ -422,8 +419,9 @@ asynStatus detectorTowerAngleAxis::doReset() {
     getAxisParamChecked(this, positionStateRBV, &positionState);
 
     // Reset the deferred movement flags
-    startingDeferredMovement_ = false;
+    receivedTarget_ = false;
-    deferredMovementWait_ = false;
+    waitingForStart_ = false;
+    liftAxis()->waitingForStart_ = false;
 
     /*
     Check which action should be performed:
@@ -552,11 +550,12 @@ static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) {
 // =============================================================================
 
 /**
- * @brief Set the setDeferredMovementWait (FFI implementation)
+ * @brief Set the setDeferredMovementWait time (FFI implementation)
  *
  * @param portName                Name of the controller
  * @param axisNo                  Axis number
- * @param scaleMovTimeout         Scaling factor (in seconds)
+ * @param deferredMovementWait    Time (in seconds) the driver waits to collect
+ * all movement commands from the different axes
  * @return asynStatus
  */
 asynStatus setDeferredMovementWait(const char *portName, int axisNo,

src/detectorTowerAngleAxis.h

@@ -136,15 +136,15 @@ class HIDDEN detectorTowerAngleAxis : public turboPmacAxis {
      */
     detectorTowerSupportAxis *supportAxis() { return supportAxis_; }
 
+    /**
+     * @brief Return a pointer to the axis controller
+     */
+    virtual detectorTowerController *pController() override { return pC_; };
+
     // If true, either this axis or one of the detectorTowerLiftAxis
     // attached to it received a movement command.
     bool receivedTarget_;
 
-    // If set to true, the virtual axis is about to start a deferred movement
-    // (but is currently still collecting movement commands from its component
-    // axes)
-    bool startingDeferredMovement_;
-
     /*
     The collector cycle waits until this time in seconds has passed before
     starting a deferred movement
@@ -157,10 +157,9 @@ class HIDDEN detectorTowerAngleAxis : public turboPmacAxis {
      */
     int error_;
 
-    /**
+    // True, if this axis has received a move command and the driver is
-     * @brief Return a pointer to the axis controller
+    // currently starting a deferred movement.
-     */
+    bool waitingForStart_;
-    virtual detectorTowerController *pController() override { return pC_; };
 
   protected:
     detectorTowerController *pC_;

src/detectorTowerController.cpp

@@ -418,15 +418,14 @@ asynStatus detectorTowerController::pollDetectorAxes(
 
     // Check if the tower is moving
     if (notInPosition == 1 || positionState > 2) {
-        // By now, the controller has actually started the movement
-        angleAxis->startingDeferredMovement_ = false;
         *moving = true;
+
+        // Since the tower is now moving, the axis by definition do not wait for
+        // movement start anymore.
+        liftAxis->waitingForStart_ = false;
+        angleAxis->waitingForStart_ = false;
     } else {
-        if (angleAxis->startingDeferredMovement_) {
+        *moving = false;
-            *moving = true;
-        } else {
-            *moving = false;
-        }
     }
 
     // Calculate the lift position /w consideration of the angle
@@ -809,14 +808,26 @@ asynStatus detectorTowerController::pollDetectorAxes(
     setAxisParamChecked(liftAxis, motorEnableRBV, true);
     setAxisParamChecked(supportAxis, motorEnableRBV, true);
 
-    // Are the axes currently moving?
+    // If the axis is starting a movement, mark it as moving. Otherwise use the
-    setAxisParamChecked(angleAxis, motorStatusMoving, *moving);
+    // moving parameter.
-    setAxisParamChecked(liftAxis, motorStatusMoving, *moving);
+    if (angleAxis->waitingForStart_) {
-    setAxisParamChecked(supportAxis, motorStatusMoving, *moving);
+        setAxisParamChecked(angleAxis, motorStatusMoving, true);
+        setAxisParamChecked(angleAxis, motorStatusDone, false);
+    } else {
+        setAxisParamChecked(angleAxis, motorStatusMoving, *moving);
+        setAxisParamChecked(angleAxis, motorStatusDone, !(*moving));
+    }
 
-    // Is the axis movement done?
+    if (liftAxis->waitingForStart_) {
-    setAxisParamChecked(angleAxis, motorStatusDone, !(*moving));
+        setAxisParamChecked(liftAxis, motorStatusMoving, true);
-    setAxisParamChecked(liftAxis, motorStatusDone, !(*moving));
+        setAxisParamChecked(liftAxis, motorStatusDone, false);
+    } else {
+        setAxisParamChecked(liftAxis, motorStatusMoving, *moving);
+        setAxisParamChecked(liftAxis, motorStatusDone, !(*moving));
+    }
+
+    // Support axis status is solely deduced from system movement state
+    setAxisParamChecked(supportAxis, motorStatusMoving, *moving);
     setAxisParamChecked(supportAxis, motorStatusDone, !(*moving));
 
     // In which direction are the axes currently moving?

src/detectorTowerLiftAxis.cpp

@@ -70,6 +70,9 @@ detectorTowerLiftAxis::detectorTowerLiftAxis(detectorTowerController *pC,
 
     angleAxis_ = angleAxis;
     angleAxis->liftAxis_ = this;
+
+    // Initialize the flag to false
+   waitingForStart_ = false;
 }
 
 detectorTowerLiftAxis::~detectorTowerLiftAxis(void) {
@@ -157,7 +160,6 @@ asynStatus detectorTowerLiftAxis::doMove(double position, int relative,
                                          double min_velocity,
                                          double max_velocity,
                                          double acceleration) {
-
     double motorRecResolution = 0.0;
     getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
 
@@ -166,7 +168,7 @@ asynStatus detectorTowerLiftAxis::doMove(double position, int relative,
     // defined target position.
     setTargetPosition(position * motorRecResolution);
     angleAxis_->receivedTarget_ = true;
-
+    waitingForStart_ = true;
     return asynSuccess;
 }
 
@@ -222,4 +224,4 @@ asynStatus detectorTowerLiftAxis::adjustOrigin(double newOrigin) {
     }
 
     return status;
-}
+}

src/detectorTowerLiftAxis.h

@@ -110,6 +110,10 @@ class HIDDEN detectorTowerLiftAxis : public turboPmacAxis {
      */
     virtual detectorTowerController *pController() override { return pC_; };
 
+    // True, if this axis has received a move command and the driver is
+    // currently starting a deferred movement.
+    bool waitingForStart_;
+
   protected:
     detectorTowerController *pC_;
     detectorTowerAngleAxis *angleAxis_;