From 7d4c2ea1e4036babd91b174e3d1112eadc260cb3 Mon Sep 17 00:00:00 2001 From: smathis Date: Mon, 23 Dec 2024 09:40:40 +0100 Subject: [PATCH] Initial commit for masterMacs --- Makefile | 27 ++ README.md | 45 ++ src/masterMacs.dbd | 4 + src/masterMacsAxis.cpp | 794 +++++++++++++++++++++++++++++++++++ src/masterMacsAxis.h | 215 ++++++++++ src/masterMacsController.cpp | 652 ++++++++++++++++++++++++++++ src/masterMacsController.h | 105 +++++ utils/decodeStatus.py | 223 ++++++++++ 8 files changed, 2065 insertions(+) create mode 100644 Makefile create mode 100644 README.md create mode 100644 src/masterMacs.dbd create mode 100644 src/masterMacsAxis.cpp create mode 100644 src/masterMacsAxis.h create mode 100644 src/masterMacsController.cpp create mode 100644 src/masterMacsController.h create mode 100644 utils/decodeStatus.py diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..524aebe --- /dev/null +++ b/Makefile @@ -0,0 +1,27 @@ +# Use the PSI build system +include /ioc/tools/driver.makefile + +MODULE=masterMacs +BUILDCLASSES=Linux +EPICS_VERSIONS=7.0.7 +ARCH_FILTER=RHEL% + +# Additional module dependencies +REQUIRED+=asynMotor +REQUIRED+=sinqMotor + +# Specify the version of sinqMotor we want to build against +sinqMotor_VERSION=mathis_s + +# These headers allow to depend on this library for derived drivers. +HEADERS += src/masterMacsAxis.h +HEADERS += src/masterMacsController.h + +# Source files to build +SOURCES += src/masterMacsAxis.cpp +SOURCES += src/masterMacsController.cpp + +# This file registers the motor-specific functions in the IOC shell. +DBDS += src/masterMacs.dbd + +USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result # -Werror diff --git a/README.md b/README.md new file mode 100644 index 0000000..d2df815 --- /dev/null +++ b/README.md @@ -0,0 +1,45 @@ +# masterMacs + +## Overview + +This is a driver for the masterMacs motion controller with the SINQ communication protocol. It is based on the sinqMotor shared library (https://git.psi.ch/sinq-epics-modules/sinqmotor). The header files contain detailed documentation for all public functions. The headers themselves are exported when building the library to allow other drivers to depend on this one. + +## User guide + +This driver is a standard sinqMotor-derived driver and does not need any specific configuration. For the general configuration, please see https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. + +The folder "utils" contains utility scripts for working with masterMacs motor controllers: +- decodeStatus.py: Take the return message of a R10 (read status) command and print it in human-readable form. + +## Developer guide + +### Usage in IOC shell + +masterMacs exposes the following IOC shell functions (all in masterMacsController.cpp): +- `masterMacsController`: Create a new controller object. +- `masterMacsAxis`: Create a new axis object. +These functions are parametrized as follows: +``` +masterMacsController( + "$(NAME)", # Name of the MCU, e.g. mcu1. This parameter should be provided by an environment variable. + "$(ASYN_PORT)", # IP-Port of the MCU. This parameter should be provided by an environment variable. + 8, # Maximum number of axes + 0.05, # Busy poll period in seconds + 1, # Idle poll period in seconds + 0.05 # Communication timeout in seconds + ); +``` +``` +masterMacsAxis( + "$(NAME)", # Name of the associated MCU, e.g. mcu1. This parameter should be provided by an environment variable. + 1 # Index of the axis. + ); +``` + +### Versioning + +Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. + +### How to build it + +Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. diff --git a/src/masterMacs.dbd b/src/masterMacs.dbd new file mode 100644 index 0000000..e54e329 --- /dev/null +++ b/src/masterMacs.dbd @@ -0,0 +1,4 @@ +#--------------------------------------------- +# SINQ specific DB definitions +#--------------------------------------------- +registrar(masterMacsRegister) diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp new file mode 100644 index 0000000..c42a9b7 --- /dev/null +++ b/src/masterMacsAxis.cpp @@ -0,0 +1,794 @@ +#include "masterMacsAxis.h" +#include "asynOctetSyncIO.h" +#include "masterMacsController.h" +#include +#include +#include +#include +#include +#include +#include + +masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo) + : sinqAxis(pC, axisNo), pC_(pC) { + + asynStatus status = asynSuccess; + + /* + The superclass constructor sinqAxis calls in turn its superclass constructor + asynMotorAxis. In the latter, a pointer to the constructed object this is + stored inside the array pAxes_: + + pC->pAxes_[axisNo] = this; + + Therefore, the axes are managed by the controller pC. If axisNo is out of + bounds, asynMotorAxis prints an error (see + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp, + line 40). However, we want the IOC creation to stop completely, since this + is a configuration error. + */ + if (axisNo >= pC->numAxes_) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:: FATAL ERROR: Axis index %d must be smaller " + "than the total number of axes %d. Call the support.", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC->numAxes_); + exit(-1); + } + + // Initialize all member variables + initial_poll_ = true; + axisStatus_ = std::bitset<16>(0); + axisError_ = std::bitset<16>(0); + + // Default values for the watchdog timeout mechanism + offsetMovTimeout_ = 30; // seconds + scaleMovTimeout_ = 2.0; + + // masterMacs motors can always be disabled + status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 1); + if (status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (setting a parameter value failed " + "with %s)\n. Terminating IOC", + __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); + exit(-1); + } + + // Assume that the motor is initially not moving + status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving_, false); + if (status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (setting a parameter value failed " + "with %s)\n. Terminating IOC", + __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); + exit(-1); + } +} + +masterMacsAxis::~masterMacsAxis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +/** +Read out the following values: +- current position +- current velocity +- maximum velocity +- acceleration +- Software limits + */ +asynStatus masterMacsAxis::atFirstPoll() { + + // Local variable declaration + asynStatus pl_status = asynSuccess; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + double motorRecResolution = 0.0; + double motorPosition = 0.0; + double motorVelocity = 0.0; + double motorVmax = 0.0; + double motorAccel = 0.0; + + // ========================================================================= + + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status == asynParamUndefined) { + return asynParamUndefined; + } else if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Read out the current position + snprintf(command, sizeof(command), "%dR12", axisNo_); + pl_status = pC_->writeRead(axisNo_, command, response, true); + if (pl_status != asynSuccess) { + return pl_status; + } + nvals = sscanf(response, "%lf", &motorPosition); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Read out the current velocity + snprintf(command, sizeof(command), "%dR05", axisNo_); + pl_status = pC_->writeRead(axisNo_, command, response, true); + if (pl_status != asynSuccess) { + return pl_status; + } + nvals = sscanf(response, "%lf", &motorVelocity); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Read out the maximum velocity + // snprintf(command, sizeof(command), "%dR26", axisNo_); + // pl_status = pC_->writeRead(axisNo_, command, response, true); + // if (pl_status != asynSuccess) { + // return pl_status; + // } + // nvals = sscanf(response, "%lf", &motorVmax); + // if (nvals != 1) { + // return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + // __PRETTY_FUNCTION__, + // __LINE__); + // } + // TODO: Temporary workaround until R26 is implemented + motorVmax = motorVelocity; + + // Read out the acceleration + snprintf(command, sizeof(command), "%dR06", axisNo_); + pl_status = pC_->writeRead(axisNo_, command, response, true); + if (pl_status != asynSuccess) { + return pl_status; + } + nvals = sscanf(response, "%lf", &motorAccel); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from motor to parameter library coordinates + motorPosition = motorPosition / motorRecResolution; + + // Store these values in the parameter library + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorPosition_, motorPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Write to the motor record fields + pl_status = setVeloFields(motorVelocity, 0.0, motorVmax); + if (pl_status != asynSuccess) { + return pl_status; + } + pl_status = setAcclField(motorAccel); + if (pl_status != asynSuccess) { + return pl_status; + } + + pl_status = readEncoderType(); + if (pl_status != asynSuccess) { + return pl_status; + } + + // Update the parameter library immediately + pl_status = callParamCallbacks(); + if (pl_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 => line %d:\ncallParamCallbacks failed with %s for " + "axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, + pC_->stringifyAsynStatus(pl_status), axisNo_); + return pl_status; + } + return pl_status; +} + +// Perform the actual poll +asynStatus masterMacsAxis::doPoll(bool *moving) { + + // Return value for the poll + asynStatus poll_status = asynSuccess; + + // 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_]; + int nvals = 0; + + int direction = 0; + bool hasError = false; + double currentPosition = 0.0; + double previousPosition = 0.0; + double motorRecResolution = 0.0; + double highLimit = 0.0; + double lowLimit = 0.0; + double limitsOffset = 0.0; + int wasMoving = 0; + + // ========================================================================= + + // Motor resolution from parameter library + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Read the previous motor position + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from EPICS to motor coordinates (see comment in + // masterMacsAxis::readConfig()) + previousPosition = previousPosition * motorRecResolution; + + // Update the axis status + rw_status = readAxisStatus(); + if (rw_status != asynSuccess) { + return rw_status; + } + + // Update the movement status + *moving = isMoving(); + + // Read the current position + snprintf(command, sizeof(command), "%dR12", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%lf", ¤tPosition); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Read the low limit + snprintf(command, sizeof(command), "%dR34", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%lf", &lowLimit); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Read the high limit + snprintf(command, sizeof(command), "%dR33", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%lf", &highLimit); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Did we just finish a movement? If yes, read out the error. + pl_status = + pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &wasMoving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + __PRETTY_FUNCTION__, __LINE__); + } + if (!isMoving() && wasMoving == 1) { + rw_status = readAxisError(); + if (rw_status != asynSuccess) { + return rw_status; + } + + // TODO> Error handling + } + + /* + The axis limits are set as: ({[]}) + where [] are the positive and negative limits set in EPICS/NICOS, {} are + the software limits set on the MCU and () are the hardware limit + switches. In other words, the EPICS/NICOS limits must be stricter than + the software limits on the MCU which in turn should be stricter than the + hardware limit switches. For example, if the hardware limit switches are + at [-10, 10], the software limits could be at [-9, 9] and the EPICS / + NICOS limits could be at + [-8, 8]. Therefore, we cannot use the software limits read from the MCU + directly, but need to shrink them a bit. In this case, we're shrinking + them by 0.1 mm or 0.1 degree (depending on the axis type) on both sides. + */ + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_, &limitsOffset); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_", + __PRETTY_FUNCTION__, __LINE__); + } + highLimit = highLimit - limitsOffset; + lowLimit = lowLimit + limitsOffset; + + // + + // Update the enablement PV + pl_status = setIntegerParam(pC_->motorEnableRBV_, enabled()); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (*moving) { + // If the axis is moving, evaluate the movement direction + if ((currentPosition - previousPosition) > 0) { + direction = 1; + } else { + direction = 0; + } + } + + // Update the parameter library + if (hasError) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + if (!*moving) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from motor to EPICS coordinates (see comment in + // masterMacsAxis::atFirstPoll()) + currentPosition = currentPosition / motorRecResolution; + + pl_status = setDoubleParam(pC_->motorPosition_, currentPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + return poll_status; +} + +asynStatus masterMacsAxis::doMove(double position, int relative, + double minVelocity, double maxVelocity, + double acceleration) { + + // 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_]; + double motorCoordinatesPosition = 0.0; + double motorRecResolution = 0.0; + double motorVelocity = 0.0; + int enabled = 0; + int motorCanSetSpeed = 0; + + // ========================================================================= + + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (enabled == 0) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is disabled.\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + return asynSuccess; + } + + // Convert from EPICS to user / motor units + motorCoordinatesPosition = position * motorRecResolution; + motorVelocity = maxVelocity * motorRecResolution; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nStart of axis %d to position %lf.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, position); + + // Check if the speed is allowed to be changed + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_, + &motorCanSetSpeed); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorCanSetSpeed_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Set the new motor speed, if the user is allowed to do so. + if (motorCanSetSpeed != 0) { + snprintf(command, sizeof(command), "%dS05=%lf", axisNo_, motorVelocity); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, + "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + return rw_status; + } + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nSetting speed of axis %d to %lf.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, motorVelocity); + } + + // Set the target position + snprintf(command, sizeof(command), "%dS02=%lf", axisNo_, + motorCoordinatesPosition); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + return rw_status; + } + + // Start the move + if (relative) { + snprintf(command, sizeof(command), "%dS00=2", axisNo_); + } else { + snprintf(command, sizeof(command), "%dS00=1", axisNo_); + } + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + return rw_status; + } + + // Waiting for a handshake is already part of the movement procedure => + // Start the watchdog + if (startMovTimeoutWatchdog() != asynSuccess) { + return asynError; + } + + return rw_status; +} + +asynStatus masterMacsAxis::stop(double acceleration) { + + // 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_]; + + // ========================================================================= + + snprintf(command, sizeof(command), "%dS00=8", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + return rw_status; +} + +/* +Home the axis. On absolute encoder systems, this is a no-op +*/ +asynStatus masterMacsAxis::doHome(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_]; + + // ========================================================================= + + pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_, + sizeof(response), response); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "encoderType_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Only send the home command if the axis has an incremental encoder + if (strcmp(response, IncrementalEncoder) == 0) { + + snprintf(command, sizeof(command), "%dS00=9", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + return rw_status; + } + + pl_status = setIntegerParam(pC_->motorMoveToHome_, 1); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setStringParam(pC_->motorMessageText_, "Homing"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } else { + pl_status = setStringParam(pC_->motorMessageText_, + "Can't home a motor with absolute encoder"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + return rw_status; +} + +/* +Read the encoder type and update the parameter library accordingly +*/ +asynStatus masterMacsAxis::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_]; + int nvals = 0; + int encoder_id = 0; + + // ========================================================================= + + // Check if this is an absolute encoder + snprintf(command, sizeof(command), "%dR60", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d", &encoder_id); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + Defined encoder IDs: + 0=INC (Incremental) + 1=SSI (Absolute encoder with SSI interface) + 2=SSI (Absolute encoder with BiSS interface) + */ + if (encoder_id == 0) { + pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder); + } else { + pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder); + } + + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "encoderType_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynSuccess; +} + +asynStatus masterMacsAxis::enable(bool on) { + + int timeout_enable_disable = 2; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + bool moving = false; + doPoll(&moving); + + // ========================================================================= + + // If the axis is currently moving, it cannot be disabled. Ignore the + // command and inform the user. We check the last known status of the + // axis instead of "moving", since status -6 is also moving, but the + // motor can actually be disabled in this state! + if (moving) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is not idle and can therefore not " + "be enabled / disabled.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Axis cannot be disabled while it is moving."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + return asynError; + } + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((axisStatus_ != -3) == on) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s => line %d:\nAxis %d on controller %s is already %s.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName, + on ? "enabled" : "disabled"); + return asynSuccess; + } + + // Enable / disable the axis if it is not moving + snprintf(command, sizeof(command), "%dS04=%d", axisNo_, on); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\n%s axis %d on controller %s\n", + __PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable", axisNo_, + pC_->portName); + if (on == 0) { + pl_status = setStringParam(pC_->motorMessageText_, "Disabling ..."); + } else { + pl_status = setStringParam(pC_->motorMessageText_, "Enabling ..."); + } + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + 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 + int startTime = time(NULL); + while (time(NULL) < startTime + timeout_enable_disable) { + + // Read the axis status + usleep(100000); + rw_status = readAxisStatus(); + if (rw_status != asynSuccess) { + return rw_status; + } + + if (switchedOn() == 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 => line %d:\nFailed to %s axis %d on controller %s within %d " + "seconds\n", + __PRETTY_FUNCTION__, __LINE__, on ? "enable" : "disable", axisNo_, + pC_->portName, timeout_enable_disable); + + // Output message to user + snprintf(command, sizeof(command), "Failed to %s within %d seconds", + on ? "enable" : "disable", timeout_enable_disable); + pl_status = setStringParam(pC_->motorMessageText_, "Enabling ..."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynError; +} + +asynStatus masterMacsAxis::readAxisStatus() { + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + + // ========================================================================= + + snprintf(command, sizeof(command), "%dR10", axisNo_); + asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status == asynSuccess) { + + int axisStatus = 0; + int nvals = sscanf(response, "%d", &axisStatus); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse( + command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + axisStatus_ = std::bitset<16>(axisStatus); + } + return rw_status; +} + +asynStatus masterMacsAxis::readAxisError() { + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + + // ========================================================================= + + snprintf(command, sizeof(command), "%dR11", axisNo_); + asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status == asynSuccess) { + + int axisError = 0; + int nvals = sscanf(response, "%d", &axisError); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse( + command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + axisError_ = std::bitset<16>(axisError); + } + return rw_status; +} \ No newline at end of file diff --git a/src/masterMacsAxis.h b/src/masterMacsAxis.h new file mode 100644 index 0000000..b21ff34 --- /dev/null +++ b/src/masterMacsAxis.h @@ -0,0 +1,215 @@ +#ifndef masterMacsAXIS_H +#define masterMacsAXIS_H +#include "sinqAxis.h" +#include + +// 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. +class masterMacsController; + +class masterMacsAxis : public sinqAxis { + public: + /** + * @brief Construct a new masterMacsAxis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + */ + masterMacsAxis(masterMacsController *pController, int axisNo); + + /** + * @brief Destroy the masterMacsAxis + * + */ + virtual ~masterMacsAxis(); + + /** + * @brief Implementation of the `stop` function from asynMotorAxis + * + * @param acceleration Acceleration ACCEL from the motor record. This + * value is currently not used. + * @return asynStatus + */ + asynStatus stop(double acceleration); + + /** + * @brief Implementation of the `doHome` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doHome`. + * + * @param minVelocity + * @param maxVelocity + * @param acceleration + * @param forwards + * @return asynStatus + */ + asynStatus doHome(double minVelocity, double maxVelocity, + double acceleration, int forwards); + + /** + * @brief Implementation of the `doPoll` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doPoll`. + * + * @param moving + * @return asynStatus + */ + asynStatus doPoll(bool *moving); + + /** + * @brief Implementation of the `doMove` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doMove`. + * + * @param position + * @param relative + * @param min_velocity + * @param max_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus doMove(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + + /** + * @brief Implementation of the `atFirstPoll` function from sinqAxis. + * + * The following steps are performed: + * - Read out the motor status, motor position, velocity and acceleration + * from the MCU and store this information in the parameter library. + * - Set the enable PV accordint to the initial status of the axis. + * + * @return asynStatus + */ + asynStatus atFirstPoll(); + + /** + * @brief Enable / disable the axis. + * + * @param on + * @return asynStatus + */ + asynStatus enable(bool on); + + /** + * @brief Read the encoder type (incremental or absolute) for this axis from + * the MCU and store the information in the PV ENCODER_TYPE. + * + * @return asynStatus + */ + asynStatus readEncoderType(); + + protected: + masterMacsController *pC_; + + asynStatus readConfig(); + bool initial_poll_; + + /* + The axis status and axis error of MasterMACS are given as an integer from + the controller. The 16 individual bits contain the actual information. + */ + std::bitset<16> axisStatus_; + std::bitset<16> axisError_; + + /** + * @brief Read the Master MACS status with the xR10 command and store the + * result in axisStatus_ + * + */ + asynStatus readAxisStatus(); + + /* + The functions below read the specified status bit from the axisStatus_ + bitset. Since a bit can either be 0 or 1, the return value is given as a + boolean. + */ + + /** + * @brief Read the property from axisStatus_ + */ + bool readyToBeSwitchedOn() { return axisStatus_[0]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool switchedOn() { return axisStatus_[1]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool enabled() { return axisStatus_[2]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool faultConditionSet() { return axisStatus_[3]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool voltagePresent() { return axisStatus_[4]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool quickStopping() { return axisStatus_[5] == 0; } + + /** + * @brief Read the property from axisStatus_ + */ + bool switchOnDisabled() { return axisStatus_[6]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool newMoveCommandWhileMoving() { return axisStatus_[7]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool isMoving() { return axisStatus_[8]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool remoteMode() { return axisStatus_[9]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool targetReached() { return axisStatus_[10]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool internalLimitActive() { return axisStatus_[11]; } + + // Bits 12 and 13 are unused + + /** + * @brief Read the property from axisStatus_ + */ + bool setEventHasOcurred() { return axisStatus_[14]; } + + /** + * @brief Read the property from axisStatus_ + */ + bool powerEnabled() { return axisStatus_[15]; } + + /** + * @brief Read the Master MACS status with the xR10 command and store the + * result in axisStatus_ + * + */ + asynStatus readAxisError(); + + /* + The functions below read the specified error bit from the axisError_ + bitset. Since a bit can either be 0 or 1, the return value is given as a + boolean. + */ + + private: + friend class masterMacsController; +}; + +#endif diff --git a/src/masterMacsController.cpp b/src/masterMacsController.cpp new file mode 100644 index 0000000..71c9f7f --- /dev/null +++ b/src/masterMacsController.cpp @@ -0,0 +1,652 @@ + +#include "masterMacsController.h" +#include "asynMotorController.h" +#include "asynOctetSyncIO.h" +#include "masterMacsAxis.h" +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Copy src into dst and replace all NULL terminators up to the carriage + * return with spaces. This allows to print *dst with asynPrint. + * + * @param dst Buffer for the modified string + * @param src Original string + */ +void adjustForPrint(char *dst, const char *src, size_t buf_length) { + for (size_t i = 0; i < buf_length; i++) { + if (src[i] == '\x0d') { + dst[i] = ' '; + break; + } else if (src[i] == '\x00') { + dst[i] = ' '; + } else { + dst[i] = src[i]; + } + } +} + +/** + * @brief Construct a new masterMacsController::masterMacsController object + * + * @param portName See documentation of sinqController + * @param ipPortConfigName See documentation of sinqController + * @param numAxes See documentation of sinqController + * @param movingPollPeriod See documentation of sinqController + * @param idlePollPeriod See documentation of sinqController + * @param comTimeout Time after which a communication timeout error + * is declared in writeRead (in seconds) + * @param extraParams See documentation of sinqController + */ +masterMacsController::masterMacsController(const char *portName, + const char *ipPortConfigName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, + double comTimeout) + : sinqController( + portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, + /* + The following parameter library entries are added in this driver: + - REREAD_ENCODER_POSITION + - READ_CONFIG + */ + NUM_masterMacs_DRIVER_PARAMS) + +{ + + // Initialization of local variables + asynStatus status = asynSuccess; + + // Initialization of all member variables + lowLevelPortUser_ = nullptr; + comTimeout_ = comTimeout; + + // =========================================================================; + + /* + We try to connect to the port via the port name provided by the constructor. + If this fails, the function is terminated via exit + */ + pasynOctetSyncIO->connect(ipPortConfigName, 0, &lowLevelPortUser_, NULL); + if (status != asynSuccess || lowLevelPortUser_ == nullptr) { + errlogPrintf( + "%s => line %d:\nFATAL ERROR (cannot connect to MCU controller).\n" + "Terminating IOC", + __PRETTY_FUNCTION__, __LINE__); + 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 = "\x03"; // Hex-code for ETX + status = pasynOctetSyncIO->setInputEos( + lowLevelPortUser_, message_from_device, strlen(message_from_device)); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (setting input EOS failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + pasynOctetSyncIO->disconnect(lowLevelPortUser_); + exit(-1); + } + + status = callParamCallbacks(); + if (status != asynSuccess) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (executing ParamLib callbacks failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + pasynOctetSyncIO->disconnect(lowLevelPortUser_); + exit(-1); + } +} + +/* +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. +*/ +masterMacsAxis *masterMacsController::getAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return masterMacsController::castToAxis(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 +*/ +masterMacsAxis *masterMacsController::getAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return masterMacsController::castToAxis(asynAxis); +} + +masterMacsAxis *masterMacsController::castToAxis(asynMotorAxis *asynAxis) { + + // ========================================================================= + + // If the axis slot of the pAxes_ array is empty, a nullptr must be returned + if (asynAxis == nullptr) { + return nullptr; + } + + // Here, an error is emitted since asyn_axis is not a nullptr but also not + // an instance of Axis + masterMacsAxis *axis = dynamic_cast(asynAxis); + if (axis == nullptr) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is not an instance of masterMacsAxis", + __PRETTY_FUNCTION__, __LINE__, axis->axisNo_); + } + return axis; +} + +asynStatus masterMacsController::writeRead(int axisNo, const char *command, + char *response, + bool expectResponse) { + + // Definition of local variables. + asynStatus status = asynSuccess; + asynStatus pl_status = asynSuccess; + std::string fullCommand = ""; + char fullResponse[MAXBUF_] = {0}; + char printableCommand[MAXBUF_] = {0}; + char printableResponse[MAXBUF_] = {0}; + char drvMessageText[MAXBUF_] = {0}; + int motorStatusProblem = 0; + + // Send the message and block the thread until either a response has + // been received or the timeout is triggered + int eomReason = 0; // Flag indicating why the message has ended + + // 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; + + // These parameters are used to remove not-needed information from the + // response. + int dataStartIndex = 0; + int validIndex = 0; + bool responseValid = false; + + // ========================================================================= + + masterMacsAxis *axis = getAxis(axisNo); + if (axis == nullptr) { + // We already did the error logging directly in getAxis + return asynError; + } + + /* + PSI SINQ uses a custom protocol which is described in + PSI_TCP_Interface_V1-8.pdf (p. // 4-17). + A special case is the message length, which is specified by two bytes LSB + and MSB: + MSB = message length / 256 + LSB = message length % 256. + For example, a message length of 47 chars would result in MSB = 0, LSB = 47, + whereas a message length of 356 would result in MSB = 1, LSB = 100. + + The full protocol looks as follows: + 0x05 -> Start of protocol frame ENQ + [LSB] + [MSB] + 0x19 -> Data type PDO1 + value [Actual message] It is not necessary to append a terminator, since + this protocol encodes the message length in LSB and MSB. + 0x0D -> Carriage return (ASCII alias \r) + 0x03 -> End of text ETX + + The rest of the telegram length is filled with 0x00 as specified in the + protocol. + */ + + // Command has four additional bytes between ENQ and ETX: + // LSB, MSB, PDO1 and CR + const size_t commandLength = strlen(command) + 4; + + // Perform both division and modulo operation at once. + div_t commandLengthSep = std::div(commandLength, 256); + + /* + Build the actual command. LSB and MSB need to be converted directly to hex, + hence they are interpolated as characters. For example, the eight hex value + is 0x08, but interpolating 8 directly via %x or %d returns the 38th hex + value, since 8 is interpreted as ASCII in those cases. + */ + fullCommand.push_back('\x05'); + fullCommand.push_back(commandLengthSep.rem); + fullCommand.push_back(commandLengthSep.quot); + fullCommand.push_back('\x19'); + for (size_t i = 0; i < strlen(command); i++) { + fullCommand.push_back(command[i]); + } + fullCommand.push_back('\x0D'); + fullCommand.push_back('\x03'); + // snprintf(fullCommand, MAXBUF_, "\x05%c%c\x19%s\x0D\x03", + // commandLengthSep.rem, commandLengthSep.quot, command); + + adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_); + asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, + "%s => line %d:\nSending command %s\n", __PRETTY_FUNCTION__, + __LINE__, printableCommand); + + // Perform the actual writeRead + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(), + fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, &eomReason); + + // ___________________________________________________________DEBUG + + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + // "=================== COMMAND ===================\n"); + + // for (int i = 0; i < 12; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // fullCommand[i], fullCommand[i]); + // } + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n"); + + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + // "=================== RESPONSE ===================\n"); + + // for (int i = 0; i < 40; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // fullResponse[i], fullResponse[i]); + // } + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n"); + + // ___________________________________________________________DEBUG + + /* + As of 19.12.2025, there is a bug in the MasterMACS hardware: If two commands + are sent in rapid succession, MasterMACS sends garbage as answer for the + second command. Crucially, this garbage does not contain an CR. Hence, the + following strategy is implemented here: + - Wait 1 ms after a pasynOctetSyncIO->writeRead + - If the message does not contain an CR, wait 50 ms and then try again. If + we receive garbage again, propagate the error. + */ + // Check for CR + bool hasEtx = false; + for (ulong i = 0; i < sizeof(fullResponse); i++) { + if (fullResponse[i] == '\x0d') { + hasEtx = true; + break; + } + } + + if (!hasEtx) { + usleep(50000); // 50 ms + + // Try again ... + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(), + fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, + &eomReason); + + // Does this message contain an CR? + for (ulong i = 0; i < sizeof(fullResponse); i++) { + if (fullResponse[i] == '\x0d') { + hasEtx = true; + break; + } + } + if (!hasEtx) { + adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_); + adjustForPrint(printableResponse, fullResponse, MAXBUF_); + // Failed for the second time => Give up and propagate the error. + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s => line %d:\nReceived garbage response '%s' for " + "command '%s' two times in a row.\n", + __PRETTY_FUNCTION__, __LINE__, printableResponse, + printableCommand); + status = asynError; + } else { + usleep(1000); // 1 ms + } + } else { + usleep(50000); // 1 ms + } + + // Create custom error messages for different failure modes + switch (status) { + case asynSuccess: + + /* + A response looks like this (note the spaces, they are part of the + message!): + - [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[ACK][CR] (No error) + - [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[NAK][CR] (Communication failed) + - [ENQ][LSB][MSB][PDO1] 1 S 10 [CAN][CR] (Driver tried to write with a + read-only command) + Read out the second-to-last char of the response and check if it is NAK + or CAN. + */ + + // We don't use strlen here since the C string terminator 0x00 occurs in + // the middle of the char array. + for (ulong i = 0; i < sizeof(fullResponse); i++) { + if (fullResponse[i] == '=') { + dataStartIndex = i + 1; + } + if (fullResponse[i] == '\x06') { + validIndex = i; + responseValid = true; + break; + } else if (fullResponse[i] == '\x15') { + // NAK + snprintf(drvMessageText, sizeof(drvMessageText), + "Communication failed."); + responseValid = false; + break; + } else if (fullResponse[i] == '\x18') { + // CAN + snprintf(drvMessageText, sizeof(drvMessageText), + "Tried to write with a read-only command. This is a " + "bug, please call the support."); + responseValid = false; + break; + } + } + + if (responseValid) { + /* + If a property has been read, we need just the part between the "=" + (0x3D) and the [ACK] (0x06). Therefore, we remove all non-needed + parts after evaluating the second-to-last char before returning the + response. + */ + for (int i = 0; i + dataStartIndex < validIndex; i++) { + response[i] = fullResponse[i + dataStartIndex]; + } + } else { + status = asynError; + } + + 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; + } + + // ___________________________________________________________DEBUG + + // adjustForPrint(printableCommand, fullCommand, MAXBUF_); + // adjustForPrint(printableResponse, fullResponse, MAXBUF_); + + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + // "\n------------------c\n"); for (int i = 0; i < 12; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // fullCommand[i], fullCommand[i]); + // } + // for (int i = 0; i < 12; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // printableCommand[i], printableCommand[i]); + // } + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + // "\n=================\n"); for (int i = 0; i < 12; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // fullResponse[i], fullResponse[i]); + // } + // for (int i = 0; i < 12; ++i) { + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n", + // printableResponse[i], printableResponse[i]); + // } + // asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + // "\n++++++++++++++++++++\n"); + + // asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, + // "%s => line %d:\nResponse '%s' for command '%s'.\n", + // __PRETTY_FUNCTION__, __LINE__, printableResponse, + // printableCommand); + // ___________________________________________________________DEBUG + + // Log the overall status (communication successfull or not) + if (status == asynSuccess) { + asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, + "%s => line %d:\nReturn value: %s\n", __PRETTY_FUNCTION__, + __LINE__, response); + pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0); + } else { + adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_); + asynPrint( + lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s => line %d:\nCommunication failed for command '%s' (%s)\n", + __PRETTY_FUNCTION__, __LINE__, printableCommand, + stringifyAsynStatus(status)); + + // Check if the axis already is in an error communication mode. If it is + // not, upstream the error. This is done to avoid "flooding" the user + // with different error messages if more than one error ocurred before + // an error-free communication + pl_status = + getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (motorStatusProblem == 0) { + pl_status = axis->setStringParam(motorMessageText_, drvMessageText); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = axis->setIntegerParam(motorStatusProblem_, 1); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorStatusProblem", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = axis->setIntegerParam(motorStatusProblem_, 1); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorStatusCommsError_", + __PRETTY_FUNCTION__, __LINE__); + } + } + } + return status; +} + +asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) { + // masterMacs can be disabled + if (pasynUser->reason == motorCanDisable_) { + *value = 1; + return asynSuccess; + } else { + return asynMotorController::readInt32(pasynUser, value); + } +} + +/*************************************************************************************/ +/** 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 +masterMacsController constructor documentation. +*/ +asynStatus masterMacsCreateController(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" + masterMacsController *pController = + new masterMacsController(portName, ipPortConfigName, numAxes, + movingPollPeriod, idlePollPeriod, comTimeout); + + return asynSuccess; +} + +/* +C wrapper for the axis constructor. Please refer to the masterMacsAxis +constructor documentation. The controller is read from the portName. +*/ +asynStatus masterMacsCreateAxis(const char *portName, int axis) { + masterMacsAxis *pAxis; + + /* + findAsynPortDriver is a asyn library FFI function which uses the C ABI. + Therefore it returns a void pointer instead of e.g. a pointer to a + superclass of the controller such as asynPortDriver. Type-safe upcasting + via dynamic_cast is therefore not possible directly. However, we do know + that the void pointer is either a pointer to asynPortDriver (if a driver + with the specified name exists) or a nullptr. Therefore, we first do a + nullptr check, then a cast to asynPortDriver and lastly a (typesafe) + dynamic_upcast to Controller + https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c + */ + void *ptr = findAsynPortDriver(portName); + if (ptr == nullptr) { + /* + We can't use asynPrint here since this macro would require us + to get a lowLevelPortUser_ from a pointer to an asynPortDriver. + However, the given pointer is a nullptr and therefore doesn't + have a lowLevelPortUser_! printf is an EPICS alternative which + works w/o that, but doesn't offer the comfort provided + by the asynTrace-facility + */ + errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__, + __LINE__, portName); + return asynError; + } + // Unsafe cast of the pointer to an asynPortDriver + asynPortDriver *apd = (asynPortDriver *)(ptr); + + // Safe downcast + masterMacsController *pC = dynamic_cast(apd); + if (pC == nullptr) { + errlogPrintf("%s => line %d:\ncontroller on port %s is not a " + "masterMacsController.", + __PRETTY_FUNCTION__, __LINE__, portName); + return asynError; + } + + // Prevent manipulation of the controller from other threads while we + // create the new axis. + pC->lock(); + + /* + We create a new instance of the axis, 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" + pAxis = new masterMacsAxis(pC, axis); + + // Allow manipulation of the controller again + pC->unlock(); + return asynSuccess; +} + +/* +This is boilerplate code which is used to make the FFI functions +CreateController and CreateAxis "known" to the IOC shell (iocsh). +*/ + +#ifdef vxWorks +#else + +/* +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. mmacs1)", + iocshArgString}; +static const iocshArg CreateControllerArg1 = { + "Asyn IP port name (e.g. pmmacs1)", 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 configMasterMacsCreateController = { + "masterMacsController", 6, CreateControllerArgs}; +static void configMasterMacsCreateControllerCallFunc(const iocshArgBuf *args) { + masterMacsCreateController(args[0].sval, args[1].sval, args[2].ival, + args[3].dval, args[4].dval, args[5].dval); +} + +/* +Same procedure as for the CreateController function, but for the axis +itself. +*/ +static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mmacs1)", + iocshArgString}; +static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0, + &CreateAxisArg1}; +static const iocshFuncDef configMasterMacsCreateAxis = {"masterMacsAxis", 2, + CreateAxisArgs}; +static void configMasterMacsCreateAxisCallFunc(const iocshArgBuf *args) { + masterMacsCreateAxis(args[0].sval, args[1].ival); +} + +// This function is made known to EPICS in masterMacs.dbd and is called by EPICS +// in order to register both functions in the IOC shell +static void masterMacsRegister(void) { + iocshRegister(&configMasterMacsCreateController, + configMasterMacsCreateControllerCallFunc); + iocshRegister(&configMasterMacsCreateAxis, + configMasterMacsCreateAxisCallFunc); +} +epicsExportRegistrar(masterMacsRegister); + +#endif + +} // extern "C" diff --git a/src/masterMacsController.h b/src/masterMacsController.h new file mode 100644 index 0000000..7d3e3c3 --- /dev/null +++ b/src/masterMacsController.h @@ -0,0 +1,105 @@ +/******************************************** + * masterMacsController.h + * + * PMAC V3 controller driver based on the asynMotorController class + * + * Stefan Mathis, September 2024 + ********************************************/ + +#ifndef masterMacsController_H +#define masterMacsController_H +#include "masterMacsAxis.h" +#include "sinqAxis.h" +#include "sinqController.h" + +class masterMacsController : public sinqController { + + public: + /** + * @brief Construct a new masterMacsController object + * + * @param portName See sinqController constructor + * @param ipPortConfigName See sinqController constructor + * @param numAxes See sinqController constructor + * @param movingPollPeriod See sinqController constructor + * @param idlePollPeriod See sinqController constructor + * @param comTimeout When trying to communicate with the device, + the underlying asynOctetSyncIO interface waits for a response until this + time (in seconds) has passed, then it declares a timeout. + */ + masterMacsController(const char *portName, const char *ipPortConfigName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, double comTimeout); + + /** + * @brief Get the axis object + * + * @param pasynUser Specify the axis via the asynUser + * @return masterMacsAxis* If no axis could be found, this is a + * nullptr + */ + masterMacsAxis *getAxis(asynUser *pasynUser); + + /** + * @brief Get the axis object + * + * @param axisNo Specify the axis via its index + * @return masterMacsAxis* If no axis could be found, this is a + * nullptr + */ + masterMacsAxis *getAxis(int axisNo); + + protected: + asynUser *lowLevelPortUser_; + + /** + * @brief Send a command to the hardware and receive a response + * + * @param axisNo Axis to which the command should be send + * @param command Command for the hardware + * @param response Buffer for the response. This buffer is + * expected to have the size MAXBUF_. + * @param numExpectedResponses The PMAC MCU can send multiple responses at + * once. The number of responses is determined by the number of + * "subcommands" within command. Therefore it is known in advance how many + * "subresponses" are expected. This can be used to check the integrity of + * the received response, since the subresponses are separated by carriage + * returns (/r). The number of carriage returns is compared to + * numExpectedResponses to determine if the communication was successfull. + * @return asynStatus + */ + asynStatus writeRead(int axisNo, const char *command, char *response, + bool expectResponse); + + /** + * @brief Save cast of the given asynAxis pointer to a masterMacsAxis + * pointer. If the cast fails, this function returns a nullptr. + * + * @param asynAxis + * @return masterMacsAxis* + */ + masterMacsAxis *castToAxis(asynMotorAxis *asynAxis); + + private: + // Set the maximum buffer size. This is an empirical value which must be + // large enough to avoid overflows for all commands to the device / + // responses from it. + static const uint32_t MAXBUF_ = 200; + + /* + Stores the constructor input comTimeout + */ + double comTimeout_; + + // Indices of additional PVs +#define FIRST_masterMacs_PARAM rereadEncoderPosition_ + int rereadEncoderPosition_; + int readConfig_; +#define LAST_masterMacs_PARAM readConfig_ + + friend class masterMacsAxis; +}; +#define NUM_masterMacs_DRIVER_PARAMS \ + (&LAST_masterMacs_PARAM - &FIRST_masterMacs_PARAM + 1) + +#endif /* masterMacsController_H */ diff --git a/utils/decodeStatus.py b/utils/decodeStatus.py new file mode 100644 index 0000000..ee27133 --- /dev/null +++ b/utils/decodeStatus.py @@ -0,0 +1,223 @@ +#!/usr/bin/env python3 +""" +The R10 status read command returns an integer, which needs to be interpreted +bitwise for various status flags. This script prints out these status flags in +human-readable formatting. + +To read the manual, simply run this script without any arguments. + +Stefan Mathis, December 2024 +""" + + +# List of tuples which encodes the states given in the file description. +# Index first with the bit index, then with the bit value +interpretation = [ + ("Not ready to be switched on", "Ready to be switched on"), # Bit 0 + ("Not switched on", "Switched on"), # Bit 1 + ("Disabled", "Enabled"), # Bit 2 + ("Ok", "Fault condition set"), # Bit 3 + ("Motor supply voltage absent ", "Motor supply voltage present"), # Bit 4 + ("Motor performs quick stop", "Ok"), # Bit 5 + ("Switch on enabled", "Switch on disabled"), # Bit 6 + ("Ok", "RWarning: Movement function was called while motor is still moving. The function call is ignored"), # Bit 7 + ("Motor is idle", "Motor is currently moving"), # Bit 8 + ("Motor does not execute command messages (local mode)", "Motor does execute command messages (remote mode)"), # Bit 9 + ("Target not reached", "Target reached"), # Bit 10 + ("Ok", "Internal limit active"), # Bit 11 + ("Not specified", "Not specified"), # Bit 12 + ("Not specified", "Not specified"), # Bit 13 + ("No event set or event has not occurred yet", "Set event has occurred"), # Bit 14 + ("Axis off (power disabled)", "Axis on (power enabled)"), # Bit 15 +] + +def decode(value, big_endian: bool = False): + + interpreted = [] + + bit_list = [(value >> shift_ind) & 1 + for shift_ind in range(value.bit_length())] # little endian + + if big_endian: + bit_list.reverse() # big endian + + for (bit, interpretations) in zip(bit_list, interpretation): + interpreted.append(interpretations[bit]) + return (bit_list, interpreted) + +def print_decoded(bit_list, interpreted): + for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)): + print(f"Bit {idx} = {bit_value}: {msg}") + +def interactive(): + + # Imported here, because curses is not available in Windows. Using the + # interactive mode therefore fails on Windows, but at least the single + # command mode can be used (which would not be possible if we would import + # curses at the top level) + import curses + + stdscr = curses.initscr() + curses.noecho() + curses.cbreak() + stdscr.keypad(True) + stdscr.scrollok(True) + + stdscr.addstr(">> ") + stdscr.refresh() + + history = [""] + ptr = len(history) - 1 + + while True: + c = stdscr.getch() + if c == curses.KEY_RIGHT: + (y, x) = stdscr.getyx() + if x < len(history[ptr]) + 3: + stdscr.move(y, x+1) + stdscr.refresh() + elif c == curses.KEY_LEFT: + (y, x) = stdscr.getyx() + if x > 3: + stdscr.move(y, x-1) + stdscr.refresh() + elif c == curses.KEY_UP: + if ptr > 0: + ptr -= 1 + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + elif c == curses.KEY_DOWN: + if ptr < len(history) - 1: + ptr += 1 + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + elif c == curses.KEY_ENTER or c == ord('\n') or c == ord('\r'): + if history[ptr] == 'quit': + break + + # because of arrow keys move back to the end of the line + (y, x) = stdscr.getyx() + stdscr.move(y, 3+len(history[ptr])) + + if history[ptr]: + result = interpret_inputs(history[ptr].split()) + if result is None: + stdscr.addstr(f"\nBAD INPUT: Expected input of 'value [big_endian]', where 'value' is an int or a float and 'big_endian' is an optional boolean argument.") + else: + (arg, big_endian) = result + (bit_list, interpreted) = decode(arg, big_endian) + for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)): + stdscr.addstr(f"\nBit {idx} = {bit_value}: {msg}") + stdscr.refresh() + + if ptr == len(history) - 1 and history[ptr] != "": + history += [""] + else: + history[-1] = "" + ptr = len(history) - 1 + + stdscr.addstr("\n>> ") + stdscr.refresh() + + else: + if ptr < len(history) - 1: # Modifying previous input + if len(history[-1]) == 0: + history[-1] = history[ptr] + ptr = len(history) - 1 + + else: + history += [history[ptr]] + ptr = len(history) - 1 + + if c == curses.KEY_BACKSPACE: + if len(history[ptr]) == 0: + continue + (y, x) = stdscr.getyx() + history[ptr] = history[ptr][0:x-4] + history[ptr][x-3:] + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + stdscr.move(y, x-1) + stdscr.refresh() + + else: + (y, x) = stdscr.getyx() + history[ptr] = history[ptr][0:x-3] + chr(c) + history[ptr][x-3:] + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + stdscr.move(y, x+1) + stdscr.refresh() + + # to quit + curses.nocbreak() + stdscr.keypad(False) + curses.echo() + curses.endwin() + +def interpret_inputs(inputs): + + number = None + big_endian = False + try: + number = int(float(inputs[0])) + if len(inputs) > 1: + second_arg = inputs[1] + if second_arg == "True" or second_arg == "true": + big_endian = True + elif second_arg == "False" or second_arg == "false": + big_endian = False + else: + big_endian = bool(int(second_arg)) + return (number, big_endian) + except: + return None + +if __name__ == "__main__": + from sys import argv + + if len(argv) == 1: + # Start interactive mode + interactive() + else: + + result = interpret_inputs(argv[1:]) + + if result is None: + print(""" + Decode R10 message of MasterMACs + ------------------ + + MasterMACs returns its status message (R10) as a floating-point number. + The bits of this float encode different states. These states are stored + in the interpretation variable. + + This script can be used in two different ways: + + Option 1: Single Command + ------------------------ + + Usage: decodeMasterMACStatusR10.py value [big_endian] + + 'value' is the return value of a R10 command. This value is interpreted + bit-wise and the result is printed out. The optional second argument can + be used to specify whether the input value needs to be interpreted as + little or big endian. Default is False. + + Option 2: CLI Mode + ------------------ + + Usage: decodeMasterMACStatusR10.py + + A prompt will be opened. Type in the return value of a R10 command, hit + enter and the interpretation will be printed in the prompt. After that, + the next value can be typed in. Type 'quit' to close the prompt. + """) + else: + print("Motor status") + print("============") + (arg, big_endian) = result + (bit_list, interpreted) = decode(arg, big_endian) + print_decoded(bit_list, interpreted)