Files
mastermacs/src/masterMacsController.cpp

560 lines
23 KiB
C++

#include "masterMacsController.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "masterMacsAxis.h"
#include <epicsExport.h>
#include <errlog.h>
#include <iocsh.h>
#include <netinet/in.h>
#include <registryFunction.h>
#include <string.h>
#include <string>
#include <unistd.h>
/**
* @brief Copy src into dst and replace all NULL terminators up to the carriage
* return with spaces. This allows to print *dst with asynPrint.
*
* @param dst Buffer for the modified string
* @param src Original string
*/
void adjustForPrint(char *dst, const char *src, size_t buf_length) {
for (size_t i = 0; i < buf_length; i++) {
if (src[i] == '\x0d') {
dst[i] = ' ';
break;
} else if (src[i] == '\x00') {
dst[i] = ' ';
} else {
dst[i] = src[i];
}
}
}
/**
* @brief Construct a new masterMacsController::masterMacsController 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
*/
masterMacsController::masterMacsController(const char *portName,
const char *ipPortConfigName,
int numAxes, double movingPollPeriod,
double idlePollPeriod,
double comTimeout)
: sinqController(portName, ipPortConfigName, numAxes, movingPollPeriod,
idlePollPeriod,
// No additional parameter library entries
0)
{
// Initialization of local variables
asynStatus status = asynSuccess;
// Initialization of all member variables
comTimeout_ = comTimeout;
// =========================================================================
/*
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.
*/
const char *message_from_device = "\x0D"; // Hex-code for CR
status = pasynOctetSyncIO->setInputEos(pasynOctetSyncIOipPort(),
message_from_device,
strlen(message_from_device));
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, 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->pasynUserSelf, 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);
}
}
/*
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.
*/
masterMacsAxis *masterMacsController::getMasterMacsAxis(asynUser *pasynUser) {
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
return dynamic_cast<masterMacsAxis *>(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
*/
masterMacsAxis *masterMacsController::getMasterMacsAxis(int axisNo) {
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
return dynamic_cast<masterMacsAxis *>(asynAxis);
}
asynStatus masterMacsController::read(int axisNo, int tcpCmd, char *response,
double comTimeout) {
return writeRead(axisNo, tcpCmd, NULL, response);
}
asynStatus masterMacsController::write(int axisNo, int tcpCmd,
const char *payload, double comTimeout) {
return writeRead(axisNo, tcpCmd, payload, NULL, comTimeout);
}
asynStatus masterMacsController::writeRead(int axisNo, int tcpCmd,
const char *payload, char *response,
double comTimeout) {
// Definition of local variables.
asynStatus status = asynSuccess;
asynStatus pl_status = asynSuccess;
char fullCommand[MAXBUF_] = {0};
char fullResponse[MAXBUF_] = {0};
char drvMessageText[MAXBUF_] = {0};
int motorStatusProblem = 0;
int valueStart = 0;
int valueStop = 0;
// Send the message and block the thread until either a response has
// been received or the timeout is triggered
int eomReason = 0; // Flag indicating why the message has ended
// 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;
// Do we expect an response?
bool isRead = response != NULL;
// =========================================================================
// Check if a custom timeout has been given
if (comTimeout < 0.0) {
comTimeout = comTimeout_;
}
masterMacsAxis *axis = getMasterMacsAxis(axisNo);
if (axis == nullptr) {
// We already did the error logging directly in getAxis
return asynError;
}
// Build the full command depending on the inputs to this function
if (isRead) {
snprintf(fullCommand, MAXBUF_ - 1, "%dR%02d\x0D", axisNo, tcpCmd);
} else {
if (strlen(payload) == 0) {
snprintf(fullCommand, MAXBUF_ - 1, "%dS%02d\x0D", axisNo, tcpCmd);
} else {
snprintf(fullCommand, MAXBUF_ - 1, "%dS%02d=%s\x0D", axisNo, tcpCmd,
payload);
}
}
// Calculate the command length
const size_t fullCommandLength = strlen(fullCommand);
// Flush the IOC-side socket, then write the command and wait for the
// response.
status = pasynOctetSyncIO->writeRead(
pasynOctetSyncIOipPort(), fullCommand, fullCommandLength, fullResponse,
MAXBUF_, comTimeout, &nbytesOut, &nbytesIn, &eomReason);
// If a communication error occured, print this message to the
msgPrintControlKey comKey =
msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
if (status != asynSuccess) {
if (msgPrintControl_.shouldBePrinted(comKey, true, pasynUserSelf)) {
char printableCommand[MAXBUF_] = {0};
adjustForPrint(printableCommand, fullCommand, MAXBUF_);
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nError "
"%s while sending command %s to the controller\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
stringifyAsynStatus(status), printableCommand);
}
} else {
msgPrintControl_.resetCount(comKey, pasynUserSelf);
}
// Create custom error messages for different failure modes
switch (status) {
case asynSuccess:
// We did get a response, but does it make sense and is it designated as
// OK from the controller? This is checked here.
status = parseResponse(fullCommand, fullResponse, drvMessageText,
&valueStart, &valueStop, axisNo, tcpCmd, isRead);
// Read out the important information from the response
if (status == asynSuccess && isRead) {
/*
If a property has been read, we need just the part between the
"=" (0x3D) and the [ACK] (0x06). Therefore, we remove all
non-needed parts after evaluating the second-to-last char before
returning the response.
*/
for (int i = 0; i + valueStart < valueStop; i++) {
response[i] = fullResponse[i + valueStart];
}
}
break;
case asynTimeout:
snprintf(drvMessageText, sizeof(drvMessageText),
"Connection timeout. Please call the support.");
break;
case asynDisconnected:
snprintf(drvMessageText, sizeof(drvMessageText),
"Axis is not connected.");
break;
case asynDisabled:
snprintf(drvMessageText, sizeof(drvMessageText), "Axis is disabled.");
break;
case asynError:
// Do nothing - error message drvMessageText has already been set.
break;
default:
snprintf(drvMessageText, sizeof(drvMessageText),
"Communication failed (%s). Please call the support.",
stringifyAsynStatus(status));
break;
}
// Log the overall status (communication successfull or not)
if (status == asynSuccess) {
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
axisNo, __PRETTY_FUNCTION__, __LINE__);
}
} else if (status == asynDisconnected) {
// Do nothing
} else {
// Set the error status bits only if the axis is not disconnected
// 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_",
axisNo, __PRETTY_FUNCTION__, __LINE__);
}
if (motorStatusProblem == 0) {
pl_status = axis->setStringParam(motorMessageText_, drvMessageText);
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorMessageText_",
axisNo, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorStatusProblem",
axisNo, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
if (pl_status != asynSuccess) {
return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
axisNo, __PRETTY_FUNCTION__,
__LINE__);
}
}
}
return status;
}
/*
A response looks like this (note the spaces, they are part of the
message!):
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[ACK][CR] (No error)
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[NAK][CR] (Communication failed)
- [ENQ][LSB][MSB][PDO1] 1 S 10 [CAN][CR] (Driver tried to write with
a read-only command) Read out the second-to-last char of the
response and check if it is NAK or CAN.
*/
asynStatus masterMacsController::parseResponse(
const char *fullCommand, const char *fullResponse, char *drvMessageText,
int *valueStart, int *valueStop, int axisNo, int tcpCmd, bool isRead) {
int responseStart = 0;
asynStatus status = asynSuccess;
int prevConnected = 0;
char printableCommand[MAXBUF_] = {0};
char printableResponse[MAXBUF_] = {0};
msgPrintControlKey parseKey =
msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
// Was the motor previously connected?
status = getIntegerParam(axisNo, motorConnected(), &prevConnected);
if (status != asynSuccess) {
return paramLibAccessFailed(status, "motorConnected", axisNo,
__PRETTY_FUNCTION__, __LINE__);
}
// We don't use strlen here since the C string terminator 0x00
// occurs in the middle of the char array.
for (uint32_t i = 0; i < MAXBUF_; i++) {
if (fullResponse[i] == '\x19') {
responseStart = i;
} else if (fullResponse[i] == '=') {
*valueStart = i + 1;
} else if (fullResponse[i] == '\x06') {
// ACK
*valueStop = i;
// Motor wasn't connected before -> Update the paramLib entry and PV
// to show it is now connected.
if (prevConnected == 0) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\nAxis connection status has changed to "
"connected.\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
masterMacsAxis *axis = getMasterMacsAxis(axisNo);
if (axis == nullptr) {
return asynError;
}
status = axis->setIntegerParam(motorConnected(), 1);
if (status != asynSuccess) {
return paramLibAccessFailed(status, "motorConnected",
axisNo, __PRETTY_FUNCTION__,
__LINE__);
}
status = callParamCallbacks();
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\nCould not update parameter library\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
return status;
}
}
msgPrintControl_.resetCount(parseKey, pasynUserSelf);
// Check if the response matches the expectations. Each response
// contains the string "axisNo R tcpCmd" (including the spaces)
char expectedResponseSubstring[MAXBUF_] = {0};
// The response does not contain a leading 0 if tcpCmd only has
// a single digit!
if (isRead) {
snprintf(expectedResponseSubstring, MAXBUF_ - 4, "%d R %d",
axisNo, tcpCmd);
} else {
snprintf(expectedResponseSubstring, MAXBUF_ - 4, "%d S %d",
axisNo, tcpCmd);
}
msgPrintControlKey responseMatchKey = msgPrintControlKey(
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
if (strstr(&fullResponse[responseStart],
expectedResponseSubstring) == NULL) {
adjustForPrint(printableCommand, fullCommand, MAXBUF_);
adjustForPrint(printableResponse, fullResponse, MAXBUF_);
if (msgPrintControl_.shouldBePrinted(parseKey, true,
pasynUserSelf)) {
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
"Controller \"%s\", axis %d => %s, line "
"%d:\nMismatched "
"response %s to command %s.%s\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
printableResponse, printableCommand,
msgPrintControl_.getSuffix());
}
snprintf(
drvMessageText, MAXBUF_,
"Mismatched response %s to command %s. Please call the "
"support.",
printableResponse, printableCommand);
return asynError;
} else {
msgPrintControl_.resetCount(responseMatchKey, pasynUserSelf);
}
return asynSuccess;
} else if (fullResponse[i] == '\x15') {
/*
NAK
This indicates that the axis is not connected. This is not an error!
*/
snprintf(drvMessageText, MAXBUF_, "Axis not connected.");
// Motor was connected before -> Update the paramLib entry and PV
// to show it is now disconnected.
if (prevConnected == 1) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\nAxis connection status has changed to "
"disconnected.\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
masterMacsAxis *axis = getMasterMacsAxis(axisNo);
if (axis == nullptr) {
return asynError;
}
status = axis->setIntegerParam(motorConnected(), 0);
if (status != asynSuccess) {
return paramLibAccessFailed(status, "motorConnected",
axisNo, __PRETTY_FUNCTION__,
__LINE__);
}
status = callParamCallbacks();
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\nCould not update parameter library\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
return status;
}
}
return asynDisconnected;
} else if (fullResponse[i] == '\x18') {
// CAN
snprintf(drvMessageText, MAXBUF_,
"Tried to write with a read-only command. This is a "
"bug, please call the support.");
if (msgPrintControl_.shouldBePrinted(parseKey, true,
pasynUserSelf)) {
adjustForPrint(printableCommand, fullCommand, MAXBUF_);
asynPrint(
this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nTried to "
"write with the read-only command %s.%s\n",
portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
printableCommand, msgPrintControl_.getSuffix());
}
return asynError;
}
}
return asynError;
}
asynStatus masterMacsController::readInt32(asynUser *pasynUser,
epicsInt32 *value) {
// masterMacs can be disabled
if (pasynUser->reason == motorCanDisable_) {
*value = 1;
return asynSuccess;
} else {
return asynMotorController::readInt32(pasynUser, value);
}
}
/***************************************************************************/
/** 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
masterMacsController constructor documentation.
*/
asynStatus masterMacsCreateController(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. 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"
masterMacsController *pController =
new masterMacsController(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).
*/
#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. mmacs1)",
iocshArgString};
static const iocshArg CreateControllerArg1 = {
"Asyn IP port name (e.g. pmmacs1)", 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 configMasterMacsCreateController = {
"masterMacsController", 6, CreateControllerArgs};
static void configMasterMacsCreateControllerCallFunc(const iocshArgBuf *args) {
masterMacsCreateController(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 masterMacs.dbd and is called by
// EPICS in order to register both functions in the IOC shell
static void masterMacsControllerRegister(void) {
iocshRegister(&configMasterMacsCreateController,
configMasterMacsCreateControllerCallFunc);
}
epicsExportRegistrar(masterMacsControllerRegister);
#endif
} // extern "C"