From 1b01be845b1bc8ced89a8a566816af24d906cc5c Mon Sep 17 00:00:00 2001 From: smathis Date: Tue, 25 Feb 2025 17:17:41 +0100 Subject: [PATCH] Initial version for the detector tower driver --- .gitlab-ci.yml | 58 ++ Makefile | 33 ++ README.md | 47 ++ db/amorDetector.db | 70 +++ src/detectorTower.dbd | 2 + src/detectorTowerAxis.cpp | 932 ++++++++++++++++++++++++++++++++ src/detectorTowerAxis.h | 122 +++++ src/detectorTowerController.cpp | 295 ++++++++++ src/detectorTowerController.h | 96 ++++ 9 files changed, 1655 insertions(+) create mode 100644 .gitlab-ci.yml create mode 100644 Makefile create mode 100644 README.md create mode 100644 db/amorDetector.db create mode 100644 src/detectorTower.dbd create mode 100644 src/detectorTowerAxis.cpp create mode 100644 src/detectorTowerAxis.h create mode 100644 src/detectorTowerController.cpp create mode 100644 src/detectorTowerController.h diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..c522389 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,58 @@ +default: + image: docker.psi.ch:5000/sinqdev/sinqepics:latest + +stages: + - lint + - build + - test + +cppcheck: + stage: lint + script: + - cppcheck --std=c++17 --addon=cert --addon=misc --error-exitcode=1 src/*.cpp + artifacts: + expire_in: 1 week + tags: + - sinq + +formatting: + stage: lint + script: + - clang-format --style=file --Werror --dry-run src/*.cpp + artifacts: + expire_in: 1 week + tags: + - sinq + +# clangtidy: +# stage: lint +# script: +# - curl https://docker.psi.ch:5000/v2/_catalog +# # - dnf update -y +# # - dnf install -y clang-tools-extra +# # - clang-tidy sinqEPICSApp/src/*.cpp sinqEPICSApp/src/*.c sinqEPICSApp/src/*.h -checks=cppcoreguidelines-*,cert-* +# # tags: +# # - sinq + +build_module: + stage: build + script: + - export TURBOPMAC_VERSION="$(grep 'turboPmac_VERSION=' Makefile | cut -d= -f2)" + - git clone --depth 1 --branch "${TURBOPMAC_VERSION}" https://gitlab-ci-token:${CI_JOB_TOKEN}@git.psi.ch/sinq-epics-modules/sinqmotor.git + - pushd sinqmotor + - sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile + - echo "LIBVERSION=${TURBOPMAC_VERSION}" >> Makefile + - make install + - popd + - sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile + - echo "LIBVERSION=${CI_COMMIT_TAG:-0.0.1}" >> Makefile + - make install + - cp -rT "/ioc/modules/detectorTower/$(ls -U /ioc/modules/detectorTower/ | head -1)" "./detectorTower-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + artifacts: + name: "detectorTower-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + paths: + - "detectorTower-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}/*" + expire_in: 1 week + when: always + tags: + - sinq diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..eb32acf --- /dev/null +++ b/Makefile @@ -0,0 +1,33 @@ +# Use the PSI build system +include /ioc/tools/driver.makefile + +MODULE=detectorTower +BUILDCLASSES=Linux +EPICS_VERSIONS=7.0.7 +ARCH_FILTER=RHEL% + +# Additional module dependencies +REQUIRED+=motorBase +REQUIRED+=turboPmac + +# Specify the version of motorBase we want to build against +motorBase_VERSION=7.2.2 + +# Specify the version of turboPmac we want to build against +turboPmac_VERSION=mathis_s + +# These headers allow to depend on this library for derived drivers. +HEADERS += src/detectorTowerAxis.h +HEADERS += src/detectorTowerController.h + +# Source files to build +SOURCES += src/detectorTowerAxis.cpp +SOURCES += src/detectorTowerController.cpp + +# Store the record files +TEMPLATES += db/detectorTower.db + +# This file registers the motor-specific functions in the IOC shell. +DBDS += src/detectorTower.dbd + +USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result -Wextra -Werror # -Wpedantic // Does not work because EPICS macros trigger warnings diff --git a/README.md b/README.md new file mode 100644 index 0000000..a27122b --- /dev/null +++ b/README.md @@ -0,0 +1,47 @@ +# detectorTower + +## Overview + +This is a driver for the Turbo PMAC 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 pmac motor controllers. To read their manual, run the scripts without any arguments. +- writeRead.py: Allows sending commands to and receiving commands from a pmac controller over an ethernet connection. +- analyzeTcpDump.py: Parse the TCP communication between an IOC and a MCU and format it into a dictionary. "demo.py" shows how this data can be easily visualized for analysis. + + +## Developer guide + +### Usage in IOC shell + +turboPmac exposes the following IOC shell functions (all in turboPmacController.cpp): +- `turboPmacController`: Create a new controller object. +- `turboPmacAxis`: Create a new axis object. +These functions are parametrized as follows: +``` +turboPmacController( + "$(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 + ); +``` +``` +turboPmacAxis( + "$(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/db/amorDetector.db b/db/amorDetector.db new file mode 100644 index 0000000..88e7758 --- /dev/null +++ b/db/amorDetector.db @@ -0,0 +1,70 @@ +# Reset any errors of the virtual axis. +# This record is coupled to the parameter library via reset_ -> RESET. +record(longout, "$(INSTR)$(M):Reset") { + field(DTYP, "asynInt32") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) RESET") + field(PINI, "NO") +} + +# Read the position state of the axis: +# 0 = not ready +# 1 = ready (in working position) +# 2 = ready (in changer position) +record(longout, "$(INSTR)$(M):PositionState") +{ + field(DTYP, "asynInt32") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) POSITION_STATE") + field(SCAN, "I/O Intr") +} + +# Set the target position for the offset of the lift (motor COZ) +# This record is coupled to the parameter library via liftOffset_ -> LIFT_OFFSET. +record(ao, "$(INSTR)$(M):LiftOffset") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) LIFT_OFFSET") + field(PINI, "NO") +} + +# Set the target position for the offset of the lift (motor COZ) +# This record is coupled to the parameter library via liftOffset_ -> LIFT_OFFSET. +record(ao, "$(INSTR)$(M):LiftOffset") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) LIFT_OFFSET") + field(PINI, "NO") +} + +# Read the lower and upper limits for the lift offset +record(ai, "$(INSTR)$(M):LiftOffsetLowLimitRBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) LIFT_OFFSET_LOW_LIMIT") + field(SCAN, "I/O Intr") +} +record(ai, "$(INSTR)$(M):LiftOffsetHighLimitRBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) LIFT_OFFSET_HIGH_LIMIT") + field(SCAN, "I/O Intr") +} + +# Set the target position for the offset of the detector tilt (motor COX) +# This record is coupled to the parameter library via tiltOffset_ -> TILT_OFFSET. +record(ao, "$(INSTR)$(M):TiltOffset") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) TILT_OFFSET") + field(PINI, "NO") +} + +# Read the lower and upper limits for the detector tilt offset +record(ai, "$(INSTR)$(M):TiltOffsetLowLimitRBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) TILT_OFFSET_LOW_LIMIT") + field(SCAN, "I/O Intr") +} +record(ai, "$(INSTR)$(M):TiltOffsetHighLimitRBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) TILT_OFFSET_HIGH_LIMIT") + field(SCAN, "I/O Intr") +} \ No newline at end of file diff --git a/src/detectorTower.dbd b/src/detectorTower.dbd new file mode 100644 index 0000000..a778c29 --- /dev/null +++ b/src/detectorTower.dbd @@ -0,0 +1,2 @@ +registrar(detectorTowerControllerRegister) +registrar(detectorTowerAxisRegister) \ No newline at end of file diff --git a/src/detectorTowerAxis.cpp b/src/detectorTowerAxis.cpp new file mode 100644 index 0000000..6125df5 --- /dev/null +++ b/src/detectorTowerAxis.cpp @@ -0,0 +1,932 @@ +#include "detectorTowerAxis.h" +#include "turboPmacController.h" +#include +#include +#include + +detectorTowerAxis::detectorTowerAxis(detectorTowerController *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, + "Controller \"%s\", axis %d => %s, line %d: FATAL ERROR: " + "Axis index %d must be smaller than the total number of axes " + "%d", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + axisNo_, pC->numAxes_); + exit(-1); + } + + // Initialize all member variables + // Assumed to be not ready by default, this is overwritten in the next poll + error_ = 0; + scheduleMoveFromParamLib_ = false; + + // Speed cannot be changed + status = pC_->setIntegerParam(axisNo_, pC_->motorCanSetSpeed_, 0); + if (status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nFATAL ERROR " + "(setting a parameter value failed with %s)\n. Terminating IOC", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + pC_->stringifyAsynStatus(status)); + exit(-1); + } + + status = pC_->setIntegerParam(axisNo_, pC_->positionState_, 1); + if (status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nFATAL ERROR " + "(setting a parameter value failed with %s)\n. Terminating IOC", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + pC_->stringifyAsynStatus(status)); + exit(-1); + } +} + +// Perform the actual poll +asynStatus detectorTowerAxis::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 response[pC_->MAXBUF_], userMessage[pC_->MAXBUF_]; + int nvals = 0; + + int direction = 0; + int error = 0; + int positionState = 0; + int inPosition = 0; + double beamTiltAngle = 0.0; + double previousBeamTiltAngle = 0.0; + double motorRecResolution = 0.0; + double limitsOffset = 0.0; + double highLimit = 0.0; + double lowLimit = 0.0; + double liftOffsetHighLimit = 0.0; + double liftOffsetLowLimit = 0.0; + double tiltOffsetHighLimit = 0.0; + double tiltOffsetLowLimit = 0.0; + + // ========================================================================= + + // Motor resolution from parameter library + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Read the previous motor position + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_, + &previousBeamTiltAngle); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from EPICS to motor coordinates (see comment in + // turboPmacAxis::atFirstPoll) + previousBeamTiltAngle = previousBeamTiltAngle * motorRecResolution; + + /* + Query the following information: + - In-position flag + - Position state + - Axis error + - Beam tilt angle + - Beam tilt angle limits + - Beam lift offset limits + - Detector tilt offset limits + */ + char command[] = "P354 P358 P359 Q510 Q513 Q514 Q353 Q354 Q553 Q554"; + rw_status = pC_->writeRead(axisNo_, command, response, 10); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = + sscanf(response, "%d %d %lf %d %lf %lf %lf %lf %lf %lf", &inPosition, + &positionState, &beamTiltAngle, &error, &highLimit, &lowLimit, + &liftOffsetHighLimit, &liftOffsetLowLimit, &tiltOffsetHighLimit, + &tiltOffsetLowLimit); + if (nvals != 10) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + 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 should 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 limitsOffset on both sides. + */ + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_, &limitsOffset); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + highLimit = highLimit - limitsOffset; + lowLimit = lowLimit + limitsOffset; + + // Store the axis status + error_ = error; + + // Update the enablement PV + pl_status = setIntegerParam(pC_->motorEnableRBV_, (positionState != 2)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Check if the motor is moving + if (inPosition == 0) { + *moving = false; + } else { + *moving = true; + } + + // Intepret the position state + switch (positionState) { + case 0: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nAxis not ready\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + break; + case 1: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nAxis ready\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + break; + case 2: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nAxis in change " + "position\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + break; + default: + *moving = false; + snprintf(userMessage, sizeof(userMessage), + "No air cushion feedback before movement start (P%2.2d01 = " + "%d). Please call the support.", + axisNo_, error); + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nReached unknown " + "state P354 = %d\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + positionState); + snprintf(userMessage, sizeof(userMessage), + "Unknown state P354 = %d has been reached. Please call " + "the support.", + positionState); + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + if (*moving) { + // If the axis is moving, evaluate the movement direction + if ((beamTiltAngle - previousBeamTiltAngle) > 0) { + direction = 1; + } else { + direction = 0; + } + } + + // Error handling + switch (error) { + case 0: + // No error + break; + case 1: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nBrake COZ not " + "released (P359=1).\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, error); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Brake COZ not released. Try resetting the axis. " + "If the problem persists, please call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 2: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMove command " + "rejected because axis is already moving.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setStringParam(pC_->motorMessageText_, + "Move command rejected because axis is " + "already moving. Please call the support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 3: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor FTZ.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Error in motor FTZ. Try resetting the axis. If " + "the problem persists, please call the support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 4: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis stopped " + "unexpectedly.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Axis stopped unexpectedly. Try resetting the axis " + "and issue the move command again. If the problem " + "persists, please call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 5: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nRelease removed " + "while moving.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setStringParam( + pC_->motorMessageText_, + "Axis release was removed while moving. Try resetting the axis " + "and issue the move command again. If the problem persists, please " + "call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 6: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nEmergency stop " + "activated.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, "Emergency stop activate"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 7: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor COZ.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Error in motor COZ. Try resetting the axis. If " + "the problem persists, please call the support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 8: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor COM.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Error in motor COM. Try resetting the axis. If " + "the problem persists, please call the support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 9: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor COX.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Error in motor COX. Try resetting the axis. If " + "the problem persists, please call the support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 10: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nHit end switch FTZ.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = + setStringParam(pC_->motorMessageText_, "Hit end switch FTZ"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + case 11: + // Following error + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMaximum allowed " + "following error FTZ exceeded.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + snprintf(userMessage, sizeof(userMessage), + "Maximum allowed following error exceeded (P359 = %d). " + "Check if movement range is blocked. " + "Otherwise please call the support.", + axisNo_, error); + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + + default: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nUnknown error " + "P359 = %d.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, error); + + snprintf(userMessage, sizeof(userMessage), + "Unknown error P359 = %d. Please call the support.", error); + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + poll_status = asynError; + break; + } + + // Update the parameter library + if (error != 0) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + if (*moving == false) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = pC_->setDoubleParam(axisNo_, pC_->liftOffsetHighLimit_, + liftOffsetHighLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "liftOffsetHighLimit_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = pC_->setDoubleParam(axisNo_, pC_->liftOffsetLowLimit_, + liftOffsetLowLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "liftOffsetLowLimit_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = pC_->setDoubleParam(axisNo_, pC_->tiltOffsetHighLimit_, + tiltOffsetHighLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "tiltOffsetHighLimit_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = pC_->setDoubleParam(axisNo_, pC_->tiltOffsetLowLimit_, + tiltOffsetLowLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "tiltOffsetLowLimit_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Transform from motor to EPICS coordinates (see comment in + // turboPmacAxis::init()) + beamTiltAngle = beamTiltAngle / motorRecResolution; + + pl_status = setDoubleParam(pC_->motorPosition_, beamTiltAngle); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->positionState_, positionState); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "positionState_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If a movement is scheduled and the axis is not moving anymore, start the + // scheduled movement. This replaces the original motor record movement + // control loop functionality. + if (scheduleMoveFromParamLib_ && !moving && poll_status == asynSuccess) { + scheduleMoveFromParamLib_ = false; + return moveFromParamLib(); + } else { + return poll_status; + } +} + +asynStatus detectorTowerAxis::moveFromParamLib() { + + // 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 motorTargetPosition = 0.0; + double motorCoordinatesPosition = 0.0; + double motorRecResolution = 0.0; + double liftOffset = 0.0; + double tiltOffset = 0.0; + double motorVelocity = 0.0; + int motorCanSetSpeed = 0; + int writeOffset = 0; + int positionState = 0; + + // ========================================================================= + + pl_status = + pC_->getIntegerParam(axisNo_, pC_->positionState_, &positionState); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "positionState_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If the axis is in changer position, it must be moved into working + // position before any move can be started. + if (positionState == 2) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " + "moved because it is in changer " + "position. Move it to working position first.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + + // Read the target values from the parameter library + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorTargetPosition_, + &motorTargetPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = pC_->getDoubleParam(axisNo_, pC_->liftOffset_, &liftOffset); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "liftOffset_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + pl_status = pC_->getDoubleParam(axisNo_, pC_->tiltOffset_, &tiltOffset); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "tiltOffset_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Set the target positions for beam tilt, detector tilt offset and lift + // offset + snprintf(&command[writeOffset], sizeof(command) - writeOffset, + "Q451=%lf Q454=%lf Qxxx=%lf P350=1", motorTargetPosition, + liftOffset, tiltOffset); + + // We don't expect an answer + rw_status = pC_->writeRead(axisNo_, command, response, 0); + if (rw_status != asynSuccess) { + + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStarting movement to " + "target position %lf failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + motorCoordinatesPosition); + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + return rw_status; +} + +asynStatus detectorTowerAxis::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 response[pC_->MAXBUF_]; + + // ========================================================================= + + rw_status = pC_->writeRead(axisNo_, "P350=8", response, 0); + + if (rw_status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStopping the movement " + "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; +} + +// The AMOR detector axis uses absolute encoders +asynStatus detectorTowerAxis::readEncoderType() { + asynStatus status = setStringParam(pC_->encoderType_, AbsoluteEncoder); + + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "encoderType_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + return asynSuccess; +} + +asynStatus detectorTowerAxis::moveToWorkingPosition(bool toWorkingPosition) { + + int timeout_enable_disable = 2; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + bool moving = false; + int positionState = 0; + + // ========================================================================= + + doPoll(&moving); + + pl_status = + pC_->getIntegerParam(axisNo_, pC_->positionState_, &positionState); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "positionState_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // 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, + "Controller \"%s\", axis %d => %s, line %d\nAxis is not " + "idle and can therefore not be moved to changer position.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setStringParam( + pC_->motorMessageText_, + "Axis cannot be moved to changer position while it is moving."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + return asynError; + } + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((toWorkingPosition == true && positionState == 1) || + (toWorkingPosition == false && positionState == 2)) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "Controller \"%s\", axis %d => %s, line %d\nAxis is already " + "in %s position.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + toWorkingPosition ? "working" : "changer"); + return asynSuccess; + } + + // Move the axis into changer or working position + if (toWorkingPosition) { + rw_status = pC_->writeRead(axisNo_, "P350=3", response, 0); + pl_status = setStringParam(pC_->motorMessageText_, + "Moving to working position ..."); + } else { + rw_status = pC_->writeRead(axisNo_, "P350=2", response, 0); + pl_status = setStringParam(pC_->motorMessageText_, + "Moving to changer position ..."); + } + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + return pC_->writeRead(axisNo_, command, response, 0); +} + +asynStatus detectorTowerAxis::reset() { + + char response[pC_->MAXBUF_]; + asynStatus pl_status = asynSuccess; + int positionState = 0; + + pl_status = + pC_->getIntegerParam(axisNo_, pC_->positionState_, &positionState); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "positionState_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + Check which action should be performed: + - If positionState_ == 0 (not ready): P352 = 1 (Set axis in closed-loop + mode) + - If error_ == 10 or 11 (FTZ motor error): P352 = 3 (Recover FTZ) + - If any other error: P352 = 2 (Reset error) + */ + if (positionState == 0) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nSetting axis in " + "closed-loop mode.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + return pC_->writeRead(axisNo_, "P352=1", response, 0); + } else if (error_ == 10 || error_ == 11) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nResetting FTZ " + "motor error.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + return pC_->writeRead(axisNo_, "P352=3", response, 0); + } else if (error_ != 0) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nResetting error.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + return pC_->writeRead(axisNo_, "P352=2", response, 0); + } else { + return asynSuccess; + } +} + +/*************************************************************************************/ +/** The following functions are C-wrappers, and can be called directly from + * iocsh */ + +extern "C" { + +/* +C wrapper for the axis constructor. Please refer to the detectorTower +constructor documentation. The controller is read from the portName. +*/ +asynStatus detectorTowerCreateAxis(const char *portName, int axis) { + + /* + 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("Controller \"%s\" => %s, line %d\nPort not found.", + portName, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + // Unsafe cast of the pointer to an asynPortDriver + asynPortDriver *apd = (asynPortDriver *)(ptr); + + // Safe downcast + detectorTowerController *pC = dynamic_cast(apd); + if (pC == nullptr) { + errlogPrintf("Controller \"%s\" => %s, line %d\nController " + "is not a turboPmacController.", + portName, __PRETTY_FUNCTION__, __LINE__); + 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" + detectorTowerAxis *pAxis = new detectorTowerAxis(pC, axis); + + // Allow manipulation of the controller again + pC->unlock(); + return asynSuccess; +} + +/* +Same procedure as for the CreateController function, but for the axis +itself. +*/ +static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)", + iocshArgString}; +static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0, + &CreateAxisArg1}; +static const iocshFuncDef configDetectorTowerCreateAxis = {"detectorTowerAxis", + 2, CreateAxisArgs}; +static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) { + detectorTowerCreateAxis(args[0].sval, args[1].ival); +} + +// This function is made known to EPICS in detectorTower.dbd and is called by +// EPICS in order to register both functions in the IOC shell +static void detectorTowerAxisRegister(void) { + iocshRegister(&configDetectorTowerCreateAxis, + configDetectorTowerCreateAxisCallFunc); +} +epicsExportRegistrar(detectorTowerAxisRegister); + +} // extern "C" \ No newline at end of file diff --git a/src/detectorTowerAxis.h b/src/detectorTowerAxis.h new file mode 100644 index 0000000..34bcdfc --- /dev/null +++ b/src/detectorTowerAxis.h @@ -0,0 +1,122 @@ +#ifndef detectorTowerAXIS_H +#define detectorTowerAXIS_H +#include "detectorTowerController.h" +#include "sinqAxis.h" + +// Forward declaration of the controller class to resolve the cyclic dependency +// between the controller and the axis .h-file. See +// https://en.cppreference.com/w/cpp/language/class. +class detectorTowerController; + +class detectorTowerAxis : public sinqAxis { + public: + /** + * @brief Construct a new turboPmacAxis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + */ + detectorTowerAxis(detectorTowerController *pController, int axisNo); + + /** + * @brief Destroy the turboPmacAxis + * + */ + virtual ~detectorTowerAxis(); + + /** + * @brief Readout of some values from the controller at IOC startup + * + * @return asynStatus + */ + asynStatus init(); + + /** + * @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 `doPoll` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doPoll`. + * + * @param moving + * @return asynStatus + */ + asynStatus doPoll(bool *moving); + + /** + * @brief Empty dummy for the move method which is called by the EPICS + * movement control loop + * + * For the detector tower, we want to start movements manually without + * using the EPICS control loop. Therefore, we call the `move` function of + * sinqMotor directly whenever a new value is written to the PV + * $(INSTR)$(M):TargetPosition". The reason is that we want to start a + * movement even if the VAL field of the motor record equals its RBV field + * because either the lift offset or the detector tilt offset might have + * changed. + * + * @param position + * @param relative + * @param min_velocity + * @param max_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration) { + return asynSuccess; + } + + /** + * @brief Start a movement based on the parameter library values for + * motorTargetPosition_, liftOffset_ and tiltOffset_ + * + * @return asynStatus + */ + asynStatus moveFromParamLib(); + + /** + * @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(); + + /** + * @brief If the input value is true, move the axis into working position, + * otherwise into changer position. + * + * @return asynStatus + */ + asynStatus moveToWorkingPosition(bool toWorkingPosition); + + /** + * @brief Reset the axis error + * + * @param on + * @return asynStatus + */ + asynStatus reset(); + + protected: + detectorTowerController *pC_; + + bool scheduleMoveFromParamLib_; + bool waitForHandshake_; + time_t timeAtHandshake_; + bool inChangerPosition_; + + int error_; + + private: + friend class detectorTowerController; +}; + +#endif diff --git a/src/detectorTowerController.cpp b/src/detectorTowerController.cpp new file mode 100644 index 0000000..cebedc1 --- /dev/null +++ b/src/detectorTowerController.cpp @@ -0,0 +1,295 @@ +#include "detectorTowerController.h" +#include "turboPmacController.h" +#include +#include + +/** + * @brief Construct a new turboPmacController::turboPmacController 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 + */ +detectorTowerController::detectorTowerController( + const char *portName, const char *ipPortConfigName, int numAxes, + double movingPollPeriod, double idlePollPeriod, double comTimeout) + : turboPmacController(portName, ipPortConfigName, numAxes, movingPollPeriod, + idlePollPeriod, NUM_detectorTower_DRIVER_PARAMS) + +{ + + asynStatus status = asynSuccess; + + // ========================================================================= + // Create additional parameter library entries + + status = createParam("LIFT_OFFSET", asynParamFloat64, &liftOffset_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("LIFT_OFFSET_LOW_LIMIT", asynParamFloat64, + &liftOffsetLowLimit_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("LIFT_OFFSET_HIGH_LIMIT", asynParamFloat64, + &liftOffsetHighLimit_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("TILT_OFFSET", asynParamFloat64, &tiltOffset_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("TILT_OFFSET_LOW_LIMIT", asynParamFloat64, + &tiltOffsetLowLimit_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("TILT_OFFSET_HIGH_LIMIT", asynParamFloat64, + &tiltOffsetHighLimit_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("POSITION_STATE", asynParamInt32, &positionState_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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("RESET", asynParamInt32, &reset_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, 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); + } +} + +asynStatus detectorTowerController::readInt32(asynUser *pasynUser, + epicsInt32 *value) { + // The detector tower cannot be disabled + if (pasynUser->reason == motorCanDisable_) { + *value = 0; + return asynSuccess; + } else { + return turboPmacController::readInt32(pasynUser, value); + } +} + +asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, + epicsInt32 value) { + int function = pasynUser->reason; + + // ===================================================================== + + detectorTowerAxis *axis = getVirtualAxis(pasynUser); + if (axis == nullptr) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an " + "instance of sinqAxis", + portName, axis->axisNo_, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + + // Handle custom PVs + if (function == motorEnable_) { + return axis->enable(value); + } else if (function == reset_) { + return axis->reset(); + } else { + return turboPmacController::writeInt32(pasynUser, value); + } +} + +asynStatus detectorTowerController::writeFloat64(asynUser *pasynUser, + epicsFloat64 value) { + int function = pasynUser->reason; + + // ===================================================================== + + detectorTowerAxis *axis = getVirtualAxis(pasynUser); + if (axis == nullptr) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nAxis is not an " + "instance of sinqAxis", + portName, axis->axisNo_, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + + if (function == motorTargetPosition_) { + // Start the movement, if the axis is not already moving. Otherwise stop + // it and schedule a movement. + bool moving = false; + axis->poll(&moving); + if (moving) { + axis->scheduleMoveFromParamLib_ = true; + return axis->stop(0.0); + } else { + return axis->moveFromParamLib(); + } + } else { + return turboPmacController::writeFloat64(pasynUser, value); + } +} + +/* +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. +*/ +detectorTowerAxis * +detectorTowerController::getVirtualAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return detectorTowerController::castToVirtualAxis(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 +*/ +detectorTowerAxis *detectorTowerController::getVirtualAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return detectorTowerController::castToVirtualAxis(asynAxis); +} + +detectorTowerAxis * +detectorTowerController::castToVirtualAxis(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 + detectorTowerAxis *axis = dynamic_cast(asynAxis); + if (axis == nullptr) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis is not " + "an instance of turboPmacAxis", + portName, axis->axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + return axis; +} + +/*************************************************************************************/ +/** 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 +detectorTowerController constructor documentation. +*/ +asynStatus detectorTowerCreateController(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" + detectorTowerController *pController = new detectorTowerController( + 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 configDetectorTowerCreateController = { + "detectorTowerController", 6, CreateControllerArgs}; +static void +configDetectorTowerCreateControllerCallFunc(const iocshArgBuf *args) { + detectorTowerCreateController(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 detectorTowerControllerRegister(void) { + iocshRegister(&configDetectorTowerCreateController, + configDetectorTowerCreateControllerCallFunc); +} +epicsExportRegistrar(detectorTowerControllerRegister); + +} // extern "C" \ No newline at end of file diff --git a/src/detectorTowerController.h b/src/detectorTowerController.h new file mode 100644 index 0000000..b62dee4 --- /dev/null +++ b/src/detectorTowerController.h @@ -0,0 +1,96 @@ +/******************************************** + * detectorTowerController.h + * + * Detector tower controller driver based on the asynMotorController class + * + * Stefan Mathis, September 2024 + ********************************************/ + +#ifndef detectorTowerController_H +#define detectorTowerController_H +#include "detectorTowerAxis.h" +#include "turboPmacController.h" + +class detectorTowerController : public turboPmacController { + + public: + /** + * @brief Construct a new detectorTowerController object + * + * This controller object can handle both "normal" TurboPMAC axes created + with the turboPmacAxis constructor and the special detectorTowerAxis. + * + * @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. + */ + detectorTowerController(const char *portName, const char *ipPortConfigName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, double comTimeout); + + asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + /** + * @brief Overloaded function of asynMotorController + * + * The function is overloaded to allow manual starting of an axis movement + * + * @param pasynUser Specify the axis via the asynUser + * @param value New value + * @return asynStatus + */ + virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + + /** + * @brief Get the axis object + * + * @param pasynUser Specify the axis via the asynUser + * @return detectorTowerAxis* If no axis could be found, this is a + * nullptr + */ + detectorTowerAxis *getVirtualAxis(asynUser *pasynUser); + + /** + * @brief Get the axis object + * + * @param axisNo Specify the axis via its index + * @return detectorTowerAxis* If no axis could be found, this is a + * nullptr + */ + detectorTowerAxis *getVirtualAxis(int axisNo); + + /** + * @brief Save cast of the given asynAxis pointer to a detectorTowerAxis + * pointer. If the cast fails, this function returns a nullptr. + * + * @param asynAxis + * @return detectorTowerAxis* + */ + detectorTowerAxis *castToVirtualAxis(asynMotorAxis *asynAxis); + + private: + // Indices of additional PVs +#define FIRST_detectorTower_PARAM liftOffset_ + int liftOffset_; + int liftOffsetLowLimit_; + int liftOffsetHighLimit_; + int tiltOffset_; + int tiltOffsetLowLimit_; + int tiltOffsetHighLimit_; + int positionState_; + int reset_; +#define LAST_detectorTower_PARAM reset_ + + friend class detectorTowerAxis; +}; +#define NUM_detectorTower_DRIVER_PARAMS \ + (&LAST_detectorTower_PARAM - &FIRST_detectorTower_PARAM + 1) + +#endif /* detectorTowerController_H */