diff --git a/src/sinqAxis.cpp b/src/sinqAxis.cpp index e9ea07c..2f6424d 100644 --- a/src/sinqAxis.cpp +++ b/src/sinqAxis.cpp @@ -6,6 +6,129 @@ #include #include +template <> +asynStatus setAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), int writeValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->setIntegerParam(indexValue, writeValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus setAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), bool writeValue, + const char *callerFunctionName, int lineNumber) { + return setAxisParam(axis, indexName, func, static_cast(writeValue), + callerFunctionName, lineNumber); +} + +template <> +asynStatus +setAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), double writeValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->setDoubleParam(indexValue, writeValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus setAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), + const char *writeValue, + const char *callerFunctionName, + int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->setStringParam(indexValue, writeValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), int *readValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->pController()->getIntegerParam( + axis->axisNo(), indexValue, readValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), bool *readValue, + const char *callerFunctionName, int lineNumber) { + return getAxisParam(axis, indexName, func, (int *)readValue, + callerFunctionName, lineNumber); +} + +template <> +asynStatus +getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), double *readValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->pController()->getDoubleParam( + axis->axisNo(), indexValue, readValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), char *readValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + asynStatus status = axis->pController()->getStringParam( + axis->axisNo(), indexValue, readValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +template <> +asynStatus +getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), std::string *readValue, + const char *callerFunctionName, int lineNumber) { + int indexValue = (axis->pController()->*func)(); + + // Convert the pointer to a reference, since getStringParam expects the + // latter. + std::string &rReadValue = *readValue; + + asynStatus status = axis->pController()->getStringParam( + axis->axisNo(), indexValue, rReadValue); + if (status != asynSuccess) { + return axis->pController()->paramLibAccessFailed( + status, indexName, axis->axisNo(), callerFunctionName, lineNumber); + } + return asynSuccess; +} + +// ============================================================================= + sinqAxis::sinqAxis(class sinqController *pC, int axisNo) : asynMotorAxis((asynMotorController *)pC, axisNo), pC_(pC) { asynStatus status = asynSuccess; @@ -130,7 +253,6 @@ asynStatus sinqAxis::poll(bool *moving) { asynStatus pl_status = asynSuccess; asynStatus poll_status = asynSuccess; int homing = 0; - int homed = 0; int adaptivePolling = 0; /* @@ -141,13 +263,7 @@ asynStatus sinqAxis::poll(bool *moving) { - If yes, perform the poll - If no, skip it */ - - pl_status = - pC_->getIntegerParam(axisNo_, pC_->adaptivePolling(), &adaptivePolling); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "adaptivePolling", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - }; + getAxisParamChecked(this, adaptivePolling, &adaptivePolling); // Using the EPICS timestamp here, see // https://docs.epics-controls.org/projects/base/en/latest/epicsTime_h.html#_CPPv414epicsTimeStamp @@ -156,7 +272,11 @@ asynStatus sinqAxis::poll(bool *moving) { epicsTimeStamp ts; epicsTimeGetCurrent(&ts); - if (adaptivePolling != 0) { + /* + Check if both adaptive polling is enabled and no forced fast polls are still + required. + */ + if (adaptivePolling != 0 && pC_->outstandingForcedFastPolls() == 0) { // Motor wasn't moving during the last poll if (!wasMoving_) { @@ -170,6 +290,7 @@ asynStatus sinqAxis::poll(bool *moving) { } } } + // Update the start time of the last poll lastPollTime_ = ts; @@ -180,22 +301,8 @@ asynStatus sinqAxis::poll(bool *moving) { The motorStatusProblem_ field changes the motor record fields SEVR and STAT. */ - pl_status = setIntegerParam(pC_->motorStatusProblem(), false); - if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - pl_status = setIntegerParam(pC_->motorStatusCommsError(), false); - if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - pl_status = setStringParam(pC_->motorMessageText(), ""); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorStatusCommsError, false); + setAxisParamChecked(this, motorMessageText, ""); // The poll function is just a wrapper around doPoll and // handles mainly the callParamCallbacks() function. This wrapper is used @@ -203,52 +310,29 @@ asynStatus sinqAxis::poll(bool *moving) { // return. poll_status = doPoll(moving); - // Motor was moving during this poll - wasMoving_ = *moving; - - // The poll did not succeed: Something went wrong and the motor has a status - // problem. - if (poll_status != asynSuccess) { - pl_status = setIntegerParam(pC_->motorStatusProblem(), true); - if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } + // If the poll did not succeed, something went wrong and the motor has a + // status problem. + if (poll_status == asynSuccess) { + setAxisParamChecked(this, motorStatusProblem, false); + } else { + setAxisParamChecked(this, motorStatusProblem, true); } - pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusHomed(), &homed); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusHome(), &homing); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHome_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); + getAxisParamChecked(this, motorStatusHome, &homing); + + /* + Motor is in homing mode, was moving, but is not moving anymore -> It can be + assumed that the homing procedure is done. + */ + if (homing == 1 && !(*moving) && wasMoving_) { + setAxisParamChecked(this, motorStatusHome, false); + setAxisParamChecked(this, motorStatusHomed, true); + setAxisParamChecked(this, motorStatusAtHome, true); } - if (homing == 1 && !(*moving)) { - - // Set the homing-related flags - pl_status = setIntegerParam(pC_->motorStatusHome(), 0); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHome_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = setIntegerParam(pC_->motorStatusHomed(), 1); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = setIntegerParam(pC_->motorStatusAtHome(), 1); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusAtHome_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + // Update the wasMoving status + if (pC_->outstandingForcedFastPolls() == 0) { + wasMoving_ = *moving; } // Check and update the watchdog @@ -284,12 +368,13 @@ asynStatus sinqAxis::move(double position, int relative, double minVelocity, // Status of parameter library operations asynStatus status = asynSuccess; - double motorRecResolution = 0.0; + double motorRecRes = 0.0; // ========================================================================= // Store the target position internally - targetPosition_ = position * motorRecResolution; + getAxisParamChecked(this, motorRecResolution, &motorRecRes); + targetPosition_ = position * motorRecRes; status = doMove(position, relative, minVelocity, maxVelocity, acceleration); if (status != asynSuccess) { @@ -303,22 +388,8 @@ asynStatus sinqAxis::move(double position, int relative, double minVelocity, // Since the move command was successfull, we assume that the motor has // started its movement. - status = setIntegerParam(pC_->motorStatusHomed(), 0); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorStatusHomed_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - status = setIntegerParam(pC_->motorStatusAtHome(), 0); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorStatusAtHome_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(), - &motorRecResolution); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorRecResolution_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } + setAxisParamChecked(this, motorStatusHomed, false); + setAxisParamChecked(this, motorStatusAtHome, false); // Needed for adaptive polling wasMoving_ = true; @@ -340,51 +411,22 @@ asynStatus sinqAxis::home(double minVelocity, double maxVelocity, if (status == asynSuccess) { - status = setStringParam(pC_->motorMessageText(), "Homing"); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Set the homing-related flags - status = setIntegerParam(pC_->motorStatusHome(), 1); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorStatusHome_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - // Set field ATHM to zero - status = setIntegerParam(pC_->motorStatusHomed(), 0); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorStatusHomed_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - status = setIntegerParam(pC_->motorStatusAtHome(), 0); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorStatusAtHome_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorMessageText, "Reference drive"); + setAxisParamChecked(this, motorStatusHome, true); + setAxisParamChecked(this, motorStatusHomed, false); + setAxisParamChecked(this, motorStatusAtHome, false); + wasMoving_ = true; status = assertConnected(); if (status != asynSuccess) { return status; } - - // Update the motor record - return callParamCallbacks(); + return pC_->wakeupPoller(); } else if (status == asynError) { // asynError means that we tried to home an absolute encoder - status = setStringParam(pC_->motorMessageText(), - "Can't home a motor with absolute encoder"); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorMessageText, + "Can't home a motor with absolute encoder"); status = assertConnected(); if (status != asynSuccess) { @@ -395,7 +437,7 @@ asynStatus sinqAxis::home(double minVelocity, double maxVelocity, return callParamCallbacks(); } else { // Bubble up all other problems - return assertConnected(); + return status; } } @@ -420,9 +462,9 @@ asynStatus sinqAxis::reset() { pC_->unlock(); } - asynStatus pl_status = assertConnected(); - if (pl_status != asynSuccess) { - return pl_status; + status = assertConnected(); + if (status != asynSuccess) { + return status; } return status; @@ -432,62 +474,40 @@ asynStatus sinqAxis::doReset() { return asynError; } asynStatus sinqAxis::enable(bool on) { return asynSuccess; } -asynStatus sinqAxis::motorPosition(double *motorPosition) { +asynStatus sinqAxis::motorPosition(double *motorPos) { asynStatus status = asynSuccess; - double motorRecResolution = 0.0; + double motorRecRes = 0.0; - status = pC_->getDoubleParam(axisNo(), pC_->motorRecResolution(), - &motorRecResolution); + getAxisParamChecked(this, motorRecResolution, &motorRecRes); + + /* + We cannot use getAxisParamChecked checked here, since the name of the index + getter function of the controller and this function have the same name. + Therefore we implement this manually + */ + status = pC_->getDoubleParam(axisNo(), pC_->motorPosition(), motorPos); if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorRecResolution_", - axisNo(), __PRETTY_FUNCTION__, - __LINE__); + return pC_->paramLibAccessFailed(status, "motorPosition", axisNo(), + __PRETTY_FUNCTION__, __LINE__); } - status = pC_->getDoubleParam(axisNo(), pC_->motorPosition(), motorPosition); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo(), - __PRETTY_FUNCTION__, - - __LINE__); - } - *motorPosition = *motorPosition * motorRecResolution; + *motorPos = *motorPos * motorRecRes; return status; } -asynStatus sinqAxis::setMotorPosition(double motorPosition) { +asynStatus sinqAxis::setMotorPosition(double motorPos) { asynStatus status = asynSuccess; - double motorRecResolution = 0.0; + double motorRecRes = 0.0; - status = pC_->getDoubleParam(axisNo(), pC_->motorRecResolution(), - &motorRecResolution); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorRecResolution_", - axisNo(), __PRETTY_FUNCTION__, - __LINE__); - } + getAxisParamChecked(this, motorRecResolution, &motorRecRes); + setAxisParamChecked(this, motorPosition, motorPos / motorRecRes); - status = setDoubleParam(pC_->motorPosition(), - motorPosition / motorRecResolution); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo(), - __PRETTY_FUNCTION__, - - __LINE__); - } return status; } asynStatus sinqAxis::assertConnected() { - asynStatus status = asynSuccess; int connected = 0; - - status = pC_->getIntegerParam(axisNo(), pC_->motorConnected(), &connected); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorConnected", axisNo(), - __PRETTY_FUNCTION__, __LINE__); - } - + getAxisParamChecked(this, motorConnected, &connected); if (connected == 0) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " @@ -502,12 +522,8 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) { int variableSpeed = 0; // Can the speed of the motor be varied? - status = - pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed(), &variableSpeed); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } + getAxisParamChecked(this, motorCanSetSpeed, &variableSpeed); + if (variableSpeed == 1) { // Check the inputs and create corresponding error messages @@ -518,15 +534,9 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) { "vmax=%lf.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, vbas, vmax); - - status = setStringParam( - pC_->motorMessageText(), + setAxisParamChecked( + this, motorMessageText, "Lower speed limit must not be smaller than upper speed limit"); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } return asynError; } if (velo < vbas || velo > vmax) { @@ -537,58 +547,19 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) { pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, velo, vbas, vmax); - status = setStringParam(pC_->motorMessageText(), - "Speed is not inside limits"); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorMessageText, + "Speed is not inside limits"); return asynError; } - status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver(), vbas); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver(), velo); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver(), vmax); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorVbasFromDriver, vbas); + setAxisParamChecked(this, motorVeloFromDriver, velo); + setAxisParamChecked(this, motorVmaxFromDriver, vmax); } else { // Set minimum and maximum speed equal to the set speed - status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver(), velo); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver(), velo); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver(), velo); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + setAxisParamChecked(this, motorVbasFromDriver, velo); + setAxisParamChecked(this, motorVeloFromDriver, velo); + setAxisParamChecked(this, motorVmaxFromDriver, velo); } return status; } @@ -599,30 +570,20 @@ asynStatus sinqAxis::setAcclField(double accl) { return asynError; } - asynStatus status = setDoubleParam(pC_->motorAcclFromDriver(), accl); - if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorAcclFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - return status; + setAxisParamChecked(this, motorAcclFromDriver, accl); + return asynSuccess; } asynStatus sinqAxis::setWatchdogEnabled(bool enable) { - return setIntegerParam(pC_->motorEnableMovWatchdog(), enable); + setAxisParamChecked(this, motorEnableMovWatchdog, enable); + return asynSuccess; } asynStatus sinqAxis::startMovTimeoutWatchdog() { asynStatus pl_status; int enableMovWatchdog = 0; - pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog(), - &enableMovWatchdog); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + getAxisParamChecked(this, motorEnableMovWatchdog, &enableMovWatchdog); if (enableMovWatchdog == 1) { // These parameters are only needed in this branch @@ -631,7 +592,7 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() { double motorVelocityRec = 0.0; double motorAccel = 0.0; double motorAccelRec = 0.0; - double motorRecResolution = 0.0; + double motorRecRes = 0.0; time_t timeContSpeed = 0; time_t timeAccel = 0; @@ -660,23 +621,16 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() { = VELO / MRES motorAccel = (motorVelocity - motorVelBase) / ACCL Therefore, we need to correct the values from the parameter library. */ - pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(), - &motorRecResolution); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + getAxisParamChecked(this, motorRecResolution, &motorRecRes); // Read the velocity - pl_status = pC_->getDoubleParam(axisNo_, pC_->motorVelocity(), - &motorVelocityRec); + getAxisParamChecked(this, motorVelocity, &motorVelocityRec); // Only calculate timeContSpeed if the motorVelocity has been populated // with a sensible value (e.g. > 0) if (pl_status == asynSuccess && motorVelocityRec > 0.0) { // Convert back to the value in the VELO field - motorVelocity = motorVelocityRec * motorRecResolution; + motorVelocity = motorVelocityRec * motorRecRes; if (pl_status == asynSuccess) { timeContSpeed = std::ceil( @@ -684,8 +638,7 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() { } } - pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorAccel(), &motorAccelRec); + getAxisParamChecked(this, motorAccel, &motorAccelRec); if (pl_status == asynSuccess && motorVelocityRec > 0.0 && motorAccelRec > 0.0) { @@ -707,16 +660,9 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() { } asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) { - asynStatus pl_status; int enableMovWatchdog = 0; - pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog(), - &enableMovWatchdog); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } + getAxisParamChecked(this, motorEnableMovWatchdog, &enableMovWatchdog); // Not moving or watchdog not active / enabled if (enableMovWatchdog == 0 || !moving || !watchdogMovActive_) { @@ -741,20 +687,10 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) { expectedArrivalTime_, time(NULL)); } - pl_status = setStringParam( - pC_->motorMessageText(), + setAxisParamChecked( + this, motorMessageText, "Exceeded expected arrival time. Check if the axis is blocked."); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - pl_status = setIntegerParam(pC_->motorStatusProblem(), true); - if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } + setAxisParamChecked(this, motorStatusProblem, true); } else { pC_->getMsgPrintControl().resetCount(key, pC_->pasynUser()); } diff --git a/src/sinqAxis.h b/src/sinqAxis.h index 44d0a41..f5a0523 100644 --- a/src/sinqAxis.h +++ b/src/sinqAxis.h @@ -8,6 +8,7 @@ Stefan Mathis, November 2024 #define sinqAxis_H #include "asynMotorAxis.h" #include +#include class epicsShareClass sinqAxis : public asynMotorAxis { public: @@ -358,6 +359,11 @@ class epicsShareClass sinqAxis : public asynMotorAxis { */ asynStatus assertConnected(); + /** + * @brief Return a pointer to the axis controller + */ + sinqController *pController() { return pC_; }; + protected: // Internal variables used in the movement timeout watchdog time_t expectedArrivalTime_; @@ -379,4 +385,128 @@ class epicsShareClass sinqAxis : public asynMotorAxis { sinqController *pC_; }; +// ============================================================================= +// Helper functions and definitions for the macro setAxisParamChecked + +/** + * @brief Helper function to set an integer / double / string parameter for an + * axis in the paramLib + * + * This function should not be used directly, but rather through its macro + * variant `setAxisParamChecked`. + * + * @tparam T + * @param axis + * @param indexName + * @param func + * @param writeValue + * @param callerFunctionName + * @param lineNumber + * @return asynStatus + */ +template +asynStatus setAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), T writeValue, + const char *callerFunctionName, int lineNumber); + +/** + * @brief Macro to set an paramLib parameter and error checking the return value + * + * This macro is a wrapper around `setIntegerParam` / `setDoubleParam` / + * `setStringParam` which checks if the operation was successfull. If it wasn't, + * it returns by calling the paramLibAccessFailed function. + * + * For example, the following input: + * ``` + * setAxisParamChecked(this, motorStatusProblem_, false) + * ``` + * expands into the following code: + * ``` + * { + * int indexValue = axis->pController()->motorStatusProblem_(); + * asynStatus status = axis->setIntegerParam(indexValue, writeValue); + * if (status != asynSuccess) { + * return axis->pController()->paramLibAccessFailed( + * status, "motorStatusProblem_", axis->axisNo(), __PRETTY_FUNCTION__, + * __LINE__); + * } + * return asynSuccess; + * } + * ``` + * ============================================================================= + */ +#define setAxisParamChecked(axis, indexGetterFunction, writeValue) \ + { \ + asynStatus setStatus = setAxisParam( \ + axis, #indexGetterFunction, \ + &std::remove_pointer< \ + decltype(axis->pController())>::type::indexGetterFunction, \ + writeValue, __PRETTY_FUNCTION__, __LINE__); \ + if (setStatus != asynSuccess) { \ + return setStatus; \ + } \ + } + +// ============================================================================= +// Helper functions and definitions for the macro getAxisParamChecked + +/** + * @brief Helper function to set an integer / double / string parameter for an + * axis in the paramLib + * + * This function should not be used directly, but rather through its macro + * variant `getAxisParamChecked`. + * + * @tparam T + * @param axis + * @param indexName + * @param func + * @param readValue + * @param callerFunctionName + * @param lineNumber + * @return asynStatus + */ +template +asynStatus getAxisParam(sinqAxis *axis, const char *indexName, + int (sinqController::*func)(), T *readValue, + const char *callerFunctionName, int lineNumber); + +/** + * @brief Macro to get an paramLib parameter and error checking the return value + * + * This macro is a wrapper around `getIntegerParam` / `getDoubleParam` / + * `getStringParam` which checks if the operation was successfull. If it wasn't, + * it returns by calling the paramLibAccessFailed function. + * + * For example, the following input: + * ``` + * getAxisParamChecked(this, motorStatusProblem_, &readValue) + * ``` + * expands into the following code: + * ``` + * { + * int indexValue = axis->pController()->motorStatusProblem_(); + * asynStatus status = axis->pController()->getIntegerParam(axis->axisNo(), + * indexValue, readValue); if (status != asynSuccess) { return + * axis->pController()->paramLibAccessFailed( status, "motorStatusProblem_", + * axis->axisNo(), __PRETTY_FUNCTION__, + * __LINE__); + * } + * return asynSuccess; + * } + * ``` + * ============================================================================= + */ +#define getAxisParamChecked(axis, indexGetterFunction, readValue) \ + { \ + asynStatus getStatus = getAxisParam( \ + axis, #indexGetterFunction, \ + &std::remove_pointer< \ + decltype(axis->pController())>::type::indexGetterFunction, \ + readValue, __PRETTY_FUNCTION__, __LINE__); \ + if (getStatus != asynSuccess) { \ + return getStatus; \ + } \ + } + #endif diff --git a/src/sinqController.cpp b/src/sinqController.cpp index 4fac795..107ae69 100644 --- a/src/sinqController.cpp +++ b/src/sinqController.cpp @@ -314,8 +314,24 @@ sinqController::~sinqController(void) { free(this->pAxes_); } -msgPrintControl &sinqController::getMsgPrintControl() { - return msgPrintControl_; +/* +Access one of the axes of the controller via the axis adress stored in asynUser. +If the axis does not exist or is not an sinqAxis, the function returns a +nullptr. +*/ +sinqAxis *sinqController::getSinqAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = sinqController::getAxis(pasynUser); + return dynamic_cast(asynAxis); +} + +/* +Access one of the axes of the controller via the axis index. +If the axis does not exist or is not an sinqAxis, the function returns a +nullptr. +*/ +sinqAxis *sinqController::getSinqAxis(int axisNo) { + asynMotorAxis *asynAxis = sinqController::getAxis(axisNo); + return dynamic_cast(asynAxis); } asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) { @@ -323,9 +339,7 @@ asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) { // ===================================================================== - asynMotorAxis *asynAxis = getAxis(pasynUser); - sinqAxis *axis = dynamic_cast(asynAxis); - + sinqAxis *axis = getSinqAxis(pasynUser); if (axis == nullptr) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an " @@ -348,22 +362,21 @@ asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) { asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) { - // Casting into a sinqAxis is necessary to get access to the field axisNo() - asynMotorAxis *asynAxis = getAxis(pasynUser); - sinqAxis *axis = dynamic_cast(asynAxis); - + sinqAxis *axis = getSinqAxis(pasynUser); if (axis == nullptr) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an " - "instance of sinqAxis.\n", + "instance of sinqAxis", portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__); return asynError; } if (pasynUser->reason == motorEnableRBV_) { - return getIntegerParam(axis->axisNo(), motorEnableRBV_, value); + getAxisParamChecked(axis, motorEnableRBV, value); + return asynSuccess; } else if (pasynUser->reason == motorCanDisable_) { - return getIntegerParam(axis->axisNo(), motorCanDisable_, value); + getAxisParamChecked(axis, motorCanDisable, value); + return asynSuccess; } else { return asynMotorController::readInt32(pasynUser, value); } @@ -374,26 +387,24 @@ asynStatus sinqController::couldNotParseResponse(const char *command, int axisNo, const char *functionName, int line) { - asynStatus pl_status = asynSuccess; - asynPrint(pasynOctetSyncIOipPort_, ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nCould not interpret " "response \"%s\" for command \"%s\".\n", portName, axisNo, functionName, line, response, command); - pl_status = setStringParam( - motorMessageText_, - "Could not interpret controller response. Please call the support"); - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorMessageText_", axisNo, - __PRETTY_FUNCTION__, __LINE__); + sinqAxis *axis = getSinqAxis(axisNo); + if (axis == nullptr) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an " + "instance of sinqAxis", + portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__); + return asynError; } - pl_status = setIntegerParam(motorStatusCommsError_, 1); - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorStatusCommsError_", axisNo, - __PRETTY_FUNCTION__, __LINE__); - } + setAxisParamChecked( + axis, motorMessageText, + "Could not interpret controller response. Please call the support"); + setAxisParamChecked(axis, motorStatusCommsError, true); return asynError; } @@ -413,7 +424,7 @@ asynStatus sinqController::paramLibAccessFailed(asynStatus status, // Log the error message and try to propagate it. If propagating fails, // there is nothing we can do here anyway. - setStringParam(motorMessageText_, + setStringParam(motorMessageText(), "Accessing paramLib failed. Please call the support."); } @@ -545,6 +556,22 @@ asynStatus sinqController::checkMaxSubsequentTimeouts(int timeoutNo, return status; } +asynStatus sinqController::poll() { + // Decrement the number of outstanding forced fast polls, if they are not + // zero + if (outstandingForcedFastPolls_ > 0) { + outstandingForcedFastPolls_--; + } + return asynMotorController::poll(); +} + +asynStatus sinqController::wakeupPoller() { + // + 1 since outstandingForcedFastPolls_ is reduced once at the start of + // a poll cycle + outstandingForcedFastPolls_ = forcedFastPolls_ + 1; + return asynMotorController::wakeupPoller(); +} + // Static pointers (valid for the entire lifetime of the IOC). The number behind // the strings gives the integer number of each variant (see also method // stringifyAsynStatus) diff --git a/src/sinqController.h b/src/sinqController.h index 9112844..231d130 100644 --- a/src/sinqController.h +++ b/src/sinqController.h @@ -219,7 +219,23 @@ class epicsShareClass sinqController : public asynMotorController { * message repetitions. See the documentation of `printRepetitionWatchdog` * in msgPrintControl.h for details. */ - msgPrintControl &getMsgPrintControl(); + msgPrintControl &getMsgPrintControl() { return msgPrintControl_; } + + /** + * @brief Get the axis object + * + * @param pasynUser Specify the axis via the asynUser + * @return sinqAxis* If no axis could be found, this is a nullptr + */ + sinqAxis *getSinqAxis(asynUser *pasynUser); + + /** + * @brief Get the axis object + * + * @param axisNo Specify the axis via its index + * @return sinqAxis* If no axis could be found, this is a nullptr + */ + sinqAxis *getSinqAxis(int axisNo); // ========================================================================= // Public getters for protected members @@ -314,9 +330,53 @@ class epicsShareClass sinqController : public asynMotorController { */ asynUser *pasynOctetSyncIOipPort() { return pasynOctetSyncIOipPort_; } + /** + * @brief Overloaded version of `asynController::poll` which decreases + * `outstandingForcedFastPolls` and then defers to the base method + */ + asynStatus poll(); + + /** + * @brief Overloaded version of `asynController::wakeupPoller` which + * initializes the `outstandingForcedFastPolls` variable and then defers to + * the base class method. + * + * @return asynStatus + */ + asynStatus wakeupPoller(); + + /** + * @brief Set the number of forced fast polls which should be performed when + * `wakeupPoller` is called. + * + * @param forcedFastPolls + */ + void setForcedFastPolls(int forcedFastPolls) { + forcedFastPolls_ = forcedFastPolls; + } + + /** + * @brief Read the number of forced fast polls currently specified + * + */ + int forcedFastPolls() { return forcedFastPolls_; } + + /** + * @brief Read the number of outstanding forced fast polls currently + * specified + * + */ + int outstandingForcedFastPolls() { return outstandingForcedFastPolls_; } + // ========================================================================= protected: + // Number of fast polls which still need to be performed before adaptive + // polling is active again. + int outstandingForcedFastPolls_; + + // Number of polls forced by wakeupPoller which are still + // Pointer to the port user which is specified by the char array // `ipPortConfigName` in the constructor asynUser *pasynOctetSyncIOipPort_;