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

Substantial rRework of 0.2.0 after the CAMEA test showed multiple

Browse Source 
problems. Also improved the documentation.
This commit is contained in:
mathis_s
2024-11-26 16:51:12 +01:00
parent 97e80814e3
commit 682325de7d
7 changed files with 423 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
src/sinqAxis.cpp
View File

@ -9,6 +9,8 @@ sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
initial_poll_ = true;
watchdogMovActive_ = false;
init_poll_counter_ = 0;
scaleMovTimeout_ = 2.0;
offsetMovTimeout_ = 30;
}
asynStatus sinqAxis::atFirstPoll() { return asynSuccess; }
@ -53,6 +55,11 @@ asynStatus sinqAxis::poll(bool *moving) {
// return.
poll_status = doPoll(moving);
// Check and update the watchdog
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
return asynError;
}
// If the poll status is ok, reset the error indicators in the parameter
// library
if (poll_status == asynSuccess) {
@ -74,6 +81,30 @@ asynStatus sinqAxis::poll(bool *moving) {
}
}
// Update the enable RBV
bool axisIsEnabled = false;
pl_status = isEnabled(&axisIsEnabled);
if (pl_status != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFunction isEnabled failed with %s.\n",
__PRETTY_FUNCTION__, __LINE__,
pC_->stringifyAsynStatus(poll_status));
pl_status = setStringParam(pC_->motorMessageText_,
"Could not check whether the motor is "
"enabled or not. Please call the support");
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
__PRETTY_FUNCTION__, __LINE__);
}
} else {
pl_status = setIntegerParam(pC_->enableMotorRBV_, axisIsEnabled);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
__PRETTY_FUNCTION__, __LINE__);
}
}
// According to the function documentation of asynMotorAxis::poll, this
// function should be called at the end of a poll implementation.
pl_status = callParamCallbacks();
@ -186,66 +217,183 @@ asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
return asynSuccess;
}
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
watchdogEnabled_ = enable;
asynStatus sinqAxis::enable(bool on) { return asynSuccess; }
asynStatus sinqAxis::isEnabled(bool *on) {
*on = true;
return asynSuccess;
}
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
return pC_->setIntegerParam(axisNo_, pC_->enableMovWatchdog_, enable);
}
asynStatus sinqAxis::startMovTimeoutWatchdog() {
if (watchdogEnabled_) {
asynStatus pl_status;
int enableMovWatchdog = 0;
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMovWatchdog_,
&enableMovWatchdog);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMovWatchdog_",
__PRETTY_FUNCTION__, __LINE__);
}
if (enableMovWatchdog == 1) {
// These parameters are only needed in this branch
double motorPosition = 0.0;
double motorPositionRec = 0.0;
double motorTargetPositionRec = 0.0;
double motorTargetPosition = 0.0;
double motorVelBase = 0.0;
double motorVelocity = 0.0;
double motorVelocityRec = 0.0;
double motorAccel = 0.0;
double motorAccelRec = 0.0;
double motorRecResolution = 0.0;
time_t timeContSpeed = 0;
time_t timeAccel = 0;
asynStatus pl_status;
// Activate the watchdog
watchdogMovActive_ = true;
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
/*
The motor record resolution (index motorRecResolution_ in the parameter
library, MRES in the motor record) is NOT a conversion factor between
user units (e.g. mm) and motor units (e.g. encoder steps), but a scaling
factor defining the resolution of the position readback field RRBV. This
is due to an implementation detail inside EPICS described here:
https://epics.anl.gov/tech-talk/2018/msg00089.php
https://github.com/epics-modules/motor/issues/8
Basically, the position value in the parameter library is a double which
is then truncated to an integer in devMotorAsyn.c (because it was
originally meant for converting from engineering units to encoder steps,
which are by definition integer values). Therefore, if we want a
precision of 1 millimeter, we need to set MRES to 1. If we want one of
1 micrometer, we need to set MRES to 0.001. The readback value needs to
be multiplied with MRES to get the actual value.
In the driver, we use user units. Therefore, when we interact with the
parameter library, we need to account for MRES. This means:
- When writing position or speed to the parameter library, we divide the
value by the motor record resolution.
- When reading position or speed from the parameter library, we multiply
the value with the motor record resolution.
Index and motor record field are coupled as follows:
The parameter motorRecResolution_ is coupled to the field MRES of the
motor record in the following manner:
- In sinqMotor.db, the PV (motor_record_pv_name) MOTOR_REC_RESOLUTION
is defined as a copy of the field (motor_record_pv_name).MRES:
record(ao,"$(P)$(M):Resolution") {
field(DESC, "$(M) resolution")
field(DOL, "$(P)$(M).MRES CP MS")
field(OMSL, "closed_loop")
field(DTYP, "asynFloat64")
field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION")
field(PREC, "$(PREC)")
}
- The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h
to the constant motorRecResolutionString
- ... which in turn is assigned to motorRecResolution_ in
asynMotorController.cpp This way of making the field visible to the
driver is described here:
https://epics.anl.gov/tech-talk/2020/msg00378.php This is a one-way
coupling, changes to the parameter library via setDoubleParam are NOT
transferred to (motor_record_pv_name).MRES or to
(motor_record_pv_name):Resolution.
NOTE: This function must not be called in the constructor (e.g. in order
to save the read result to the member variable earlier), since the
parameter library is updated at a later stage!
*/
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
&motorRecResolution);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_,
&motorPositionRec);
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
motorPosition = motorPositionRec * motorRecResolution;
/*
We use motorVelocity, which corresponds to the record field VELO.
From https://epics.anl.gov/docs/APS2015/14-Motor-Record.pdf:
* VELO = motorVelocity_ = Slew velocity
* VBAS = motorVelBase_ = Only used for stepper motors to minimize
resonance.
As documented in
https://epics.anl.gov/docs/APS2015/17-Motor-Driver.pdf, the
following relations apply: motorVelBase = VBAS / MRES motorVelocity
= VELO / MRES motorAccel = (motorVelocity - motorVelBase) / ACCL
Therefore, we need to correct the values from the parameter library.
*/
// Read the velocity
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorVelocity_,
&motorVelocityRec);
// Only calculate timeContSpeed if the motorVelocity has been populated
// with a sensible value (e.g. > 0)
if (pl_status == asynSuccess && motorVelBase > 0.0) {
if (pl_status == asynSuccess && motorVelocityRec > 0.0) {
// Convert back to the value in the VELO field
motorVelocity = motorVelocityRec * motorRecResolution;
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorTargetPosition_,
&motorTargetPosition);
&motorTargetPositionRec);
motorTargetPosition = motorTargetPositionRec * motorRecResolution;
if (pl_status == asynSuccess) {
timeContSpeed =
std::ceil(std::fabs(motorTargetPosition - motorPosition) /
motorVelBase);
motorVelocity);
}
}
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorAccel_, &motorAccel);
if (pl_status == asynSuccess && motorVelBase > 0.0 &&
motorAccel > 0.0) {
timeAccel = 2 * std::ceil(motorVelBase / motorAccel);
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorAccel_, &motorAccelRec);
if (pl_status == asynSuccess && motorVelocityRec > 0.0 &&
motorAccelRec > 0.0) {
// Convert back to the value in the ACCL field
motorAccel = motorVelocityRec / motorAccelRec;
// Calculate the time
timeAccel = 2 * std::ceil(motorVelocity / motorAccel);
}
// Calculate the expected arrival time
expectedArrivalTime_ =
time(NULL) + offsetMovTimeout_ +
scaleMovTimeout_ * (timeContSpeed + 2 * timeAccel);
} else {
watchdogMovActive_ = false;
}
return asynSuccess;
}
asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
asynStatus pl_status;
int enableMovWatchdog = 0;
// Not moving or watchdog not active
if (!watchdogEnabled_ || !moving) {
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMovWatchdog_,
&enableMovWatchdog);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMovWatchdog_",
__PRETTY_FUNCTION__, __LINE__);
}
// Not moving or watchdog not active / enabled
if (enableMovWatchdog == 0 || !moving || !watchdogMovActive_) {
watchdogMovActive_ = false;
return asynSuccess;
}
@ -272,7 +420,8 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
__PRETTY_FUNCTION__, __LINE__);
}
return asynError;
// Even if the movement timed out, the rest of the poll should continue.
return asynSuccess;
}
return asynSuccess;
}

22
src/sinqAxis.h
View File

@ -42,6 +42,8 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
After calling doPoll:
- Reset motorStatusProblem_, motorStatusCommsError_ and motorMessageText_ if
doPoll returned asynSuccess
- If the movement timeout watchdog has been started, check it.
- Update the parameter library entry enableMotorRBV_ by calling isEnabled.
- Run `callParamCallbacks`
- Return the status of `doPoll`
*
@ -141,6 +143,25 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
virtual asynStatus doHome(double minVelocity, double maxVelocity,
double acceleration, int forwards);
/**
* @brief This function enables / disables an axis. It should be implemented
* by a child class of sinqAxis.
*
* @param on
* @return asynStatus
*/
virtual asynStatus enable(bool on);
/**
* @brief This function should set "on" to true, if the motor is enabled,
* and false otherwise. It should be implemented by a child class of
* sinqAxis.
*
* @param on
* @return asynStatus
*/
virtual asynStatus isEnabled(bool *on);
/**
* @brief Start the watchdog for the movement, if the watchdog is not
disabled. See the documentation of checkMovTimeoutWatchdog for more details.
@ -235,7 +256,6 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
time_t expectedArrivalTime_;
time_t offsetMovTimeout_;
double scaleMovTimeout_;
bool watchdogEnabled_;
bool watchdogMovActive_;
private:

122
src/sinqController.cpp
View File

@ -16,11 +16,16 @@ sinqController::sinqController(const char *portName,
// added for better readability of the configuration.
numAxes + 1,
/*
2 Parameters are added in sinqController:
4 Parameters are added in sinqController:
- MOTOR_MESSAGE_TEXT
- MOTOR_TARGET_POSITION
- ENABLE_AXIS
- ENABLE_AXIS_RBV
- ENABLE_MOV_WATCHDOG
- LIMITS_OFFSET
*/
NUM_MOTOR_DRIVER_PARAMS + numExtraParams + 2,
NUM_MOTOR_DRIVER_PARAMS + NUM_SINQMOTOR_DRIVER_PARAMS +
numExtraParams,
0, // No additional interfaces beyond those in base class
0, // No additional callback interfaces beyond those in base class
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
@ -76,6 +81,69 @@ sinqController::sinqController(const char *portName,
exit(-1);
}
status = createParam("ENABLE_AXIS", asynParamInt32, &enableMotor_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
status = createParam("ENABLE_AXIS_RBV", asynParamInt32, &enableMotorRBV_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
/*
We need to introduce 2 new parameters in order to write the limits from the
driver to the EPICS record. See the comment in sinqController.h next to
the declaration of motorHighLimitFromDriver_.
*/
status = createParam("MOTOR_HIGH_LIMIT_FROM_DRIVER", asynParamFloat64,
&motorHighLimitFromDriver_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
status = createParam("MOTOR_LOW_LIMIT_FROM_DRIVER", asynParamFloat64,
&motorLowLimitFromDriver_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
status =
createParam("ENABLE_MOV_WATCHDOG", asynParamInt32, &enableMovWatchdog_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
status =
createParam("LIMITS_OFFSET", asynParamFloat64, &motorLimitsOffset_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (creating a parameter failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
exit(-1);
}
// Poller configuration
status = startPoller(movingPollPeriod, idlePollPeriod, 1);
if (status != asynSuccess) {
@ -95,19 +163,53 @@ sinqController::~sinqController(void) {
free(this->pAxes_);
}
asynStatus sinqController::writeInt32(asynUser *pasynUser, epicsInt32 value) {
int function = pasynUser->reason;
// =====================================================================
asynMotorAxis *asynAxis = getAxis(pasynUser);
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
if (axis == nullptr) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nAxis %d is not an instance of sinqAxis",
__PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
return asynError;
}
// Handle custom PVs
if (function == enableMotor_) {
return axis->enable(value != 0);
} else {
return asynMotorController::writeInt32(pasynUser, value);
}
}
/*
Overloaded from asynMotorController to cover the readback value of enableMotor.
*/
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
if (pasynUser->reason == enableMotorRBV_) {
// Value is updated in the poll function of an axis
return asynSuccess;
} else {
return asynMotorController::readInt32(pasynUser, value);
}
}
asynStatus sinqController::errMsgCouldNotParseResponse(const char *command,
const char *response,
int axisNo_,
const char *functionName,
int lineNumber) {
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\n Could not interpret response %s for "
"command %s.\n",
functionName, lineNumber, response, command);
setStringParam(
motorMessageText_,
"Could not interpret MCU response. Please call the software support");
setStringParam(motorMessageText_,
"Could not interpret MCU response. Please call the support");
setIntegerParam(motorStatusCommsError_, 1);
return asynError;
}
@ -121,11 +223,11 @@ asynStatus sinqController::paramLibAccessFailed(asynStatus status,
// Log the error message and try to propagate it
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\n Accessing the parameter library failed for "
"parameter %s.\n",
functionName, lineNumber, parameter);
setStringParam(
motorMessageText_,
"Accessing paramLib failed. Please call the software support.");
"parameter %s with error %s.\n",
functionName, lineNumber, parameter,
stringifyAsynStatus(status));
setStringParam(motorMessageText_,
"Accessing paramLib failed. Please call the support.");
}
return status;

44
src/sinqController.h
View File

@ -46,7 +46,28 @@ class epicsShareClass sinqController : public asynMotorController {
*/
virtual ~sinqController(void);
friend class sinqAxis;
/**
* @brief Overloaded function of asynMotorController
*
* The function is overloaded to allow enabling / disabling the motor.
*
* @param pasynUser Specify the axis via the asynUser
* @param value New value
* @return asynStatus
*/
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
/**
* @brief Overloaded function of asynMotorController
*
* The function is overloaded to get readback values for the enabling /
* disabling status.
*
* @param pasynUser Specify the axis via the asynUser
* @param value Read-out value
* @return asynStatus
*/
asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
/**
* @brief Error handling in case accessing the parameter library failed.
@ -98,10 +119,31 @@ class epicsShareClass sinqController : public asynMotorController {
*/
const char *stringifyAsynStatus(asynStatus status);
friend class sinqAxis;
protected:
asynUser *lowLevelPortUser_;
#define FIRST_SINQMOTOR_PARAM motorMessageText_
int motorMessageText_;
int motorTargetPosition_;
int enableMotor_;
int enableMotorRBV_;
int enableMovWatchdog_;
int motorLimitsOffset_;
/*
These parameters are here to write the high and low limits from the MCU to
the EPICS motor record. Using motorHighLimit_ / motorLowLimit_ does not
work: https://epics.anl.gov/tech-talk/2023/msg00576.php.
Therefore, some additional records are introduced which read from these
parameters and write into the motor record. See the sinq_asyn_motor.db file.
*/
int motorHighLimitFromDriver_;
int motorLowLimitFromDriver_;
#define LAST_SINQMOTOR_PARAM motorLowLimitFromDriver_
};
#define NUM_SINQMOTOR_DRIVER_PARAMS \
(&LAST_SINQMOTOR_PARAM - &FIRST_SINQMOTOR_PARAM + 1)
#endif