lin-epics-modules/mastermacs
0
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit for masterMacs

Browse Source
This commit is contained in:
mathis_s
2024-12-23 09:40:40 +01:00
commit 7d4c2ea1e4
8 changed files with 2065 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

652
src/masterMacsController.cpp Normal file
View File

@@ -0,0 +1,652 @@
#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,
/*
The following parameter library entries are added in this driver:
- REREAD_ENCODER_POSITION
- READ_CONFIG
*/
NUM_masterMacs_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);
}
/*
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 = "\x03"; // Hex-code for ETX
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.
*/
masterMacsAxis *masterMacsController::getAxis(asynUser *pasynUser) {
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
return masterMacsController::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
*/
masterMacsAxis *masterMacsController::getAxis(int axisNo) {
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
return masterMacsController::castToAxis(asynAxis);
}
masterMacsAxis *masterMacsController::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
masterMacsAxis *axis = dynamic_cast<masterMacsAxis *>(asynAxis);
if (axis == nullptr) {
asynPrint(
this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nAxis %d is not an instance of masterMacsAxis",
__PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
}
return axis;
}
asynStatus masterMacsController::writeRead(int axisNo, const char *command,
char *response,
bool expectResponse) {
// Definition of local variables.
asynStatus status = asynSuccess;
asynStatus pl_status = asynSuccess;
std::string fullCommand = "";
char fullResponse[MAXBUF_] = {0};
char printableCommand[MAXBUF_] = {0};
char printableResponse[MAXBUF_] = {0};
char drvMessageText[MAXBUF_] = {0};
int motorStatusProblem = 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;
// These parameters are used to remove not-needed information from the
// response.
int dataStartIndex = 0;
int validIndex = 0;
bool responseValid = false;
// =========================================================================
masterMacsAxis *axis = getAxis(axisNo);
if (axis == nullptr) {
// We already did the error logging directly in getAxis
return asynError;
}
/*
PSI SINQ uses a custom protocol which is described in
PSI_TCP_Interface_V1-8.pdf (p. // 4-17).
A special case is the message length, which is specified by two bytes LSB
and MSB:
MSB = message length / 256
LSB = message length % 256.
For example, a message length of 47 chars would result in MSB = 0, LSB = 47,
whereas a message length of 356 would result in MSB = 1, LSB = 100.
The full protocol looks as follows:
0x05 -> Start of protocol frame ENQ
[LSB]
[MSB]
0x19 -> Data type PDO1
value [Actual message] It is not necessary to append a terminator, since
this protocol encodes the message length in LSB and MSB.
0x0D -> Carriage return (ASCII alias \r)
0x03 -> End of text ETX
The rest of the telegram length is filled with 0x00 as specified in the
protocol.
*/
// Command has four additional bytes between ENQ and ETX:
// LSB, MSB, PDO1 and CR
const size_t commandLength = strlen(command) + 4;
// Perform both division and modulo operation at once.
div_t commandLengthSep = std::div(commandLength, 256);
/*
Build the actual command. LSB and MSB need to be converted directly to hex,
hence they are interpolated as characters. For example, the eight hex value
is 0x08, but interpolating 8 directly via %x or %d returns the 38th hex
value, since 8 is interpreted as ASCII in those cases.
*/
fullCommand.push_back('\x05');
fullCommand.push_back(commandLengthSep.rem);
fullCommand.push_back(commandLengthSep.quot);
fullCommand.push_back('\x19');
for (size_t i = 0; i < strlen(command); i++) {
fullCommand.push_back(command[i]);
}
fullCommand.push_back('\x0D');
fullCommand.push_back('\x03');
// snprintf(fullCommand, MAXBUF_, "\x05%c%c\x19%s\x0D\x03",
// commandLengthSep.rem, commandLengthSep.quot, command);
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
"%s => line %d:\nSending command %s\n", __PRETTY_FUNCTION__,
__LINE__, printableCommand);
// Perform the actual writeRead
status = pasynOctetSyncIO->writeRead(
lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(),
fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, &eomReason);
// ___________________________________________________________DEBUG
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "=================== COMMAND ===================\n");
// for (int i = 0; i < 12; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// fullCommand[i], fullCommand[i]);
// }
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n");
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "=================== RESPONSE ===================\n");
// for (int i = 0; i < 40; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// fullResponse[i], fullResponse[i]);
// }
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n");
// ___________________________________________________________DEBUG
/*
As of 19.12.2025, there is a bug in the MasterMACS hardware: If two commands
are sent in rapid succession, MasterMACS sends garbage as answer for the
second command. Crucially, this garbage does not contain an CR. Hence, the
following strategy is implemented here:
- Wait 1 ms after a pasynOctetSyncIO->writeRead
- If the message does not contain an CR, wait 50 ms and then try again. If
we receive garbage again, propagate the error.
*/
// Check for CR
bool hasEtx = false;
for (ulong i = 0; i < sizeof(fullResponse); i++) {
if (fullResponse[i] == '\x0d') {
hasEtx = true;
break;
}
}
if (!hasEtx) {
usleep(50000); // 50 ms
// Try again ...
status = pasynOctetSyncIO->writeRead(
lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(),
fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn,
&eomReason);
// Does this message contain an CR?
for (ulong i = 0; i < sizeof(fullResponse); i++) {
if (fullResponse[i] == '\x0d') {
hasEtx = true;
break;
}
}
if (!hasEtx) {
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
adjustForPrint(printableResponse, fullResponse, MAXBUF_);
// Failed for the second time => Give up and propagate the error.
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\nReceived garbage response '%s' for "
"command '%s' two times in a row.\n",
__PRETTY_FUNCTION__, __LINE__, printableResponse,
printableCommand);
status = asynError;
} else {
usleep(1000); // 1 ms
}
} else {
usleep(50000); // 1 ms
}
// Create custom error messages for different failure modes
switch (status) {
case asynSuccess:
/*
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.
*/
// We don't use strlen here since the C string terminator 0x00 occurs in
// the middle of the char array.
for (ulong i = 0; i < sizeof(fullResponse); i++) {
if (fullResponse[i] == '=') {
dataStartIndex = i + 1;
}
if (fullResponse[i] == '\x06') {
validIndex = i;
responseValid = true;
break;
} else if (fullResponse[i] == '\x15') {
// NAK
snprintf(drvMessageText, sizeof(drvMessageText),
"Communication failed.");
responseValid = false;
break;
} else if (fullResponse[i] == '\x18') {
// CAN
snprintf(drvMessageText, sizeof(drvMessageText),
"Tried to write with a read-only command. This is a "
"bug, please call the support.");
responseValid = false;
break;
}
}
if (responseValid) {
/*
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 + dataStartIndex < validIndex; i++) {
response[i] = fullResponse[i + dataStartIndex];
}
} else {
status = asynError;
}
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;
}
// ___________________________________________________________DEBUG
// adjustForPrint(printableCommand, fullCommand, MAXBUF_);
// adjustForPrint(printableResponse, fullResponse, MAXBUF_);
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "\n------------------c\n"); for (int i = 0; i < 12; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// fullCommand[i], fullCommand[i]);
// }
// for (int i = 0; i < 12; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// printableCommand[i], printableCommand[i]);
// }
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "\n=================\n"); for (int i = 0; i < 12; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// fullResponse[i], fullResponse[i]);
// }
// for (int i = 0; i < 12; ++i) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
// printableResponse[i], printableResponse[i]);
// }
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "\n++++++++++++++++++++\n");
// asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
// "%s => line %d:\nResponse '%s' for command '%s'.\n",
// __PRETTY_FUNCTION__, __LINE__, printableResponse,
// printableCommand);
// ___________________________________________________________DEBUG
// Log the overall status (communication successfull or not)
if (status == asynSuccess) {
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
"%s => line %d:\nReturn value: %s\n", __PRETTY_FUNCTION__,
__LINE__, response);
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
} else {
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
asynPrint(
lowLevelPortUser_, ASYN_TRACE_ERROR,
"%s => line %d:\nCommunication failed for command '%s' (%s)\n",
__PRETTY_FUNCTION__, __LINE__, printableCommand,
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 sinqController::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.
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"
masterMacsController *pController =
new masterMacsController(portName, ipPortConfigName, numAxes,
movingPollPeriod, idlePollPeriod, comTimeout);
return asynSuccess;
}
/*
C wrapper for the axis constructor. Please refer to the masterMacsAxis
constructor documentation. The controller is read from the portName.
*/
asynStatus masterMacsCreateAxis(const char *portName, int axis) {
masterMacsAxis *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
masterMacsController *pC = dynamic_cast<masterMacsController *>(apd);
if (pC == nullptr) {
errlogPrintf("%s => line %d:\ncontroller on port %s is not a "
"masterMacsController.",
__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 masterMacsAxis(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. 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);
}
/*
Same procedure as for the CreateController function, but for the axis
itself.
*/
static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mmacs1)",
iocshArgString};
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
&CreateAxisArg1};
static const iocshFuncDef configMasterMacsCreateAxis = {"masterMacsAxis", 2,
CreateAxisArgs};
static void configMasterMacsCreateAxisCallFunc(const iocshArgBuf *args) {
masterMacsCreateAxis(args[0].sval, args[1].ival);
}
// 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 masterMacsRegister(void) {
iocshRegister(&configMasterMacsCreateController,
configMasterMacsCreateControllerCallFunc);
iocshRegister(&configMasterMacsCreateAxis,
configMasterMacsCreateAxisCallFunc);
}
epicsExportRegistrar(masterMacsRegister);
#endif
} // extern "C"