Files
sinqMotor/src/sinqController.cpp
2024-12-04 13:38:13 +01:00

524 lines
21 KiB
C++

#include "sinqController.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include "sinqAxis.h"
#include <errlog.h>
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,
/*
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 + 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
{
// Initialization of local variables
asynStatus status = asynSuccess;
// Initialization of all member variables
lowLevelPortUser_ = nullptr;
// =========================================================================;
/*
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, &lowLevelPortUser_, NULL);
if (status != asynSuccess || lowLevelPortUser_ == nullptr) {
errlogPrintf(
"%s => line %d:\nFATAL ERROR (cannot connect to MCU controller).\n"
"Terminating IOC",
__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, &motorMessageText_);
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);
}
// Internal parameter library entry which stores the movement target
status = createParam("MOTOR_TARGET_POSITION", asynParamFloat64,
&motorTargetPosition_);
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_ENABLE", asynParamInt32, &motorEnable_);
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_ENABLE_RBV", asynParamInt32, &motorEnableRBV_);
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_CAN_DISABLE", asynParamInt32, &motorCanDisable_);
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_CAN_SET_SPEED", asynParamInt32, &motorCanSetSpeed_);
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_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);
}
/*
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("MOTOR_ENABLE_MOV_WATCHDOG", asynParamInt32,
&motorEnableMovWatchdog_);
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_VELO_FROM_DRIVER", asynParamFloat64,
&motorVeloFromDriver_);
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_VBAS_FROM_DRIVER", asynParamFloat64,
&motorVbasFromDriver_);
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_VMAX_FROM_DRIVER", asynParamFloat64,
&motorVmaxFromDriver_);
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_ACCL_FROM_DRIVER", asynParamFloat64,
&motorAcclFromDriver_);
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) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFATAL ERROR (starting the poller failed "
"with %s).\nTerminating IOC",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
pasynOctetSyncIO->disconnect(lowLevelPortUser_);
exit(-1);
}
}
sinqController::~sinqController(void) {
/*
Cleanup of the memory allocated in the asynMotorController constructor
*/
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 == motorEnable_) {
return axis->enable(value != 0);
} else {
return asynMotorController::writeInt32(pasynUser, value);
}
}
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
if (pasynUser->reason == motorEnableRBV_) {
// Value is updated in the poll function of an axis
return asynSuccess;
} else if (pasynUser->reason == motorCanDisable_) {
// By default, motors cannot be disabled
*value = 1;
return asynSuccess;
} else {
return asynMotorController::readInt32(pasynUser, value);
}
}
asynStatus sinqController::errMsgCouldNotParseResponse(const char *command,
const char *response,
int axisNo,
const char *functionName,
int lineNumber) {
asynStatus pl_status = asynSuccess;
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\nCould not interpret response %s for "
"command %s.\n",
functionName, lineNumber, response, command);
pl_status = setStringParam(
motorMessageText_,
"Could not interpret MCU response. Please call the support");
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorMessageText_",
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = setIntegerParam(motorStatusCommsError_, 1);
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
__PRETTY_FUNCTION__, __LINE__);
}
return asynError;
}
asynStatus sinqController::paramLibAccessFailed(asynStatus status,
const char *parameter,
const char *functionName,
int lineNumber) {
if (status != asynSuccess) {
// Log the error message and try to propagate it. If propagating fails,
// there is nothing we can do here anyway.
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\n Accessing the parameter library failed for "
"parameter %s with error %s.\n",
functionName, lineNumber, parameter,
stringifyAsynStatus(status));
setStringParam(motorMessageText_,
"Accessing paramLib failed. Please call the support.");
}
return status;
}
// 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;
}
// =============================================================================
// Provide the setters to IOC shell
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;
pC = (sinqController *)findAsynPortDriver(portName);
if (pC == nullptr) {
errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
__LINE__, portName);
return asynError;
}
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
if (axis == nullptr) {
errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
"instance of sinqAxis.",
__PRETTY_FUNCTION__, __LINE__, axisNo);
}
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};
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("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
__LINE__, portName);
return asynError;
}
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
if (axis == nullptr) {
errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
"instance of sinqAxis.",
__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};
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("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
__LINE__, portName);
return asynError;
}
asynMotorAxis *asynAxis = pC->getAxis(axisNo);
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
if (axis == nullptr) {
errlogPrintf("%s => line %d:\nAxis %d does not exist or is not an "
"instance of sinqAxis.",
__PRETTY_FUNCTION__, __LINE__, axisNo);
}
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};
static void setScaleMovTimeoutCallFunc(const iocshArgBuf *args) {
setScaleMovTimeout(args[0].sval, args[1].ival, args[2].dval);
}
// =============================================================================
static void sinqControllerRegister(void) {
iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
iocshRegister(&setScaleMovTimeoutDef, setScaleMovTimeoutCallFunc);
iocshRegister(&setWatchdogEnabledDef, setWatchdogEnabledCallFunc);
}
epicsExportRegistrar(sinqControllerRegister);
} // extern C