lin-epics-modules/mastermacs
0
0
Fork 0
Code Issues 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

4
Makefile
View File

@ -11,7 +11,7 @@ REQUIRED+=asynMotor
REQUIRED+=sinqMotor
# Specify the version of sinqMotor we want to build against
sinqMotor_VERSION=mathis_s
sinqMotor_VERSION=0.6.3
# These headers allow to depend on this library for derived drivers.
HEADERS += src/masterMacsAxis.h
@ -24,4 +24,4 @@ SOURCES += src/masterMacsController.cpp
# This file registers the motor-specific functions in the IOC shell.
DBDS += src/masterMacs.dbd
USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result # -Werror
USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result -Wpedantic -Wextra -Werror

285
src/masterMacsAxis.cpp
View File

@ -40,9 +40,8 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
axisStatus_ = std::bitset<16>(0);
axisError_ = std::bitset<16>(0);
// Default values for the watchdog timeout mechanism
offsetMovTimeout_ = 30; // seconds
scaleMovTimeout_ = 2.0;
// Initial value for the motor speed, is overwritten in atFirstPoll.
lastSetSpeed_ = 0.0;
// masterMacs motors can always be disabled
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 1);
@ -84,7 +83,7 @@ asynStatus masterMacsAxis::atFirstPoll() {
// Local variable declaration
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
int nvals = 0;
double motorRecResolution = 0.0;
double motorPosition = 0.0;
@ -104,56 +103,54 @@ asynStatus masterMacsAxis::atFirstPoll() {
}
// Read out the current position
snprintf(command, sizeof(command), "%dR12", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 12, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorPosition);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R12", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read out the current velocity
snprintf(command, sizeof(command), "%dR05", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 05, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorVelocity);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R05", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read out the maximum velocity
// snprintf(command, sizeof(command), "%dR26", axisNo_);
// pl_status = pC_->writeRead(axisNo_, command, response, true);
// if (pl_status != asynSuccess) {
// return pl_status;
// }
// nvals = sscanf(response, "%lf", &motorVmax);
// if (nvals != 1) {
// return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
// __PRETTY_FUNCTION__,
// __LINE__);
// }
// TODO: Temporary workaround until R26 is implemented
motorVmax = motorVelocity;
pl_status = pC_->read(axisNo_, 26, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorVmax);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse("R26", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read out the acceleration
snprintf(command, sizeof(command), "%dR06", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 06, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorAccel);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R06", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Cache the motor speed. If this value differs from the one in the motor
// record at the start of a movement, the motor record value is sent to
// MasterMACS.
lastSetSpeed_ = motorVelocity;
// Transform from motor to parameter library coordinates
motorPosition = motorPosition / motorRecResolution;
@ -208,7 +205,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
int nvals = 0;
int direction = 0;
@ -219,7 +216,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
double highLimit = 0.0;
double lowLimit = 0.0;
double limitsOffset = 0.0;
int wasMoving = 0;
// =========================================================================
@ -249,86 +245,113 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
return rw_status;
}
if (faultConditionSet()) {
rw_status = readAxisError();
// TODO: Handling
// pl_status =
// pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_,
// &wasMoving);
// if (pl_status != asynSuccess) {
// return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
// __PRETTY_FUNCTION__, __LINE__);
// }
// if (!isMoving() && wasMoving == 1) {
// rw_status = readAxisError();
// if (rw_status != asynSuccess) {
// return rw_status;
// }
// // TODO> Error handling
// }
}
// Update the movement status
*moving = isMoving();
if (*moving) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nMOVMOVMOV\n", __PRETTY_FUNCTION__, __LINE__);
}
if (enabled()) {
}
// Read the current position
snprintf(command, sizeof(command), "%dR12", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
rw_status = pC_->read(axisNo_, 12, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &currentPosition);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R12", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read the low limit
snprintf(command, sizeof(command), "%dR34", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &lowLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read the high limit
snprintf(command, sizeof(command), "%dR33", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &highLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Did we just finish a movement? If yes, read out the error.
pl_status =
pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &wasMoving);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
__PRETTY_FUNCTION__, __LINE__);
}
if (!isMoving() && wasMoving == 1) {
rw_status = readAxisError();
// Read out the limits, if the motor is not moving
if (!isMoving()) {
rw_status = pC_->read(axisNo_, 34, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &lowLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
"R34", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
// TODO> Error handling
rw_status = pC_->read(axisNo_, 33, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &highLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
"R33", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
/*
The axis limits are set as: ({[]})
where [] are the positive and negative limits set in EPICS/NICOS, {} are
the software limits set on the MCU and () are the hardware limit
switches. In other words, the EPICS/NICOS limits must be stricter than
the software limits on the MCU which in turn should be stricter than the
hardware limit switches. For example, if the hardware limit switches are
at [-10, 10], the software limits could be at [-9, 9] and the EPICS /
NICOS limits could be at
[-8, 8]. Therefore, we cannot use the software limits read from the MCU
directly, but need to shrink them a bit. In this case, we're shrinking
them by 0.1 mm or 0.1 degree (depending on the axis type) on both sides.
*/
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_,
&limitsOffset);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_",
__PRETTY_FUNCTION__, __LINE__);
}
highLimit = highLimit - limitsOffset;
lowLimit = lowLimit + limitsOffset;
pl_status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_,
highLimit);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status,
"motorHighLimitFromDriver_",
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_,
lowLimit);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
__PRETTY_FUNCTION__, __LINE__);
}
}
/*
The axis limits are set as: ({[]})
where [] are the positive and negative limits set in EPICS/NICOS, {} are
the software limits set on the MCU and () are the hardware limit
switches. In other words, the EPICS/NICOS limits must be stricter than
the software limits on the MCU which in turn should be stricter than the
hardware limit switches. For example, if the hardware limit switches are
at [-10, 10], the software limits could be at [-9, 9] and the EPICS /
NICOS limits could be at
[-8, 8]. Therefore, we cannot use the software limits read from the MCU
directly, but need to shrink them a bit. In this case, we're shrinking
them by 0.1 mm or 0.1 degree (depending on the axis type) on both sides.
*/
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_, &limitsOffset);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_",
__PRETTY_FUNCTION__, __LINE__);
}
highLimit = highLimit - limitsOffset;
lowLimit = lowLimit + limitsOffset;
//
// Update the enablement PV
// Update the enable PV
pl_status = setIntegerParam(pC_->motorEnableRBV_, enabled());
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
@ -379,20 +402,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
__PRETTY_FUNCTION__, __LINE__);
}
pl_status =
pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_",
__PRETTY_FUNCTION__, __LINE__);
}
pl_status =
pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
__PRETTY_FUNCTION__, __LINE__);
}
// Transform from motor to EPICS coordinates (see comment in
// masterMacsAxis::atFirstPoll())
currentPosition = currentPosition / motorRecResolution;
@ -416,7 +425,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char value[pC_->MAXBUF_];
double motorCoordinatesPosition = 0.0;
double motorRecResolution = 0.0;
double motorVelocity = 0.0;
@ -461,10 +470,14 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
__PRETTY_FUNCTION__, __LINE__);
}
// Set the new motor speed, if the user is allowed to do so.
if (motorCanSetSpeed != 0) {
snprintf(command, sizeof(command), "%dS05=%lf", axisNo_, motorVelocity);
rw_status = pC_->writeRead(axisNo_, command, response, false);
// Set the new motor speed, if the user is allowed to do so and if the motor
// speed changed since the last move command.
if (motorCanSetSpeed != 0 && lastSetSpeed_ != motorVelocity) {
lastSetSpeed_ = motorVelocity;
snprintf(value, sizeof(value), "%lf", motorVelocity);
rw_status = pC_->write(axisNo_, 05, value);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@ -481,9 +494,8 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
}
// Set the target position
snprintf(command, sizeof(command), "%dS02=%lf", axisNo_,
motorCoordinatesPosition);
rw_status = pC_->writeRead(axisNo_, command, response, false);
snprintf(value, sizeof(value), "%lf", motorCoordinatesPosition);
rw_status = pC_->write(axisNo_, 02, value);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@ -495,11 +507,10 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
// Start the move
if (relative) {
snprintf(command, sizeof(command), "%dS00=2", axisNo_);
rw_status = pC_->write(axisNo_, 00, "2");
} else {
snprintf(command, sizeof(command), "%dS00=1", axisNo_);
rw_status = pC_->write(axisNo_, 00, "1");
}
rw_status = pC_->writeRead(axisNo_, command, response, false);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@ -526,12 +537,9 @@ asynStatus masterMacsAxis::stop(double acceleration) {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
// =========================================================================
snprintf(command, sizeof(command), "%dS00=8", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, false);
rw_status = pC_->write(axisNo_, 00, "8");
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@ -555,7 +563,7 @@ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
@ -569,8 +577,7 @@ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
// Only send the home command if the axis has an incremental encoder
if (strcmp(response, IncrementalEncoder) == 0) {
snprintf(command, sizeof(command), "%dS00=9", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, false);
rw_status = pC_->write(axisNo_, 00, "9");
if (rw_status != asynSuccess) {
return rw_status;
}
@ -616,8 +623,7 @@ asynStatus masterMacsAxis::readEncoderType() {
// =========================================================================
// Check if this is an absolute encoder
snprintf(command, sizeof(command), "%dR60", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
rw_status = pC_->read(axisNo_, 60, response);
if (rw_status != asynSuccess) {
return rw_status;
}
@ -650,7 +656,7 @@ asynStatus masterMacsAxis::readEncoderType() {
asynStatus masterMacsAxis::enable(bool on) {
int timeout_enable_disable = 2;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char value[pC_->MAXBUF_];
// Status of read-write-operations of ASCII commands to the controller
asynStatus rw_status = asynSuccess;
@ -659,10 +665,11 @@ asynStatus masterMacsAxis::enable(bool on) {
asynStatus pl_status = asynSuccess;
bool moving = false;
doPoll(&moving);
// =========================================================================
doPoll(&moving);
// If the axis is currently moving, it cannot be disabled. Ignore the
// command and inform the user. We check the last known status of the
// axis instead of "moving", since status -6 is also moving, but the
@ -686,7 +693,7 @@ asynStatus masterMacsAxis::enable(bool on) {
// Axis is already enabled / disabled and a new enable / disable command
// was sent => Do nothing
if ((axisStatus_ != -3) == on) {
if (enabled() == on) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
"%s => line %d:\nAxis %d on controller %s is already %s.\n",
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName,
@ -695,7 +702,7 @@ asynStatus masterMacsAxis::enable(bool on) {
}
// Enable / disable the axis if it is not moving
snprintf(command, sizeof(command), "%dS04=%d", axisNo_, on);
snprintf(value, sizeof(value), "%d", on);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
"%s => line %d:\n%s axis %d on controller %s\n",
__PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable", axisNo_,
@ -709,7 +716,7 @@ asynStatus masterMacsAxis::enable(bool on) {
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
__PRETTY_FUNCTION__, __LINE__);
}
rw_status = pC_->writeRead(axisNo_, command, response, false);
rw_status = pC_->write(axisNo_, 04, value);
if (rw_status != asynSuccess) {
return rw_status;
}
@ -736,14 +743,14 @@ asynStatus masterMacsAxis::enable(bool on) {
// Failed to change axis status within timeout_enable_disable => Send a
// corresponding message
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nFailed to %s axis %d on controller %s within %d "
"seconds\n",
__PRETTY_FUNCTION__, __LINE__, on ? "enable" : "disable", axisNo_,
pC_->portName, timeout_enable_disable);
// Output message to user
snprintf(command, sizeof(command), "Failed to %s within %d seconds",
snprintf(value, sizeof(value), "Failed to %s within %d seconds",
on ? "enable" : "disable", timeout_enable_disable);
pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
if (pl_status != asynSuccess) {
@ -754,19 +761,18 @@ asynStatus masterMacsAxis::enable(bool on) {
}
asynStatus masterMacsAxis::readAxisStatus() {
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
snprintf(command, sizeof(command), "%dR10", axisNo_);
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
asynStatus rw_status = pC_->read(axisNo_, 10, response);
if (rw_status == asynSuccess) {
int axisStatus = 0;
int nvals = sscanf(response, "%d", &axisStatus);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
"R10", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
axisStatus_ = std::bitset<16>(axisStatus);
}
@ -774,21 +780,20 @@ asynStatus masterMacsAxis::readAxisStatus() {
}
asynStatus masterMacsAxis::readAxisError() {
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
snprintf(command, sizeof(command), "%dR11", axisNo_);
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
asynStatus rw_status = pC_->read(axisNo_, 11, response);
if (rw_status == asynSuccess) {
int axisError = 0;
int nvals = sscanf(response, "%d", &axisError);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
"R11", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
axisError_ = std::bitset<16>(axisError);
}
return rw_status;
}
}

1
src/masterMacsAxis.h
View File

@ -99,6 +99,7 @@ class masterMacsAxis : public sinqAxis {
protected:
masterMacsController *pC_;
double lastSetSpeed_;
asynStatus readConfig();
bool initial_poll_;

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);

69
src/masterMacsController.h
View File

@ -53,23 +53,66 @@ class masterMacsController : public sinqController {
asynUser *lowLevelPortUser_;
/**
* @brief Send a command to the hardware and receive a response
* @brief Send a command to the hardware (S mode)
*
* @param axisNo Axis to which the command should be send
* @param command Command for the hardware
* @param response Buffer for the response. This buffer is
* expected to have the size MAXBUF_.
* @param numExpectedResponses The PMAC MCU can send multiple responses at
* once. The number of responses is determined by the number of
* "subcommands" within command. Therefore it is known in advance how many
* "subresponses" are expected. This can be used to check the integrity of
* the received response, since the subresponses are separated by carriage
* returns (/r). The number of carriage returns is compared to
* numExpectedResponses to determine if the communication was successfull.
* @param tcpCmd TCP command key
* @param payload Value send to MasterMACS.
* @return asynStatus
*/
asynStatus writeRead(int axisNo, const char *command, char *response,
bool expectResponse);
asynStatus write(int axisNo, int tcpCmd, const char *payload);
/**
* @brief Send a command to the hardware and receive a response (R mode)
*
* @param axisNo Axis to which the command should be send
* @param tcpCmd TCP command key
* @param response Buffer for the response. This buffer is
* expected to have the size MAXBUF_.
* @return asynStatus
*/
asynStatus read(int axisNo, int tcpCmd, char *response);
/**
* @brief Send a command to the hardware (R or S mode) and receive a
* response in the former case.
*
* This function is the internal implementation of the write and read
* methods.
*
* @param axisNo Axis to which the command should be send
* @param tcpCmd TCP command key
* @param payload Value send to MasterMACS.
* @param response Buffer for the response. This buffer is
* expected to have the size MAXBUF_.
* @return asynStatus
*/
asynStatus writeRead(int axisNo, int tcpCmd, const char *payload,
char *response);
/**
* @brief Parse "fullResponse" received upon sending "fullCommand".
*
* Returns asynSuccess if the response is valid and asynError otherwise.
* This is an internal function used in writeRead.
*
* @param fullCommand Command which was send to the controller
* @param fullResponse Response which was received
* @param drvMessageText Buffer for messages send to the user
* @param valueStart If a payload was included in fullResponse,
* return the index where it starts.
* @param valueStop If a payload was included in fullResponse,
* return the index where it stops.
* @param axisNo Axis to which the command should be send
* @param tcpCmd TCP command key
* @param isRead true, if a payload is expect as part of the
* answer
* @return asynStatus
*/
asynStatus parseResponse(const char *fullCommand, const char *fullResponse,
char *drvMessageText, int *valueStart,
int *valueStop, int axisNo, int tcpCmd,
bool isRead);
/**
* @brief Save cast of the given asynAxis pointer to a masterMacsAxis