From b700fca383244117606d24f20c84a66559875133 Mon Sep 17 00:00:00 2001 From: smathis Date: Fri, 11 Oct 2024 17:19:41 +0200 Subject: [PATCH] Commit first working version of the new PMAC-V3 driver. This version has been tested on the SINQTEST instrument. However, some important improvements (such as central polling in the controller) are still missing. They will be implemented in the next commit. --- Makefile.RHEL8 | 5 +- sinqEPICSApp/src/C804Axis.cpp | 6 +- sinqEPICSApp/src/C804Axis.h | 55 +- sinqEPICSApp/src/newPmacV3Axis.cpp | 762 +++++++++++++++++------ sinqEPICSApp/src/newPmacV3Axis.h | 15 +- sinqEPICSApp/src/newPmacV3Controller.cpp | 306 ++++----- sinqEPICSApp/src/newPmacV3Controller.h | 29 +- 7 files changed, 749 insertions(+), 429 deletions(-) diff --git a/Makefile.RHEL8 b/Makefile.RHEL8 index 9f0713d..62ede3e 100644 --- a/Makefile.RHEL8 +++ b/Makefile.RHEL8 @@ -13,7 +13,7 @@ REQUIRED+=scaler REQUIRED+=asynMotor # Release version -LIBVERSION=2024-dev +LIBVERSION=2024-newPmacV3 # DB files to include in the release TEMPLATES += sinqEPICSApp/Db/dimetix.db @@ -38,6 +38,9 @@ SOURCES += sinqEPICSApp/src/pmacController.cpp SOURCES += sinqEPICSApp/src/MasterMACSDriver.cpp SOURCES += sinqEPICSApp/src/C804Axis.cpp SOURCES += sinqEPICSApp/src/C804Controller.cpp +SOURCES += sinqEPICSApp/src/newPmacV3Axis.cpp +SOURCES += sinqEPICSApp/src/newPmacV3Controller.cpp +SOURCES += sinqEPICSApp/src/pmacController.cpp USR_CFLAGS += -Wall -Wextra # -Werror diff --git a/sinqEPICSApp/src/C804Axis.cpp b/sinqEPICSApp/src/C804Axis.cpp index acbd396..03fda0b 100644 --- a/sinqEPICSApp/src/C804Axis.cpp +++ b/sinqEPICSApp/src/C804Axis.cpp @@ -46,9 +46,9 @@ asynStatus C804Axis::poll(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; - // The poll function is just a wrapper around poll_no_param_lib_update and + // The poll function is just a wrapper around pollNoUpdate and // handles mainly the callParamCallbacks() function - asynStatus status_poll = C804Axis::poll_no_param_lib_update(moving); + asynStatus status_poll = C804Axis::pollNoUpdate(moving); // According to the function documentation of asynMotorAxis::poll, this // function should be called at the end of a poll implementation. @@ -64,7 +64,7 @@ asynStatus C804Axis::poll(bool *moving) { } // Perform the actual poll -asynStatus C804Axis::poll_no_param_lib_update(bool *moving) { +asynStatus C804Axis::pollNoUpdate(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; asynStatus status; diff --git a/sinqEPICSApp/src/C804Axis.h b/sinqEPICSApp/src/C804Axis.h index 6e5bdc9..0e4eddd 100644 --- a/sinqEPICSApp/src/C804Axis.h +++ b/sinqEPICSApp/src/C804Axis.h @@ -1,40 +1,43 @@ #ifndef C804Axis_H #define C804Axis_H -#include "SINQController.h" #include "SINQAxis.h" +#include "SINQController.h" // Forward declaration of the controller class to resolve the cyclic dependency -// between C804Controller.h and C804Axis.h. See https://en.cppreference.com/w/cpp/language/class. +// between C804Controller.h and C804Axis.h. See +// https://en.cppreference.com/w/cpp/language/class. class C804Controller; -class C804Axis : public SINQAxis -{ -public: - /* These are the methods we override from the base class */ - C804Axis(C804Controller *pController, int axisNo); - virtual ~C804Axis(); - asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); - asynStatus stop(double acceleration); - asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); - asynStatus poll(bool *moving); - asynStatus poll_no_param_lib_update(bool *moving); - asynStatus enable(int on); +class C804Axis : public SINQAxis { + public: + /* These are the methods we override from the base class */ + C804Axis(C804Controller *pController, int axisNo); + virtual ~C804Axis(); + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + asynStatus moveVelocity(double min_velocity, double max_velocity, + double acceleration); + asynStatus stop(double acceleration); + asynStatus home(double minVelocity, double maxVelocity, double acceleration, + int forwards); + asynStatus poll(bool *moving); + asynStatus pollNoUpdate(bool *moving); + asynStatus enable(int on); -protected: - C804Controller *pC_; + protected: + C804Controller *pC_; - void checkBounds(C804Controller *pController, int axisNo); - int last_position_steps_; - double motorRecResolution_; - time_t estimatedArrivalTime_; - time_t last_poll_; - int errorReported_; - bool enabled_; + void checkBounds(C804Controller *pController, int axisNo); + int last_position_steps_; + double motorRecResolution_; + time_t estimatedArrivalTime_; + time_t last_poll_; + int errorReported_; + bool enabled_; -private: - friend class C804Controller; + private: + friend class C804Controller; }; #endif diff --git a/sinqEPICSApp/src/newPmacV3Axis.cpp b/sinqEPICSApp/src/newPmacV3Axis.cpp index ccb1f01..80c60b2 100644 --- a/sinqEPICSApp/src/newPmacV3Axis.cpp +++ b/sinqEPICSApp/src/newPmacV3Axis.cpp @@ -1,4 +1,5 @@ #include "newPmacV3Axis.h" +#include "asynOctetSyncIO.h" #include "newPmacV3Controller.h" #include #include @@ -11,6 +12,7 @@ newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo) : asynMotorAxis(pC, axisNo), pC_(pC) { static const char *functionName = "newPmacV3Axis::newPmacV3Axis"; + asynStatus status = asynSuccess; /* The superclass constructor SINQAxis calls in turn its superclass constructor @@ -36,8 +38,47 @@ newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo) // Initialize all member variables initial_poll_ = true; + waitForHandshake_ = false; + + // Wait 10 seconds for the handshake until declaring a timeout + handshakeTimeout_ = 10; + + // Placeholder, is overwritten later time_at_init_poll_ = 0; - timeout_param_lib_init_ = 10; + + // After 3 idle polls, the parameter library definitely had enough time to + // be initialized + timeout_param_lib_init_ = 3 * pC->idlePollPeriod_; + + // Provide initial values for some parameter library entries + status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition_, 0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for rereadEncoderPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } + + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for motorPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } + + // This value is updated in the poll. Initially, we assume that the motor is + // not enabled. + status = pC_->setIntegerParam(axisNo_, pC_->motorEnabled_, 0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for motorPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } } newPmacV3Axis::~newPmacV3Axis(void) { @@ -45,16 +86,22 @@ newPmacV3Axis::~newPmacV3Axis(void) { // clean up the pointer pC here. } -// Read the configuration from the motor control unit and the parameter library +/* +Read the configuration from the motor control unit and the parameter library. +This operation is only allowed if the motor is not moving +*/ asynStatus newPmacV3Axis::readConfig() { + // Local variable declaration static const char *functionName = "newPmacV3Axis::readConfig"; asynStatus status = asynSuccess; char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; int nvals = 0; - double positive_limit = 0.0; - double negative_limit = 0.0; + double highLimit = 0.0; + double lowLimit = 0.0; double motorRecResolution = 0.0; + double position = 0.0; + int axStatus = 0; // ========================================================================= @@ -64,32 +111,75 @@ asynStatus newPmacV3Axis::readConfig() { if (status == asynParamUndefined) { return asynParamUndefined; } else if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorRecResolution_"); } - // Software limits - snprintf(command, sizeof(command), "Q%2.2d08 Q%2.2d09", axisNo_, axisNo_); - status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%lf %lf", &positive_limit, &negative_limit); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { + // Software limits and current position + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 Q%2.2d13 Q%2.2d14", + axisNo_, axisNo_, axisNo_, axisNo_); + status = pC_->writeRead(axisNo_, command, response, true); + nvals = sscanf(response, "%d %lf %lf %lf", &axStatus, &position, &highLimit, + &lowLimit); + if (pC_->checkNumExpectedReads(4, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + // If the motor is not in idle status, do not read the configuration + if (axStatus != 0) { return asynError; } // Transform from motor to user coordinates - positive_limit = positive_limit * motorRecResolution; - negative_limit = negative_limit * motorRecResolution; + position = position * motorRecResolution; + highLimit = highLimit * motorRecResolution; + lowLimit = lowLimit * motorRecResolution; + + /* + 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. + */ + highLimit = highLimit - 0.1; + lowLimit = lowLimit + 0.1; // Store these values in the parameter library - status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, positive_limit); + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, position); if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorHighLimit_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorPosition_"); } - status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, negative_limit); + status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, lowLimit); if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorLowLimit_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorLowLimit_"); } - return asynSuccess; + status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, highLimit); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, functionName, + "motorHighLimit_"); + } + + // Update the parameter library immediately + status = callParamCallbacks(); + if (status != asynSuccess) { + // If we can't communicate with the parameter library, it doesn't make + // sense to try and upstream this to the user -> Just log the error + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Updating the parameter library failed for axis %d\n", + functionName, axisNo_); + return status; + } + + return this->readEncoderType(); } asynStatus newPmacV3Axis::poll(bool *moving) { @@ -138,22 +228,24 @@ asynStatus newPmacV3Axis::poll(bool *moving) { } } - // The poll function is just a wrapper around poll_no_param_lib_update and + // The poll function is just a wrapper around pollNoUpdate and // handles mainly the callParamCallbacks() function. This wrapper is used // to make sure callParamCallbacks() is called in case of a premature // return. - poll_status = newPmacV3Axis::poll_no_param_lib_update(moving); + poll_status = newPmacV3Axis::pollNoUpdate(moving); // If the poll status is ok, reset the error indicators in the parameter // library if (poll_status == asynSuccess) { pl_status = setIntegerParam(pC_->motorStatusProblem_, false); if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } pl_status = setIntegerParam(pC_->motorStatusCommsError_, false); if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusCommsError_"); } } @@ -173,7 +265,7 @@ asynStatus newPmacV3Axis::poll(bool *moving) { } // Perform the actual poll -asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { +asynStatus newPmacV3Axis::pollNoUpdate(bool *moving) { // Return value for the poll asynStatus poll_status = asynSuccess; @@ -190,43 +282,88 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { int direction = 0; int error = 0; - int ax_status = 0; - double current_position = 0.0; - double previous_position = 0.0; - int low_limit_reached = 0; - int high_limit_reached = 0; + int axStatus = 0; + double currentPosition = 0.0; + double previousPosition = 0.0; double motorRecResolution = 0.0; + int handshakePerformed = 0; // ========================================================================= + // Are we currently waiting for a handshake? + if (waitForHandshake_) { + snprintf(command, sizeof(command), "P%2.2d23", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d", &handshakePerformed); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, + response, axisNo_) != asynSuccess) { + return asynError; + } + + if (handshakePerformed == 1) { + // Handshake has been performed successfully -> Continue with the + // poll + waitForHandshake_ = false; + } else { + // Still waiting for the handshake. This is already part of the + // movement procedure! + if (time(NULL) < timeAtHandshake_ + handshakeTimeout_) { + *moving = true; + return asynSuccess; + } else { + // Timed out when waiting for the handshake + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d timed out when waiting for the " + "handshake with the MCU.\n", + functionName, axisNo_); + pl_status = setStringParam( + pC_->messageText_, "Handshake timed out. This is a bug."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + waitForHandshake_ = false; + return asynError; + } + } + } + // Motor resolution from parameter library pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, &motorRecResolution); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorRecResolution_"); } // Read the previous motor position pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previous_position); + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorPosition_"); } // Check the axis status (Pxx00) and the current motor position (Qxx10) - snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10", axisNo_, axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%d %lf", &ax_status, ¤t_position); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 P%2.2d01", axisNo_, + axisNo_, axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d %lf %d", &axStatus, ¤tPosition, &error); + if (pC_->checkNumExpectedReads(3, nvals, functionName, command, response, + axisNo_) != asynSuccess) { return asynError; } - // Change the position values from motor to user coordinates. - current_position = current_position * motorRecResolution; - // Intepret the status - switch (ax_status) { + switch (axStatus) { case -6: asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Axis %d is stopping\n", functionName, axisNo_); @@ -237,6 +374,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is deactivated\n", functionName, axisNo_); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Deactivated"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case -4: @@ -246,6 +389,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { functionName); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Emergency stop"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case -3: @@ -253,6 +402,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is inhibited\n", functionName, axisNo_); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Disabled"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case 0: @@ -260,31 +415,58 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is ready for movement\n", functionName, axisNo_); *moving = false; break; + case 1: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Move order for %d acknowledged\n", functionName, + axisNo_); + *moving = true; + break; + case 2: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Move order for %d is possible\n", functionName, axisNo_); + *moving = true; + break; + case 3: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %d in Air Cushion Outout status\n", functionName, + axisNo_); + *moving = true; + break; + case 4: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %d in Air Cushion Input status\n", functionName, + axisNo_); + *moving = true; + break; case 5: asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Axis %d is moving\n", functionName, axisNo_); *moving = true; break; + default: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Reached unreachable state P%2.2d00 = %d.\n", + functionName, axisNo_, axStatus); + pl_status = + setStringParam(pC_->messageText_, + "Unreachable state has been reached. This is a bug"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + *moving = false; } if (*moving) { // If the axis is moving, evaluate the movement direction - if ((current_position - previous_position) > 0) { + if ((currentPosition - previousPosition) > 0) { direction = 1; } else { direction = 0; } } - // Read the axis error - snprintf(command, sizeof(command), "P%2.2d01", axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%d", &error); - if (pC_->checkNumExpectedReads(1, nvals, functionName, axisNo_) != - asynSuccess) { - return asynError; - } - + // Error handling switch (error) { case 0: // No error @@ -292,19 +474,19 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { case 1: // EPICS should already prevent this issue in the first place, // since it contains the user limits - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Target position would exceed user limits in axis %d. EPICS " - "should prevent an out-of-bounds target position, this error " - "therefore indicates a bug.\n", - functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Target position would exceed user limits in axis " + "%d. EPICS should prevent an out-of-bounds target " + "position, this is a bug.\n", + functionName, axisNo_); pl_status = setStringParam(pC_->messageText_, "Target position would exceed software limits. This " - "is a bug, please contact software support"); + "is a bug."); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -313,92 +495,42 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { // Command not possible asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Axis %d is still moving, but received another move " - "command. EPICS should prevent this, this error therefore " - "indicates a bug.\n", + "command. EPICS should prevent this, this is a bug.\n", functionName, axisNo_); - pl_status = setStringParam( - pC_->messageText_, - "Axis received move command while it is still moving. This is a " - "bug, please contact software support"); + pl_status = setStringParam(pC_->messageText_, + "Axis received move command while it is " + "still moving. This is a bug."); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; break; case 10: - // Limits reached - snprintf(command, sizeof(command), "M%2.2d21 M%2.2d22", axisNo_, - axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = - sscanf(response, "%d %d", &high_limit_reached, &low_limit_reached); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { - return asynError; - } - - if (high_limit_reached && !low_limit_reached) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Axis %d reached its high limit.\n", functionName, - axisNo_); - - pl_status = setStringParam(pC_->messageText_, "High limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - } else if (!high_limit_reached && low_limit_reached) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Axis %d reached its low limit.\n", functionName, - axisNo_); - - pl_status = setStringParam(pC_->messageText_, "Low limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - } else if (high_limit_reached && low_limit_reached) { - // If the execution has hit this branch, both high and low - // limit are apparently hit at the same time. This most likely - // indicates an error in the configuration! - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Axis %d reached both its higher and lower limit.\n", - functionName, axisNo_); - - pl_status = setStringParam(pC_->messageText_, - "Both high and low limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - poll_status = asynError; - } else { - // If the execution has hit this branch, neither upper nor lower - // limit has been reached, but the MCU states that limits have been - // reached. This is definitely a bug! - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Axis %d states that it has hit its limits, but at " - "the same time states that neither high or low limit " - "have been hit. This is a bug.\n", - functionName, axisNo_); - - snprintf(command, sizeof(command), - "Limits have been hit and not hit at the " - "same time " - "(P%2.2d01 = 10). This is a bug, please " - "contact Software " - "support.", - axisNo_); - pl_status = setStringParam(pC_->messageText_, command); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - poll_status = asynError; + /* + Software limits of the controller have been hit. Since the EPICS limits + are derived from the software limits and are a little bit smaller, this + error case can only happen if either the axis has an incremental encoder + which is not properly homed or if a bug occured. + */ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d hit the controller limits. Try homing the axis, " + "if that doesn't remove the error, this is a bug.\n", + functionName, axisNo_); + + snprintf(command, sizeof(command), + "Software limits hit (P%2.2d01 = %d). Try homing the motor. " + "If that doesn't work, contact the support", + axisNo_, error); + pl_status = setStringParam(pC_->messageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } + poll_status = asynError; break; case 11: // Following error @@ -412,25 +544,26 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { axisNo_); pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; break; case 13: // Watchdog of the controller final stage triggered - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Controller final stage watchdog triggered for axis %d.\n", - functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Driver hardware error triggered for axis %d.\n", + functionName, axisNo_); snprintf(command, sizeof(command), - "Controller final stage watchdog triggered (P%2.2d01 = 13). " + "Driver hardware error (P%2.2d01 = 13). " "Please contact Electronics support.", axisNo_); pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -440,16 +573,17 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { // since it contains the user limits asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Target position would exceed hardware limits in axis " - "%d. EPICS should prevent an out-of-bounds target position, " - "this error therefore indicates a bug.\n", + "%d. Homing might be necessary.\n", functionName, axisNo_); - pl_status = setStringParam( - pC_->messageText_, - "Controller final stage watchdog triggered (Pxx01 = 13). " - "Please contact Electronics support."); + snprintf(command, sizeof(command), + "Move command exceeds hardware limits (P%2.2d01 = %d). Try " + "homing the motor. If that doesn't work, contact the support", + axisNo_, error); + pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -461,9 +595,10 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { pl_status = setStringParam(pC_->messageText_, "Axis reached an unreachable state. Please " - "contact software support"); + "contact electronics support"); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -474,44 +609,50 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { if (error != 0) { pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } } - pl_status = setIntegerParam(pC_->motorEnabled_, (ax_status != -3)); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + if (*moving == false) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorMoveToHome_"); + } } - pl_status = setIntegerParam(pC_->motorStatusHighLimit_, high_limit_reached); + pl_status = + setIntegerParam(pC_->motorEnabled_, (axStatus != -3 && axStatus != -5)); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHighLimit_"); - } - - pl_status = setIntegerParam(pC_->motorStatusLowLimit_, low_limit_reached); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusLowLimit_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorEnabled_"); } pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusMoving_"); } pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusDone_"); } pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusDirection_"); } - pl_status = setDoubleParam(pC_->motorPosition_, current_position); + pl_status = setDoubleParam(pC_->motorPosition_, currentPosition); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorPosition_"); } + return poll_status; } @@ -527,8 +668,7 @@ asynStatus newPmacV3Axis::move(double position, int relative, static const char *functionName = "newPmacV3Axis::move"; char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; - double motor_coordinates_position = 0.0; - double motor_position = 0.0; + double motorCoordinatesPosition = 0.0; int enabled = 0; double motorRecResolution = 0.0; @@ -536,13 +676,15 @@ asynStatus newPmacV3Axis::move(double position, int relative, pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorEnabled_"); } pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, &motorRecResolution); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorRecResolution_"); } if (enabled == 0) { @@ -551,52 +693,43 @@ asynStatus newPmacV3Axis::move(double position, int relative, return asynSuccess; } - // Use only absolute values - if (relative == 1) { - - pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motor_position); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); - } - - motor_position = motor_position + position; - } - // Convert from user to motor units - motor_coordinates_position = motor_position / motorRecResolution; + motorCoordinatesPosition = position / motorRecResolution; - // Set the axis speed - snprintf(command, sizeof(command), "Q%d04=%lf", axisNo_, maxVelocity); - rw_status = pC_->writeRead(axisNo_, command, response); - if (rw_status != asynSuccess) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Setting the velocity failed for axis %d\n", functionName, - axisNo_); - pl_status = setIntegerParam(pC_->motorStatusProblem_, true); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); - } - - return rw_status; - } + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Start of axis %d to position %lf\n", functionName, axisNo_, + position); // Perform handshake, Set target position and start the move command - snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", - axisNo_, axisNo_, motor_coordinates_position, axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); + + if (relative) { + snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2", + axisNo_, axisNo_, motorCoordinatesPosition, axisNo_); + } else { + snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", + axisNo_, axisNo_, motorCoordinatesPosition, axisNo_); + } + + // We don't expect an answer + rw_status = pC_->writeRead(axisNo_, command, response, false); if (rw_status != asynSuccess) { asynPrint( pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Starting movement to target position %lf failed for axis %d\n", - functionName, motor_position, axisNo_); + functionName, position, axisNo_); pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } return rw_status; } + // In the next poll, we will check if the handshake has been performed in a + // reasonable time + waitForHandshake_ = true; + timeAtHandshake_ = time(NULL); + return rw_status; } @@ -614,7 +747,7 @@ asynStatus newPmacV3Axis::stop(double acceleration) { // ========================================================================= - pl_status = poll_no_param_lib_update(&moving); + pl_status = pollNoUpdate(&moving); if (pl_status != asynSuccess) { return pl_status; } @@ -622,7 +755,7 @@ asynStatus newPmacV3Axis::stop(double acceleration) { if (moving) { // only send a stop when actually moving snprintf(command, sizeof(command), "M%2.2d=8", axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); + rw_status = pC_->writeRead(axisNo_, command, response, false); if (rw_status != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, @@ -631,10 +764,227 @@ asynStatus newPmacV3Axis::stop(double acceleration) { pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, + return pC_->paramLibAccessFailed(pl_status, functionName, "motorStatusProblem_"); } } } return rw_status; -} \ No newline at end of file +} + +/* +Home the axis. On absolute encoder systems, this is a no-op +*/ +asynStatus newPmacV3Axis::home(double min_velocity, double max_velocity, + double acceleration, int forwards) { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + static const char *functionName = "newPmacV3Axis::home"; + + // ========================================================================= + + pl_status = + pC_->getStringParam(axisNo_, pC_->encoderType_, pC_->MAXBUF_, response); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "encoderType_"); + } + + // Only send the home command if the axis has an incremental encoder + if (strcmp(response, IncrementalEncoder) == 0) { + snprintf(command, sizeof(command), "M%2.2d=9", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + return rw_status; + } + + pl_status = setIntegerParam(pC_->motorMoveToHome_, 1); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorMoveToHome_"); + } + + pl_status = setStringParam(pC_->messageText_, "Homing"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + } + + return rw_status; +} + +/* +Read the encoder type and update the parameter library accordingly +*/ +asynStatus newPmacV3Axis::readEncoderType() { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + static const char *functionName = "newPmacV3Axis::readEncoderType"; + int nvals = 0; + int encoder_id = 0; + char encoderType[pC_->MAXBUF_]; + + // ========================================================================= + + // Check if this is an absolute encoder + snprintf(command, sizeof(command), "I%2.2X04", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + int reponse_length = strlen(response); + if (reponse_length < 3) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Unexpected reponse '%s' from axis %d on " + "controller %s while reading the encoder type. Aborting...\n", + functionName, response, axisNo_, pC_->portName); + return asynError; + } + + // We are only interested in the last two digits and the last value in + // the string before the terminator is \r + nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + snprintf(command, sizeof(command), "P46"); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + int number_of_axes = strtol(response, NULL, 10); + + // If true, the encoder is incremental + if (encoder_id <= number_of_axes) { + pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder); + } else { + pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder); + } + + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "encoderType_"); + } + return asynSuccess; +} + +asynStatus newPmacV3Axis::enable(int on) { + + static const char *functionName = "newPmacV3Axis::enable"; + int ax_status = 0; + int timeout_enable_disable = 2; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + // ========================================================================= + + // Check if the axis is currently enabled + snprintf(command, sizeof(command), "P%2.2d00", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d", &ax_status); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis status %d, new input %d \n", functionName, ax_status, + on); + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((ax_status != -3) == on) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: axis %d on controller %s is already %s\n", functionName, + axisNo_, pC_->portName, on ? "enabled" : "disabled"); + return asynSuccess; + } + + // Enable / disable the axis + snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %s axis %d on controller %s\n", functionName, + on ? "Enable" : "Disable", axisNo_, pC_->portName); + if (on == 0) { + pl_status = setStringParam(pC_->messageText_, "Disabling ..."); + } else { + pl_status = setStringParam(pC_->messageText_, "Enabling ..."); + } + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + return rw_status; + } + + // Query the axis status every few milliseconds until the axis has been + // enabled or until the timeout has been reached + snprintf(command, sizeof(command), "P%2.2d00", axisNo_); + int startTime = time(NULL); + while (time(NULL) < startTime + timeout_enable_disable) { + + // Read the axis status + usleep(100000); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%d", &ax_status); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, + response, axisNo_) != asynSuccess) { + return asynError; + } + + if ((ax_status != -3) == on) { + bool moving = false; + // Perform a poll to update the parameter library + poll(&moving); + return asynSuccess; + } + } + + // Failed to change axis status within timeout_enable_disable => Send a + // corresponding message + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Failed to %s axis %d on controller %s within %d seconds\n", + functionName, on ? "enable" : "disable", axisNo_, pC_->portName, + timeout_enable_disable); + + // Output message to user + snprintf(command, sizeof(command), "Failed to %s within %d seconds", + on ? "enable" : "disable", timeout_enable_disable); + pl_status = setStringParam(pC_->messageText_, "Enabling ..."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + return asynError; +} diff --git a/sinqEPICSApp/src/newPmacV3Axis.h b/sinqEPICSApp/src/newPmacV3Axis.h index e3e2330..332f38c 100644 --- a/sinqEPICSApp/src/newPmacV3Axis.h +++ b/sinqEPICSApp/src/newPmacV3Axis.h @@ -14,15 +14,13 @@ class newPmacV3Axis : public asynMotorAxis { virtual ~newPmacV3Axis(); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - // asynStatus moveVelocity(double min_velocity, double max_velocity, - // double acceleration); asynStatus stop(double acceleration); - // asynStatus home(double minVelocity, double maxVelocity, double - // acceleration, - // int forwards); + asynStatus home(double minVelocity, double maxVelocity, double acceleration, + int forwards); asynStatus poll(bool *moving); - asynStatus poll_no_param_lib_update(bool *moving); - // asynStatus enable(int on); + asynStatus pollNoUpdate(bool *moving); + asynStatus enable(int on); + asynStatus readEncoderType(); protected: newPmacV3Controller *pC_; @@ -30,6 +28,9 @@ class newPmacV3Axis : public asynMotorAxis { void checkBounds(newPmacV3Controller *pController, int axisNo); asynStatus readConfig(); bool initial_poll_; + bool waitForHandshake_; + time_t timeAtHandshake_; + time_t handshakeTimeout_; time_t time_at_init_poll_; time_t timeout_param_lib_init_; diff --git a/sinqEPICSApp/src/newPmacV3Controller.cpp b/sinqEPICSApp/src/newPmacV3Controller.cpp index e389a24..868baed 100644 --- a/sinqEPICSApp/src/newPmacV3Controller.cpp +++ b/sinqEPICSApp/src/newPmacV3Controller.cpp @@ -85,7 +85,7 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, // NICOS and in turn to the user status = createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &messageText_); if (status != asynSuccess) { - paramLibAccessFailed(status, "messageText_"); + paramLibAccessFailed(status, functionName, "messageText_"); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%s: FATAL ERROR: unable to create parameter (%s). " "Terminating IOC.\n", @@ -111,6 +111,15 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, exit(-1); } + status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, &rereadEncoderPosition_); if (status != asynSuccess) { @@ -140,6 +149,16 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, exit(-1); } + status = + createParam("MOTOR_POSITION_RBV", asynParamFloat64, &motorPositionRBV_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + /* Define the end-of-string of a message coming from the device to EPICS. It is not necessary to append a terminator to outgoing messages, since @@ -230,14 +249,14 @@ Sends the given command to the axis specified by axisNo and returns the response of the axis. */ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, - char *response) { + char *response, + bool expect_response) { // Definition of local variables. static const char *functionName = "newPmacV3Controller::writeRead"; asynStatus status = asynSuccess; asynStatus pl_status = asynSuccess; char full_command[MAXBUF_] = {0}; char user_message[MAXBUF_] = {0}; - int motorStatusCommsError = 0; int motorStatusProblem = 0; // Send the message and block the thread until either a response has been @@ -294,15 +313,48 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, "%s: Sending command: %s\n", functionName, full_command); // Perform the actual writeRead - // status = - // pasynOctetSyncIO->write(lowLevelPortUser_, full_command, - // commandLength + offset, TIMEOUT_, - // &nbytesOut); - status = pasynOctetSyncIO->writeRead( lowLevelPortUser_, full_command, commandLength + offset, response, MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason); + /* + If we expect a response, check if we got one. If no response was received, + flush the PMAC and try again. If that fails as well, return an error + */ + if (expect_response && strlen(response) == 0) { + // Flush message as defined in Turbo PMAC User Manual, p. 430: + // \x40\xB3000 + // VR_DOWNLOAD = \x40 + // VR_PMAC_FLUSH = \xB3 + char flush_msg[5] = {0}; + flush_msg[0] = '\x40'; + flush_msg[1] = '\xB3'; + size_t nbytesOut = 0; + status = pasynOctetSyncIO->write(lowLevelPortUser_, flush_msg, 5, + TIMEOUT_, &nbytesOut); + + // Wait after the flush so the MCU has time to prepare for the + // next command + usleep(100000); + + if (status == asynSuccess) { + // If flushing the MCU succeded, try to send the command again + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, full_command, commandLength + offset, + response, MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason); + + // If the command returned an empty string for the second time, give + // up and propagate the error. + if (strlen(response) == 0) { + status = asynError; + } + } else { + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Unable to flush MCU (%s).\n", functionName, + stringifyAsynStatus(status)); + } + } + // Create custom error messages for different failure modes switch (status) { case asynSuccess: @@ -312,22 +364,19 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, "connection timeout for axis %d", axisNo); break; case asynDisconnected: - snprintf(user_message, sizeof(user_message), "axis %d is not connected", - axisNo); + snprintf(user_message, sizeof(user_message), "axis is not connected"); break; case asynDisabled: - snprintf(user_message, sizeof(user_message), "axis %d is disabled", - axisNo); + snprintf(user_message, sizeof(user_message), "axis is disabled"); break; default: snprintf(user_message, sizeof(user_message), - "Communication with axis %d failed (%s)", axisNo, - stringifyAsynStatus(status)); + "Communication failed (%s)", stringifyAsynStatus(status)); break; } if (status != asynSuccess) { - // Check if the axis aleady is in an error communication mode. If it is + // 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 @@ -335,13 +384,15 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, pl_status = getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem); if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } if (motorStatusProblem == 0) { pl_status = axis->setStringParam(this->messageText_, user_message); if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); + return paramLibAccessFailed(pl_status, functionName, + "messageText_"); } } } @@ -360,8 +411,10 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, } if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + return paramLibAccessFailed(pl_status, functionName, + "motorStatusCommsError_"); } + return asynSuccess; } @@ -370,10 +423,7 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, int function = pasynUser->reason; asynStatus status = asynSuccess; - char command[MAXBUF_] = {0}; - char response[MAXBUF_] = {0}; static const char *functionName = "newPmacV3Controller::writeInt32"; - int nvals = 0; // ========================================================================= @@ -385,92 +435,12 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // Handle custom PVs if (function == enableMotor_) { - - int ax_status = 0; - asynStatus pl_status = asynSuccess; - int timeout_enable_disable = 2; - char message[MAXBUF_] = {0}; - - // ===================================================================== - - // Check if the axis is currently enabled - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - status = writeRead(axis->axisNo_, command, response); - nvals = sscanf(response, "%d", &ax_status); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - // Axis is already enabled / disabled and a new enable / disable command - // was sent => Do nothing - if ((ax_status != -3) == value) { - return asynSuccess; - } - - // Enable / disable the axis - snprintf(command, sizeof(command), "M%2.2d14=%d", axis->axisNo_, value); - asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: %s axis %d on controller %s\n", functionName, - value ? "Enable" : "Disable", axis->axisNo_, portName); - if (value == 0) { - pl_status = setStringParam(messageText_, "Disabling ..."); - } else { - pl_status = setStringParam(messageText_, "Enabling ..."); - } - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); - } - status = writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - - // Query the axis status every few milliseconds until the axis has been - // enabled or until the timeout has been reached - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - int startTime = time(NULL); - while (time(NULL) < startTime + timeout_enable_disable) { - - // Read the axis status - usleep(100); - status = writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - nvals = sscanf(response, "%d", &ax_status); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - if ((ax_status != -3) == value) { - bool moving = false; - // Perform a poll to update the parameter library - axis->poll(&moving); - return asynSuccess; - } - } - - // Failed to change axis status within timeout_enable_disable => Send a - // corresponding message - asynPrint( - this->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Failed to %s axis %d on controller %s within %d seconds\n", - functionName, value ? "enable" : "disable", axis->axisNo_, portName, - timeout_enable_disable); - - // Output message to user - snprintf(command, sizeof(command), "Failed to %s within %d seconds", - value ? "enable" : "disable", timeout_enable_disable); - pl_status = setStringParam(messageText_, "Enabling ..."); - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); - } - return asynError; + return axis->enable(value); } else if (function == rereadEncoderPosition_) { + char encoderType[MAXBUF_] = {0}; + /* This is not a command that can always be run when enabling a motor as it also causes relative encoders to reread a position @@ -488,68 +458,56 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // Poll the current status of the axis bool moving = false; - axis->poll(&moving); + status = axis->poll(&moving); + if (status != asynSuccess) { + return status; + } // Check if this is an absolute encoder - snprintf(command, sizeof(command), "I%2.2X04", axis->axisNo_); - status = writeRead(axis->axisNo_, command, response); + status = axis->readEncoderType(); if (status != asynSuccess) { return status; } - int reponse_length = strlen(response); - if (reponse_length < 3) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Unexpected reponse '%s' from axis %d on " - "controller %s while rereading encoder. Aborting...\n", - functionName, response, axis->axisNo_, portName); - return asynError; - } - - // We are only interested in the last two digits and the last value in - // the string before the terminator is \r - int encoder_id = 0; - nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - snprintf(command, sizeof(command), "P46"); - status = writeRead(axis->axisNo_, command, response); + status = getStringParam(axis->axisNo_, encoderType_, encoderType); if (status != asynSuccess) { - return status; + return paramLibAccessFailed(status, functionName, "encoderType_"); } - int number_of_axes = strtol(response, NULL, 10); - if (encoder_id <= number_of_axes) { - asynPrint( - this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Trying to reread absolute encoder of axis %d on " - "controller %s, but it is a relative encoder. Aborting...\n", - functionName, axis->axisNo_, portName); + // Abort if the axis is incremental + if (strcmp(encoderType, IncrementalEncoder) == 0) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Trying to reread absolute encoder of axis %d on " + "controller %s, but it is a relative encoder.\n", + functionName, axis->axisNo_, portName); + status = setStringParam(messageText_, + "Cannot reread an incremental encoder."); + if (status != asynSuccess) { + return paramLibAccessFailed(status, functionName, + "messageText_"); + } return asynError; } - // Check if the axis is disabled. If not, inform the user that this is - // necessary + // Check if the axis is disabled. If not, inform the user that this + // is necessary int enabled = 0; status = getIntegerParam(axis->axisNo_, motorEnabled_, &enabled); if (status != asynSuccess) { - return paramLibAccessFailed(status, "motorEnabled_"); + return paramLibAccessFailed(status, functionName, "motorEnabled_"); } if (enabled == 1) { - asynPrint( - this->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Trying to reread absolute encoder of axis %d on " - "controller %s, but it is a relative encoder. Aborting...\n", - functionName, axis->axisNo_, portName); + asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d on controller %s must be disabled before " + "rereading the encoder.\n", + functionName, axis->axisNo_, portName); status = setStringParam( messageText_, "Axis must be disabled before rereading the encoder."); if (status != asynSuccess) { - return paramLibAccessFailed(status, "messageText_"); + return paramLibAccessFailed(status, functionName, + "messageText_"); } return asynError; } @@ -564,7 +522,8 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // though status = axis->setIntegerParam(rereadEncoderPosition_, 0); if (status != asynSuccess) { - return paramLibAccessFailed(status, "rereadEncoderPosition_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPosition_"); } return asynSuccess; @@ -581,11 +540,9 @@ and "rereading the encoder" must be covered. asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { - int function = pasynUser->reason, axStat; + int function = pasynUser->reason; asynStatus status = asynError; static const char *functionName = "newPmacV3Controller::readInt32"; - char command[MAXBUF_]; - char response[MAXBUF_]; // ===================================================================== @@ -595,42 +552,22 @@ asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser, return asynError; } - if (function == motorEnabled_) { - // Readback value for motorEnabled - - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - status = this->writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - - int nvals = sscanf(response, "%d", value); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - status = setIntegerParam(motorEnabled_, (*value != -3)); - if (status != asynSuccess) { - return paramLibAccessFailed(status, "motorEnabled_"); - } - return callParamCallbacks(); // Update the PVs from the parameter - // library - - } else if (function == rereadEncoderPositionRBV_) { + if (function == rereadEncoderPositionRBV_) { // Readback value for rereadEncoderPosition - status = getIntegerParam(axis->axisNo_, rereadEncoderPosition_, value); if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "HERE\n"); - return paramLibAccessFailed(status, "rereadEncoderPosition_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPosition_"); } status = setIntegerParam(axis->axisNo_, rereadEncoderPositionRBV_, *value); if (status != asynSuccess) { - return paramLibAccessFailed(status, "rereadEncoderPositionRBV_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPositionRBV_"); } - return callParamCallbacks(); // Update the PVs from the parameter - // library + + // Update the PVs from the parameter library + return callParamCallbacks(); } else { return asynMotorController::readInt32(pasynUser, value); } @@ -676,6 +613,7 @@ const char *newPmacV3Controller::stringifyAsynStatus(asynStatus status) { } asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status, + const char *functionName, const char *parameter) { char message[MAXBUF_] = {0}; snprintf(message, sizeof(message), @@ -684,23 +622,27 @@ asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status, parameter, stringifyAsynStatus(status)); // Log the error message and try to propagate it - asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName, + message); setStringParam(messageText_, message); return status; } asynStatus newPmacV3Controller::checkNumExpectedReads(int expected, int read, const char *functionName, + const char *command, + const char *response, int axisNo_) { if (expected == read) { return asynSuccess; } else { char message[MAXBUF_] = {0}; snprintf(message, sizeof(message), - "Could not interpret controller response in function %s for " - "axis %d. This is a bug, please inform the software support.", - functionName, axisNo_); - asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + "Could not interpret response %s for command %s (axis %d). " + "This is a bug.", + response, command, axisNo_); + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName, + message); setStringParam(messageText_, message); setIntegerParam(motorStatusCommsError_, 1); return asynError; @@ -868,4 +810,4 @@ epicsExportRegistrar(newPmacV3ControllerRegister); #endif -} // extern "C" \ No newline at end of file +} // extern "C" diff --git a/sinqEPICSApp/src/newPmacV3Controller.h b/sinqEPICSApp/src/newPmacV3Controller.h index 4f2ffe3..0d6fc23 100644 --- a/sinqEPICSApp/src/newPmacV3Controller.h +++ b/sinqEPICSApp/src/newPmacV3Controller.h @@ -12,6 +12,9 @@ #include "asynMotorController.h" #include "newPmacV3Axis.h" +#define IncrementalEncoder "Incremental encoder" +#define AbsoluteEncoder "Absolute encoder" + class newPmacV3Controller : public asynMotorController { public: @@ -31,17 +34,25 @@ class newPmacV3Controller : public asynMotorController { // overloaded because we want to read the axis state asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + // // Overloaded to read configuration details from the motor records + // asynStatus drvUserCreate(asynUser *pasynUser, const char *drvInfo, + // const char **pptypeName, size_t *psize); + protected: asynUser *lowLevelPortUser_; - asynStatus writeRead(int axisNo, const char *command, char *response); + asynStatus writeRead(int axisNo, const char *command, char *response, + bool expect_response); // Create a descriptive string out of an asynStatus which can be used for // logging or communicating with the user const char *stringifyAsynStatus(asynStatus status); - asynStatus paramLibAccessFailed(asynStatus status, const char *parameter); + asynStatus paramLibAccessFailed(asynStatus status, const char *functionName, + const char *parameter); asynStatus checkNumExpectedReads(int expected, int read, - const char *function, int axisNo_); + const char *functionName, + const char *command, const char *response, + int axisNo_); newPmacV3Axis *castToAxis(asynMotorAxis *asynAxis); private: @@ -58,14 +69,24 @@ class newPmacV3Controller : public asynMotorController { */ static const double TIMEOUT_; + // Indices of additional PVs int messageText_; int enableMotor_; int motorEnabled_; int rereadEncoderPosition_; int rereadEncoderPositionRBV_; int readConfig_; + int encoderType_; + int motorPositionRBV_; + + /* + If the time between two sent messages is too short, the MCU communication + module might "lose" an answer. To prevent this, a small delay is introduced + in EPICS after each message exchange. Unit is microseconds. + */ + int afterMessageSleep_; friend class newPmacV3Axis; }; -#endif /* pmacV3Controller_H */ \ No newline at end of file +#endif /* pmacV3Controller_H */