From 967613447b84ee5420fb266139d30c72333291e7 Mon Sep 17 00:00:00 2001 From: smathis Date: Mon, 10 Mar 2025 14:31:15 +0100 Subject: [PATCH] Added error reset function. --- Makefile | 2 +- src/turboPmacAxis.cpp | 95 +++++++++++++++++++++++++++++++++++-- src/turboPmacAxis.h | 8 ++++ src/turboPmacController.cpp | 8 ++-- 4 files changed, 104 insertions(+), 9 deletions(-) diff --git a/Makefile b/Makefile index 06efa83..a9d4d74 100644 --- a/Makefile +++ b/Makefile @@ -14,7 +14,7 @@ REQUIRED+=sinqMotor motorBase_VERSION=7.2.2 # Specify the version of sinqMotor we want to build against -sinqMotor_VERSION=0.8.0 +sinqMotor_VERSION=mathis_s # These headers allow to depend on this library for derived drivers. HEADERS += src/turboPmacAxis.h diff --git a/src/turboPmacAxis.cpp b/src/turboPmacAxis.cpp index d13930c..c297f8c 100644 --- a/src/turboPmacAxis.cpp +++ b/src/turboPmacAxis.cpp @@ -78,6 +78,7 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo, // Initialize all member variables waitForHandshake_ = false; + timeAtHandshake_ = 0; axisStatus_ = 0; // Provide initial values for some parameter library entries @@ -191,6 +192,13 @@ asynStatus turboPmacAxis::init() { __PRETTY_FUNCTION__, __LINE__); } + // Initial motor status is idle + status = pC_->setIntegerParam(axisNo_, pC_->motorStatusDone_, 1); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusDone_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + // Write to the motor record fields status = setVeloFields(motorVelocity, 0.0, motorVmax); if (status != asynSuccess) { @@ -245,11 +253,41 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { double lowLimit = 0.0; double limitsOffset = 0.0; + // Was the axis idle during the previous poll? + int previousStatusDone = 1; + // ========================================================================= // Are we currently waiting for a handshake? if (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); + + if (pC_->msgPrintControl_.shouldBePrinted( + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, timedOut, + pC_->pasynUserSelf)) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAsked for a " + "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); + } + + if (timedOut) { + pl_status = + setStringParam(pC_->motorMessageText_, + "Timed out while waiting for a handshake"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + snprintf(command, sizeof(command), "P%2.2d23 P%2.2d01", axisNo_, axisNo_); rw_status = pC_->writeRead(axisNo_, command, response, 2); @@ -312,6 +350,14 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { __PRETTY_FUNCTION__, __LINE__); } + // Read the previous motor position + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusDone_, + &previousStatusDone); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + // Transform from EPICS to motor coordinates (see comment in // turboPmacAxis::atFirstPoll) previousPosition = previousPosition * motorRecResolution; @@ -375,7 +421,13 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { switch (axStatus) { case -6: // Axis is stopping - *moving = true; + + // If the axis was already idle during the last poll, it is not moving + if (previousStatusDone == 0) { + *moving = true; + } else { + *moving = false; + } break; case -5: // Axis is deactivated @@ -479,7 +531,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { } if (resetCountStatus) { - pC_->msgPrintControl_.resetCount(keyStatus); + pC_->msgPrintControl_.resetCount(keyStatus, pC_->pasynUserSelf); } if (*moving) { @@ -767,7 +819,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { } if (resetError) { - pC_->msgPrintControl_.resetCount(keyError); + pC_->msgPrintControl_.resetCount(keyError, pC_->pasynUserSelf); } // Update the parameter library @@ -990,6 +1042,41 @@ asynStatus turboPmacAxis::stop(double acceleration) { return rw_status; } +asynStatus turboPmacAxis::reset() { + + // 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_]; + + // ========================================================================= + + // Reset the error for this axis manually + snprintf(command, sizeof(command), "P%2.2d01=0", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, 0); + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Resetting\n"); + + if (rw_status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nResetting the " + "error failed\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return rw_status; +} + /* Home the axis. On absolute encoder systems, this is a no-op */ @@ -1421,4 +1508,4 @@ static void turboPmacAxisRegister(void) { } epicsExportRegistrar(turboPmacAxisRegister); -} // extern "C" \ No newline at end of file +} // extern "C" diff --git a/src/turboPmacAxis.h b/src/turboPmacAxis.h index 858c2e6..1a76dd9 100644 --- a/src/turboPmacAxis.h +++ b/src/turboPmacAxis.h @@ -81,6 +81,14 @@ class turboPmacAxis : public sinqAxis { */ asynStatus init(); + /** + * @brief Reset the axis error + * + * @param on + * @return asynStatus + */ + asynStatus reset(); + /** * @brief Enable / disable the axis. * diff --git a/src/turboPmacController.cpp b/src/turboPmacController.cpp index 982f231..e378497 100644 --- a/src/turboPmacController.cpp +++ b/src/turboPmacController.cpp @@ -300,7 +300,7 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, stringifyAsynStatus(status), msgPrintControl_.getSuffix()); } } else { - msgPrintControl_.resetCount(writeKey); + msgPrintControl_.resetCount(writeKey, pasynUserSelf); } // Read the response from the MCU buffer @@ -351,7 +351,7 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, stringifyAsynStatus(status), msgPrintControl_.getSuffix()); } } else { - msgPrintControl_.resetCount(readKey); + msgPrintControl_.resetCount(readKey, pasynUserSelf); } if (timeoutStatus == asynError) { @@ -377,7 +377,7 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, eomReason, msgPrintControl_.getSuffix()); } } else { - msgPrintControl_.resetCount(terminateKey); + msgPrintControl_.resetCount(terminateKey, pasynUserSelf); } /* @@ -412,7 +412,7 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command, modResponse, command); status = asynError; } else { - msgPrintControl_.resetCount(numResponsesKey); + msgPrintControl_.resetCount(numResponsesKey, pasynUserSelf); } // Create custom error messages for different failure modes, if no error