Renamed from pmacV3 to turboPmac
This commit is contained in:
651
src/turboPmacController.cpp
Normal file
651
src/turboPmacController.cpp
Normal file
@@ -0,0 +1,651 @@
|
||||
#include "turboPmacController.h"
|
||||
#include "asynMotorController.h"
|
||||
#include "asynOctetSyncIO.h"
|
||||
#include "turboPmacAxis.h"
|
||||
#include <epicsExport.h>
|
||||
#include <errlog.h>
|
||||
#include <iocsh.h>
|
||||
#include <netinet/in.h>
|
||||
#include <registryFunction.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/**
|
||||
* @brief Copy src into dst and replace all carriage returns with spaces. This
|
||||
* allows to print *dst with asynPrint.
|
||||
*
|
||||
*
|
||||
* @param dst Buffer for the modified string
|
||||
* @param src Original string
|
||||
*/
|
||||
void adjustResponseForPrint(char *dst, const char *src, size_t buf_length) {
|
||||
for (size_t i = 0; i < buf_length; i++) {
|
||||
if (src[i] == '\r') {
|
||||
dst[i] = ' ';
|
||||
} else {
|
||||
dst[i] = src[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct a new turboPmacController::turboPmacController object
|
||||
*
|
||||
* @param portName See documentation of sinqController
|
||||
* @param ipPortConfigName See documentation of sinqController
|
||||
* @param numAxes See documentation of sinqController
|
||||
* @param movingPollPeriod See documentation of sinqController
|
||||
* @param idlePollPeriod See documentation of sinqController
|
||||
* @param comTimeout Time after which a communication timeout error
|
||||
* is declared in writeRead (in seconds)
|
||||
* @param extraParams See documentation of sinqController
|
||||
*/
|
||||
turboPmacController::turboPmacController(const char *portName,
|
||||
const char *ipPortConfigName,
|
||||
int numAxes, double movingPollPeriod,
|
||||
double idlePollPeriod,
|
||||
double comTimeout)
|
||||
: sinqController(
|
||||
portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod,
|
||||
/*
|
||||
The following parameter library entries are added in this driver:
|
||||
- REREAD_ENCODER_POSITION
|
||||
- READ_CONFIG
|
||||
*/
|
||||
NUM_turboPmac_DRIVER_PARAMS)
|
||||
|
||||
{
|
||||
|
||||
// Initialization of local variables
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
// Initialization of all member variables
|
||||
lowLevelPortUser_ = nullptr;
|
||||
comTimeout_ = comTimeout;
|
||||
|
||||
// =========================================================================;
|
||||
|
||||
/*
|
||||
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);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Create additional parameter library entries
|
||||
|
||||
status = createParam("REREAD_ENCODER_POSITION", asynParamInt32,
|
||||
&rereadEncoderPosition_);
|
||||
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("READ_CONFIG", asynParamInt32, &readConfig_);
|
||||
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);
|
||||
}
|
||||
|
||||
/*
|
||||
Define the end-of-string of a message coming from the device to EPICS.
|
||||
It is not necessary to append a terminator to outgoing messages, since
|
||||
the message length is encoded in the message header in the getSetResponse
|
||||
method.
|
||||
*/
|
||||
const char *message_from_device =
|
||||
"\006"; // Hex-code for ACK (acknowledge) -> Each message from the MCU
|
||||
// is terminated by this value
|
||||
status = pasynOctetSyncIO->setInputEos(
|
||||
lowLevelPortUser_, message_from_device, strlen(message_from_device));
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (setting input EOS failed "
|
||||
"with %s).\nTerminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
|
||||
pasynOctetSyncIO->disconnect(lowLevelPortUser_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = callParamCallbacks();
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (executing ParamLib callbacks failed "
|
||||
"with %s).\nTerminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
|
||||
pasynOctetSyncIO->disconnect(lowLevelPortUser_);
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
turboPmacAxis *turboPmacController::getAxis(asynUser *pasynUser) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
|
||||
return turboPmacController::castToAxis(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
|
||||
*/
|
||||
turboPmacAxis *turboPmacController::getAxis(int axisNo) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
|
||||
return turboPmacController::castToAxis(asynAxis);
|
||||
}
|
||||
|
||||
turboPmacAxis *turboPmacController::castToAxis(asynMotorAxis *asynAxis) {
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// If the axis slot of the pAxes_ array is empty, a nullptr must be returned
|
||||
if (asynAxis == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Here, an error is emitted since asyn_axis is not a nullptr but also not
|
||||
// an instance of Axis
|
||||
turboPmacAxis *axis = dynamic_cast<turboPmacAxis *>(asynAxis);
|
||||
if (axis == nullptr) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nAxis %d is not an instance of turboPmacAxis",
|
||||
__PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
|
||||
}
|
||||
return axis;
|
||||
}
|
||||
|
||||
asynStatus turboPmacController::writeRead(int axisNo, const char *command,
|
||||
char *response,
|
||||
int numExpectedResponses) {
|
||||
|
||||
// Definition of local variables.
|
||||
asynStatus status = asynSuccess;
|
||||
asynStatus pl_status = asynSuccess;
|
||||
char fullCommand[MAXBUF_] = {0};
|
||||
char drvMessageText[MAXBUF_] = {0};
|
||||
char modResponse[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;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
turboPmacAxis *axis = getAxis(axisNo);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
/*
|
||||
The message protocol of the turboPmac used at PSI looks as follows (all
|
||||
characters immediately following each other without a newline):
|
||||
0x40 (ASCII value of @) -> Request for download
|
||||
0xBF (ASCII value of ¿) -> Select mode "get_response"
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
[message length in network byte order] -> Use the htons function for this
|
||||
value [Actual message] It is not necessary to append a terminator, since
|
||||
this protocol encodes the message length at the beginning. See Turbo PMAC
|
||||
User Manual, page 418 in VR_PMAC_GETRESPONSE
|
||||
|
||||
The message has to be build manually into the buffer fullCommand, since it
|
||||
contains NULL terminators in its middle, therefore the string manipulation
|
||||
methods of C don't work.
|
||||
*/
|
||||
|
||||
const size_t commandLength = strlen(command);
|
||||
const int offset = 9;
|
||||
|
||||
// Positions 2 to 6 must have the value 0. Since fullCommand is initialized
|
||||
// as an array of zeros, we don't need to set these bits manually.
|
||||
fullCommand[0] = '\x40';
|
||||
fullCommand[1] = '\xBF';
|
||||
|
||||
// The size of size_t is platform dependant (pointers-sized), while htons
|
||||
// needs an unsigned int. The byte order is then converted from host to
|
||||
// network order.
|
||||
u_int16_t len = htons(static_cast<u_int16_t>(commandLength));
|
||||
|
||||
// Split up into the upper and the lower byte
|
||||
fullCommand[7] = (char)(len >> 8); // Shift the 8 higher bits to the right
|
||||
fullCommand[8] = (char)(len & 0xFF); // Mask the higher bits
|
||||
|
||||
// Write the actual command behind the protocol
|
||||
for (int i = 0; i < commandLength; i++) {
|
||||
fullCommand[i + offset] = command[i];
|
||||
}
|
||||
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
|
||||
"%s => line %d:\nSending command %s", __PRETTY_FUNCTION__,
|
||||
__LINE__, fullCommand);
|
||||
|
||||
// TEST CODE
|
||||
/*
|
||||
Check if there is data on the MCU which is waiting to be read
|
||||
TBD: Flush if there is something?
|
||||
*/
|
||||
u_int16_t val = htons(2);
|
||||
char readReadyCommand[MAXBUF_] = {0};
|
||||
char readReadyResponse[MAXBUF_] = {0};
|
||||
readReadyCommand[0] = '\xC0';
|
||||
readReadyCommand[1] = '\xC2';
|
||||
readReadyCommand[4] = (char)(val >> 8);
|
||||
readReadyCommand[5] = (char)(val & 0xFF);
|
||||
status = pasynOctetSyncIO->write(lowLevelPortUser_, readReadyCommand, 6,
|
||||
comTimeout_, &nbytesOut);
|
||||
|
||||
status =
|
||||
pasynOctetSyncIO->read(lowLevelPortUser_, readReadyResponse, MAXBUF_,
|
||||
comTimeout_, &nbytesIn, &eomReason);
|
||||
|
||||
if (readReadyResponse[1] != 0) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nBytes: %x %x\n", __PRETTY_FUNCTION__,
|
||||
__LINE__, readReadyResponse[0], readReadyResponse[1]);
|
||||
}
|
||||
|
||||
/*
|
||||
We use separated write and read commands here, not the combined writeRead
|
||||
method, because the latter is actually a flushWriteRead (see
|
||||
https://epics.anl.gov/modules/soft/asyn/R4-14/asynDriver.pdf, p. 31) ->
|
||||
Calls the flush command, then the write command, then the read command.
|
||||
The flush itself reads repeatedly from the MCU until no messages are there
|
||||
anymore. (The Diamond Light Source driver first send a PMAC flush command
|
||||
and then does the same as the asyn flush). We don't want this behaviour.
|
||||
(https://www.slac.stanford.edu/grp/lcls/controls/global/doc/epics-modules/R3-14-12/asyn/asyn-R4-18-lcls2/asyn/interfaces/asynOctetBase.c):
|
||||
*/
|
||||
|
||||
// Send out the command
|
||||
status = pasynOctetSyncIO->write(lowLevelPortUser_, fullCommand,
|
||||
commandLength + offset, comTimeout_,
|
||||
&nbytesOut);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nError %s while writing to the controller\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
|
||||
pl_status = setStringParam(
|
||||
motorMessageText_, "Communication timeout between IOC and "
|
||||
"motor controller. Please call the support.");
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
}
|
||||
|
||||
// Read the response from the MCU buffer
|
||||
status = pasynOctetSyncIO->read(lowLevelPortUser_, response, MAXBUF_,
|
||||
comTimeout_, &nbytesIn, &eomReason);
|
||||
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nError %s while reading from the controller\n ",
|
||||
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
|
||||
pl_status = setStringParam(
|
||||
motorMessageText_, "Communication timeout between IOC and "
|
||||
"motor controller. Please call the support.");
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
}
|
||||
|
||||
// // TEST CODE
|
||||
// if (response == lastResponse) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "%s => line %d:\nBytes: %x %x\n", __PRETTY_FUNCTION__,
|
||||
// __LINE__, readReadyResponse[0], readReadyResponse[1]);
|
||||
// } else {
|
||||
// for (int i = 0; i < MAXBUF_; i++) {
|
||||
// lastResponse[i] = response[i];
|
||||
// }
|
||||
// }
|
||||
|
||||
// TBD: The message should only ever terminate due to reason 2 -> Should we
|
||||
// indicate an error otherwise?
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
|
||||
"%s => line %d:\nMessage terminated due to reason %i\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, eomReason);
|
||||
|
||||
/*
|
||||
Calculate the number of received responses by counting the number of
|
||||
carriage returns "\r" in the response.
|
||||
*/
|
||||
for (size_t i = 0; i < strlen(response); i++) {
|
||||
if (response[i] == '\r') {
|
||||
numReceivedResponses++;
|
||||
}
|
||||
}
|
||||
if (numExpectedResponses != numReceivedResponses) {
|
||||
adjustResponseForPrint(modResponse, response, MAXBUF_);
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nUnexpected response '%s' (carriage "
|
||||
"returns are replaced with spaces) for command %s\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, modResponse, command);
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"Received unexpected response '%s' (carriage returns "
|
||||
"are replaced with spaces) for command %s. "
|
||||
"Please call the support",
|
||||
modResponse, command);
|
||||
pl_status = setStringParam(motorMessageText_, drvMessageText);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
status = asynError;
|
||||
}
|
||||
|
||||
// Create custom error messages for different failure modes
|
||||
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) {
|
||||
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
|
||||
"%s => line %d:\nDevice response: %s (_ are "
|
||||
"carriage returns)\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, modResponse);
|
||||
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
|
||||
} else {
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nCommunication failed for command %s (%s)\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, fullCommand,
|
||||
stringifyAsynStatus(status));
|
||||
|
||||
// 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
|
||||
pl_status =
|
||||
getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
if (motorStatusProblem == 0) {
|
||||
pl_status = axis->setStringParam(motorMessageText_, drvMessageText);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorStatusProblem",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus turboPmacController::writeInt32(asynUser *pasynUser,
|
||||
epicsInt32 value) {
|
||||
int function = pasynUser->reason;
|
||||
|
||||
// =====================================================================
|
||||
|
||||
turboPmacAxis *axis = getAxis(pasynUser);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Handle custom PVs
|
||||
if (function == rereadEncoderPosition_) {
|
||||
return axis->rereadEncoder();
|
||||
} else if (function == readConfig_) {
|
||||
return axis->atFirstPoll();
|
||||
} else {
|
||||
return sinqController::writeInt32(pasynUser, value);
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
|
||||
// PMACs can be disabled
|
||||
if (pasynUser->reason == motorCanDisable_) {
|
||||
*value = 1;
|
||||
return asynSuccess;
|
||||
} else {
|
||||
return asynMotorController::readInt32(pasynUser, value);
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus turboPmacController::errMsgCouldNotParseResponse(
|
||||
const char *command, const char *response, int axisNo,
|
||||
const char *functionName, int lineNumber) {
|
||||
char modifiedResponse[MAXBUF_] = {0};
|
||||
adjustResponseForPrint(modifiedResponse, response, MAXBUF_);
|
||||
return sinqController::errMsgCouldNotParseResponse(
|
||||
command, modifiedResponse, axisNo, functionName, lineNumber);
|
||||
}
|
||||
|
||||
/*************************************************************************************/
|
||||
/** 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
|
||||
turboPmacController constructor documentation.
|
||||
*/
|
||||
asynStatus turboPmacCreateController(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"
|
||||
turboPmacController *pController =
|
||||
new turboPmacController(portName, ipPortConfigName, numAxes,
|
||||
movingPollPeriod, idlePollPeriod, comTimeout);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
C wrapper for the axis constructor. Please refer to the turboPmacAxis
|
||||
constructor documentation. The controller is read from the portName.
|
||||
*/
|
||||
asynStatus turboPmacCreateAxis(const char *portName, int axis) {
|
||||
turboPmacAxis *pAxis;
|
||||
|
||||
/*
|
||||
findAsynPortDriver is a asyn library FFI function which uses the C ABI.
|
||||
Therefore it returns a void pointer instead of e.g. a pointer to a
|
||||
superclass of the controller such as asynPortDriver. Type-safe upcasting
|
||||
via dynamic_cast is therefore not possible directly. However, we do know
|
||||
that the void pointer is either a pointer to asynPortDriver (if a driver
|
||||
with the specified name exists) or a nullptr. Therefore, we first do a
|
||||
nullptr check, then a cast to asynPortDriver and lastly a (typesafe)
|
||||
dynamic_upcast to Controller
|
||||
https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c
|
||||
*/
|
||||
void *ptr = findAsynPortDriver(portName);
|
||||
if (ptr == nullptr) {
|
||||
/*
|
||||
We can't use asynPrint here since this macro would require us
|
||||
to get a lowLevelPortUser_ from a pointer to an asynPortDriver.
|
||||
However, the given pointer is a nullptr and therefore doesn't
|
||||
have a lowLevelPortUser_! printf is an EPICS alternative which
|
||||
works w/o that, but doesn't offer the comfort provided
|
||||
by the asynTrace-facility
|
||||
*/
|
||||
errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__,
|
||||
__LINE__, portName);
|
||||
return asynError;
|
||||
}
|
||||
// Unsafe cast of the pointer to an asynPortDriver
|
||||
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
||||
|
||||
// Safe downcast
|
||||
turboPmacController *pC = dynamic_cast<turboPmacController *>(apd);
|
||||
if (pC == nullptr) {
|
||||
errlogPrintf("%s => line %d:\ncontroller on port %s is not a "
|
||||
"turboPmacController.",
|
||||
__PRETTY_FUNCTION__, __LINE__, portName);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Prevent manipulation of the controller from other threads while we
|
||||
// create the new axis.
|
||||
pC->lock();
|
||||
|
||||
/*
|
||||
We create a new instance of the axis, 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"
|
||||
pAxis = new turboPmacAxis(pC, axis);
|
||||
|
||||
// Allow manipulation of the controller again
|
||||
pC->unlock();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
This is boilerplate code which is used to make the FFI functions
|
||||
CreateController and CreateAxis "known" to the IOC shell (iocsh).
|
||||
*/
|
||||
|
||||
#ifdef vxWorks
|
||||
#else
|
||||
|
||||
/*
|
||||
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 configturboPmacCreateController = {
|
||||
"turboPmacController", 6, CreateControllerArgs};
|
||||
static void configturboPmacCreateControllerCallFunc(const iocshArgBuf *args) {
|
||||
turboPmacCreateController(args[0].sval, args[1].sval, args[2].ival,
|
||||
args[3].dval, args[4].dval, args[5].dval);
|
||||
}
|
||||
|
||||
/*
|
||||
Same procedure as for the CreateController function, but for the axis
|
||||
itself.
|
||||
*/
|
||||
static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)",
|
||||
iocshArgString};
|
||||
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
|
||||
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
|
||||
&CreateAxisArg1};
|
||||
static const iocshFuncDef configturboPmacCreateAxis = {"turboPmacAxis", 2,
|
||||
CreateAxisArgs};
|
||||
static void configturboPmacCreateAxisCallFunc(const iocshArgBuf *args) {
|
||||
turboPmacCreateAxis(args[0].sval, args[1].ival);
|
||||
}
|
||||
|
||||
// This function is made known to EPICS in turboPmac.dbd and is called by EPICS
|
||||
// in order to register both functions in the IOC shell
|
||||
static void turboPmacRegister(void) {
|
||||
iocshRegister(&configturboPmacCreateController,
|
||||
configturboPmacCreateControllerCallFunc);
|
||||
iocshRegister(&configturboPmacCreateAxis,
|
||||
configturboPmacCreateAxisCallFunc);
|
||||
}
|
||||
epicsExportRegistrar(turboPmacRegister);
|
||||
|
||||
#endif
|
||||
|
||||
} // extern "C"
|
||||
Reference in New Issue
Block a user