Moved shared functionality from the pmacV3 driver into this library.

README.md

@@ -25,12 +25,16 @@ sinqMotor offers a variety of additional methods for children classes to standar
   - Run `callParamCallbacks`
   - Return the status of `doPoll`
 - `doPoll`: This is an empty function which should be overwritten by concrete driver implementations.
+- `move`: This function sets the absolute target position in the parameter library and then calls `doMove`.
+- `doMove`: This is an empty function which should be overwritten by concrete driver implementations.
+- `movementTimeoutWatchdog`: Manages a watchdog mechanism for movement operations
+
 
 ## Versioning
 
 The versioning is done via git tags. Git tags are recognized by the PSI build system: If you tag a version as 1.0, it will be built into the directory /ioc/modules/sinqMotor/1.0. The tag is directly coupled to a commit so that it is always clear which source code was used to build which binary.
 
-All existing tags can be listed with `git tag` in the sinqMotor directory. Detailed information (author, data, commit number, commit message) regarding a specific tag can be shown with `git show X.X`, where X.X is the name of your version. To create a new tag, use `git tag -a X.X`. If the tag `X.X` is already used by another commit, git will show a corresponding error.
+All existing tags can be listed with `git tag` in the sinqMotor directory. Detailed information (author, data, commit number, commit message) regarding a specific tag can be shown with `git show X.X`, where X.X is the name of your version. To create a new tag, use `git tag X.X`. If the tag `X.X` is already used by another commit, git will show a corresponding error.
 
 ## How to build it
 

src/sinqAxis.cpp

@@ -1,12 +1,14 @@
 #include "sinqAxis.h"
 #include "sinqController.h"
+#include <math.h>
 #include <unistd.h>
 
 sinqAxis::sinqAxis(class sinqController *pC, int axis)
     : asynMotorAxis((asynMotorController *)pC, axis), pC_(pC) {
 
-    bool initial_poll_ = true;
-    int init_poll_counter_ = 0;
+    initial_poll_ = true;
+    watchdogMovActive_ = false;
+    init_poll_counter_ = 0;
 }
 
 asynStatus sinqAxis::atFirstPoll() { return asynSuccess; }
@@ -88,4 +90,128 @@ asynStatus sinqAxis::poll(bool *moving) {
     return poll_status;
 }
 
-asynStatus sinqAxis::doPoll(bool *moving) { return asynSuccess; }
+asynStatus sinqAxis::doPoll(bool *moving) { return asynSuccess; }
+
+asynStatus sinqAxis::move(double position, int relative, double minVelocity,
+                          double maxVelocity, double acceleration) {
+
+    double motorRecResolution = 0.0;
+    double target_position = 0.0;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    // =========================================================================
+
+    // Calculate the (absolute) target position
+    if (relative) {
+
+        double motorPosition = 0.0;
+        pl_status =
+            pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        target_position = position + motorPosition;
+    } else {
+        target_position = position;
+    }
+
+    // Set the target position
+    pl_status = setDoubleParam(pC_->motorTargetPosition_, target_position);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    return doMove(position, relative, minVelocity, maxVelocity, acceleration);
+}
+
+asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,
+                            double maxVelocity, double acceleration) {
+    return asynSuccess;
+}
+
+asynStatus sinqAxis::movementTimeoutWatchdog(bool moving) {
+    asynStatus pl_status;
+
+    // Not moving -> Watchdog inactive
+    if (!moving) {
+        watchdogMovActive_ = false;
+        return asynSuccess;
+    }
+
+    if (!watchdogMovActive_) {
+
+        // These parameters are only needed in this branch
+        double motorPosition = 0.0;
+        double motorTargetPosition = 0.0;
+        double motorRecResolution = 0.0;
+        double motorVelBase = 0.0;
+        double motorAccel = 0.0;
+        time_t timeContSpeed = 0;
+        time_t timeAccel = 0;
+
+        // Activate the watchdog
+        watchdogMovActive_ = true;
+
+        pl_status =
+            pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorPosition",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        pl_status =
+            pC_->getDoubleParam(axisNo_, pC_->motorVelBase_, &motorVelBase);
+        // Only calculate timeContSpeed if the motorVelBase_ has been populated
+        // with a sensible value (e.g. > 0)
+        if (pl_status == asynSuccess && motorVelBase > 0.0) {
+
+            pl_status = pC_->getDoubleParam(axisNo_, pC_->motorTargetPosition_,
+                                            &motorTargetPosition);
+            if (pl_status == asynSuccess) {
+                timeContSpeed =
+                    std::ceil(std::fabs(motorTargetPosition - motorPosition) /
+                              motorVelBase);
+            }
+        }
+
+        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorAccel_, &motorAccel);
+        if (pl_status == asynSuccess && motorVelBase > 0.0 &&
+            motorAccel > 0.0) {
+            timeAccel = 2 * std::ceil(motorVelBase / motorAccel);
+        }
+
+        // Calculate the expected arrival time
+        expectedArrivalTime_ =
+            time(NULL) + offsetMovTimeout_ +
+            scaleMovTimeout_ * (timeContSpeed + 2 * timeAccel);
+
+    } else if (expectedArrivalTime_ < time(NULL)) {
+        // Check the watchdog
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\nAxis %d exceeded the expected arrival time "
+                  "(%ld).\n",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_, expectedArrivalTime_);
+
+        pl_status = setStringParam(
+            pC_->motorMessageText_,
+            "Exceeded expected arrival time. Check if the axis is blocked.");
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+        if (pl_status != asynSuccess) {
+            pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
+                                      __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        return asynError;
+    }
+    return asynSuccess;
+}

src/sinqAxis.h

@@ -18,6 +18,17 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
      */
     asynStatus atFirstPoll();
 
+    /**
+    Wrapper around doPoll which performs the following operations;
+    Before calling doPoll:
+    - Try to execute atFirstPoll once (and retry, if that failed)
+
+    After calling doPoll:
+    - Reset motorStatusProblem_, motorStatusCommsError_ and motorMessageText_ if
+    doPoll returned asynSuccess
+    - Run `callParamCallbacks`
+    - Return the status of `doPoll`
+     */
     asynStatus poll(bool *moving);
 
     /**
@@ -26,12 +37,69 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
      */
     asynStatus doPoll(bool *moving);
 
+    /**
+    Wrapper around move which calculates the (absolute) target position and
+    stores it in the parameter library. After that, it calls and returns doMove
+     */
+    asynStatus move(double position, int relative, double minVelocity,
+                    double maxVelocity, double acceleration);
+
+    /**
+    Implementation of the "proper", device-specific poll method. This method
+    should be implemented by a child class of sinqAxis.
+    */
+    asynStatus doMove(double position, int relative, double minVelocity,
+                      double maxVelocity, double acceleration);
+
+    /**
+    Manages a timeout mechanism for the movement:
+    If the movement takes too long, create an error message and return
+    asynError. The watchdog is started when moving switches from "false" to
+    "true" and stopped when moving switches from "true" to "false". At the
+    watchdog start, the estimated movement time is calculated as
+
+    t = offsetMovTimeout_ + scaleMovTime_ * [timeContSpeed + 2*timeAccel]
+
+    with
+
+    timeContSpeed = abs(motorTargetPosition - motorPosition) / motorVelBase
+    timeAcc = motorVelBase / motorAccel
+
+    The values motorTargetPosition, motorVelBase, motorAccel and
+    positionAtMovementStart are taken from the parameter library. Therefore it
+    is necessary to populate them before using this function. If they are not
+    given, both speed and velocity are assumed to be infinity. This means that
+    timeContSpeed and/or timeAcc are set to zero. motorTargetPosition is
+    populated automatically when using the doMove function.
+
+    The values offsetMovTimeout_ and scaleMovTimeout_ can be set directly from
+    the IOC shell with the functions setScaleMovTimeout and setOffsetMovTimeout,
+    if sinqMotor is loaded via the "require" mechanism.
+     */
+    asynStatus movementTimeoutWatchdog(bool moving);
+
+    // Setter for offsetMovTimeout
+    asynStatus setOffsetMovTimeout(time_t offsetMovTimeout) {
+        offsetMovTimeout_ = offsetMovTimeout;
+    }
+
+    // Setter for scaleMovTimeout
+    asynStatus setScaleMovTimeout(time_t scaleMovTimeout) {
+        scaleMovTimeout_ = scaleMovTimeout;
+    }
+
     friend class sinqController;
 
   protected:
     bool initial_poll_;
     int init_poll_counter_;
 
+    // Helper variables for movementTimeoutWatchdog
+    time_t expectedArrivalTime_;
+    time_t offsetMovTimeout_;
+    double scaleMovTimeout_;
+    bool watchdogMovActive_;
+
   private:
     sinqController *pC_;
 };

src/sinqController.cpp

@@ -15,6 +15,7 @@
 #include "asynMotorController.h"
 #include "epicsExport.h"
 #include "iocsh.h"
+#include "sinqAxis.h"
 #include <errlog.h>
 
 sinqController::sinqController(const char *portName, const char *SINQPortName,
@@ -27,7 +28,42 @@ sinqController::sinqController(const char *portName, const char *SINQPortName,
           1,    // autoconnect
           0, 0) // Default priority and stack size
 {
-    createParam(motorMessageTextString, asynParamOctet, &motorMessageText_);
+
+    // Initialization of local variables
+    asynStatus status = asynSuccess;
+
+    // =========================================================================;
+
+    // MOTOR_MESSAGE_TEXT corresponds to the PV definition inside
+    // sinqn_asyn_motor.db. This text is used to forward status messages to
+    // NICOS and in turn to the user.
+    status =
+        createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &motorMessageText_);
+    if (status != asynSuccess) {
+        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
+                  "with %s)\n. Terminating IOC",
+                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
+        exit(-1);
+    }
+
+    // Internal parameter library entry which stores the movement target
+    status = createParam("MOTOR_TARGET_POSITION", asynParamOctet,
+                         &motorTargetPosition_);
+    if (status != asynSuccess) {
+        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
+                  "with %s)\n. Terminating IOC",
+                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
+        exit(-1);
+    }
+}
+
+sinqController::~sinqController(void) {
+    /*
+    Cleanup of the memory allocated in the asynMotorController constructor
+    */
+    free(this->pAxes_);
 }
 
 asynStatus sinqController::errMsgCouldNotParseResponse(const char *command,
@@ -118,4 +154,91 @@ const char *sinqController::stringifyAsynStatus(asynStatus status) {
     errlogPrintf("%s => line %d:\nReached unreachable code.",
                  __PRETTY_FUNCTION__, __LINE__);
     return "unreachable code reached";
-}
+}
+
+// =============================================================================
+// Provide the setters to IOC shell
+extern "C" {
+
+asynStatus setOffsetMovTimeout(const char *portName, int axisNo,
+                               double offsetMovTimeout) {
+
+    sinqController *pC;
+    pC = (sinqController *)findAsynPortDriver(portName);
+    if (pC == nullptr) {
+        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
+                     __LINE__, portName);
+        return asynError;
+    }
+
+    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
+    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
+    if (axis == nullptr) {
+        errlogPrintf("%s => line %d:\nPAxis %d does not exist or is not an "
+                     "instance of sinqAxis.",
+                     __PRETTY_FUNCTION__, __LINE__, portName, axisNo);
+    }
+
+    return axis->setOffsetMovTimeout(offsetMovTimeout);
+}
+
+static const iocshArg setOffsetMovTimeoutArg0 = {"Controller port name",
+                                                 iocshArgString};
+static const iocshArg setOffsetMovTimeoutArg1 = {"Axis number", iocshArgInt};
+static const iocshArg setOffsetMovTimeoutArg2 = {"Offset timeout for movement",
+                                                 iocshArgDouble};
+static const iocshArg *const setOffsetMovTimeoutArgs[] = {
+    &setOffsetMovTimeoutArg0, &setOffsetMovTimeoutArg1,
+    &setOffsetMovTimeoutArg2};
+static const iocshFuncDef setOffsetMovTimeoutDef = {"setOffsetMovTimeout", 3,
+                                                    setOffsetMovTimeoutArgs};
+
+static void setOffsetMovTimeoutCallFunc(const iocshArgBuf *args) {
+    setOffsetMovTimeout(args[0].sval, args[1].ival, args[1].dval);
+}
+
+// ============================================================================
+
+asynStatus setScaleMovTimeout(const char *portName, int axisNo,
+                              double scaleMovTimeout) {
+
+    sinqController *pC;
+    pC = (sinqController *)findAsynPortDriver(portName);
+    if (pC == nullptr) {
+        errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
+                     __LINE__, portName);
+        return asynError;
+    }
+
+    asynMotorAxis *asynAxis = pC->getAxis(axisNo);
+    sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
+    if (axis == nullptr) {
+        errlogPrintf("%s => line %d:\nPAxis %d does not exist or is not an "
+                     "instance of sinqAxis.",
+                     __PRETTY_FUNCTION__, __LINE__, portName, axisNo);
+    }
+
+    return axis->setScaleMovTimeout(scaleMovTimeout);
+}
+
+static const iocshArg setScaleMovTimeoutArg0 = {"Controller port name",
+                                                iocshArgString};
+static const iocshArg setScaleMovTimeoutArg1 = {"Axis number", iocshArgInt};
+static const iocshArg setScaleMovTimeoutArg2 = {
+    "Multiplier for calculated move time", iocshArgDouble};
+static const iocshArg *const setScaleMovTimeoutArgs[] = {
+    &setScaleMovTimeoutArg0, &setScaleMovTimeoutArg1, &setScaleMovTimeoutArg2};
+static const iocshFuncDef setScaleMovTimeoutDef = {"setScaleMovTimeout", 3,
+                                                   setScaleMovTimeoutArgs};
+
+static void setScaleMovTimeoutCallFunc(const iocshArgBuf *args) {
+    setScaleMovTimeout(args[0].sval, args[1].ival, args[1].dval);
+}
+
+static void sinqControllerRegister(void) {
+    iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
+    iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
+}
+epicsExportRegistrar(sinqControllerRegister);
+
+} // extern C

src/sinqController.h

@@ -48,6 +48,7 @@ class epicsShareClass sinqController : public asynMotorController {
   protected:
     asynUser *lowLevelPortUser_;
     int motorMessageText_;
+    int motorTargetPosition_;
 };
 
 #endif