From ea8c34ab84a1ededb4be708577a5d5ec570c1052 Mon Sep 17 00:00:00 2001 From: smathis Date: Tue, 28 Jan 2025 13:15:53 +0100 Subject: [PATCH] WIP version of the MasterMACS driver --- Makefile | 4 +- src/masterMacsAxis.cpp | 285 +++++++++++++------------ src/masterMacsAxis.h | 1 + src/masterMacsController.cpp | 399 +++++++++++++++++------------------ src/masterMacsController.h | 69 ++++-- 5 files changed, 399 insertions(+), 359 deletions(-) diff --git a/Makefile b/Makefile index 524aebe..dd2e4ee 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp index c42a9b7..9d96f64 100644 --- a/src/masterMacsAxis.cpp +++ b/src/masterMacsAxis.cpp @@ -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", ¤tPosition); 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; -} \ No newline at end of file +} diff --git a/src/masterMacsAxis.h b/src/masterMacsAxis.h index b21ff34..f6e2c3d 100644 --- a/src/masterMacsAxis.h +++ b/src/masterMacsAxis.h @@ -99,6 +99,7 @@ class masterMacsAxis : public sinqAxis { protected: masterMacsController *pC_; + double lastSetSpeed_; asynStatus readConfig(); bool initial_poll_; diff --git a/src/masterMacsController.cpp b/src/masterMacsController.cpp index 71c9f7f..66a10b1 100644 --- a/src/masterMacsController.cpp +++ b/src/masterMacsController.cpp @@ -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); diff --git a/src/masterMacsController.h b/src/masterMacsController.h index 7d3e3c3..1e4e78e 100644 --- a/src/masterMacsController.h +++ b/src/masterMacsController.h @@ -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