Add PMAC V3

- limit switches: use PXX37, PXX38 insteam of MXX21, MXX22
- if the motor is running query for axis position (QXX10) and status (PXX00)
- else query also error status (PXX01) and limit switches (PXX37-8)
- make the requests in 1+1 transactions

sinqEPICSApp/src/pmacAxis.cpp

@@ -23,6 +23,11 @@
  * Modified to use the MsgTxt field for SINQ
  *
  * Mark Koennecke, January 2019
+ * 
+ * Add driver for V3 protocol
+ * 
+ * Michele Brambilla, February 2022
+ * 
  ********************************************/
 
 #include <stdlib.h>
@@ -363,6 +368,34 @@ static char *translateAxisError(int axErr)
   }
 }
 
+asynStatus pmacV3Axis::getAxisStatus(bool *moving)
+{
+  char command[pC_->PMAC_MAXBUF_];
+  char response[pC_->PMAC_MAXBUF_];
+  int cmdStatus = 0;
+  int done = 0, posChanging = 0;
+  double position = 0;
+  int nvals = 0;
+  int axisProblemFlag = 0;
+  epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
+  char message[132], *axMessage;
+
+  /* read our status items one by one: our PMAC does not seem to give multiple
+   * responses ..*/
+  sprintf(command, "P%2.2d01", axisNo_);
+  cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
+  nvals = sscanf(response, "%d", &axErr);
+  if (cmdStatus || nvals != 1) {
+    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "drvPmacAxisGetStatus: Failed to read axis Error  Status: "
+              "%d\nCommand :%s\nResponse:%s\n",
+              cmdStatus, command, response);
+    updateMsgTxtFromDriver("Cannot read Axis Error Status");
+  }
+}
+
+
+
 asynStatus pmacAxis::getAxisStatus(bool *moving)
 {
     char command[pC_->PMAC_MAXBUF_];
@@ -847,3 +880,188 @@ asynStatus LiftAxis::stop(double acceleration)
   return pmacAxis::stop(acceleration);
 }
 /*-------------------------------------------------------------------------------------------------------------*/
+
+asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
+  char command[pC_->PMAC_MAXBUF_];
+  char response[pC_->PMAC_MAXBUF_];
+  int cmdStatus = 0;
+
+  int done = 0, posChanging = 0;
+  double position = 0;
+  int nvals = 0;
+  int axisProblemFlag = 0;
+  epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
+  char message[132], *axMessage;
+
+  sprintf(command, "Q%2.2d10 P%2.2d00", axisNo_, axisNo_);
+  cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
+  nvals = sscanf(response, "%lf %d", &position, &axStat);
+  if (cmdStatus || nvals != 2) {
+    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "drvPmacAxisGetStatus: Failed to read position and status,  "
+              "Status: %d\nCommand :%s\nResponse:%s\n",
+              cmdStatus, command, response);
+    updateMsgTxtFromDriver("Cannot read Axis position and status");
+  }
+
+  setDoubleParam(pC_->motorPosition_, position * MULT);
+  setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
+
+  /* Use previous position and current position to calculate direction.*/
+  int direction = previous_direction_;
+  if ((position - previous_position_) > 0) {
+    direction = 1;
+  }
+  if ((position - previous_position_) < 0) {
+    direction = 0;
+  }
+  setIntegerParam(pC_->motorStatusDirection_, direction);
+
+  /*Store position to calculate direction for next poll.*/
+  previous_position_ = position;
+  previous_direction_ = direction;
+
+  // errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position);
+
+  /* are we done? */
+  if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) {
+    done = 1;
+  } else {
+    starting = 0;
+    done = 0;
+  }
+
+  if (starting && time(NULL) > startTime + 10) {
+    /*
+      did not start in 10 seconds: messed up: cause an alarm
+    */
+    done = 1;
+    *moving = false;
+    setIntegerParam(pC_->motorStatusMoving_, false);
+    setIntegerParam(pC_->motorStatusDone_, true);
+    setIntegerParam(pC_->motorStatusProblem_, true);
+    errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
+    updateMsgTxtFromDriver("Axis did not start within 10 seconds");
+    starting = 0;
+    return asynSuccess;
+  }
+
+  /*
+    code to test against too long status 5 or 6
+  */
+  if (axStat == 5 || axStat == 6) {
+    if (status6Time == 0) {
+      status6Time = time(NULL);
+      statusPos = position;
+    } else {
+      if (time(NULL) > status6Time + 60) {
+        /* trigger error only when not moving */
+        if (abs(position - statusPos) < .1) {
+          done = 1;
+          *moving = false;
+          setIntegerParam(pC_->motorStatusMoving_, false);
+          setIntegerParam(pC_->motorStatusDone_, true);
+          setIntegerParam(pC_->motorStatusProblem_, true);
+          errlogPrintf(
+              "Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n",
+              axisNo_);
+          updateMsgTxtFromDriver(
+              "Axis stayed in 5,6 for more then 60 seconds: BROKEN");
+          status6Time = 0;
+          return asynSuccess;
+        } else {
+          status6Time = time(NULL);
+          statusPos = position;
+        }
+      }
+    }
+  }
+
+  if (!done) {
+    *moving = true;
+    setIntegerParam(pC_->motorStatusMoving_, true);
+    setIntegerParam(pC_->motorStatusDone_, false);
+  } else {
+    *moving = false;
+    setIntegerParam(pC_->motorStatusMoving_, false);
+    setIntegerParam(pC_->motorStatusDone_, true);
+    if (homing) {
+      setIntegerParam(pC_->motorStatusHomed_, done);
+      homing = 0;
+    }
+  }
+
+  sprintf(message, "poll results: axis %d, status %d, done = %d",
+          axisNo_, axStat, done);
+  pC_->debugFlow(message);
+
+  if (*moving == false) {
+
+    sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
+    cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
+    nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
+    if (cmdStatus || nvals != 1) {
+      asynPrint(
+          pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+          "drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
+          "flags    Status: %d\nCommand :%s\nResponse:%s\n",
+          cmdStatus, command, response);
+      updateMsgTxtFromDriver(
+          "Cannot read Axis Error, high and low limit flag Status");
+    }
+
+    // errlogPrintf("Polling, axErr = %d, highLim = %d, lowLim = %d\n", axStat,
+    // axErr, highLim, lowLim);
+
+    sprintf(message, "poll results: axis %d, axErr %d", axisNo_, axErr);
+    pC_->debugFlow(message);
+
+    /* search for trouble */
+    if (highLim) {
+      setIntegerParam(pC_->motorStatusHighLimit_, true);
+      updateMsgTxtFromDriver("High Limit Hit");
+    } else {
+      setIntegerParam(pC_->motorStatusHighLimit_, false);
+    }
+    if (lowLim) {
+      setIntegerParam(pC_->motorStatusLowLimit_, true);
+      updateMsgTxtFromDriver("Low Limit Hit");
+    } else {
+      setIntegerParam(pC_->motorStatusLowLimit_, false);
+    }
+    if (axErr == 11) {
+      setIntegerParam(pC_->motorStatusFollowingError_, true);
+      updateMsgTxtFromDriver("Following error triggered");
+    } else {
+      setIntegerParam(pC_->motorStatusFollowingError_, false);
+    }
+    /* Set any axis specific general problem bits. */
+  }
+
+  if (axStat < 0 || axErr != 0) {
+    axisProblemFlag = 1;
+    if (axisErrorCount < 10) {
+      axMessage = translateAxisError(axErr);
+      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                "drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error "
+                "code %d, translated: %s:, status code = %d\n",
+                axisNo_, axErr, axMessage, axStat);
+      snprintf(message, sizeof(message), "PMAC Axis error: %s", axMessage);
+      updateMsgTxtFromDriver(message);
+      if (axMessage != NULL) {
+        free(axMessage);
+      }
+      axisErrorCount++;
+    } else if (axisErrorCount == 10) {
+      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                "Suppressing further axis error messages\n");
+      axisErrorCount++;
+    }
+  } else {
+    axisProblemFlag = 0;
+    axisErrorCount = 0;
+  }
+  setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
+
+  return asynSuccess;
+}

sinqEPICSApp/src/pmacAxis.h

@@ -131,4 +131,16 @@ class LiftAxis : public pmacAxis
   int waitStart;
 };
 
+ /********************************************
+ * Protocol version 3 requires just some minor change
+ *
+ * Michele Brambilla, February 2022
+ ********************************************/
+
+class pmacV3Axis : public pmacAxis {
+public:
+  using pmacAxis::pmacAxis;
+  friend class pmacController;
+ };
+
 #endif /* pmacAxis_H */

sinqEPICSApp/src/pmacController.cpp

@@ -843,6 +843,18 @@ static void configLiftAxisCallFunc(const iocshArgBuf *args)
   LiftCreateAxis(args[0].sval, args[1].ival);
 }
 
+/* pmacV3CreateAxis */
+static const iocshArg pmacV3CreateAxisArg0 = {"Controller port name",
+                                              iocshArgString};
+static const iocshArg pmacV3CreateAxisArg1 = {"Axis number", iocshArgInt};
+static const iocshArg *const pmacV3CreateAxisArgs[] = {&pmacV3CreateAxisArg0,
+                                                       &pmacV3CreateAxisArg1};
+static const iocshFuncDef configpmacV3Axis = {"pmacV3CreateAxis", 2,
+                                              pmacV3CreateAxisArgs};
+
+static void configpmacV3AxisCallFunc(const iocshArgBuf *args) {
+  pmacV3CreateAxis(args[0].sval, args[1].ival);
+}
 
 /* pmacCreateAxes */
 static const iocshArg pmacCreateAxesArg0 = {"Controller port name", iocshArgString};

sinqEPICSApp/src/pmacController.h

@@ -139,6 +139,7 @@ class pmacController : public SINQController {
   friend class pmacHRPTAxis;
   friend class SeleneAxis;
   friend class LiftAxis;
+  friend class pmacV3Axis;
 };
 
 #define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1)
@@ -163,5 +164,4 @@ class SeleneController : public pmacController {
     
 };
 
-
 #endif /* pmacController_H */