#include "turboPmacController.h" #include "asynInt32SyncIO.h" #include "asynMotorController.h" #include "asynOctetSyncIO.h" #include "pmacAsynIPPort.h" #include "turboPmacAxis.h" #include #include #include #include #include #include #include #include /** * @brief Copy src into dst and replace all carriage returns with spaces. This * allows to print *dst with asynPrint. * * * @param dst Buffer for the modified string * @param src Original string */ void adjustResponseForPrint(char *dst, const char *src, size_t buf_length) { for (size_t i = 0; i < buf_length; i++) { if (src[i] == '\r') { dst[i] = ' '; } else { dst[i] = src[i]; } } } 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 ParamLib entries int rereadEncoderPosition; int readConfig; int flushHardware; int limFromHardware; }; #define NUM_turboPmac_DRIVER_PARAMS 3 turboPmacController::turboPmacController(const char *portName, const char *ipPortConfigName, int numAxes, double movingPollPeriod, double idlePollPeriod, double comTimeout, int numExtraParams) : sinqController( portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, /* The following parameter library entries are added in this driver: - REREAD_ENCODER_POSITION - READ_CONFIG */ numExtraParams + NUM_turboPmac_DRIVER_PARAMS) { // 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; // Maximum allowed number of subsequent timeouts before the user is // informed. setMaxSubsequentTimeouts(10); // ========================================================================= // Create additional parameter library entries status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, &pTurboPmacC_->rereadEncoderPosition); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " "parameter failed with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); exit(-1); } 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 " "parameter failed with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); exit(-1); } 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 " "parameter failed with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); exit(-1); } status = createParam("LIM_FROM_HARDWARE", asynParamInt32, &pTurboPmacC_->limFromHardware); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " "parameter failed with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, 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 the message length is encoded in the message header in the getSetResponse method. */ const char *message_from_device = "\006"; // Hex-code for ACK (acknowledge) -> Each message from the MCU // is terminated by this value status = pasynOctetSyncIO->setInputEos(pasynOctetSyncIOipPort(), message_from_device, strlen(message_from_device)); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR " "(setting input EOS failed with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort()); exit(-1); } status = callParamCallbacks(); if (status != asynSuccess) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR " "(executing ParamLib callbacks failed " "with %s).\nTerminating IOC", portName, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); pasynOctetSyncIO->disconnect(pasynOctetSyncIOipPort()); exit(-1); } // =========================================================================; /* 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, &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); } } turboPmacController::~turboPmacController() {} /* Access one of the axes of the controller via the axis adress stored in asynUser. If the axis does not exist or is not a Axis, a nullptr is returned and an error is emitted. */ turboPmacAxis *turboPmacController::getTurboPmacAxis(asynUser *pasynUser) { asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); return dynamic_cast(asynAxis); } /* Access one of the axes of the controller via the axis index. If the axis does not exist or is not a Axis, the function must return Null */ turboPmacAxis *turboPmacController::getTurboPmacAxis(int axisNo) { asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); return dynamic_cast(asynAxis); } asynStatus turboPmacController::writeRead(int axisNo, const char *command, char *response, int numExpectedResponses) { // Definition of local variables. asynStatus status = asynSuccess; asynStatus timeoutStatus = asynSuccess; char drvMessageText[MAXBUF_] = {0}; char modResponse[MAXBUF_] = {0}; int motorStatusProblem = 0; int numReceivedResponses = 0; /* asyn defines the following reasons for an end-of-message coming from the MCU (https://epics.anl.gov/modules/soft/asyn/R4-14/asynDriver.pdf, p. 28): 0: Timeout 1: Request count reached 2: End of string detected -> In this driver, this is the "normal" case 4: End indicator detected Combinations of reasons are also possible, e.g. eomReason = 5 would mean that both the request count was reached and an end indicator was detected. */ int eomReason = 0; // Number of bytes of the outgoing message (which is command + the // end-of-string terminator defined in the constructor) size_t nbytesOut = 0; // Number of bytes of the incoming message (which is response + the // end-of-string terminator defined in the constructor) size_t nbytesIn = 0; // ========================================================================= turboPmacAxis *axis = getTurboPmacAxis(axisNo); if (axis == nullptr) { // We already did the error logging directly in getAxis return asynError; } const size_t commandLength = strlen(command); /* The writeRead command performs the following steps: 1) Flush the socket buffer on the IOC side (not the controller!) 2) Write a command to the controller 3) Read the response If a timeout occurs during writing or reading, inform the user that we're trying to reconnect. If the problem persists, ask them to call the support */ status = pasynOctetSyncIO->writeRead( pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_, pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, &eomReason); /* If sth. is written to the controller, it needs some time to process the command. However, the controller returns the acknowledgment via pasynOctetSyncIO->writeRead immediately, signalling to the driver that it is ready to receive the next command. In practice, this can result in commands getting discarded on the driver side or in bringing the driver in undefined states (e.g. stuck in status 1). To prevent this, we wait for 20 ms after a write command to give the controller enough time to process everything. A write command can be identified by looking for the equal sign. */ if (strchr(command, '=')) { usleep(20000); } msgPrintControlKey comKey = msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__); if (status == asynTimeout) { if (getMsgPrintControl().shouldBePrinted(comKey, true, pasynUser())) { asynPrint( this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nTimeout while " "writing to the controller. Retrying ...%s\n", portName, axisNo, __PRETTY_FUNCTION__, __LINE__, getMsgPrintControl().getSuffix()); } timeoutStatus = checkComTimeoutWatchdog(axisNo, drvMessageText, sizeof(drvMessageText)); int timeoutCounter = 0; while (1) { checkMaxSubsequentTimeouts(timeoutCounter, axis); timeoutCounter += 1; if (maxSubsequentTimeoutsExceeded()) { break; } status = pasynOctetSyncIO->writeRead( pasynOctetSyncIOipPort(), command, commandLength, response, MAXBUF_, pTurboPmacC_->comTimeout, &nbytesOut, &nbytesIn, &eomReason); if (status != asynTimeout) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d\nReconnected after write timeout\n", portName, axisNo, __PRETTY_FUNCTION__, __LINE__); break; } } } else if (status != asynSuccess) { if (getMsgPrintControl().shouldBePrinted(comKey, true, pasynUser())) { asynPrint( this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nError %s while " "writing to the controller.%s\n", portName, axisNo, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status), getMsgPrintControl().getSuffix()); } } else { 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; } // The message should only ever terminate due to reason 2 msgPrintControlKey terminateKey = msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__); if (eomReason != 2) { status = asynError; char reasonStringified[30] = {0}; switch (eomReason) { case 0: snprintf(reasonStringified, sizeof(reasonStringified), "Timeout"); break; case 1: snprintf(reasonStringified, sizeof(reasonStringified), "Request count reached"); break; case 2: snprintf(reasonStringified, sizeof(reasonStringified), "End of string detected"); break; case 3: snprintf(reasonStringified, sizeof(reasonStringified), "End indicator detected"); break; } snprintf(drvMessageText, sizeof(drvMessageText), "Terminated message due to reason %s (should be \"End of " "string detected\"). Please call the support.", reasonStringified); if (getMsgPrintControl().shouldBePrinted(terminateKey, true, pasynUser())) { asynPrint(this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nMessage " "terminated due to reason %s.%s\n", portName, axisNo, __PRETTY_FUNCTION__, __LINE__, reasonStringified, getMsgPrintControl().getSuffix()); } } else { getMsgPrintControl().resetCount(terminateKey, pasynUser()); } /* Calculate the number of received responses by counting the number of carriage returns "\r" in the response. */ for (size_t i = 0; i < strlen(response); i++) { if (response[i] == '\r') { numReceivedResponses++; } } msgPrintControlKey numResponsesKey = msgPrintControlKey(portName, axisNo, __PRETTY_FUNCTION__, __LINE__); if (numExpectedResponses != numReceivedResponses) { adjustResponseForPrint(modResponse, response, MAXBUF_); if (getMsgPrintControl().shouldBePrinted(numResponsesKey, true, pasynUser())) { asynPrint( this->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nUnexpected " "response '%s' (carriage returns are replaced with spaces) " "for command %s.%s\n", portName, axisNo, __PRETTY_FUNCTION__, __LINE__, modResponse, command, getMsgPrintControl().getSuffix()); } snprintf(drvMessageText, sizeof(drvMessageText), "Received unexpected response '%s' (carriage returns " "are replaced with spaces) for command %s. " "Please call the support", modResponse, command); status = asynError; } else { getMsgPrintControl().resetCount(numResponsesKey, pasynUser()); } // Create custom error messages for different failure modes, if no error // message has been set yet if (strlen(drvMessageText) == 0) { switch (status) { case asynSuccess: break; // Communicate nothing case asynTimeout: snprintf(drvMessageText, sizeof(drvMessageText), "connection timeout for axis %d", axisNo); break; case asynDisconnected: snprintf(drvMessageText, sizeof(drvMessageText), "axis is not connected"); break; case asynDisabled: snprintf(drvMessageText, sizeof(drvMessageText), "axis is disabled"); break; default: snprintf(drvMessageText, sizeof(drvMessageText), "Communication failed (%s)", stringifyAsynStatus(status)); break; } } // Log the overall status (communication successfull or not) if (status == asynSuccess) { setAxisParamChecked(axis, motorStatusCommsError, false); } else { // 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 getAxisParamChecked(axis, motorStatusProblem, &motorStatusProblem); if (motorStatusProblem == 0) { setAxisParamChecked(axis, motorMessageText, drvMessageText); setAxisParamChecked(axis, motorStatusProblem, true); setAxisParamChecked(axis, motorStatusCommsError, true); } } return status; } asynStatus turboPmacController::doFlushHardware() { /* Temporarily overwrite the "reason" field with the FLUSH_HARDWARE constant defined in pmacAsynIPPort.c. This reason is then used within the write method of pasynInt32SyncIO to select the flush function. */ int temp = pTurboPmacC_->pasynInt32SyncIOipPort->reason; pTurboPmacC_->pasynInt32SyncIOipPort->reason = FLUSH_HARDWARE; asynStatus status = (asynStatus)pasynInt32SyncIO->write( pTurboPmacC_->pasynInt32SyncIOipPort, 1, pTurboPmacC_->comTimeout); // Reset the status afterwards pTurboPmacC_->pasynInt32SyncIOipPort->reason = temp; return status; } asynStatus turboPmacController::writeInt32(asynUser *pasynUser, epicsInt32 value) { int function = pasynUser->reason; // ===================================================================== turboPmacAxis *axis = getTurboPmacAxis(pasynUser); // Handle custom PVs if (function == rereadEncoderPosition()) { return axis->rereadEncoder(); } else if (function == readConfig()) { return axis->init(); } else if (function == flushHardware()) { return doFlushHardware(); } else { return sinqController::writeInt32(pasynUser, value); } } asynStatus turboPmacController::couldNotParseResponse(const char *command, const char *response, int axisNo, const char *functionName, int lineNumber) { char modifiedResponse[MAXBUF_] = {0}; adjustResponseForPrint(modifiedResponse, response, MAXBUF_); return sinqController::couldNotParseResponse( command, modifiedResponse, axisNo, functionName, lineNumber); } int turboPmacController::rereadEncoderPosition() { return pTurboPmacC_->rereadEncoderPosition; } int turboPmacController::readConfig() { return pTurboPmacC_->readConfig; } int turboPmacController::flushHardware() { return pTurboPmacC_->flushHardware; } int turboPmacController::limFromHardware() { return pTurboPmacC_->limFromHardware; } asynUser *turboPmacController::pasynInt32SyncIOipPort() { return pTurboPmacC_->pasynInt32SyncIOipPort; } /*************************************************************************************/ /** The following functions are C-wrappers, and can be called directly from * iocsh */ extern "C" { /* C wrapper for the controller constructor. Please refer to the turboPmacController constructor documentation. */ asynStatus turboPmacCreateController(const char *portName, const char *ipPortConfigName, int numAxes, double movingPollPeriod, double idlePollPeriod, double comTimeout) { /* We create a new instance of the controller, using the "new" keyword to allocate it on the heap while avoiding RAII. 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. */ #pragma GCC diagnostic ignored "-Wunused-but-set-variable" #pragma GCC diagnostic ignored "-Wunused-variable" turboPmacController *pController = new turboPmacController(portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, comTimeout); return asynSuccess; } /* Define name and type of the arguments for the CreateController function in the iocsh. This is done by creating structs with the argument names and types and then providing "factory" functions (configCreateControllerCallFunc). These factory functions are used to register the constructors during compilation. */ static const iocshArg CreateControllerArg0 = {"Controller name (e.g. mcu1)", iocshArgString}; static const iocshArg CreateControllerArg1 = {"Asyn IP port name (e.g. pmcu1)", iocshArgString}; static const iocshArg CreateControllerArg2 = {"Number of axes", iocshArgInt}; static const iocshArg CreateControllerArg3 = {"Moving poll rate (s)", iocshArgDouble}; static const iocshArg CreateControllerArg4 = {"Idle poll rate (s)", iocshArgDouble}; static const iocshArg CreateControllerArg5 = {"Communication timeout (s)", iocshArgDouble}; static const iocshArg *const CreateControllerArgs[] = { &CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2, &CreateControllerArg3, &CreateControllerArg4, &CreateControllerArg5}; static const iocshFuncDef configTurboPmacCreateController = { "turboPmacController", 6, CreateControllerArgs}; static void configTurboPmacCreateControllerCallFunc(const iocshArgBuf *args) { turboPmacCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].dval, args[4].dval, args[5].dval); } // This function is made known to EPICS in turboPmac.dbd and is called by // EPICS in order to register both functions in the IOC shell static void turboPmacControllerRegister(void) { iocshRegister(&configTurboPmacCreateController, configTurboPmacCreateControllerCallFunc); } epicsExportRegistrar(turboPmacControllerRegister); } // extern "C"