From 18bc3dfc5f17115900d5cdab68fae8dd3c9e2539 Mon Sep 17 00:00:00 2001 From: smathis Date: Fri, 23 May 2025 11:51:33 +0200 Subject: [PATCH] Removed exit(-1) from init When the initialization fails, the motor will now try again during each poll. --- src/turboPmacAxis.cpp | 132 ++++++++++++++++++++++++++++-------- src/turboPmacAxis.h | 27 ++++++-- src/turboPmacController.cpp | 84 ++++++++++++++++++----- src/turboPmacController.h | 30 +++----- 4 files changed, 200 insertions(+), 73 deletions(-) diff --git a/src/turboPmacAxis.cpp b/src/turboPmacAxis.cpp index ce4977e..94675b8 100644 --- a/src/turboPmacAxis.cpp +++ b/src/turboPmacAxis.cpp @@ -13,6 +13,14 @@ #include #include +struct turboPmacAxisImpl { + bool waitForHandshake; + time_t timeAtHandshake; + // The axis status is used when enabling / disabling the motor + int axisStatus; + bool needInit; +}; + /* Contains all instances of turboPmacAxis which have been created and is used in the initialization hook function. @@ -54,10 +62,11 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo, axes.push_back(this); } - // Initialize all member variables - waitForHandshake_ = false; - timeAtHandshake_ = 0; - axisStatus_ = 0; + pTurboPmacA_ = std::make_unique( + (turboPmacAxisImpl){.waitForHandshake = false, + .timeAtHandshake = 0, + .axisStatus = 0, + .needInit = false}); // Provide initial values for some parameter library entries status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition(), 0); @@ -158,9 +167,9 @@ asynStatus turboPmacAxis::init() { "with controller during IOC initialization. Check if you used " "\"pmacAsynIPPortConfigure\" instead of the standard " "\"drvAsynIPPortConfigure\" function in the .cmd file in order to " - "create the port driver.\nTerminating IOC.\n", + "create the port driver.\n", pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__); - exit(-1); + pTurboPmacA_->needInit = true; } nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPos, &motorVmax, &motorVelocity, &motorAccel, &acoDelay); @@ -215,7 +224,12 @@ asynStatus turboPmacAxis::init() { return status; } - return this->readEncoderType(); + status = this->readEncoderType(); + if (status == asynSuccess) { + pTurboPmacA_->needInit = false; + } + + return status; } // Perform the actual poll @@ -251,13 +265,20 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { // ========================================================================= + if (pTurboPmacA_->needInit) { + rw_status = init(); + if (rw_status != asynSuccess) { + return rw_status; + } + } + // Are we currently waiting for a handshake? - if (waitForHandshake_) { + if (pTurboPmacA_->waitForHandshake) { // Check if the handshake takes too long and communicate an error in // this case. A handshake should not take more than 5 seconds. time_t currentTime = time(NULL); - bool timedOut = (currentTime > timeAtHandshake_ + 5); + bool timedOut = (currentTime > pTurboPmacA_->timeAtHandshake + 5); if (pC_->getMsgPrintControl().shouldBePrinted( pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, timedOut, @@ -267,7 +288,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { "handshake at %ld s and didn't get a positive reply yet " "(current time is %ld s).\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - timeAtHandshake_, currentTime); + pTurboPmacA_->timeAtHandshake, currentTime); } if (timedOut) { @@ -279,6 +300,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { axisNo_, __PRETTY_FUNCTION__, __LINE__); } + pTurboPmacA_->waitForHandshake = false; } snprintf(command, sizeof(command), "P%2.2d23 P%2.2d01", axisNo_, @@ -298,11 +320,11 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { // If an error occurred while waiting for the handshake, abort the // waiting procedure and continue with the poll in order to handle // the error. - waitForHandshake_ = false; + pTurboPmacA_->waitForHandshake = false; } else if (handshakePerformed == 1) { // Handshake has been performed successfully -> Continue with the // poll - waitForHandshake_ = false; + pTurboPmacA_->waitForHandshake = false; } else { // Still waiting for the handshake - try again in the next busy // poll. This is already part of the movement procedure. @@ -386,7 +408,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { lowLimit = lowLimit + limitsOffset; // Store the axis status - axisStatus_ = axStatus; + pTurboPmacA_->axisStatus = axStatus; // Update the enablement PV pl_status = setIntegerParam(pC_->motorEnableRBV(), @@ -459,13 +481,53 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { *moving = true; break; case 5: + // Motor is moving normally + *moving = true; + break; + case 6: + // Jog start + *moving = true; + break; + case 7: + // Jog forward + *moving = true; + break; + case 8: + // Jog backward + *moving = true; + break; + case 9: + // Jog break + *moving = true; + break; + case 10: + // Jog step forward + *moving = true; + break; + case 11: + // Jog step backward + *moving = true; + break; + case 12: + // Jog stop + *moving = true; + break; + case 13: + // Backlash + *moving = true; + break; + case 14: + // Move order out of range - this is also handled as an error + *moving = false; + break; + case 15: + // Move is interrupted, but the motor is not ready to receive another + // move command. Therefore it is treated as still moving. + *moving = true; + break; + case 16: + // Move is resumed *moving = true; - - asynPrint( - pC_->pasynUser(), ASYN_TRACE_FLOW, - "Controller \"%s\", axis %d => %s, line %d\nAxis is moving\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); - break; default: *moving = false; @@ -962,8 +1024,8 @@ asynStatus turboPmacAxis::doMove(double position, int relative, // In the next poll, we will check if the handshake has been performed in a // reasonable time - waitForHandshake_ = true; - timeAtHandshake_ = time(NULL); + pTurboPmacA_->waitForHandshake = true; + pTurboPmacA_->timeAtHandshake = time(NULL); // Waiting for a handshake is already part of the movement procedure => // Start the watchdog @@ -1007,7 +1069,7 @@ asynStatus turboPmacAxis::stop(double acceleration) { // Reset the driver to idle state and move out of the handshake wait loop, // if we're currently inside it. - waitForHandshake_ = false; + pTurboPmacA_->waitForHandshake = false; return rw_status; } @@ -1045,7 +1107,7 @@ asynStatus turboPmacAxis::doReset() { // Reset the driver to idle state and move out of the handshake wait loop, // if we're currently inside it. - waitForHandshake_ = false; + pTurboPmacA_->waitForHandshake = false; return rw_status; } @@ -1276,8 +1338,11 @@ asynStatus turboPmacAxis::enable(bool on) { // 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 motor can // actually be disabled in this state! - if (axisStatus_ == 1 || axisStatus_ == 2 || axisStatus_ == 3 || - axisStatus_ == 4 || axisStatus_ == 5) { + int axStatus = pTurboPmacA_->axisStatus; + if (axStatus == 1 || axStatus == 2 || axStatus == 3 || axStatus == 4 || + axStatus == 5 || axStatus == 6 || axStatus == 7 || axStatus == 8 || + axStatus == 9 || axStatus == 10 || axStatus == 11 || axStatus == 12 || + axStatus == 13 || axStatus == 15 || axStatus == 16) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nAxis is not " "idle and can therefore not be enabled / disabled.\n", @@ -1297,7 +1362,7 @@ asynStatus turboPmacAxis::enable(bool on) { // Axis is already enabled / disabled and a new enable / disable command // was sent => Do nothing - if ((axisStatus_ != -3) == on) { + if ((axStatus != -3) == on) { asynPrint( pC_->pasynUser(), ASYN_TRACE_WARNING, "Controller \"%s\", axis %d => %s, line %d\nAxis is already %s.\n", @@ -1338,13 +1403,13 @@ asynStatus turboPmacAxis::enable(bool on) { if (rw_status != asynSuccess) { return rw_status; } - nvals = sscanf(response, "%d", &axisStatus_); + nvals = sscanf(response, "%d", &pTurboPmacA_->axisStatus); if (nvals != 1) { return pC_->couldNotParseResponse(command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } - if ((axisStatus_ != -3) == on) { + if ((pTurboPmacA_->axisStatus != -3) == on) { bool moving = false; // Perform a poll to update the parameter library poll(&moving); @@ -1372,6 +1437,17 @@ asynStatus turboPmacAxis::enable(bool on) { return asynError; } +bool turboPmacAxis::needInit() { return pTurboPmacA_->needInit; } + +/** + * @brief Instruct the axis to run its init() function during the next poll + * + * @param needInit + */ +void turboPmacAxis::setNeedInit(bool needInit) { + pTurboPmacA_->needInit = needInit; +} + /*************************************************************************************/ /** The following functions are C-wrappers, and can be called directly from * iocsh */ diff --git a/src/turboPmacAxis.h b/src/turboPmacAxis.h index a2ddcf2..932de99 100644 --- a/src/turboPmacAxis.h +++ b/src/turboPmacAxis.h @@ -1,6 +1,9 @@ #ifndef turboPmacAXIS_H #define turboPmacAXIS_H #include "sinqAxis.h" +#include + +struct turboPmacAxisImpl; // Forward declaration of the controller class to resolve the cyclic dependency // between the controller and the axis .h-file. See @@ -123,14 +126,24 @@ class turboPmacAxis : public sinqAxis { */ asynStatus handleError(int error, char *userMessage, int sizeUserMessage); - protected: + /** + * @brief Check if the axis needs to run its initialization function + * + * @return true + * @return false + */ + bool needInit(); + + /** + * @brief Instruct the axis to run its init() function during the next poll + * + * @param needInit + */ + void setNeedInit(bool needInit); + + private: turboPmacController *pC_; - - bool waitForHandshake_; - time_t timeAtHandshake_; - - // The axis status is used when enabling / disabling the motor - int axisStatus_; + std::unique_ptr pTurboPmacA_; }; #endif diff --git a/src/turboPmacController.cpp b/src/turboPmacController.cpp index a3fd6b0..81f0b5a 100644 --- a/src/turboPmacController.cpp +++ b/src/turboPmacController.cpp @@ -31,6 +31,23 @@ void adjustResponseForPrint(char *dst, const char *src, size_t buf_length) { } } +struct turboPmacControllerImpl { + + // Timeout for the communication process in seconds + double comTimeout; + + char lastResponse[sinqController::MAXBUF_]; + + // User for writing int32 values to the port driver. + asynUser *pasynInt32SyncIOipPort; + + // Indices of additional PVs + int rereadEncoderPosition; + int readConfig; + int flushHardware; +}; +#define NUM_turboPmac_DRIVER_PARAMS 3 + turboPmacController::turboPmacController(const char *portName, const char *ipPortConfigName, int numAxes, double movingPollPeriod, @@ -47,12 +64,16 @@ turboPmacController::turboPmacController(const char *portName, { + // The paramLib indices are populated with the calls to createParam + pTurboPmacC_ = + std::make_unique((turboPmacControllerImpl){ + .comTimeout = comTimeout, + .lastResponse = {0}, + }); + // Initialization of local variables asynStatus status = asynSuccess; - // Initialization of all member variables - comTimeout_ = comTimeout; - // Maximum allowed number of subsequent timeouts before the user is // informed. setMaxSubsequentTimeouts(10); @@ -61,7 +82,7 @@ turboPmacController::turboPmacController(const char *portName, // Create additional parameter library entries status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, - &rereadEncoderPosition_); + &pTurboPmacC_->rereadEncoderPosition); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " @@ -71,7 +92,8 @@ turboPmacController::turboPmacController(const char *portName, exit(-1); } - status = createParam("READ_CONFIG", asynParamInt32, &readConfig_); + status = + createParam("READ_CONFIG", asynParamInt32, &pTurboPmacC_->readConfig); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " @@ -81,7 +103,8 @@ turboPmacController::turboPmacController(const char *portName, exit(-1); } - status = createParam("FLUSH_HARDWARE", asynParamInt32, &flushHardware_); + status = createParam("FLUSH_HARDWARE", asynParamInt32, + &pTurboPmacC_->flushHardware); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " @@ -131,13 +154,15 @@ turboPmacController::turboPmacController(const char *portName, We try to connect to the port via the port name provided by the constructor. If this fails, the function is terminated via exit. */ - pasynInt32SyncIO->connect(ipPortConfigName, 0, &pasynInt32SyncIOipPort_, - NULL); - if (status != asynSuccess || pasynInt32SyncIOipPort_ == nullptr) { + pasynInt32SyncIO->connect(ipPortConfigName, 0, + &pTurboPmacC_->pasynInt32SyncIOipPort, NULL); + if (status != asynSuccess || + pTurboPmacC_->pasynInt32SyncIOipPort == nullptr) { errlogPrintf("Controller \"%s\" => %s, line %d:\nFATAL ERROR (cannot " "connect to MCU controller).\n" "Terminating IOC", portName, __PRETTY_FUNCTION__, __LINE__); + pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort()); exit(-1); } } @@ -215,7 +240,7 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, */ status = pasynOctetSyncIO->writeRead( pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_, - comTimeout_, &nbytesOut, &nbytesIn, &eomReason); + pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, &eomReason); msgPrintControlKey comKey = msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__); @@ -245,7 +270,8 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, status = pasynOctetSyncIO->writeRead( pasynOctetSyncIOipPort(), command, commandLength, response, - MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, &eomReason); + MAXBUF_, pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, + &eomReason); if (status != asynTimeout) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " @@ -267,6 +293,18 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, getMsgPrintControl().resetCount(comKey, pasynUser()); } + if (status != asynSuccess) { + /* + Since the communication failed, there is the possibility that the + controller is not connected at all to the network. In that case, we + cannot be sure that the information read out in the init method of the + axis is still up-to-date the next time we get a connection. Therefore, + an info flag is set which the axis object can use at the start of its + poll method to try to initialize itself. + */ + axis->setNeedInit(true); + } + if (timeoutStatus == asynError) { status = asynError; } @@ -422,13 +460,13 @@ asynStatus turboPmacController::doFlushHardware() { constant defined in pmacAsynIPPort.c. This reason is then used within the write method of pasynInt32SyncIO to select the flush function. */ - int temp = pasynInt32SyncIOipPort_->reason; - pasynInt32SyncIOipPort_->reason = FLUSH_HARDWARE; + int temp = pTurboPmacC_->pasynInt32SyncIOipPort->reason; + pTurboPmacC_->pasynInt32SyncIOipPort->reason = FLUSH_HARDWARE; asynStatus status = (asynStatus)pasynInt32SyncIO->write( - pasynInt32SyncIOipPort_, 1, comTimeout_); + pTurboPmacC_->pasynInt32SyncIOipPort, 1, pTurboPmacC_->comTimeout); // Reset the status afterwards - pasynInt32SyncIOipPort_->reason = temp; + pTurboPmacC_->pasynInt32SyncIOipPort->reason = temp; return status; } @@ -441,11 +479,11 @@ asynStatus turboPmacController::writeInt32(asynUser *pasynUser, turboPmacAxis *axis = getTurboPmacAxis(pasynUser); // Handle custom PVs - if (function == rereadEncoderPosition_) { + if (function == rereadEncoderPosition()) { return axis->rereadEncoder(); - } else if (function == readConfig_) { + } else if (function == readConfig()) { return axis->init(); - } else if (function == flushHardware_) { + } else if (function == flushHardware()) { return doFlushHardware(); } else { return sinqController::writeInt32(pasynUser, value); @@ -463,6 +501,16 @@ asynStatus turboPmacController::couldNotParseResponse(const char *command, command, modifiedResponse, axisNo, functionName, lineNumber); } +int turboPmacController::rereadEncoderPosition() { + return pTurboPmacC_->rereadEncoderPosition; +} +int turboPmacController::readConfig() { return pTurboPmacC_->readConfig; } +int turboPmacController::flushHardware() { return pTurboPmacC_->flushHardware; } + +asynUser *turboPmacController::pasynInt32SyncIOipPort() { + return pTurboPmacC_->pasynInt32SyncIOipPort; +} + /*************************************************************************************/ /** The following functions are C-wrappers, and can be called directly from * iocsh */ diff --git a/src/turboPmacController.h b/src/turboPmacController.h index 9636d24..9d43e5d 100644 --- a/src/turboPmacController.h +++ b/src/turboPmacController.h @@ -11,6 +11,9 @@ #include "sinqAxis.h" #include "sinqController.h" #include "turboPmacAxis.h" +#include + +struct turboPmacControllerImpl; class turboPmacController : public sinqController { public: @@ -116,29 +119,16 @@ class turboPmacController : public sinqController { asynStatus doFlushHardware(); // Accessors for additional PVs - int rereadEncoderPosition() { return rereadEncoderPosition_; } - int readConfig() { return readConfig_; } - int flushHardware() { return flushHardware_; } + int rereadEncoderPosition(); + int readConfig(); + int flushHardware(); - asynUser *pasynInt32SyncIOipPort() { return pasynInt32SyncIOipPort_; } + asynUser *pasynInt32SyncIOipPort(); + + private: + std::unique_ptr pTurboPmacC_; protected: - // Timeout for the communication process in seconds - double comTimeout_; - - char lastResponse[MAXBUF_]; - - // User for writing int32 values to the port driver. - asynUser *pasynInt32SyncIOipPort_; - - // Indices of additional PVs -#define FIRST_turboPmac_PARAM rereadEncoderPosition_ - int rereadEncoderPosition_; - int readConfig_; - int flushHardware_; -#define LAST_turboPmac_PARAM flushHardware_ }; -#define NUM_turboPmac_DRIVER_PARAMS \ - (&LAST_turboPmac_PARAM - &FIRST_turboPmac_PARAM + 1) #endif /* turboPmacController_H */