995 lines
38 KiB
C++
995 lines
38 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 "asynMotorController.h"
|
|
#include "asynOctetSyncIO.h"
|
|
#include "epicsExport.h"
|
|
#include "iocsh.h"
|
|
#include <errlog.h>
|
|
#include <initHooks.h>
|
|
|
|
#pragma GCC diagnostic pop
|
|
|
|
#include "msgPrintControl.h"
|
|
#include "sinqAxis.h"
|
|
#include "sinqController.h"
|
|
#include <deque>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
|
|
/*
|
|
Contains all instances of turboPmacAxis which have been created and is used in
|
|
the initialization hook function.
|
|
*/
|
|
static std::vector<sinqController *> controller;
|
|
|
|
/**
|
|
* @brief Hook function to perform certain actions during the IOC initialization
|
|
*
|
|
* @param iState
|
|
*/
|
|
void sinqController::epicsInithookFunction(initHookState iState) {
|
|
if (iState == initHookAfterIocRunning) {
|
|
// Iterate through all axes of each and call the initialization method
|
|
// on each one of them.
|
|
for (std::vector<sinqController *>::iterator itC = controller.begin();
|
|
itC != controller.end(); ++itC) {
|
|
|
|
sinqController *controller = *itC;
|
|
controller->startPoller(controller->movingPollPeriod_,
|
|
controller->idlePollPeriod_, 1);
|
|
}
|
|
}
|
|
}
|
|
|
|
struct sinqControllerImpl {
|
|
// 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;
|
|
|
|
// Message print control
|
|
msgPrintControl msgPrintC;
|
|
|
|
// Internal variables used in the communication timeout frequency watchdog
|
|
time_t comTimeoutWindow; // Size of the time window
|
|
size_t maxNumberTimeouts; // Maximum acceptable number of events within the
|
|
// time window
|
|
|
|
// Deque holding the timestamps of the individual events
|
|
std::deque<time_t> timeoutEvents;
|
|
|
|
// Communicate a timeout to the user after it has happened this many times
|
|
// in a row
|
|
int maxSubsequentTimeouts;
|
|
bool maxSubsequentTimeoutsExceeded;
|
|
|
|
/*
|
|
These integers are indices to paramLib entries and are populated when the
|
|
parameters are created. See the documentation in db/sinqMotor.db.
|
|
*/
|
|
int motorMessageText;
|
|
int motorReset;
|
|
int motorEnable;
|
|
int motorEnableRBV;
|
|
int motorCanDisable;
|
|
int motorEnableMovWatchdog;
|
|
int motorCanSetSpeed;
|
|
int motorLimitsOffset;
|
|
int motorForceStop;
|
|
int motorConnected;
|
|
/*
|
|
These parameters are here to write values from the hardware 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.
|
|
*/
|
|
int motorVeloFromDriver;
|
|
int motorVbasFromDriver;
|
|
int motorVmaxFromDriver;
|
|
int motorAcclFromDriver;
|
|
int motorHighLimitFromDriver;
|
|
int motorLowLimitFromDriver;
|
|
int motorPositionDeadband;
|
|
int adaptivePolling;
|
|
int encoderType;
|
|
};
|
|
#define NUM_SINQMOTOR_DRIVER_PARAMS 18
|
|
|
|
sinqController::sinqController(const char *portName,
|
|
const char *ipPortConfigName, int numAxes,
|
|
double movingPollPeriod, double idlePollPeriod,
|
|
int numExtraParams)
|
|
: asynMotorController(
|
|
portName,
|
|
// As described in the function documentation, an offset of 1 is
|
|
// added for better readability of the configuration.
|
|
numAxes + 1,
|
|
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,
|
|
1, // autoconnect
|
|
0, 0), // Default priority and stack size
|
|
pSinqC_(std::make_unique<sinqControllerImpl>((sinqControllerImpl){
|
|
.outstandingForcedFastPolls = 0,
|
|
.pasynOctetSyncIOipPort = nullptr,
|
|
.msgPrintC = msgPrintControl(),
|
|
.comTimeoutWindow = 3600,
|
|
.maxNumberTimeouts = 60,
|
|
.timeoutEvents = {},
|
|
.maxSubsequentTimeouts = 10,
|
|
.maxSubsequentTimeoutsExceeded = false,
|
|
.motorMessageText = 0,
|
|
.motorReset = 0,
|
|
.motorEnable = 0,
|
|
.motorEnableRBV = 0,
|
|
.motorCanDisable = 0,
|
|
.motorEnableMovWatchdog = 0,
|
|
.motorCanSetSpeed = 0,
|
|
.motorLimitsOffset = 0,
|
|
.motorForceStop = 0,
|
|
.motorConnected = 0,
|
|
.motorVeloFromDriver = 0,
|
|
.motorVbasFromDriver = 0,
|
|
.motorVmaxFromDriver = 0,
|
|
.motorAcclFromDriver = 0,
|
|
.motorHighLimitFromDriver = 0,
|
|
.motorLowLimitFromDriver = 0,
|
|
.motorPositionDeadband = 0,
|
|
.adaptivePolling = 0,
|
|
.encoderType = 0,
|
|
})) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
// Store the poll period information. The poller itself will be started
|
|
// later (after the IOC is running in epicsInithookFunction)
|
|
movingPollPeriod_ = movingPollPeriod;
|
|
idlePollPeriod_ = idlePollPeriod;
|
|
|
|
// =========================================================================;
|
|
|
|
/*
|
|
We try to connect to the port via the port name provided by the constructor.
|
|
If this fails, the function is terminated via exit.
|
|
*/
|
|
pasynOctetSyncIO->connect(ipPortConfigName, 0,
|
|
&pSinqC_->pasynOctetSyncIOipPort, NULL);
|
|
if (status != asynSuccess || pSinqC_->pasynOctetSyncIOipPort == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nFATAL ERROR (cannot "
|
|
"connect to MCU controller).\n"
|
|
"Terminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__);
|
|
exit(-1);
|
|
}
|
|
|
|
// =========================================================================;
|
|
|
|
// MOTOR_MESSAGE_TEXT corresponds to the PV definition inside sinqMotor.db.
|
|
// This text is used to forward status messages to NICOS and in turn to the
|
|
// user.
|
|
status = createParam("MOTOR_MESSAGE_TEXT", asynParamOctet,
|
|
&pSinqC_->motorMessageText);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_ENABLE", asynParamInt32, &pSinqC_->motorEnable);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_RESET", asynParamInt32, &pSinqC_->motorReset);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_ENABLE_RBV", asynParamInt32,
|
|
&pSinqC_->motorEnableRBV);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_CAN_DISABLE", asynParamInt32,
|
|
&pSinqC_->motorCanDisable);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_CAN_SET_SPEED", asynParamInt32,
|
|
&pSinqC_->motorCanSetSpeed);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_LIMITS_OFFSET", asynParamFloat64,
|
|
&pSinqC_->motorLimitsOffset);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_CONNECTED", asynParamInt32,
|
|
&pSinqC_->motorConnected);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __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,
|
|
&pSinqC_->motorHighLimitFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_LOW_LIMIT_FROM_DRIVER", asynParamFloat64,
|
|
&pSinqC_->motorLowLimitFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_POSITION_DEADBAND", asynParamFloat64,
|
|
&pSinqC_->motorPositionDeadband);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_ENABLE_MOV_WATCHDOG", asynParamInt32,
|
|
&pSinqC_->motorEnableMovWatchdog);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_VELO_FROM_DRIVER", asynParamFloat64,
|
|
&pSinqC_->motorVeloFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_VBAS_FROM_DRIVER", asynParamFloat64,
|
|
&pSinqC_->motorVbasFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_VMAX_FROM_DRIVER", asynParamFloat64,
|
|
&pSinqC_->motorVmaxFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_ACCL_FROM_DRIVER", asynParamFloat64,
|
|
&pSinqC_->motorAcclFromDriver);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("ADAPTIVE_POLLING", asynParamInt32,
|
|
&pSinqC_->adaptivePolling);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("ENCODER_TYPE", asynParamOctet, &pSinqC_->encoderType);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
status = createParam("MOTOR_FORCE_STOP", asynParamInt32,
|
|
&pSinqC_->motorForceStop);
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
|
|
"parameter failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Register the hook function during construction of the first axis object
|
|
if (controller.empty()) {
|
|
initHookRegister(&epicsInithookFunction);
|
|
}
|
|
|
|
// Collect all axes into this list which will be used in the hook function
|
|
controller.push_back(this);
|
|
}
|
|
|
|
sinqController::~sinqController(void) {
|
|
|
|
// Free all axes
|
|
for (int axisNo = 0; axisNo < numAxes_; axisNo++) {
|
|
if (pAxes_[axisNo] != nullptr) {
|
|
delete pAxes_[axisNo];
|
|
}
|
|
}
|
|
|
|
// Cleanup of the array allocated in the asynMotorController constructor
|
|
free(this->pAxes_);
|
|
}
|
|
|
|
/*
|
|
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) {
|
|
int function = pasynUser->reason;
|
|
|
|
// =====================================================================
|
|
|
|
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",
|
|
portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__);
|
|
return asynError;
|
|
}
|
|
|
|
// Handle custom PVs
|
|
if (function == motorEnable()) {
|
|
return axis->enable(value != 0);
|
|
} else if (function == motorReset()) {
|
|
return axis->reset();
|
|
} else if (function == motorForceStop()) {
|
|
return axis->stop(0.0);
|
|
} else {
|
|
return asynMotorController::writeInt32(pasynUser, value);
|
|
}
|
|
}
|
|
|
|
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
|
|
|
|
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",
|
|
portName, axis->axisNo(), __PRETTY_FUNCTION__, __LINE__);
|
|
return asynError;
|
|
}
|
|
|
|
if (pasynUser->reason == motorEnableRBV()) {
|
|
getAxisParamChecked(axis, motorEnableRBV, value);
|
|
return asynSuccess;
|
|
} else if (pasynUser->reason == motorCanDisable()) {
|
|
getAxisParamChecked(axis, motorCanDisable, value);
|
|
return asynSuccess;
|
|
} else {
|
|
return asynMotorController::readInt32(pasynUser, value);
|
|
}
|
|
}
|
|
|
|
asynStatus sinqController::couldNotParseResponse(const char *command,
|
|
const char *response,
|
|
int axisNo,
|
|
const char *functionName,
|
|
int line) {
|
|
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);
|
|
|
|
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;
|
|
}
|
|
|
|
setAxisParamChecked(
|
|
axis, motorMessageText,
|
|
"Could not interpret controller response. Please call the support");
|
|
setAxisParamChecked(axis, motorStatusCommsError, true);
|
|
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus sinqController::paramLibAccessFailed(asynStatus status,
|
|
const char *parameter,
|
|
int axisNo,
|
|
const char *functionName,
|
|
int line) {
|
|
|
|
if (status != asynSuccess) {
|
|
asynPrint(pasynOctetSyncIOipPort(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\n Accessing the "
|
|
"parameter library failed for parameter %s with error %s.\n",
|
|
portName, axisNo, functionName, line, parameter,
|
|
stringifyAsynStatus(status));
|
|
|
|
// Log the error message and try to propagate it. If propagating fails,
|
|
// there is nothing we can do here anyway.
|
|
setStringParam(motorMessageText(),
|
|
"Accessing paramLib failed. Please call the support.");
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqController::checkComTimeoutWatchdog(int axisNo,
|
|
char *motorMessage,
|
|
size_t motorMessageSize) {
|
|
|
|
asynStatus paramLibStatus = asynSuccess;
|
|
|
|
// Add a new timeout event to the queue
|
|
pSinqC_->timeoutEvents.push_back(time(NULL));
|
|
|
|
// Remove every event which is older than the time window from the deque
|
|
while (1) {
|
|
if (pSinqC_->timeoutEvents.empty()) {
|
|
break;
|
|
}
|
|
if (pSinqC_->timeoutEvents[0] + pSinqC_->comTimeoutWindow <=
|
|
time(NULL)) {
|
|
pSinqC_->timeoutEvents.pop_front();
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Check if the maximum allowed number of events has been exceeded
|
|
bool wantToPrint =
|
|
pSinqC_->timeoutEvents.size() > pSinqC_->maxNumberTimeouts;
|
|
|
|
if (pSinqC_->msgPrintC.shouldBePrinted(portName, axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
wantToPrint, pasynUserSelf)) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nMore than %ld "
|
|
"communication timeouts in %ld "
|
|
"seconds.%s\n",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
|
|
pSinqC_->maxNumberTimeouts, pSinqC_->comTimeoutWindow,
|
|
pSinqC_->msgPrintC.getSuffix());
|
|
}
|
|
|
|
if (wantToPrint) {
|
|
snprintf(motorMessage, motorMessageSize,
|
|
"More than %ld communication timeouts in %ld seconds. Please "
|
|
"call the support.",
|
|
pSinqC_->maxNumberTimeouts, pSinqC_->comTimeoutWindow);
|
|
|
|
paramLibStatus = setIntegerParam(motorStatusCommsError_, 1);
|
|
if (paramLibStatus != asynSuccess) {
|
|
return paramLibAccessFailed(paramLibStatus,
|
|
"motorStatusCommsError_", axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return asynError;
|
|
} else {
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
asynStatus sinqController::checkComTimeoutWatchdog(sinqAxis *axis) {
|
|
|
|
char errorMessage[MAXBUF_] = {0};
|
|
|
|
asynStatus status =
|
|
checkComTimeoutWatchdog(axis->axisNo(), errorMessage, MAXBUF_);
|
|
if (status == asynError) {
|
|
setAxisParamChecked(axis, motorMessageText, errorMessage);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqController::checkMaxSubsequentTimeouts(int timeoutNo, int axisNo,
|
|
char *motorMessage,
|
|
size_t motorMessageSize) {
|
|
asynStatus paramLibStatus = asynSuccess;
|
|
|
|
if (timeoutNo >= pSinqC_->maxSubsequentTimeouts) {
|
|
if (!pSinqC_->maxSubsequentTimeoutsExceeded) {
|
|
snprintf(motorMessage, motorMessageSize,
|
|
"Communication timeout between IOC and motor controller. "
|
|
"Trying to reconnect ...");
|
|
asynPrint(
|
|
this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nMore than %d "
|
|
"subsequent communication timeouts. Check whether the "
|
|
"controller is still running and connected to the network.\n",
|
|
this->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
|
|
pSinqC_->maxSubsequentTimeouts);
|
|
|
|
paramLibStatus = setIntegerParam(motorStatusCommsError_, 1);
|
|
if (paramLibStatus != asynSuccess) {
|
|
return paramLibAccessFailed(paramLibStatus,
|
|
"motorStatusCommsError_", axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
pSinqC_->maxSubsequentTimeoutsExceeded = true;
|
|
}
|
|
|
|
return asynError;
|
|
} else {
|
|
pSinqC_->maxSubsequentTimeoutsExceeded = false;
|
|
motorMessage[0] = '\0';
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
asynStatus sinqController::checkMaxSubsequentTimeouts(int timeoutNo,
|
|
sinqAxis *axis) {
|
|
|
|
char motorMessage[MAXBUF_] = {0};
|
|
|
|
asynStatus status = checkMaxSubsequentTimeouts(timeoutNo, axis->axisNo(),
|
|
motorMessage, MAXBUF_);
|
|
if (status == asynError) {
|
|
setAxisParamChecked(axis, motorMessageText, motorMessage);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqController::poll() {
|
|
// Decrement the number of outstanding forced fast polls, if they are not
|
|
// zero
|
|
if (pSinqC_->outstandingForcedFastPolls > 0) {
|
|
pSinqC_->outstandingForcedFastPolls--;
|
|
}
|
|
return asynMotorController::poll();
|
|
}
|
|
|
|
asynStatus sinqController::wakeupPoller() {
|
|
// + 1 since outstandingForcedFastPolls_ is reduced once at the start of
|
|
// a poll cycle
|
|
pSinqC_->outstandingForcedFastPolls = forcedFastPolls_ + 1;
|
|
return asynMotorController::wakeupPoller();
|
|
}
|
|
|
|
asynStatus sinqController::setMaxSubsequentTimeouts(int maxSubsequentTimeouts) {
|
|
pSinqC_->maxSubsequentTimeouts = maxSubsequentTimeouts;
|
|
return asynSuccess;
|
|
}
|
|
|
|
bool sinqController::maxSubsequentTimeoutsExceeded() {
|
|
return pSinqC_->maxSubsequentTimeoutsExceeded;
|
|
}
|
|
|
|
/**
|
|
* @brief Get a reference to the map used to control the maximum number of
|
|
* message repetitions. See the documentation of `printRepetitionWatchdog`
|
|
* in msgPrintControl.h for details.
|
|
*/
|
|
msgPrintControl &sinqController::getMsgPrintControl() {
|
|
return pSinqC_->msgPrintC;
|
|
}
|
|
|
|
/**
|
|
* @brief Read the number of outstanding forced fast polls currently
|
|
* specified
|
|
*
|
|
*/
|
|
int sinqController::outstandingForcedFastPolls() {
|
|
return pSinqC_->outstandingForcedFastPolls;
|
|
}
|
|
|
|
/**
|
|
* @brief Return a pointer to the low-level octet (string) IP Port
|
|
*
|
|
* @return asynUser*
|
|
*/
|
|
asynUser *sinqController::pasynOctetSyncIOipPort() {
|
|
return pSinqC_->pasynOctetSyncIOipPort;
|
|
}
|
|
|
|
asynStatus sinqController::setThresholdComTimeout(time_t comTimeoutWindow,
|
|
size_t maxNumberTimeouts) {
|
|
pSinqC_->comTimeoutWindow = comTimeoutWindow;
|
|
pSinqC_->maxNumberTimeouts = maxNumberTimeouts;
|
|
return asynSuccess;
|
|
}
|
|
|
|
int sinqController::motorMessageText() { return pSinqC_->motorMessageText; }
|
|
int sinqController::motorReset() { return pSinqC_->motorReset; }
|
|
int sinqController::motorEnable() { return pSinqC_->motorEnable; }
|
|
int sinqController::motorEnableRBV() { return pSinqC_->motorEnableRBV; }
|
|
int sinqController::motorCanDisable() { return pSinqC_->motorCanDisable; }
|
|
int sinqController::motorEnableMovWatchdog() {
|
|
return pSinqC_->motorEnableMovWatchdog;
|
|
}
|
|
int sinqController::motorCanSetSpeed() { return pSinqC_->motorCanSetSpeed; }
|
|
int sinqController::motorLimitsOffset() { return pSinqC_->motorLimitsOffset; }
|
|
int sinqController::motorForceStop() { return pSinqC_->motorForceStop; }
|
|
int sinqController::motorConnected() { return pSinqC_->motorConnected; }
|
|
int sinqController::motorVeloFromDriver() {
|
|
return pSinqC_->motorVeloFromDriver;
|
|
}
|
|
int sinqController::motorVbasFromDriver() {
|
|
return pSinqC_->motorVbasFromDriver;
|
|
}
|
|
int sinqController::motorVmaxFromDriver() {
|
|
return pSinqC_->motorVmaxFromDriver;
|
|
}
|
|
int sinqController::motorAcclFromDriver() {
|
|
return pSinqC_->motorAcclFromDriver;
|
|
}
|
|
int sinqController::motorHighLimitFromDriver() {
|
|
return pSinqC_->motorHighLimitFromDriver;
|
|
}
|
|
int sinqController::motorLowLimitFromDriver() {
|
|
return pSinqC_->motorLowLimitFromDriver;
|
|
}
|
|
int sinqController::motorPositionDeadband() {
|
|
return pSinqC_->motorPositionDeadband;
|
|
}
|
|
int sinqController::adaptivePolling() { return pSinqC_->adaptivePolling; }
|
|
int sinqController::encoderType() { return pSinqC_->encoderType; }
|
|
|
|
// 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)
|
|
const char asynSuccessStringified[] = "success"; // 0
|
|
const char asynTimeoutStringified[] = "timeout"; // 1
|
|
const char asynOverflowStringified[] = "overflow"; // 2
|
|
const char asynErrorStringified[] = "error"; // 3
|
|
const char asynDisconnectedStringified[] = "disconnected"; // 4
|
|
const char asynDisabledStringified[] = "disabled"; // 5
|
|
const char asynParamAlreadyExistsStringified[] =
|
|
"parameter already exists"; // 6
|
|
const char asynParamNotFoundStringified[] = "parameter not found"; // 7
|
|
const char asynParamWrongTypeStringified[] = "wrong type"; // 8
|
|
const char asynParamBadIndexStringified[] = "bad index"; // 9
|
|
const char asynParamUndefinedStringified[] = "parameter undefined"; // 10
|
|
const char asynParamInvalidListStringified[] = "invalid list"; // 11
|
|
const char inputDidNotMatchAsynStatus[] =
|
|
"Input did not match any variant of asynStatus";
|
|
|
|
const char *sinqController::stringifyAsynStatus(asynStatus status) {
|
|
// See
|
|
// https://github.com/epics-modules/asyn/blob/master/asyn/asynDriver/asynDriver.h
|
|
// and
|
|
// https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/paramErrors.h
|
|
// for the definition of the error codes
|
|
// The pragma is necessary since the param lib error codes are "tacked onto"
|
|
// the enum, which results in compiler warnings otherwise.
|
|
#pragma GCC diagnostic ignored "-Wswitch"
|
|
switch (status) {
|
|
case asynSuccess:
|
|
return asynSuccessStringified;
|
|
case asynTimeout:
|
|
return asynTimeoutStringified;
|
|
case asynOverflow:
|
|
return asynOverflowStringified;
|
|
case asynError:
|
|
return asynErrorStringified;
|
|
case asynDisconnected:
|
|
return asynDisconnectedStringified;
|
|
case asynDisabled:
|
|
return asynDisabledStringified;
|
|
case asynParamAlreadyExists:
|
|
return asynParamAlreadyExistsStringified;
|
|
case asynParamNotFound:
|
|
return asynParamNotFoundStringified;
|
|
case asynParamWrongType:
|
|
return asynParamWrongTypeStringified;
|
|
case asynParamBadIndex:
|
|
return asynParamBadIndexStringified;
|
|
case asynParamUndefined:
|
|
return asynParamUndefinedStringified;
|
|
case asynParamInvalidList:
|
|
return asynParamInvalidListStringified;
|
|
}
|
|
|
|
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s, line %d:\nInput did not match any "
|
|
"variant of asynStatus.\n",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
|
|
return inputDidNotMatchAsynStatus;
|
|
}
|
|
|
|
// =============================================================================
|
|
// IOC shell functions
|
|
extern "C" {
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the threshold for the communication timeout frequency (FFI
|
|
* implementation)
|
|
*
|
|
* @param portName Name of the low-level asyn port the controller is
|
|
* using.
|
|
* @param comTimeoutWindow Size of the time window used to calculate
|
|
* the moving average of timeout events in seconds. Set this value to 0 to
|
|
* deactivate the watchdog.
|
|
* @param maxNumberTimeouts Maximum number of timeouts which may occur
|
|
* within the time window before the watchdog is triggered.
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setThresholdComTimeout(const char *portName, int comTimeoutWindow,
|
|
int maxNumberTimeouts) {
|
|
|
|
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;
|
|
}
|
|
|
|
return pC->setThresholdComTimeout(comTimeoutWindow, maxNumberTimeouts);
|
|
}
|
|
|
|
static const iocshArg setThresholdComTimeoutArg0 = {"Controller port name",
|
|
iocshArgString};
|
|
static const iocshArg setThresholdComTimeoutArg1 = {
|
|
"Time window communication timeout frequency", iocshArgInt};
|
|
static const iocshArg setThresholdComTimeoutArg2 = {
|
|
"Maximum allowed number of communication timeouts within the window",
|
|
iocshArgInt};
|
|
static const iocshArg *const setThresholdComTimeoutArgs[] = {
|
|
&setThresholdComTimeoutArg0, &setThresholdComTimeoutArg1,
|
|
&setThresholdComTimeoutArg2};
|
|
static const iocshFuncDef setThresholdComTimeoutDef = {
|
|
"setThresholdComTimeout", 3, setThresholdComTimeoutArgs,
|
|
"Set the communication timeout threshold in seconds"};
|
|
|
|
static void setThresholdComTimeoutCallFunc(const iocshArgBuf *args) {
|
|
setThresholdComTimeout(args[0].sval, args[1].ival, args[2].ival);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the maximum number of subsequent timeouts (FFI implementation)
|
|
*
|
|
* @param portName Name of the low-level asyn port the controller is
|
|
* using.
|
|
* @param maxSubsequentTimeouts Maximum number of timeouts which may occur
|
|
* subsequently before an error is reported.
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setMaxSubsequentTimeouts(const char *portName,
|
|
int maxSubsequentTimeouts) {
|
|
void *ptr = findAsynPortDriver(portName);
|
|
if (ptr == nullptr) {
|
|
/*
|
|
We can't use asynPrint here since this macro would require us
|
|
to get a pasynOctetSyncIOipPort_ from a pointer to an asynPortDriver.
|
|
However, the given pointer is a nullptr and therefore doesn't
|
|
have a pasynOctetSyncIOipPort_! printf is an EPICS alternative which
|
|
works w/o that, but doesn't offer the comfort provided
|
|
by the asynTrace-facility
|
|
*/
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
// Unsafe cast of the pointer to an asynPortDriver
|
|
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
|
|
|
// Safe downcast
|
|
sinqController *pC = dynamic_cast<sinqController *>(apd);
|
|
if (pC == nullptr) {
|
|
errlogPrintf(
|
|
"Controller \"%s\" => %s, line %d:\ncontroller on port %s is not a "
|
|
"turboPmacController.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
|
|
// Set the new value
|
|
pC->setMaxSubsequentTimeouts(maxSubsequentTimeouts);
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
static const iocshArg SetMaxSubsequentTimeoutsArg0 = {
|
|
"Controller name (e.g. mcu1)", iocshArgString};
|
|
static const iocshArg SetMaxSubsequentTimeoutsArg1 = {
|
|
"Maximum number of subsequent timeouts before the user receives an error "
|
|
"message",
|
|
iocshArgInt};
|
|
static const iocshArg *const SetMaxSubsequentTimeoutsArgs[] = {
|
|
&SetMaxSubsequentTimeoutsArg0, &SetMaxSubsequentTimeoutsArg1};
|
|
static const iocshFuncDef setMaxSubsequentTimeoutsDef = {
|
|
"setMaxSubsequentTimeouts", 2, SetMaxSubsequentTimeoutsArgs,
|
|
"Set the maximum number of subsequent timeouts before the user receives an "
|
|
"error message"};
|
|
static void setMaxSubsequentTimeoutsCallFunc(const iocshArgBuf *args) {
|
|
setMaxSubsequentTimeouts(args[0].sval, args[1].ival);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the number of forced fast polls which happen after a call to
|
|
* `wakePoller`.
|
|
*
|
|
* @param portName Name of the low-level asyn port the controller is
|
|
* using.
|
|
* @param forcedFastPolls Number of fast polls done after calling
|
|
* `wakePoller`.
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setForcedFastPolls(const char *portName, int forcedFastPolls) {
|
|
void *ptr = findAsynPortDriver(portName);
|
|
if (ptr == nullptr) {
|
|
/*
|
|
We can't use asynPrint here since this macro would require us
|
|
to get a pasynOctetSyncIOipPort_ from a pointer to an asynPortDriver.
|
|
However, the given pointer is a nullptr and therefore doesn't
|
|
have a pasynOctetSyncIOipPort_! printf is an EPICS alternative which
|
|
works w/o that, but doesn't offer the comfort provided
|
|
by the asynTrace-facility
|
|
*/
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
// Unsafe cast of the pointer to an asynPortDriver
|
|
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
|
|
|
// Safe downcast
|
|
sinqController *pC = dynamic_cast<sinqController *>(apd);
|
|
if (pC == nullptr) {
|
|
errlogPrintf(
|
|
"Controller \"%s\" => %s, line %d:\ncontroller on port %s is not a "
|
|
"turboPmacController.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, portName);
|
|
return asynError;
|
|
}
|
|
|
|
// Set the new value
|
|
pC->setForcedFastPolls(forcedFastPolls);
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
static const iocshArg SetForcedFastPollsArg0 = {"Controller name (e.g. mcu1)",
|
|
iocshArgString};
|
|
static const iocshArg SetForcedFastPollsArg1 = {
|
|
"Number of fast polls after \"waking\" the poller (e.g. after issuing a "
|
|
"move command).",
|
|
iocshArgInt};
|
|
static const iocshArg *const SetForcedFastPollsArgs[] = {
|
|
&SetForcedFastPollsArg0, &SetForcedFastPollsArg1};
|
|
static const iocshFuncDef setForcedFastPollsDef = {
|
|
"setForcedFastPolls", 2, SetForcedFastPollsArgs,
|
|
"Set the number of fast polls after \"waking\" the poller (e.g. after "
|
|
"issuing a move command)."};
|
|
static void setForcedFastPollsCallFunc(const iocshArgBuf *args) {
|
|
setForcedFastPolls(args[0].sval, args[1].ival);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
// 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 sinqControllerRegister(void) {
|
|
iocshRegister(&setThresholdComTimeoutDef, setThresholdComTimeoutCallFunc);
|
|
iocshRegister(&setMaxSubsequentTimeoutsDef,
|
|
setMaxSubsequentTimeoutsCallFunc);
|
|
iocshRegister(&setForcedFastPollsDef, setForcedFastPollsCallFunc);
|
|
}
|
|
epicsExportRegistrar(sinqControllerRegister);
|
|
|
|
} // extern C
|