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

WIP version of the MasterMACS driver

Browse Source
This commit is contained in:
mathis_s
2025-01-28 13:15:53 +01:00
parent 2a74b6a478
commit ea8c34ab84
5 changed files with 399 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

399
src/masterMacsController.cpp
View File

@@ -152,20 +152,32 @@ masterMacsAxis *masterMacsController::castToAxis(asynMotorAxis *asynAxis) {
return axis;
}
asynStatus masterMacsController::writeRead(int axisNo, const char *command,
char *response,
bool expectResponse) {
asynStatus masterMacsController::read(int axisNo, int tcpCmd, char *response) {
return writeRead(axisNo, tcpCmd, NULL, response);
}
asynStatus masterMacsController::write(int axisNo, int tcpCmd,
const char *payload) {
return writeRead(axisNo, tcpCmd, payload, NULL);
}
asynStatus masterMacsController::writeRead(int axisNo, int tcpCmd,
const char *payload,
char *response) {
// Definition of local variables.
asynStatus status = asynSuccess;
asynStatus pl_status = asynSuccess;
std::string fullCommand = "";
char fullCommand[MAXBUF_] = {0};
char fullResponse[MAXBUF_] = {0};
char printableCommand[MAXBUF_] = {0};
char printableResponse[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
@@ -178,11 +190,8 @@ asynStatus masterMacsController::writeRead(int axisNo, const char *command,
// 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;
// Do we expect an response?
bool isRead = response != NULL;
// =========================================================================
@@ -195,11 +204,9 @@ asynStatus masterMacsController::writeRead(int axisNo, const char *command,
/*
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,
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:
@@ -211,175 +218,106 @@ asynStatus masterMacsController::writeRead(int axisNo, const char *command,
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;
fullCommand[0] = '\x05'; // ENQ
fullCommand[1] = 1; // Placeholder value, can be anything other than 0
fullCommand[2] = 1; // Placeholder value, can be anything other than 0
fullCommand[3] = '\x19'; // PD01
// Create the command and add CR and ETX at the end
if (isRead) {
snprintf(&fullCommand[4], MAXBUF_ - 4, "%dR%02d\x0D\x03", axisNo,
tcpCmd);
} else {
snprintf(&fullCommand[4], MAXBUF_ - 4, "%dS%02d=%s\x0D\x03", axisNo,
tcpCmd, payload);
}
// Calculate the command length
const size_t fullCommandLength = strlen(fullCommand);
// Length of the command without ENQ and ETX
const size_t lenWithMetadata = fullCommandLength - 2;
// Perform both division and modulo operation at once.
div_t commandLengthSep = std::div(commandLength, 256);
div_t lenWithMetadataSep = std::div(lenWithMetadata, 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);
// Now set the actual command length
fullCommand[1] = lenWithMetadataSep.rem; // LSB
fullCommand[2] = lenWithMetadataSep.quot; // MSB
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
adjustForPrint(printableCommand, fullCommand, 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);
// Send out the command
status =
pasynOctetSyncIO->write(lowLevelPortUser_, fullCommand,
fullCommandLength, comTimeout_, &nbytesOut);
// ___________________________________________________________DEBUG
if (status == asynSuccess) {
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
// "=================== COMMAND ===================\n");
// Try to read the answer repeatedly
int maxTrials = 2;
for (int i = 0; i < maxTrials; i++) {
status =
pasynOctetSyncIO->read(lowLevelPortUser_, fullResponse, MAXBUF_,
comTimeout_, &nbytesIn, &eomReason);
// 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;
if (status == asynSuccess) {
status = parseResponse(fullCommand, fullResponse,
drvMessageText, &valueStart, &valueStop,
axisNo, tcpCmd, isRead);
if (status == asynSuccess) {
// Received the correct message
break;
}
} else {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nError %s while reading from the "
"controller\n",
__PRETTY_FUNCTION__, __LINE__,
stringifyAsynStatus(status));
break;
}
if (i + 1 == maxTrials && status == asynError) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFailed %d times to get the "
"correct response. Aborting read.\n",
__PRETTY_FUNCTION__, __LINE__, maxTrials);
}
}
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
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nError %s while writing to the controller\n",
__PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
}
// MasterMACS needs a bit of time between messages, therefore thr program
// execution is paused after the communication happened.
// usleep(1500);
// 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 (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.
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];
for (int i = 0; i + valueStart < valueStop; i++) {
response[i] = fullResponse[i + valueStart];
}
} else {
status = asynError;
}
break; // Communicate nothing
break;
case asynTimeout:
snprintf(drvMessageText, sizeof(drvMessageText),
"connection timeout for axis %d", axisNo);
@@ -391,62 +329,28 @@ asynStatus masterMacsController::writeRead(int axisNo, const char *command,
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)", 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) {
adjustForPrint(printableResponse, fullResponse, MAXBUF_);
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
"%s => line %d:\nReturn value: %s\n", __PRETTY_FUNCTION__,
__LINE__, response);
__LINE__, printableResponse);
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
// 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) {
@@ -474,9 +378,96 @@ asynStatus masterMacsController::writeRead(int axisNo, const char *command,
}
}
}
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) {
bool responseValid = false;
int responseStart = 0;
char printableCommand[MAXBUF_] = {0};
char printableResponse[MAXBUF_] = {0};
// 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') {
*valueStop = i;
responseValid = true;
break;
} else if (fullResponse[i] == '\x15') {
// NAK
snprintf(drvMessageText, MAXBUF_, "Communication failed.");
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nCommunication failed\n",
__PRETTY_FUNCTION__, __LINE__);
break;
} 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.");
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nTried to write with the read-only "
"command %s\n",
__PRETTY_FUNCTION__, __LINE__, printableCommand);
responseValid = false;
break;
}
}
if (responseValid) {
// 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);
}
if (strstr(&fullResponse[responseStart], expectedResponseSubstring) ==
NULL) {
adjustForPrint(printableCommand, fullCommand, MAXBUF_);
adjustForPrint(printableResponse, fullResponse, MAXBUF_);
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nMismatched response %s to "
"command %s\n",
__PRETTY_FUNCTION__, __LINE__, printableResponse,
printableCommand);
snprintf(drvMessageText, MAXBUF_,
"Mismatched response %s to command %s. Please call the "
"support.",
printableResponse, printableCommand);
return asynError;
}
}
return asynSuccess;
}
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
// masterMacs can be disabled
if (pasynUser->reason == motorCanDisable_) {
@@ -508,8 +499,8 @@ asynStatus masterMacsCreateController(const char *portName,
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.
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"
@@ -574,8 +565,8 @@ asynStatus masterMacsCreateAxis(const char *portName, int axis) {
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.
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"
@@ -637,8 +628,8 @@ 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
// 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);