375 lines
15 KiB
C++
375 lines
15 KiB
C++
#include "el734Controller.h"
|
|
#include "asynInt32SyncIO.h"
|
|
#include "asynMotorController.h"
|
|
#include "asynOctetSyncIO.h"
|
|
#include "el734Axis.h"
|
|
#include <epicsExport.h>
|
|
#include <errlog.h>
|
|
#include <initHooks.h>
|
|
#include <iocsh.h>
|
|
#include <netinet/in.h>
|
|
#include <registryFunction.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
|
|
struct el734ControllerImpl {
|
|
|
|
// Timeout for the communication process in seconds
|
|
double comTimeout;
|
|
|
|
char lastResponse[sinqController::MAXBUF_];
|
|
|
|
// User for writing int32 values to the port driver.
|
|
asynUser *pasynInt32SyncIOipPort;
|
|
|
|
// Indices of additional ParamLib entries
|
|
int limFromHardware;
|
|
};
|
|
#define NUM_el734_DRIVER_PARAMS 1
|
|
|
|
el734Controller::el734Controller(const char *portName,
|
|
const char *ipPortConfigName, int numAxes,
|
|
double movingPollPeriod, double idlePollPeriod,
|
|
double comTimeout, int numExtraParams)
|
|
: sinqController(portName, ipPortConfigName, numAxes, movingPollPeriod,
|
|
idlePollPeriod, numExtraParams + NUM_el734_DRIVER_PARAMS),
|
|
pEl734C_(std::make_unique<el734ControllerImpl>((el734ControllerImpl){
|
|
.comTimeout = comTimeout,
|
|
.lastResponse = {0},
|
|
.limFromHardware = 0,
|
|
}))
|
|
|
|
{
|
|
// Initialization of local variables
|
|
asynStatus status = asynSuccess;
|
|
|
|
// Maximum allowed number of subsequent timeouts before the user is
|
|
// informed.
|
|
setMaxSubsequentTimeouts(10);
|
|
|
|
// =========================================================================
|
|
// Create additional parameter library entries
|
|
|
|
status = createParam("LIM_FROM_HARDWARE", asynParamInt32,
|
|
&pEl734C_->limFromHardware);
|
|
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);
|
|
}
|
|
|
|
/*
|
|
The el734 controller expects a carriage return as terminator and terminates
|
|
each reply with a carriage return (el734_manual.pdf, p. 58).
|
|
*/
|
|
pasynOctetSyncIO->setOutputEos(pasynOctetSyncIOipPort(), "\r",
|
|
strlen("\r"));
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d\nFATAL ERROR "
|
|
"(setting input EOS failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort());
|
|
exit(-1);
|
|
}
|
|
|
|
pasynOctetSyncIO->setInputEos(pasynOctetSyncIOipPort(), "\r", strlen("\r"));
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d\nFATAL ERROR "
|
|
"(setting input EOS failed with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort());
|
|
exit(-1);
|
|
}
|
|
|
|
status = callParamCallbacks();
|
|
if (status != asynSuccess) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\" => %s, line %d\nFATAL ERROR "
|
|
"(executing ParamLib callbacks failed "
|
|
"with %s).\nTerminating IOC",
|
|
portName, __PRETTY_FUNCTION__, __LINE__,
|
|
stringifyAsynStatus(status));
|
|
pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort());
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
el734Controller::~el734Controller() {}
|
|
|
|
/*
|
|
Access one of the axes of the controller via the axis adress stored in asynUser.
|
|
If the axis does not exist or is not a Axis, a nullptr is returned and an
|
|
error is emitted.
|
|
*/
|
|
el734Axis *el734Controller::getEl734Axis(asynUser *pasynUser) {
|
|
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
|
|
return dynamic_cast<el734Axis *>(asynAxis);
|
|
}
|
|
|
|
/*
|
|
Access one of the axes of the controller via the axis index.
|
|
If the axis does not exist or is not a Axis, the function must return Null
|
|
*/
|
|
el734Axis *el734Controller::getEl734Axis(int axisNo) {
|
|
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
|
|
return dynamic_cast<el734Axis *>(asynAxis);
|
|
}
|
|
|
|
asynStatus el734Controller::writeRead(int axisNo, const char *command,
|
|
char *response) {
|
|
|
|
// Definition of local variables.
|
|
asynStatus status = asynSuccess;
|
|
asynStatus timeoutStatus = asynSuccess;
|
|
char drvMessageText[MAXBUF_] = {0};
|
|
int motorStatusProblem = 0;
|
|
int numReceivedResponses = 0;
|
|
|
|
/*
|
|
asyn defines the following reasons for an end-of-message coming from the MCU
|
|
(https://epics.anl.gov/modules/soft/asyn/R4-14/asynDriver.pdf, p. 28):
|
|
0: Timeout
|
|
1: Request count reached
|
|
2: End of string detected -> In this driver, this is the "normal" case
|
|
4: End indicator detected
|
|
Combinations of reasons are also possible, e.g. eomReason = 5 would mean
|
|
that both the request count was reached and an end indicator was detected.
|
|
*/
|
|
int eomReason = 0;
|
|
|
|
// Number of bytes of the outgoing message (which is command + the
|
|
// end-of-string terminator defined in the constructor)
|
|
size_t nbytesOut = 0;
|
|
|
|
// Number of bytes of the incoming message (which is response + the
|
|
// end-of-string terminator defined in the constructor)
|
|
size_t nbytesIn = 0;
|
|
|
|
// =========================================================================
|
|
|
|
el734Axis *axis = getEl734Axis(axisNo);
|
|
if (axis == nullptr) {
|
|
// We already did the error logging directly in getAxis
|
|
return asynError;
|
|
}
|
|
const size_t commandLength = strlen(command);
|
|
|
|
/*
|
|
The writeRead command performs the following steps:
|
|
1) Flush the socket buffer on the IOC side (not the controller!)
|
|
2) Write a command to the controller
|
|
3) Read the response
|
|
|
|
If a timeout occurs during writing or reading, inform the user that we're
|
|
trying to reconnect. If the problem persists, ask them to call the support
|
|
*/
|
|
status = pasynOctetSyncIO->writeRead(
|
|
pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_,
|
|
pEl734C_->comTimeout, &nbytesOut, &nbytesIn, &eomReason);
|
|
|
|
/*
|
|
Check if the return message is ?LOC - this means that the controller is in
|
|
local mode and does not accept commands from the driver. Report an error
|
|
and return
|
|
*/
|
|
if (strstr(response, "?LOC") != NULL) {
|
|
if (getMsgPrintControl().shouldBePrinted(portName, axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
true, pasynUser())) {
|
|
asynPrint(
|
|
this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nController is in "
|
|
"local mode, use the RESET PV to put it into remote mode.%s\n",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
|
|
getMsgPrintControl().getSuffix());
|
|
}
|
|
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"Controller is in local mode, use the reset button to put it "
|
|
"into remote mode.");
|
|
status = asynError;
|
|
} else if (strstr(response, "?CMD") != NULL) {
|
|
if (getMsgPrintControl().shouldBePrinted(portName, axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
true, pasynUser())) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nCould not "
|
|
"interpret command %s.%s\n",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__, command,
|
|
getMsgPrintControl().getSuffix());
|
|
}
|
|
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"Could not interpret command %s. Please call the support.",
|
|
command);
|
|
status = asynError;
|
|
} else if (strstr(response, "?PAR") != NULL) {
|
|
if (getMsgPrintControl().shouldBePrinted(portName, axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
true, pasynUser())) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nInvalid "
|
|
"parameters in command %s.%s\n",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__, command,
|
|
getMsgPrintControl().getSuffix());
|
|
}
|
|
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"Invalid parameters in command %s. Please call the support.",
|
|
command);
|
|
status = asynError;
|
|
} else if (strstr(response, "?RNG") != NULL) {
|
|
if (getMsgPrintControl().shouldBePrinted(portName, axisNo,
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
true, pasynUser())) {
|
|
asynPrint(this->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nParameter in "
|
|
"command %s out of range.%s\n",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__, command,
|
|
getMsgPrintControl().getSuffix());
|
|
}
|
|
|
|
snprintf(
|
|
drvMessageText, sizeof(drvMessageText),
|
|
"Parameter in command %s out of range. Please call the support.",
|
|
command);
|
|
status = asynError;
|
|
}
|
|
|
|
// Create custom error messages for different failure modes, if no error
|
|
// message has been set yet
|
|
if (strlen(drvMessageText) == 0) {
|
|
switch (status) {
|
|
case asynSuccess:
|
|
break; // Communicate nothing
|
|
case asynTimeout:
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"connection timeout for axis %d", axisNo);
|
|
break;
|
|
case asynDisconnected:
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"axis is not connected");
|
|
break;
|
|
case asynDisabled:
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"axis is disabled");
|
|
break;
|
|
default:
|
|
snprintf(drvMessageText, sizeof(drvMessageText),
|
|
"Communication failed (%s)", stringifyAsynStatus(status));
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Log the overall status (communication successfull or not)
|
|
if (status == asynSuccess) {
|
|
setAxisParamChecked(axis, motorStatusCommsError, false);
|
|
} else {
|
|
// Check if the axis already is in an error communication mode. If
|
|
// it is not, upstream the error. This is done to avoid "flooding"
|
|
// the user with different error messages if more than one error
|
|
// ocurred before an error-free communication
|
|
getAxisParamChecked(axis, motorStatusProblem, &motorStatusProblem);
|
|
|
|
if (motorStatusProblem == 0) {
|
|
setAxisParamChecked(axis, motorMessageText, drvMessageText);
|
|
setAxisParamChecked(axis, motorStatusProblem, true);
|
|
setAxisParamChecked(axis, motorStatusCommsError, true);
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
int el734Controller::limFromHardware() { return pEl734C_->limFromHardware; }
|
|
|
|
asynUser *el734Controller::pasynInt32SyncIOipPort() {
|
|
return pEl734C_->pasynInt32SyncIOipPort;
|
|
}
|
|
|
|
/*************************************************************************************/
|
|
/** The following functions are C-wrappers, and can be called directly from
|
|
* iocsh */
|
|
|
|
extern "C" {
|
|
|
|
/*
|
|
C wrapper for the controller constructor. Please refer to the
|
|
el734Controller constructor documentation.
|
|
*/
|
|
asynStatus el734CreateController(const char *portName,
|
|
const char *ipPortConfigName, int numAxes,
|
|
double movingPollPeriod, double idlePollPeriod,
|
|
double comTimeout) {
|
|
/*
|
|
We create a new instance of the controller, using the "new" keyword to
|
|
allocate it on the heap while avoiding RAII.
|
|
https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp
|
|
https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp
|
|
|
|
The created object is registered in EPICS in its constructor and can
|
|
safely be "leaked" here.
|
|
*/
|
|
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
|
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
|
|
|
el734Controller *pController =
|
|
new el734Controller(portName, ipPortConfigName, numAxes,
|
|
movingPollPeriod, idlePollPeriod, comTimeout);
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
/*
|
|
This is boilerplate code which is used to make the FFI functions
|
|
CreateController and CreateAxis "known" to the IOC shell (iocsh).
|
|
*/
|
|
|
|
#ifndef vxWorks
|
|
|
|
/*
|
|
Define name and type of the arguments for the CreateController function
|
|
in the iocsh. This is done by creating structs with the argument names and
|
|
types and then providing "factory" functions
|
|
(configCreateControllerCallFunc). These factory functions are used to
|
|
register the constructors during compilation.
|
|
*/
|
|
static const iocshArg CreateControllerArg0 = {"Controller name (e.g. mcu1)",
|
|
iocshArgString};
|
|
static const iocshArg CreateControllerArg1 = {"Asyn IP port name (e.g. pmcu1)",
|
|
iocshArgString};
|
|
static const iocshArg CreateControllerArg2 = {"Number of axes", iocshArgInt};
|
|
static const iocshArg CreateControllerArg3 = {"Moving poll rate (s)",
|
|
iocshArgDouble};
|
|
static const iocshArg CreateControllerArg4 = {"Idle poll rate (s)",
|
|
iocshArgDouble};
|
|
static const iocshArg CreateControllerArg5 = {"Communication timeout (s)",
|
|
iocshArgDouble};
|
|
static const iocshArg *const CreateControllerArgs[] = {
|
|
&CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2,
|
|
&CreateControllerArg3, &CreateControllerArg4, &CreateControllerArg5};
|
|
static const iocshFuncDef configEl734CreateController = {"el734Controller", 6,
|
|
CreateControllerArgs};
|
|
static void configEl734CreateControllerCallFunc(const iocshArgBuf *args) {
|
|
el734CreateController(args[0].sval, args[1].sval, args[2].ival,
|
|
args[3].dval, args[4].dval, args[5].dval);
|
|
}
|
|
|
|
// This function is made known to EPICS in el734.dbd and is called by
|
|
// EPICS in order to register both functions in the IOC shell
|
|
static void el734ControllerRegister(void) {
|
|
iocshRegister(&configEl734CreateController,
|
|
configEl734CreateControllerCallFunc);
|
|
}
|
|
epicsExportRegistrar(el734ControllerRegister);
|
|
|
|
#endif
|
|
|
|
} // extern "C"
|