lin-epics-modules/sinqMotor
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added macros for adding and retrieving paramlib entries in order to make

Browse Source 
the code less cluttered. Also built in a mechanism which makes sure that
forced fast polls are not ignored anymore when adaptive polling is
enabled.
This commit is contained in:
mathis_s
2025-05-08 17:04:23 +02:00
parent 5689402375
commit b89fe41c6e
4 changed files with 456 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

488
src/sinqAxis.cpp
View File

@ -6,6 +6,129 @@
#include <math.h>
#include <unistd.h>
template <>
asynStatus setAxisParam<int>(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<bool>(sinqAxis *axis, const char *indexName,
int (sinqController::*func)(), bool writeValue,
const char *callerFunctionName, int lineNumber) {
return setAxisParam(axis, indexName, func, static_cast<int>(writeValue),
callerFunctionName, lineNumber);
}
template <>
asynStatus
setAxisParam<double>(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<const char *>(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<int>(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<bool>(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<double>(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<char>(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<std::string>(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());
}

130
src/sinqAxis.h
View File

@ -8,6 +8,7 @@ Stefan Mathis, November 2024
#define sinqAxis_H
#include "asynMotorAxis.h"
#include <epicsTime.h>
#include <type_traits>
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 <typename T>
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 <typename T>
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

79
src/sinqController.cpp
View File

@ -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<sinqAxis *>(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<sinqAxis *>(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<sinqAxis *>(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<sinqAxis *>(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)

62
src/sinqController.h
View File

@ -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_;