From a4501b4517ed0ac933a369c139fe8629223278a4 Mon Sep 17 00:00:00 2001 From: smathis Date: Fri, 15 Nov 2024 12:08:23 +0100 Subject: [PATCH] Moved shared functionality from the pmacV3 driver into this library. --- README.md | 6 +- src/sinqAxis.cpp | 132 ++++++++++++++++++++++++++++++++++++++++- src/sinqAxis.h | 68 +++++++++++++++++++++ src/sinqController.cpp | 127 ++++++++++++++++++++++++++++++++++++++- src/sinqController.h | 1 + 5 files changed, 328 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index b3a3fb3..1822d6e 100644 --- a/README.md +++ b/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 diff --git a/src/sinqAxis.cpp b/src/sinqAxis.cpp index c1794c1..ac12ff9 100644 --- a/src/sinqAxis.cpp +++ b/src/sinqAxis.cpp @@ -1,12 +1,14 @@ #include "sinqAxis.h" #include "sinqController.h" +#include #include 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; } \ No newline at end of file +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; +} \ No newline at end of file diff --git a/src/sinqAxis.h b/src/sinqAxis.h index 7a522ca..be2d9e0 100644 --- a/src/sinqAxis.h +++ b/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_; }; diff --git a/src/sinqController.cpp b/src/sinqController.cpp index 90e7f9a..4c8d753 100644 --- a/src/sinqController.cpp +++ b/src/sinqController.cpp @@ -15,6 +15,7 @@ #include "asynMotorController.h" #include "epicsExport.h" #include "iocsh.h" +#include "sinqAxis.h" #include 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"; -} \ No newline at end of file +} + +// ============================================================================= +// 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(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(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 diff --git a/src/sinqController.h b/src/sinqController.h index 406158b..498789e 100644 --- a/src/sinqController.h +++ b/src/sinqController.h @@ -48,6 +48,7 @@ class epicsShareClass sinqController : public asynMotorController { protected: asynUser *lowLevelPortUser_; int motorMessageText_; + int motorTargetPosition_; }; #endif