Fixed various bugs found by the automated tests

src/turboPmacAxis.cpp

@@ -13,17 +13,9 @@
 #include <unistd.h>
 #include <vector>
 
-#include <sys/time.h>
-
-long long timeInMilliseconds(void) {
-    struct timeval tv;
-
-    gettimeofday(&tv, NULL);
-    return (((long long)tv.tv_sec) * 1000) + (tv.tv_usec / 1000);
-}
-
 struct turboPmacAxisImpl {
     bool waitForHandshake;
+    bool waitForEnable;
     time_t timeAtHandshake;
     // The axis status is used when enabling / disabling the motor
     int axisStatus;
@@ -73,6 +65,7 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo,
 
     pTurboPmacA_ = std::make_unique<turboPmacAxisImpl>(
         (turboPmacAxisImpl){.waitForHandshake = false,
+                            .waitForEnable = false,
                             .timeAtHandshake = 0,
                             .axisStatus = 0,
                             .needInit = false});
@@ -262,14 +255,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
     double lowLimit = 0.0;
     double limitsOffset = 0.0;
 
-    // Was the axis idle during the previous poll?
-    int previousStatusDone = 1;
-
-    if (axisNo() == 1) {
-        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "START POLL: at %lli\n",
-                  timeInMilliseconds());
-    }
-
     // =========================================================================
 
     if (pTurboPmacA_->needInit) {
@@ -346,8 +331,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
         return status;
     }
 
-    getAxisParamChecked(this, motorStatusDone, &previousStatusDone);
-
     // Query the axis status
     snprintf(command, sizeof(command),
              "P%2.2d00 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_,
@@ -383,18 +366,11 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
     // Store the axis status
     pTurboPmacA_->axisStatus = axStatus;
 
-    // if (axStatus == -3 || axStatus == -5) {
-    if (axisNo() == 1) {
-        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
-                  "POLL: Axis status %i at %lli\n", axStatus,
-                  timeInMilliseconds());
-    }
-
-    //}
-
     // Update the enablement PV
+    if (!(pTurboPmacA_->waitForEnable)) {
         setAxisParamChecked(this, motorEnableRBV,
                             (axStatus != -3 && axStatus != -5));
+    }
 
     // Create the unique callsite identifier manually so it can be used later in
     // the shouldBePrinted calls.
@@ -762,9 +738,6 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
                                  double minVelocity, double maxVelocity,
                                  double acceleration) {
 
-    asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
-              "Received move command at %lli\n", timeInMilliseconds());
-
     // Status of read-write-operations of ASCII commands to the controller
     asynStatus status = asynSuccess;
 
@@ -859,7 +832,6 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
 
 asynStatus turboPmacAxis::stop(double acceleration) {
 
-    // Status of read-write-operations of ASCII commands to the controller
     asynStatus status = asynSuccess;
 
     char command[pC_->MAXBUF_] = {0};
@@ -883,6 +855,19 @@ asynStatus turboPmacAxis::stop(double acceleration) {
     // if we're currently inside it.
     pTurboPmacA_->waitForHandshake = false;
 
+    /*
+    Stopping the motor results in a movement and further move commands have to
+    wait until the stopping movement is done. Therefore, we need to wait until
+    the poller "sees" the changed state (otherwise, we risk issuing move
+    commands while the motor is stopping). To ensure that at least one poll is
+    done, this thread (which also runs move commands) is paused for twice the
+    idle poll period.
+    */
+    pC_->forcedFastPolls();
+    unsigned int idlePollMicros =
+        (unsigned int)ceil(pC_->idlePollPeriod() * 1e6);
+    usleep(2 * idlePollMicros);
+
     return status;
 }
 
@@ -1072,7 +1057,7 @@ asynStatus turboPmacAxis::rereadEncoder() {
 
 asynStatus turboPmacAxis::enable(bool on) {
 
-    int timeout_enable_disable = 2;
+    int timeout_enable_disable = 5;
     char command[pC_->MAXBUF_] = {0};
     char response[pC_->MAXBUF_] = {0};
     int nvals = 0;
@@ -1082,6 +1067,8 @@ asynStatus turboPmacAxis::enable(bool on) {
 
     // =========================================================================
 
+    pTurboPmacA_->waitForEnable = true;
+
     /*
     Continue regardless of the status returned by the poll; we just want to
     find out whether the motor is currently moving or not. If the poll
@@ -1100,13 +1087,16 @@ asynStatus turboPmacAxis::enable(bool on) {
         axStatus == 5 || axStatus == 6 || axStatus == 7 || axStatus == 8 ||
         axStatus == 9 || axStatus == 10 || axStatus == 11 || axStatus == 12 ||
         axStatus == 13 || axStatus == 15 || axStatus == 16) {
-        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+
+        asynPrint(
+            pC_->pasynUser(), ASYN_TRACE_ERROR,
             "Controller \"%s\", axis %d  => %s, line %d\nAxis is not "
-                  "idle and can therefore not be enabled / disabled.\n",
+            "idle (status %d) and can therefore not be enabled / disabled.\n",
-                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+            pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axStatus);
 
         setAxisParamChecked(this, motorMessageText,
                             "Axis cannot be disabled while it is moving.");
+        pTurboPmacA_->waitForEnable = false;
         return asynError;
     }
 
@@ -1118,6 +1108,7 @@ asynStatus turboPmacAxis::enable(bool on) {
             "Controller \"%s\", axis %d  => %s, line %d\nAxis is already %s.\n",
             pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
             on ? "enabled" : "disabled");
+        pTurboPmacA_->waitForEnable = false;
         return asynSuccess;
     }
 
@@ -1125,14 +1116,12 @@ asynStatus turboPmacAxis::enable(bool on) {
     if (on != 0) {
         status = rereadEncoder();
         if (status != asynSuccess) {
+            pTurboPmacA_->waitForEnable = false;
             return status;
         }
     }
 
-    asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Sending enable %i at %lli\n",
+    // Enable / disable the axis
-              on, timeInMilliseconds());
-
-    // Enable / disable the axis if it is not moving
     snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on);
     asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
               "Controller \"%s\", axis %d  => %s, line %d\n%s axis\n",
@@ -1150,10 +1139,13 @@ asynStatus turboPmacAxis::enable(bool on) {
     int startTime = time(NULL);
     while (time(NULL) < startTime + timeout_enable_disable) {
 
+        // Wait a bit between status updates from the controller.
+        usleep(10000);
+
         // Read the axis status
-        // usleep(100000);
         status = pC_->writeRead(axisNo_, command, response, 1);
         if (status != asynSuccess) {
+            pTurboPmacA_->waitForEnable = false;
             return status;
         }
         nvals = sscanf(response, "%d", &pTurboPmacA_->axisStatus);
@@ -1162,21 +1154,13 @@ asynStatus turboPmacAxis::enable(bool on) {
                                               __PRETTY_FUNCTION__, __LINE__);
         }
 
-        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
-                  "Axis status %i at %lli\n", pTurboPmacA_->axisStatus,
-                  timeInMilliseconds());
-
         if ((pTurboPmacA_->axisStatus != -3) == on) {
+            usleep(500000);
+
             bool moving = false;
             forcedPoll(&moving);
 
-            int enabled = 0;
+            pTurboPmacA_->waitForEnable = false;
-            getAxisParamChecked(this, motorEnableRBV, &enabled);
-
-            asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
-                      "Finished enable %i at %lli, value motorEnableRBV = %i\n",
-                      on, timeInMilliseconds(), enabled);
-
             return asynSuccess;
         }
     }
@@ -1193,6 +1177,7 @@ asynStatus turboPmacAxis::enable(bool on) {
     snprintf(command, sizeof(command), "Failed to %s within %d seconds",
              on ? "enable" : "disable", timeout_enable_disable);
     setAxisParamChecked(this, motorMessageText, command);
+    pTurboPmacA_->waitForEnable = false;
     return asynError;
 }
 

src/turboPmacController.cpp

@@ -254,6 +254,10 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command,
         pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_,
         pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, &eomReason);
 
+    // if (strchr(command, '=')) {
+    //     usleep(20000);
+    // }
+
     msgPrintControlKey comKey =
         msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);