861 lines
31 KiB
C++
861 lines
31 KiB
C++
// SPDX-License-Identifier: GPL-3.0-only
|
|
|
|
// The EPICS libaries do not follow -Weffc++
|
|
#pragma GCC diagnostic push
|
|
#pragma GCC diagnostic ignored "-Weffc++"
|
|
|
|
#include "epicsExport.h"
|
|
#include "iocsh.h"
|
|
#include <epicsTime.h>
|
|
#include <errlog.h>
|
|
|
|
#pragma GCC diagnostic pop
|
|
|
|
#include "msgPrintControl.h"
|
|
#include "sinqAxis.h"
|
|
#include "sinqController.h"
|
|
#include <math.h>
|
|
#include <unistd.h>
|
|
|
|
#define getControllerMethod pController
|
|
|
|
struct sinqAxisImpl {
|
|
// Internal variables used in the movement timeout watchdog
|
|
time_t expectedArrivalTime;
|
|
time_t offsetMovTimeout;
|
|
|
|
double scaleMovTimeout;
|
|
bool watchdogMovActive;
|
|
// Store the motor target position for the movement time calculation
|
|
double targetPosition;
|
|
|
|
bool wasMoving;
|
|
|
|
/*
|
|
Store the time since the last poll
|
|
*/
|
|
epicsTimeStamp lastPollTime;
|
|
};
|
|
|
|
sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
|
|
: asynMotorAxis((asynMotorController *)pC, axisNo), pC_(pC), pSinqA_([] {
|
|
epicsTimeStamp lastPollTime;
|
|
epicsTimeGetCurrent(&lastPollTime);
|
|
return std::make_unique<sinqAxisImpl>(sinqAxisImpl{
|
|
.expectedArrivalTime = 0,
|
|
.offsetMovTimeout = 30,
|
|
.scaleMovTimeout = 2.0,
|
|
.watchdogMovActive = false,
|
|
.targetPosition = 0.0,
|
|
.wasMoving = false,
|
|
.lastPollTime = lastPollTime,
|
|
});
|
|
}()) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
/*
|
|
This check is also done in asynMotorAxis, but there the IOC continues
|
|
running even though the configuration is incorrect. When failing this check,
|
|
the IOC is stopped, since this is definitely a configuration problem.
|
|
*/
|
|
if ((axisNo < 0) || (axisNo >= pC->numAxes())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(axis index %d is not in range 0 to %d)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo,
|
|
pC->numAxes() - 1);
|
|
exit(-1);
|
|
}
|
|
|
|
/*
|
|
Initialize the parameter library entry for the motor message text, because
|
|
it is read during the first poll before it has been written to.
|
|
*/
|
|
status = setStringParam(pC_->motorMessageText(), "");
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Motor is assumed to be enabled
|
|
status = setIntegerParam(pC_->motorEnableRBV(), 1);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// By default, motors cannot be disabled
|
|
status = setIntegerParam(pC_->motorCanDisable(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Provide a default value for the motor position.
|
|
status = setDoubleParam(pC_->motorPosition(), 0.0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Assume that the motor is connected initially
|
|
status = setIntegerParam(pC_->motorConnected(), 1);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// We assume that the motor has no status problems initially
|
|
status = setIntegerParam(pC_->motorStatusProblem(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Set the homing-related flags
|
|
status = setIntegerParam(pC_->motorStatusHome(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
status = setIntegerParam(pC_->motorStatusHomed(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
status = setIntegerParam(pC_->motorStatusAtHome(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
sinqAxis::~sinqAxis() = default;
|
|
|
|
asynStatus sinqAxis::poll(bool *moving) {
|
|
int adaptivePolling = 0;
|
|
|
|
/*
|
|
If adaptive polling is enabled:
|
|
- Check if the motor was moving during the last poll
|
|
- If yes, perform the poll
|
|
- If no, check if the last poll was at least an idlePollPeriod ago
|
|
- If yes, perform the poll
|
|
- If no, skip it
|
|
*/
|
|
getAxisParamChecked(this, adaptivePolling, &adaptivePolling);
|
|
|
|
// Using the EPICS timestamp here, see
|
|
// https://docs.epics-controls.org/projects/base/en/latest/epicsTime_h.html#_CPPv414epicsTimeStamp
|
|
|
|
// Get the current time
|
|
epicsTimeStamp ts;
|
|
epicsTimeGetCurrent(&ts);
|
|
|
|
/*
|
|
Check if both adaptive polling is enabled and no forced fast polls are still
|
|
required.
|
|
*/
|
|
if (adaptivePolling != 0 && pC_->outstandingForcedFastPolls() == 0 &&
|
|
!pSinqA_->wasMoving) {
|
|
|
|
// Add the idle poll period
|
|
epicsTimeStamp earliestTimeNextPoll = pSinqA_->lastPollTime;
|
|
epicsTimeAddSeconds(&earliestTimeNextPoll, pC_->idlePollPeriod());
|
|
|
|
if (epicsTimeLessThanEqual(&earliestTimeNextPoll, &ts) == 0) {
|
|
*moving = false;
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
return forcedPoll(moving);
|
|
}
|
|
|
|
asynStatus sinqAxis::forcedPoll(bool *moving) {
|
|
|
|
// Local variable declaration
|
|
asynStatus pl_status = asynSuccess;
|
|
asynStatus poll_status = asynSuccess;
|
|
int homing = 0;
|
|
char waitingMessage[pC_->MAXBUF_] = {0};
|
|
char newMessage[pC_->MAXBUF_] = {0};
|
|
|
|
// Update the start time of the last poll
|
|
epicsTimeStamp ts;
|
|
epicsTimeGetCurrent(&ts);
|
|
pSinqA_->lastPollTime = ts;
|
|
|
|
/*
|
|
If the "motorMessageText" record currently contains an error message, it
|
|
should be shown for at least one poll period. To assure this, it is read out
|
|
here from the paramLib into "waitingMessage". If no new error message was
|
|
added to the parameter library at the end of the poll cycle, the
|
|
"waitingMessage" is briefly put into the paramLib again, then the PVs are
|
|
updated and then the message text is cleared again.
|
|
*/
|
|
getAxisParamChecked(this, motorMessageText,
|
|
static_cast<char *>(waitingMessage));
|
|
|
|
// Clear the communication
|
|
setAxisParamChecked(this, motorStatusCommsError, false);
|
|
|
|
/*
|
|
The poll function is just a wrapper around doPoll and handles mainly the
|
|
callParamCallbacks() function. This wrapper is used to make sure
|
|
callParamCallbacks() is called in case of a premature return.
|
|
*/
|
|
poll_status = doPoll(moving);
|
|
|
|
/*
|
|
If the poll did not succeed OR if an error message is waiting, something
|
|
went wrong and the motor has a status problem. Otherwise, delete the error
|
|
message entry which is currently in the paramLib.
|
|
*/
|
|
if (poll_status != asynSuccess || waitingMessage[0] != '\0') {
|
|
/*
|
|
If doPoll cleared the error message paramLib entry, but an old message
|
|
is still waiting, set the old message.
|
|
*/
|
|
getAxisParamChecked(this, motorMessageText,
|
|
static_cast<char *>(newMessage));
|
|
if (newMessage[0] == '\0') {
|
|
setAxisParamChecked(this, motorMessageText,
|
|
static_cast<char *>(waitingMessage));
|
|
}
|
|
setAxisParamChecked(this, motorStatusProblem, true);
|
|
} else {
|
|
// No errors are waiting -> Clear everything.
|
|
setAxisParamChecked(this, motorMessageText, "");
|
|
setAxisParamChecked(this, motorStatusProblem, false);
|
|
}
|
|
|
|
/*
|
|
Motor is in homing mode, was moving, but is not moving anymore -> It can be
|
|
assumed that the homing procedure is done.
|
|
*/
|
|
getAxisParamChecked(this, motorStatusHome, &homing);
|
|
if (homing == 1 && !(*moving) && pSinqA_->wasMoving) {
|
|
setAxisParamChecked(this, motorStatusHome, false);
|
|
setAxisParamChecked(this, motorStatusHomed, true);
|
|
setAxisParamChecked(this, motorStatusAtHome, true);
|
|
}
|
|
|
|
// Update the wasMoving status
|
|
if (pC_->outstandingForcedFastPolls() == 0) {
|
|
pSinqA_->wasMoving = *moving;
|
|
}
|
|
|
|
// Check and update the watchdog as well as the general poll status IF the
|
|
// poll did not fail already.
|
|
if (poll_status == asynSuccess) {
|
|
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
|
|
poll_status = asynError;
|
|
}
|
|
}
|
|
|
|
// According to the function documentation of asynMotorAxis::poll, this
|
|
// function should be called at the end of a poll implementation.
|
|
pl_status = callParamCallbacks();
|
|
bool wantToPrint = pl_status != asynSuccess;
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, wantToPrint,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line "
|
|
"%d:\ncallParamCallbacks failed with %s.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(poll_status),
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
if (wantToPrint) {
|
|
poll_status = pl_status;
|
|
}
|
|
|
|
/*
|
|
Delete the error message AFTER updating the PVs so it is not there anymore
|
|
during the next poll.
|
|
*/
|
|
setAxisParamChecked(this, motorMessageText, "");
|
|
|
|
return poll_status;
|
|
}
|
|
|
|
asynStatus sinqAxis::doPoll(bool *moving) {
|
|
// Suppress unused variable warning - this is just a default fallback
|
|
// function.
|
|
(void)moving;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::move(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
|
|
// Status of parameter library operations
|
|
asynStatus status = asynSuccess;
|
|
double motorRecRes = 0.0;
|
|
|
|
// =========================================================================
|
|
|
|
// Store the target position internally
|
|
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
|
pSinqA_->targetPosition = position * motorRecRes;
|
|
|
|
status = doMove(position, relative, minVelocity, maxVelocity, acceleration);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
status = assertConnected();
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// Since the move command was successfull, we assume that the motor has
|
|
// started its movement.
|
|
setAxisParamChecked(this, motorStatusHomed, false);
|
|
setAxisParamChecked(this, motorStatusAtHome, false);
|
|
|
|
// Needed for adaptive polling
|
|
pSinqA_->wasMoving = true;
|
|
|
|
return pC_->callParamCallbacks();
|
|
}
|
|
|
|
asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
// Suppress unused variable warning - this is just a default fallback
|
|
// function.
|
|
(void)position;
|
|
(void)relative;
|
|
(void)minVelocity;
|
|
(void)maxVelocity;
|
|
(void)acceleration;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::home(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
status = doHome(minVelocity, maxVelocity, acceleration, forwards);
|
|
|
|
if (status == asynSuccess) {
|
|
|
|
setAxisParamChecked(this, motorStatusHome, true);
|
|
setAxisParamChecked(this, motorStatusHomed, false);
|
|
setAxisParamChecked(this, motorStatusAtHome, false);
|
|
pSinqA_->wasMoving = true;
|
|
|
|
status = assertConnected();
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
return pC_->wakeupPoller();
|
|
|
|
} else if (status == asynError) {
|
|
// asynError means that we tried to home an absolute encoder
|
|
setAxisParamChecked(this, motorMessageText,
|
|
"Can't home a motor with absolute encoder");
|
|
|
|
status = assertConnected();
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// Update the motor record
|
|
return callParamCallbacks();
|
|
} else {
|
|
// Bubble up all other problems
|
|
return status;
|
|
}
|
|
}
|
|
|
|
asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
// Suppress unused variable warning - this is just a default fallback
|
|
// function.
|
|
(void)minVelocity;
|
|
(void)maxVelocity;
|
|
(void)acceleration;
|
|
(void)forwards;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::reset() {
|
|
asynStatus status = doReset();
|
|
|
|
if (status == asynSuccess) {
|
|
// Perform some fast polls
|
|
pC_->wakeupPoller();
|
|
}
|
|
|
|
status = assertConnected();
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::doReset() { return asynError; }
|
|
|
|
asynStatus sinqAxis::enable(bool on) {
|
|
// Suppress unused variable warning - this is just a default fallback
|
|
// function.
|
|
(void)on;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::motorPosition(double *motorPos) {
|
|
asynStatus status = asynSuccess;
|
|
double motorRecRes = 0.0;
|
|
|
|
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, "motorPosition", axisNo(),
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
*motorPos = *motorPos * motorRecRes;
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setMotorPosition(double motorPos) {
|
|
asynStatus status = asynSuccess;
|
|
double motorRecRes = 0.0;
|
|
|
|
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
|
setAxisParamChecked(this, motorPosition, motorPos / motorRecRes);
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::assertConnected() {
|
|
int connected = 0;
|
|
getAxisParamChecked(this, motorConnected, &connected);
|
|
if (connected == 0) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line "
|
|
"%d:\nAxis is not connected, all commands are ignored.\n",
|
|
pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
|
|
asynStatus status = asynSuccess;
|
|
int variableSpeed = 0;
|
|
|
|
// Can the speed of the motor be varied?
|
|
getAxisParamChecked(this, motorCanSetSpeed, &variableSpeed);
|
|
|
|
if (variableSpeed == 1) {
|
|
|
|
// Check the inputs and create corresponding error messages
|
|
if (vbas > vmax) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nLower speed "
|
|
"limit vbas=%lf must not be smaller than upper limit "
|
|
"vmax=%lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
vbas, vmax);
|
|
setAxisParamChecked(this, motorMessageText,
|
|
"Lower speed limit must not be smaller than "
|
|
"upper speed limit. Please call the support.");
|
|
return asynError;
|
|
}
|
|
if (velo < vbas || velo > vmax) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nActual "
|
|
"speed velo=%lf must be between lower limit vbas=%lf and "
|
|
"upper limit vmax=%lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
velo, vbas, vmax);
|
|
|
|
setAxisParamChecked(
|
|
this, motorMessageText,
|
|
"Speed is not inside limits. Set a new valid speed and try "
|
|
"to move the motor. Otherwise, please call the support.");
|
|
return asynError;
|
|
}
|
|
|
|
setAxisParamChecked(this, motorVbasFromDriver, vbas);
|
|
setAxisParamChecked(this, motorVeloFromDriver, velo);
|
|
setAxisParamChecked(this, motorVmaxFromDriver, vmax);
|
|
} else {
|
|
// Set minimum and maximum speed equal to the set speed
|
|
setAxisParamChecked(this, motorVbasFromDriver, velo);
|
|
setAxisParamChecked(this, motorVeloFromDriver, velo);
|
|
setAxisParamChecked(this, motorVmaxFromDriver, velo);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setAcclField(double accl) {
|
|
|
|
if (accl <= 0.0) {
|
|
return asynError;
|
|
}
|
|
|
|
setAxisParamChecked(this, motorAcclFromDriver, accl);
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
|
|
setAxisParamChecked(this, motorEnableMovWatchdog, enable);
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
|
asynStatus pl_status;
|
|
int enableMovWatchdog = 0;
|
|
|
|
getAxisParamChecked(this, motorEnableMovWatchdog, &enableMovWatchdog);
|
|
|
|
if (enableMovWatchdog == 1) {
|
|
// These parameters are only needed in this branch
|
|
double motorPos = 0.0;
|
|
double motorVelocity = 0.0;
|
|
double motorVelocityRec = 0.0;
|
|
double motorAccel = 0.0;
|
|
double motorAccelRec = 0.0;
|
|
double motorRecRes = 0.0;
|
|
time_t timeContSpeed = 0;
|
|
time_t timeAccel = 0;
|
|
|
|
// Activate the watchdog
|
|
pSinqA_->watchdogMovActive = true;
|
|
|
|
/*
|
|
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 = motorPosition(&motorPos);
|
|
if (pl_status != asynSuccess) {
|
|
return pl_status;
|
|
}
|
|
|
|
/*
|
|
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.
|
|
*/
|
|
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
|
|
|
// Read the velocity
|
|
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 * motorRecRes;
|
|
if (pl_status == asynSuccess) {
|
|
|
|
timeContSpeed =
|
|
std::ceil(std::fabs(pSinqA_->targetPosition - motorPos) /
|
|
motorVelocity);
|
|
}
|
|
}
|
|
|
|
getAxisParamChecked(this, 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
|
|
pSinqA_->expectedArrivalTime =
|
|
time(NULL) + pSinqA_->offsetMovTimeout +
|
|
pSinqA_->scaleMovTimeout * (timeContSpeed + 2 * timeAccel);
|
|
} else {
|
|
pSinqA_->watchdogMovActive = false;
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
|
|
int enableMovWatchdog = 0;
|
|
|
|
getAxisParamChecked(this, motorEnableMovWatchdog, &enableMovWatchdog);
|
|
|
|
// Not moving or watchdog not active / enabled
|
|
if (enableMovWatchdog == 0 || !moving || !pSinqA_->watchdogMovActive) {
|
|
pSinqA_->watchdogMovActive = false;
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Create the unique callsite identifier manually so it can be used later in
|
|
// the shouldBePrinted calls.
|
|
msgPrintControlKey key = msgPrintControlKey(pC_->portName, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
|
|
// Check if the expected time of arrival has been exceeded.
|
|
if (pSinqA_->expectedArrivalTime < time(NULL)) {
|
|
// Check the watchdog
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(key, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nExceeded "
|
|
"expected arrival time %ld (current time is %ld).\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pSinqA_->expectedArrivalTime, time(NULL));
|
|
}
|
|
|
|
setAxisParamChecked(
|
|
this, motorMessageText,
|
|
"Exceeded expected arrival time. Check if the axis is blocked.");
|
|
setAxisParamChecked(this, motorStatusProblem, true);
|
|
} else {
|
|
pC_->getMsgPrintControl().resetCount(key, pC_->pasynUser());
|
|
}
|
|
|
|
// Even if the movement timed out, the rest of the poll should continue.
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::setOffsetMovTimeout(time_t offsetMovTimeout) {
|
|
pSinqA_->offsetMovTimeout = offsetMovTimeout;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::setScaleMovTimeout(time_t scaleMovTimeout) {
|
|
pSinqA_->scaleMovTimeout = scaleMovTimeout;
|
|
return asynSuccess;
|
|
}
|
|
|
|
bool sinqAxis::wasMoving() { return pSinqA_->wasMoving; }
|
|
|
|
void sinqAxis::setWasMoving(bool wasMoving) { pSinqA_->wasMoving = wasMoving; }
|
|
|
|
double sinqAxis::targetPosition() { return pSinqA_->targetPosition; }
|
|
|
|
void sinqAxis::setTargetPosition(double targetPosition) {
|
|
pSinqA_->targetPosition = targetPosition;
|
|
}
|
|
|
|
// =============================================================================
|
|
// IOC shell functions
|
|
extern "C" {
|
|
|
|
/**
|
|
* @brief Enable / disable the watchdog (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param enable If 0, disable the watchdog, otherwise enable
|
|
* it
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setWatchdogEnabled(const char *portName, int axisNo, int enable) {
|
|
|
|
sinqController *pC = (sinqController *)findAsynPortDriver(portName);
|
|
if (pC == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
|
|
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf(
|
|
"Controller \"%s\", axis %d => %s, line %d:\nAxis does not "
|
|
"exist or is not an instance of sinqAxis.",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return axis->setWatchdogEnabled(enable != 0);
|
|
}
|
|
|
|
static const iocshArg setWatchdogEnabledArg0 = {"Controller port name",
|
|
iocshArgString};
|
|
static const iocshArg setWatchdogEnabledArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg setWatchdogEnabledArg2 = {
|
|
"Enabling / disabling the watchdog", iocshArgInt};
|
|
static const iocshArg *const setWatchdogEnabledArgs[] = {
|
|
&setWatchdogEnabledArg0, &setWatchdogEnabledArg1, &setWatchdogEnabledArg2};
|
|
static const iocshFuncDef setWatchdogEnabledDef = {
|
|
"setWatchdogEnabled", 3, setWatchdogEnabledArgs,
|
|
"Set to 0 to disable the watchdog and to any other value to enable it."};
|
|
|
|
static void setWatchdogEnabledCallFunc(const iocshArgBuf *args) {
|
|
setWatchdogEnabled(args[0].sval, args[1].ival, args[2].ival);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the offsetMovTimeout (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param offsetMovTimeout Offset (in seconds)
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setOffsetMovTimeout(const char *portName, int axisNo,
|
|
double offsetMovTimeout) {
|
|
|
|
sinqController *pC;
|
|
pC = (sinqController *)findAsynPortDriver(portName);
|
|
if (pC == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
|
|
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not "
|
|
"exist or is not an instance of sinqAxis.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, 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,
|
|
"Specify an offset (in seconds) for the movement timeout watchdog"};
|
|
|
|
static void setOffsetMovTimeoutCallFunc(const iocshArgBuf *args) {
|
|
setOffsetMovTimeout(args[0].sval, args[1].ival, args[2].dval);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the setScaleMovTimeout (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param scaleMovTimeout Scaling factor (in seconds)
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setScaleMovTimeout(const char *portName, int axisNo,
|
|
double scaleMovTimeout) {
|
|
|
|
sinqController *pC;
|
|
pC = (sinqController *)findAsynPortDriver(portName);
|
|
if (pC == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
|
|
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not "
|
|
"exist or is not an instance of sinqAxis.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, axisNo);
|
|
return asynError;
|
|
}
|
|
|
|
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,
|
|
"Set a scaling factor for the maximum expected movement time."};
|
|
|
|
static void setScaleMovTimeoutCallFunc(const iocshArgBuf *args) {
|
|
setScaleMovTimeout(args[0].sval, args[1].ival, args[2].dval);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
// This function is made known to EPICS in sinqMotor.dbd and is called by EPICS
|
|
// in order to register all functions in the IOC shell
|
|
static void sinqAxisRegister(void) {
|
|
iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
|
|
iocshRegister(&setScaleMovTimeoutDef, setScaleMovTimeoutCallFunc);
|
|
iocshRegister(&setWatchdogEnabledDef, setWatchdogEnabledCallFunc);
|
|
}
|
|
epicsExportRegistrar(sinqAxisRegister);
|
|
|
|
} // extern C
|