diff --git a/sinqEPICSApp/src/C804Axis.cpp b/sinqEPICSApp/src/C804Axis.cpp index c1e1500..acbd396 100644 --- a/sinqEPICSApp/src/C804Axis.cpp +++ b/sinqEPICSApp/src/C804Axis.cpp @@ -1,14 +1,14 @@ #include "C804Axis.h" #include "C804Controller.h" -#include -#include -#include #include -#include +#include #include +#include +#include +#include -C804Axis::C804Axis(C804Controller *pC, int axisNo) : SINQAxis(pC, axisNo), pC_(pC) -{ +C804Axis::C804Axis(C804Controller *pC, int axisNo) + : SINQAxis(pC, axisNo), pC_(pC) { /* The superclass constructor SINQAxis calls in turn its superclass constructor asynMotorAxis. In the latter, a pointer to the constructed object this is @@ -16,13 +16,14 @@ C804Axis::C804Axis(C804Controller *pC, int axisNo) : SINQAxis(pC, axisNo), pC_(p pC->pAxes_[axisNo] = this; - Therefore, the axes are managed by the controller pC. See C804Controller.cpp for further explanation. - 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. + Therefore, the axes are managed by the controller pC. See C804Controller.cpp + for further explanation. 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_) - { + if (axisNo >= pC->numAxes_) { exit(-1); } last_position_steps_ = 0; @@ -30,8 +31,7 @@ C804Axis::C804Axis(C804Controller *pC, int axisNo) : SINQAxis(pC, axisNo), pC_(p last_poll_ = 0.0; } -C804Axis::~C804Axis(void) -{ +C804Axis::~C804Axis(void) { // Since the controller memory is managed somewhere else, we don't need to // clean up the pointer pC here. } @@ -39,47 +39,44 @@ C804Axis::~C804Axis(void) /* The polling function informs us about the state of the axis, in particular if it is currently moving. It is called periodically, with the period defined by -the controller constructor arguments idlePollPeriod and movingPollPeriod depending on -the current axis state. +the controller constructor arguments idlePollPeriod and movingPollPeriod +depending on the current axis state. */ -asynStatus C804Axis::poll(bool *moving) -{ +asynStatus C804Axis::poll(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; - // The poll function is just a wrapper around poll_no_param_lib_update and handles mainly - // the callParamCallbacks() function + // The poll function is just a wrapper around poll_no_param_lib_update and + // handles mainly the callParamCallbacks() function asynStatus status_poll = C804Axis::poll_no_param_lib_update(moving); // According to the function documentation of asynMotorAxis::poll, this // function should be called at the end of a poll implementation. asynStatus status_callback = callParamCallbacks(); - if (status_callback != asynSuccess) - {d + if (status_callback != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Updating the parameter library failed for axis %d\n", functionName, axisNo_); + "%s: Updating the parameter library failed for axis %d\n", + functionName, axisNo_); return status_callback; - } - else - { + } else { return status_poll; } } // Perform the actual poll -asynStatus C804Axis::poll_no_param_lib_update(bool *moving) -{ +asynStatus C804Axis::poll_no_param_lib_update(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; asynStatus status; int axis_status = 0; // The controller returns the position and velocity in encoder steps. - // This value needs to be converted in user units (engineering units EGU) via - // the record field MRES of the motor record. This field has already been read - // by the constructor into the member variable motorRecResolution_. - // To go from steps to user units, multiply with motorRecResolution_ - // Example: If 10 steps correspond to 1 mm, MRES should be 0.1. + // This value needs to be converted in user units (engineering units EGU) + // via the record field MRES of the motor record. This field has already + // been read by the constructor into the member variable + // motorRecResolution_. To go from steps to user units, multiply with + // motorRecResolution_ Example: If 10 steps correspond to 1 mm, MRES should + // be 0.1. int position_error_steps = 0; int motor_position_steps = 0; int motor_velocity_steps = 0; @@ -89,28 +86,28 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) double motor_velocity = .0; double programmed_motor_velocity = .0; - // The buffer sizes for command and response are defined in the controller (see the corresponding source code files) + // The buffer sizes for command and response are defined in the controller + // (see the corresponding source code files) char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_]; /* Cancel the poll if the last poll has "just" happened. */ - if (time(NULL) < last_poll_ + 0.5 * pC_->movingPollPeriod_) - { + if (time(NULL) < last_poll_ + 0.5 * pC_->movingPollPeriod_) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Aborted poll since the last poll for axis %d happened a short time ago\n", functionName, axisNo_); + "%s: Aborted poll since the last poll for axis %d happened a " + "short time ago\n", + functionName, axisNo_); return asynSuccess; - } - else - { + } else { last_poll_ = time(NULL); } /* The parameter motorRecResolution_ is coupled to the field MRES of the motor record in the following manner: - - In sinq_asyn_motor.db, the PV (motor_record_pv_name)MOTOR_REC_RESOLUTION is - defined as a copy of the field (motor_record_pv_name).MRES: + - In sinq_asyn_motor.db, the PV (motor_record_pv_name)MOTOR_REC_RESOLUTION + is defined as a copy of the field (motor_record_pv_name).MRES: record(ao,"$(P)$(M):Resolution") { field(DESC, "$(M) resolution") @@ -121,27 +118,31 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) field(PREC, "$(PREC)") } - - The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h to the constant motorRecResolutionString - - ... which in turn is assigned to motorRecResolution_ in asynMotorController.cpp - This way of making the field visible to the driver is described here: https://epics.anl.gov/tech-talk/2020/msg00378.php - This is a one-way coupling, changes to the parameter library via setDoubleParam - are NOT transferred to (motor_record_pv_name).MRES or to (motor_record_pv_name):Resolution. + - The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h to + the constant motorRecResolutionString + - ... which in turn is assigned to motorRecResolution_ in + asynMotorController.cpp This way of making the field visible to the driver + is described here: https://epics.anl.gov/tech-talk/2020/msg00378.php This is + a one-way coupling, changes to the parameter library via setDoubleParam are + NOT transferred to (motor_record_pv_name).MRES or to + (motor_record_pv_name):Resolution. NOTE: This function must not be called in the constructor (e.g. in order to - save the read result to the member variable earlier), since the parameter library - is updated at a later stage! + save the read result to the member variable earlier), since the parameter + library is updated at a later stage! */ - pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, &motorRecResolution_); + pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution_); - asynPrint(pC_->pasynUserSelf,ASYN_TRACE_FLOW,"Poll axis %d\n", axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "Polling axis %d\n", + axisNo_); /* - We know that the motor resolution must not be zero. During the startup of the - IOC, polls can happen before the record is fully initialized. In that case, - all values are zero. + We know that the motor resolution must not be zero. During the startup of + the IOC, polls can happen before the record is fully initialized. In that + case, all values are zero. */ - if (motorRecResolution_ == 0) - { + if (motorRecResolution_ == 0) { return asynError; } @@ -154,11 +155,11 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) */ setIntegerParam(pC_->motorStatusProblem_, false); - // Read out the position error of the axis (delta of target position to actual position) + // Read out the position error of the axis (delta of target position to + // actual position) snprintf(command, pC_->C804_MAXBUF_ - 1, "%dTE", axisNo_); status = pC_->lowLevelWriteRead(axisNo_, command, response, true); - if (status == asynSuccess) - { + if (status == asynSuccess) { int parsed_axis; sscanf(response, "%2dE%10d", &parsed_axis, &position_error_steps); @@ -166,12 +167,12 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) position_error = double(position_error_steps) * motorRecResolution_; asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Axis %d, response %s, position error %f\n", functionName, axisNo_, response, position_error); - } - else - { + "%s: Axis %d, response %s, position error %f\n", functionName, + axisNo_, response, position_error); + } else { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Reading the position error failed for axis %d\n", functionName, axisNo_); + "%s: Reading the position error failed for axis %d\n", + functionName, axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); // Stop the evaluation prematurely @@ -180,9 +181,9 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) // Read the current position. snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTP", this->axisNo_); - status = this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); - if (status == asynSuccess) - { + status = + this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); + if (status == asynSuccess) { int parsed_axis; sscanf(response, "%2dP%10d", &parsed_axis, &motor_position_steps); @@ -190,14 +191,14 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) motor_position = double(motor_position_steps) * motorRecResolution_; asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Axis %d, response %s, position %f\n", functionName, axisNo_, response, motor_position); + "%s: Axis %d, response %s, position %f\n", functionName, + axisNo_, response, motor_position); setDoubleParam(pC_->motorPosition_, motor_position); setDoubleParam(pC_->motorEncoderPosition_, motor_position); - } - else - { + } else { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Reading the position failed for axis %d\n", functionName, axisNo_); + "%s: Reading the position failed for axis %d\n", functionName, + axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); return status; @@ -205,9 +206,9 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) // Read the current velocity snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTV", this->axisNo_); - status = this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); - if (status == asynSuccess) - { + status = + this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); + if (status == asynSuccess) { int parsed_axis; sscanf(response, "%2dV%10d", &parsed_axis, &motor_velocity_steps); @@ -215,12 +216,12 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) motor_velocity = double(motor_velocity_steps) * motorRecResolution_; asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Axis %d, response %s, velocity %f\n", functionName, axisNo_, response, motor_velocity); - } - else - { + "%s: Axis %d, response %s, velocity %f\n", functionName, + axisNo_, response, motor_velocity); + } else { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Reading the velocity failed for axis %d\n", functionName, axisNo_); + "%s: Reading the velocity failed for axis %d\n", functionName, + axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); return status; @@ -228,23 +229,25 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) // Read the programmed velocity snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTY", this->axisNo_); - status = this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); - if (status == asynSuccess) - { + status = + this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true); + if (status == asynSuccess) { int parsed_axis; - sscanf(response, "%2dY%10d", &parsed_axis, &programmed_motor_velocity_steps); + sscanf(response, "%2dY%10d", &parsed_axis, + &programmed_motor_velocity_steps); // Scale from the encoder resultion to user units - programmed_motor_velocity = double(programmed_motor_velocity_steps) * motorRecResolution_; + programmed_motor_velocity = + double(programmed_motor_velocity_steps) * motorRecResolution_; asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Axis %d, response %s, programmed velocity %f\n", functionName, axisNo_, response, programmed_motor_velocity); - } - else - { + "%s: Axis %d, response %s, programmed velocity %f\n", + functionName, axisNo_, response, programmed_motor_velocity); + } else { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Reading the programmed velocity failed for axis %d\n", functionName, axisNo_); + "%s: Reading the programmed velocity failed for axis %d\n", + functionName, axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); return status; @@ -253,15 +256,13 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) // Read the motor status snprintf(command, pC_->C804_MAXBUF_ - 1, "%dTS", axisNo_); status = pC_->lowLevelWriteRead(this->axisNo_, command, response, true); - if (status == asynSuccess) - { + if (status == asynSuccess) { int parsed_axis; sscanf(response, "%2dS%10d", &parsed_axis, &axis_status); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Axis %d, response %s, status %d\n", functionName, axisNo_, response, axis_status); - } - else - { + "%s: Axis %d, response %s, status %d\n", functionName, + axisNo_, response, axis_status); + } else { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Reading the motor status %d\n", functionName, axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); @@ -270,24 +271,22 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) return status; } - // Check if the axis is enabled by reading out bit 2 (see https://stackoverflow.com/questions/2249731/how-do-i-get-bit-by-bit-data-from-an-integer-value-in-c) + // Check if the axis is enabled by reading out bit 2 (see + // https://stackoverflow.com/questions/2249731/how-do-i-get-bit-by-bit-data-from-an-integer-value-in-c) int mask = 1 << 2; int masked_n = axis_status & mask; // Is 1 if the axis is disabled int disabled = masked_n >> 2; - if (disabled) - { + if (disabled) { enabled_ = false; - } - else - { + } else { enabled_ = true; } /* - Determine if the motor is moving. This is determined by the following criteria: - 1) The motor position changes from poll to poll - 2) The motor is enabled + Determine if the motor is moving. This is determined by the following + criteria: 1) The motor position changes from poll to poll 2) The motor is + enabled */ *moving = enabled_ && motor_position_steps != this->last_position_steps_; @@ -300,22 +299,21 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) moving. If it has spent too much time in a moving state without reaching the target, stop the motor and return an error. */ - if (*moving) - { + if (*moving) { int motorStatusMoving = 0; - pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &motorStatusMoving); + pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, + &motorStatusMoving); // motor is moving, but didn't move in the last poll - if (motorStatusMoving == 0) - { + if (motorStatusMoving == 0) { time_t current_time = time(NULL); // Factor 2 of the calculated moving time - estimatedArrivalTime_ = current_time + std::ceil(2 * std::fabs(position_error) / programmed_motor_velocity); - } - else - { + estimatedArrivalTime_ = + current_time + std::ceil(2 * std::fabs(position_error) / + programmed_motor_velocity); + } else { // /* // Motor is moving for a longer time than it should: Stop it // */ @@ -323,7 +321,9 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) // { // snprintf(command, pC_->C804_MAXBUF_ - 1, "%dST", axisNo_); // status = pC_->lowLevelWriteRead(axisNo_, command, response); - // asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Stopped axis %d since it moved for double the time it should to reach its target\n", functionName, axisNo_); + // asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Stopped + // axis %d since it moved for double the time it should to reach + // its target\n", functionName, axisNo_); // } } } @@ -339,54 +339,58 @@ asynStatus C804Axis::poll_no_param_lib_update(bool *moving) return status; } -asynStatus C804Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) -{ +asynStatus C804Axis::move(double position, int relative, double minVelocity, + double maxVelocity, double acceleration) { asynStatus status; static const char *functionName = "C804Axis::move"; char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_]; double position_c_units = 0.0; // Controller units int position_steps = 0; - // Convert from user coordinates (EGU) to controller coordinates (steps). Check for overflow - if (motorRecResolution_ == 0.0) - { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: MRES must not be zero. Movement is aborted", functionName); + // Convert from user coordinates (EGU) to controller coordinates (steps). + // Check for overflow + if (motorRecResolution_ == 0.0) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: MRES must not be zero. Movement is aborted", + functionName); return asynError; } position_c_units = position / motorRecResolution_; // Check for overflow during the division - if (position_c_units * motorRecResolution_ != position) - { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: could not convert from user units (%f) to controller units (user units divided by resolution MRES %f) due to overflow.", - functionName, position, motorRecResolution_); + if (position_c_units * motorRecResolution_ != position) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: could not convert from user units (%f) to controller units " + "(user units divided by resolution MRES %f) due to overflow.", + functionName, position, motorRecResolution_); return asynError; } - // Steps can only be integer values => cast to integer while checking for overflow - if (std::numeric_limits::max() < position_c_units || std::numeric_limits::min() > position_c_units) - { + // Steps can only be integer values => cast to integer while checking for + // overflow + if (std::numeric_limits::max() < position_c_units || + std::numeric_limits::min() > position_c_units) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: target position %f cannot be converted to int (overflow). Check target value %f and MRES %f", - functionName, position_c_units, position_c_units, motorRecResolution_); + "%s: target position %f cannot be converted to int " + "(overflow). Check target value %f and MRES %f", + functionName, position_c_units, position_c_units, + motorRecResolution_); return asynError; } position_steps = static_cast(position_c_units); // Convert from relative to absolute values - if (relative) - { + if (relative) { position_steps += last_position_steps_; } // If the axis is currently disabled, enable it - if (!enabled_) - { + if (!enabled_) { snprintf(command, pC_->C804_MAXBUF_ - 1, "%dGO", axisNo_); - status = pC_->lowLevelWriteRead(this->axisNo_, command, response, false); - if (status != asynSuccess) - { + status = + pC_->lowLevelWriteRead(this->axisNo_, command, response, false); + if (status != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Enabling axis %d\n failed", functionName, axisNo_); return status; @@ -396,10 +400,10 @@ asynStatus C804Axis::move(double position, int relative, double minVelocity, dou // Start movement snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMA%d", axisNo_, position_steps); status = pC_->lowLevelWriteRead(this->axisNo_, command, response, false); - if (status != asynSuccess) - { + if (status != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Setting the target position %d failed for axis %d\n", functionName, position_steps, axisNo_); + "%s: Setting the target position %d failed for axis %d\n", + functionName, position_steps, axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); return status; } @@ -411,14 +415,13 @@ asynStatus C804Axis::move(double position, int relative, double minVelocity, dou return status; } -asynStatus C804Axis::moveVelocity(double min_velocity, double max_velocity, double acceleration) -{ +asynStatus C804Axis::moveVelocity(double min_velocity, double max_velocity, + double acceleration) { static const char *functionName = "C804Axis::moveVelocity"; return asynError; } -asynStatus C804Axis::stop(double acceleration) -{ +asynStatus C804Axis::stop(double acceleration) { asynStatus status = asynSuccess; static const char *functionName = "C804Axis::stop"; char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_]; @@ -426,26 +429,28 @@ asynStatus C804Axis::stop(double acceleration) bool moving = false; poll(&moving); - if (moving) - { + if (moving) { // ST = Stop snprintf(command, pC_->C804_MAXBUF_ - 1, "%dST", axisNo_); status = pC_->lowLevelWriteRead(axisNo_, command, response, false); - asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Stop axis %d\n", functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Stop axis %d\n", + functionName, axisNo_); } return status; } -asynStatus C804Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) -{ +asynStatus C804Axis::home(double minVelocity, double maxVelocity, + double acceleration, int forwards) { asynStatus status = asynSuccess; static const char *functionName = "C804Axis::home"; char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_]; - snprintf(command, pC_->C804_MAXBUF_ - 1, "%dFE0", axisNo_); // Home to the upper limit of the axis (25 mm) + snprintf(command, pC_->C804_MAXBUF_ - 1, "%dFE0", + axisNo_); // Home to the upper limit of the axis (25 mm) status = pC_->lowLevelWriteRead(axisNo_, command, response, false); - asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Homing axis %d\n", functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Homing axis %d\n", + functionName, axisNo_); return status; } @@ -453,23 +458,21 @@ asynStatus C804Axis::home(double minVelocity, double maxVelocity, double acceler /** If on is 0, disable the motor, otherwise enable it. */ -asynStatus C804Axis::enable(int on) -{ +asynStatus C804Axis::enable(int on) { asynStatus status = asynSuccess; static const char *functionName = "C804Axis::enable"; char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_]; - if (on == 0) - { + if (on == 0) { snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMF", axisNo_); status = pC_->lowLevelWriteRead(axisNo_, command, response, false); - asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Disable axis %d\n", functionName, axisNo_); - } - else - { + asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, + "%s: Disable axis %d\n", functionName, axisNo_); + } else { snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMN", axisNo_); status = pC_->lowLevelWriteRead(axisNo_, command, response, false); - asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Enable axis %d\n", functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, + "%s: Enable axis %d\n", functionName, axisNo_); } return status; } diff --git a/sinqEPICSApp/src/C804Controller.cpp b/sinqEPICSApp/src/C804Controller.cpp index 8305b29..ba41857 100644 --- a/sinqEPICSApp/src/C804Controller.cpp +++ b/sinqEPICSApp/src/C804Controller.cpp @@ -176,9 +176,7 @@ C804Controller::C804Controller(const char *portName, const char *lowLevelPortNam C804Controller::~C804Controller(void) { /* - Cleanup of the memory allocated in this->pAxes_. As discussed in the constructor, - this is not strictly necessary due to the way EPICS works, but it is good - practice anyway to properly clean up resources. + Cleanup of the memory allocated in the asynMotorController constructor */ free(this->pAxes_); } diff --git a/sinqEPICSApp/src/C804Controller.h b/sinqEPICSApp/src/C804Controller.h index 24c05c2..2ecb93c 100644 --- a/sinqEPICSApp/src/C804Controller.h +++ b/sinqEPICSApp/src/C804Controller.h @@ -16,7 +16,6 @@ public: /* These are the methods that we override */ C804Axis *getAxis(asynUser *pasynUser); C804Axis *getAxis(int axisNo); - C804Axis *castToC804Axis(asynMotorAxis *asynAxis); protected: asynUser *lowLevelPortUser_; @@ -26,6 +25,7 @@ protected: time_t idlePollPeriod_; void log(const char *message); + C804Axis *castToC804Axis(asynMotorAxis *asynAxis); asynStatus lowLevelWriteRead(int axisNo, const char *command, char *response, bool expect_response); private: diff --git a/sinqEPICSApp/src/clang-format b/sinqEPICSApp/src/clang-format new file mode 100644 index 0000000..da3868c --- /dev/null +++ b/sinqEPICSApp/src/clang-format @@ -0,0 +1,246 @@ +--- +Language: Cpp +# BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseColons: false +AlignEscapedNewlines: Right +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: MultiLine +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: false +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +KeepEmptyLinesAtEOF: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +PPIndentWidth: -1 +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Latest +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +... + diff --git a/sinqEPICSApp/src/newPmacV3Axis.cpp b/sinqEPICSApp/src/newPmacV3Axis.cpp new file mode 100644 index 0000000..ccb1f01 --- /dev/null +++ b/sinqEPICSApp/src/newPmacV3Axis.cpp @@ -0,0 +1,640 @@ +#include "newPmacV3Axis.h" +#include "newPmacV3Controller.h" +#include +#include +#include +#include +#include +#include + +newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo) + : asynMotorAxis(pC, axisNo), pC_(pC) { + + static const char *functionName = "newPmacV3Axis::newPmacV3Axis"; + + /* + 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. See C804Controller.cpp + for further explanation. 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: FATAL ERROR: Axis index %d must be smaller than the " + "total number of axes %d. Terminating IOC.", + functionName, axisNo_, pC->numAxes_); + exit(-1); + } + + // Initialize all member variables + initial_poll_ = true; + time_at_init_poll_ = 0; + timeout_param_lib_init_ = 10; +} + +newPmacV3Axis::~newPmacV3Axis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +// Read the configuration from the motor control unit and the parameter library +asynStatus newPmacV3Axis::readConfig() { + // Local variable declaration + static const char *functionName = "newPmacV3Axis::readConfig"; + asynStatus status = asynSuccess; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + double positive_limit = 0.0; + double negative_limit = 0.0; + double motorRecResolution = 0.0; + + // ========================================================================= + + // Motor resolution from parameter library + status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (status == asynParamUndefined) { + return asynParamUndefined; + } else if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorRecResolution_"); + } + + // Software limits + snprintf(command, sizeof(command), "Q%2.2d08 Q%2.2d09", axisNo_, axisNo_); + status = pC_->writeRead(axisNo_, command, response); + nvals = sscanf(response, "%lf %lf", &positive_limit, &negative_limit); + if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != + asynSuccess) { + return asynError; + } + + // Transform from motor to user coordinates + positive_limit = positive_limit * motorRecResolution; + negative_limit = negative_limit * motorRecResolution; + + // Store these values in the parameter library + status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, positive_limit); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorHighLimit_"); + } + status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, negative_limit); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorLowLimit_"); + } + return asynSuccess; +} + +asynStatus newPmacV3Axis::poll(bool *moving) { + // Local variable declaration + static const char *functionName = "newPmacV3Axis::poll"; + asynStatus pl_status = asynSuccess; + asynStatus poll_status = asynSuccess; + + // ========================================================================= + + // If this poll is the initial poll, check if the parameter library has + // already been initialized. If not, force EPCIS to repeat the poll until + // the initialization is complete (or until a timeout is reached). Once the + // parameter library has been initialized, read configuration data from the + // motor controller into it. + if (initial_poll_) { + + if (time_at_init_poll_ == 0) { + time_at_init_poll_ = time(NULL); + } + + if (time(NULL) > (time_at_init_poll_ + timeout_param_lib_init_)) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Could not initialize the parameter " + "library until the timeout of %ld seconds after IOC " + "startup. Terminating IOC.", + functionName, timeout_param_lib_init_); + exit(-1); + } + + poll_status = readConfig(); + if (poll_status == asynSuccess) { + initial_poll_ = false; + } else if (poll_status == asynParamUndefined) { + // Wait for 100 ms until trying the entire poll again + usleep(100000); + return poll_status; + } else { + // Something else went completly wrong => Abort the program + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Reading a value from the parameter " + "library failed for axis %d (%s). Terminating", + functionName, axisNo_, + pC_->stringifyAsynStatus(poll_status)); + exit(-1); + } + } + + // The poll function is just a wrapper around poll_no_param_lib_update and + // handles mainly the callParamCallbacks() function. This wrapper is used + // to make sure callParamCallbacks() is called in case of a premature + // return. + poll_status = newPmacV3Axis::poll_no_param_lib_update(moving); + + // If the poll status is ok, reset the error indicators in the parameter + // library + if (poll_status == asynSuccess) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, false); + if (pl_status != asynSuccess) { + pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + } + pl_status = setIntegerParam(pC_->motorStatusCommsError_, false); + if (pl_status != asynSuccess) { + pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + } + } + + // According to the function documentation of asynMotorAxis::poll, this + // function should be called at the end of a poll implementation. + 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: Updating the parameter library failed for axis %d\n", + functionName, axisNo_); + poll_status = pl_status; + } + + return poll_status; +} + +// Perform the actual poll +asynStatus newPmacV3Axis::poll_no_param_lib_update(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; + + static const char *functionName = "newPmacV3Axis::poll"; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + + int direction = 0; + int error = 0; + int ax_status = 0; + double current_position = 0.0; + double previous_position = 0.0; + int low_limit_reached = 0; + int high_limit_reached = 0; + double motorRecResolution = 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_"); + } + + // Read the previous motor position + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previous_position); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + } + + // Check the axis status (Pxx00) and the current motor position (Qxx10) + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10", axisNo_, axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response); + nvals = sscanf(response, "%d %lf", &ax_status, ¤t_position); + if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != + asynSuccess) { + return asynError; + } + + // Change the position values from motor to user coordinates. + current_position = current_position * motorRecResolution; + + // Intepret the status + switch (ax_status) { + case -6: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d is stopping\n", functionName, axisNo_); + *moving = true; + break; + case -5: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d is deactivated\n", functionName, axisNo_); + *moving = false; + + // No further evaluation of the axis status is necessary + return asynSuccess; + case -4: + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Emergency stop has been activated. All axes are stopped.\n", + functionName); + *moving = false; + + // No further evaluation of the axis status is necessary + return asynSuccess; + case -3: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d is inhibited\n", functionName, axisNo_); + *moving = false; + + // No further evaluation of the axis status is necessary + return asynSuccess; + case 0: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d is ready for movement\n", functionName, axisNo_); + *moving = false; + break; + case 5: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d is moving\n", functionName, axisNo_); + *moving = true; + break; + } + + if (*moving) { + // If the axis is moving, evaluate the movement direction + if ((current_position - previous_position) > 0) { + direction = 1; + } else { + direction = 0; + } + } + + // Read the axis error + snprintf(command, sizeof(command), "P%2.2d01", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response); + nvals = sscanf(response, "%d", &error); + if (pC_->checkNumExpectedReads(1, nvals, functionName, axisNo_) != + asynSuccess) { + return asynError; + } + + switch (error) { + case 0: + // No error + break; + case 1: + // EPICS should already prevent this issue in the first place, + // since it contains the user limits + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Target position would exceed user limits in axis %d. EPICS " + "should prevent an out-of-bounds target position, this error " + "therefore indicates a bug.\n", + functionName, axisNo_); + + pl_status = + setStringParam(pC_->messageText_, + "Target position would exceed software limits. This " + "is a bug, please contact software support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + break; + case 5: + // Command not possible + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d is still moving, but received another move " + "command. EPICS should prevent this, this error therefore " + "indicates a bug.\n", + functionName, axisNo_); + + pl_status = setStringParam( + pC_->messageText_, + "Axis received move command while it is still moving. This is a " + "bug, please contact software support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + break; + case 10: + // Limits reached + snprintf(command, sizeof(command), "M%2.2d21 M%2.2d22", axisNo_, + axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response); + nvals = + sscanf(response, "%d %d", &high_limit_reached, &low_limit_reached); + if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != + asynSuccess) { + return asynError; + } + + if (high_limit_reached && !low_limit_reached) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d reached its high limit.\n", functionName, + axisNo_); + + pl_status = setStringParam(pC_->messageText_, "High limit hit"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + } else if (!high_limit_reached && low_limit_reached) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d reached its low limit.\n", functionName, + axisNo_); + + pl_status = setStringParam(pC_->messageText_, "Low limit hit"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + } else if (high_limit_reached && low_limit_reached) { + // If the execution has hit this branch, both high and low + // limit are apparently hit at the same time. This most likely + // indicates an error in the configuration! + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d reached both its higher and lower limit.\n", + functionName, axisNo_); + + pl_status = setStringParam(pC_->messageText_, + "Both high and low limit hit"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + } else { + // If the execution has hit this branch, neither upper nor lower + // limit has been reached, but the MCU states that limits have been + // reached. This is definitely a bug! + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d states that it has hit its limits, but at " + "the same time states that neither high or low limit " + "have been hit. This is a bug.\n", + functionName, axisNo_); + + snprintf(command, sizeof(command), + "Limits have been hit and not hit at the " + "same time " + "(P%2.2d01 = 10). This is a bug, please " + "contact Software " + "support.", + axisNo_); + pl_status = setStringParam(pC_->messageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + } + + break; + case 11: + // Following error + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Maximum allowed following error exceeded for axis %d.\n", + functionName, axisNo_); + + snprintf(command, sizeof(command), + "Maximum allowed following error exceeded (P%2.2d01 = 11). " + "Please contact Electronics support.", + axisNo_); + pl_status = setStringParam(pC_->messageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + break; + case 13: + // Watchdog of the controller final stage triggered + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Controller final stage watchdog triggered for axis %d.\n", + functionName, axisNo_); + + snprintf(command, sizeof(command), + "Controller final stage watchdog triggered (P%2.2d01 = 13). " + "Please contact Electronics support.", + axisNo_); + pl_status = setStringParam(pC_->messageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + break; + case 14: + // EPICS should already prevent this issue in the first place, + // since it contains the user limits + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Target position would exceed hardware limits in axis " + "%d. EPICS should prevent an out-of-bounds target position, " + "this error therefore indicates a bug.\n", + functionName, axisNo_); + + pl_status = setStringParam( + pC_->messageText_, + "Controller final stage watchdog triggered (Pxx01 = 13). " + "Please contact Electronics support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + poll_status = asynError; + break; + default: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d reached an unreachable state (P%2.2d01 = %d).\n", + functionName, axisNo_, axisNo_, error); + + pl_status = setStringParam(pC_->messageText_, + "Axis reached an unreachable state. Please " + "contact software support"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "messageText_"); + } + + 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_"); + } + } + + pl_status = setIntegerParam(pC_->motorEnabled_, (ax_status != -3)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + } + + pl_status = setIntegerParam(pC_->motorStatusHighLimit_, high_limit_reached); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusHighLimit_"); + } + + pl_status = setIntegerParam(pC_->motorStatusLowLimit_, low_limit_reached); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusLowLimit_"); + } + + pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_"); + } + + pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_"); + } + + pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_"); + } + + pl_status = setDoubleParam(pC_->motorPosition_, current_position); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + } + return poll_status; +} + +asynStatus newPmacV3Axis::move(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; + + static const char *functionName = "newPmacV3Axis::move"; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + double motor_coordinates_position = 0.0; + double motor_position = 0.0; + int enabled = 0; + double motorRecResolution = 0.0; + + // ========================================================================= + + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + } + + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_"); + } + + if (enabled == 0) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d is disabled\n", functionName, axisNo_); + return asynSuccess; + } + + // Use only absolute values + if (relative == 1) { + + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motor_position); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + } + + motor_position = motor_position + position; + } + + // Convert from user to motor units + motor_coordinates_position = motor_position / motorRecResolution; + + // Set the axis speed + snprintf(command, sizeof(command), "Q%d04=%lf", axisNo_, maxVelocity); + rw_status = pC_->writeRead(axisNo_, command, response); + if (rw_status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Setting the velocity failed for axis %d\n", functionName, + axisNo_); + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + } + + return rw_status; + } + + // Perform handshake, Set target position and start the move command + snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", + axisNo_, axisNo_, motor_coordinates_position, axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response); + if (rw_status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Starting movement to target position %lf failed for axis %d\n", + functionName, motor_position, axisNo_); + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + } + return rw_status; + } + + return rw_status; +} + +asynStatus newPmacV3Axis::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; + + static const char *functionName = "newPmacV3Axis::stopAxis"; + bool moving = false; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + + // ========================================================================= + + pl_status = poll_no_param_lib_update(&moving); + if (pl_status != asynSuccess) { + return pl_status; + } + + if (moving) { + // only send a stop when actually moving + snprintf(command, sizeof(command), "M%2.2d=8", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response); + + if (rw_status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Stopping the movement failed for axis %d\n", + functionName, axisNo_); + + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, + "motorStatusProblem_"); + } + } + } + return rw_status; +} \ No newline at end of file diff --git a/sinqEPICSApp/src/newPmacV3Axis.h b/sinqEPICSApp/src/newPmacV3Axis.h new file mode 100644 index 0000000..e3e2330 --- /dev/null +++ b/sinqEPICSApp/src/newPmacV3Axis.h @@ -0,0 +1,40 @@ +#ifndef pmacV3AXIS_H +#define pmacV3AXIS_H +#include "asynMotorAxis.h" + +// 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 newPmacV3Controller; + +class newPmacV3Axis : public asynMotorAxis { + public: + /* These are the methods we override from the base class */ + newPmacV3Axis(newPmacV3Controller *pController, int axisNo); + virtual ~newPmacV3Axis(); + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + // asynStatus moveVelocity(double min_velocity, double max_velocity, + // double acceleration); + asynStatus stop(double acceleration); + // asynStatus home(double minVelocity, double maxVelocity, double + // acceleration, + // int forwards); + asynStatus poll(bool *moving); + asynStatus poll_no_param_lib_update(bool *moving); + // asynStatus enable(int on); + + protected: + newPmacV3Controller *pC_; + + void checkBounds(newPmacV3Controller *pController, int axisNo); + asynStatus readConfig(); + bool initial_poll_; + time_t time_at_init_poll_; + time_t timeout_param_lib_init_; + + private: + friend class newPmacV3Controller; +}; + +#endif diff --git a/sinqEPICSApp/src/newPmacV3Controller.cpp b/sinqEPICSApp/src/newPmacV3Controller.cpp new file mode 100644 index 0000000..e389a24 --- /dev/null +++ b/sinqEPICSApp/src/newPmacV3Controller.cpp @@ -0,0 +1,871 @@ + +#include "newPmacV3Controller.h" +#include "asynMotorController.h" +#include "asynOctetSyncIO.h" +#include "newPmacV3Axis.h" +#include +#include +#include +#include +#include +#include +#include + +static const char *driverName = "newPmacV3Controller"; + +// Static pointers (valid for the entire lifetime of the IOC). The number behind +// the strings gives the integer number of each variant (see also method +// stringifyAsynStatus) +static const char *asynSuccessStringified = "success"; // 0 +static const char *asynTimeoutStringified = "timeout"; // 1 +static const char *asynOverflowStringified = "overflow"; // 2 +static const char *asynErrorStringified = "error"; // 3 +static const char *asynDisconnectedStringified = "disconnected"; // 4 +static const char *asynDisabledStringified = "disabled"; // 5 +static const char *asynParamAlreadyExistsStringified = + "parameter already exists"; // 6 +static const char *asynParamNotFoundStringified = "parameter not found"; // 7 +static const char *asynParamWrongTypeStringified = "wrong type"; // 8 +static const char *asynParamBadIndexStringified = "bad index"; // 9 +static const char *asynParamUndefinedStringified = "parameter undefined"; // 10 +static const char *asynParamInvalidListStringified = "invalid list"; // 11 + +const double newPmacV3Controller::TIMEOUT_ = 5.0; // seconds + +/* +Constructor arguments +- portName: +- lowLevelPortName +- numAxes +- movingPollPeriod: Time between polls when moving (in seconds) +- idlePollPeriod: Time between polls when not moving (in seconds) +*/ +newPmacV3Controller::newPmacV3Controller(const char *portName, + const char *lowLevelPortName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, + const int &extraParams) + : asynMotorController( + portName, numAxes, NUM_MOTOR_DRIVER_PARAMS + extraParams, + 0, // No additional interfaces beyond those in base class + 0, // No additional callback interfaces beyond those in base class + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, 0) // Default priority and stack size + +{ + + // Initialization of local variables + static const char *functionName = + "newPmacV3Controller::newPmacV3Controller"; + asynStatus status = asynSuccess; + + // Initialization of all member variables + lowLevelPortUser_ = nullptr; + + // =========================================================================; + + /* + 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(lowLevelPortName, 0, &lowLevelPortUser_, NULL); + if (status != asynSuccess || lowLevelPortUser_ == nullptr) { + errlogPrintf("FATAL ERROR: in %s: cannot connect to MCU controller\n. " + "Terminating", + functionName); + exit(-1); + } + + // ========================================================================= + // Create additional PVs + + // MOTOR_MESSAGE_TEXT corresponds to the PV definition inside + // sinqn_asyn_motor.db. This text is used to forward status messages to + // NICOS and in turn to the user + status = createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &messageText_); + if (status != asynSuccess) { + paramLibAccessFailed(status, "messageText_"); + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("ENABLE_AXIS", asynParamInt32, &enableMotor_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("AXIS_ENABLED", asynParamInt32, &motorEnabled_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, + &rereadEncoderPosition_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("REREAD_ENCODER_POSITION_RBV", asynParamInt32, + &rereadEncoderPositionRBV_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("READ_CONFIG", asynParamInt32, &readConfig_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + + /* + Define the end-of-string of a message coming from the device to EPICS. + It is not necessary to append a terminator to outgoing messages, since + the message length is encoded in the message header in the getSetResponse + method. + */ + const char *message_from_device = + "\006"; // Hex-code for ACK (acknowledge) -> Each message from the MCU + // is terminated by this value + status = pasynOctetSyncIO->setInputEos( + lowLevelPortUser_, message_from_device, strlen(message_from_device)); + if (status != asynSuccess) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Unable to set input EOS (%s). Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + pasynOctetSyncIO->disconnect(lowLevelPortUser_); + exit(-1); + } + + status = startPoller(movingPollPeriod, idlePollPeriod, 1); + if (status != asynSuccess) { + asynPrint( + lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Could not start poller (%s). Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + status = callParamCallbacks(); + if (status != asynSuccess) { + asynPrint( + lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Could not start poller (%s). Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } +} + +newPmacV3Controller::~newPmacV3Controller(void) { + /* + Cleanup of the memory allocated in the asynMotorController constructor + */ + free(this->pAxes_); +} + +/* +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. +*/ +newPmacV3Axis *newPmacV3Controller::getAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return newPmacV3Controller::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 +*/ +newPmacV3Axis *newPmacV3Controller::getAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return newPmacV3Controller::castToAxis(asynAxis); +} + +newPmacV3Axis *newPmacV3Controller::castToAxis(asynMotorAxis *asynAxis) { + static const char *functionName = "newPmacV3Controller::getAxis"; + + // ========================================================================= + + // 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 + newPmacV3Axis *axis = dynamic_cast(asynAxis); + if (axis == nullptr) { + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s: Axis %d is not an instance of newPmacV3Axis", + functionName, axis->axisNo_); + } + return axis; +} + +/* +Sends the given command to the axis specified by axisNo and returns the response +of the axis. +*/ +asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, + char *response) { + // Definition of local variables. + static const char *functionName = "newPmacV3Controller::writeRead"; + asynStatus status = asynSuccess; + asynStatus pl_status = asynSuccess; + char full_command[MAXBUF_] = {0}; + char user_message[MAXBUF_] = {0}; + int motorStatusCommsError = 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; + + // ========================================================================= + + newPmacV3Axis *axis = getAxis(axisNo); + if (axis == nullptr) { + // We already did the error logging directly in getAxis + return asynError; + } + + /* + The message protocol of the pmacV3 used at PSI looks as follows (all + characters immediately following each other without a newline): + 0x40 (ASCII value of @) -> Request for download + 0xBF (ASCII value of ¿) -> Select mode "get_response" + 0x00 (ASCII value of 0) + 0x00 (ASCII value of 0) + 0x00 (ASCII value of 0) + 0x00 (ASCII value of 0) + 0x00 (ASCII value of 0) + [message length in network byte order] -> Use the htons function for this + value [Actual message] It is not necessary to append a terminator, since + this protocol encodes the message length at the beginning. See Turbo PMAC + User Manual, page 418 in VR_PMAC_GETRESPONSE + + The message has to be build manually into the buffer full_command, since it + contains NULL terminators in its middle, therefore the string manipulation + methods of C don't work. + */ + + // The entire message is equal to the command length + const size_t commandLength = + strlen(command) + 1; // +1 because of the appended /r + const int offset = 8; + + // Positions 2 to 6 must have the value 0. Since full_command is initialized + // as an array of zeros, we don't need to set these bits manually. + full_command[0] = '\x40'; + full_command[1] = '\xBF'; + full_command[7] = commandLength; + + snprintf((char *)full_command + offset, MAXBUF_ - offset, "%s\r", command); + asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, + "%s: Sending command: %s\n", functionName, full_command); + + // Perform the actual writeRead + // status = + // pasynOctetSyncIO->write(lowLevelPortUser_, full_command, + // commandLength + offset, TIMEOUT_, + // &nbytesOut); + + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, full_command, commandLength + offset, response, + MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason); + + // Create custom error messages for different failure modes + switch (status) { + case asynSuccess: + break; // Communicate nothing + case asynTimeout: + snprintf(user_message, sizeof(user_message), + "connection timeout for axis %d", axisNo); + break; + case asynDisconnected: + snprintf(user_message, sizeof(user_message), "axis %d is not connected", + axisNo); + break; + case asynDisabled: + snprintf(user_message, sizeof(user_message), "axis %d is disabled", + axisNo); + break; + default: + snprintf(user_message, sizeof(user_message), + "Communication with axis %d failed (%s)", axisNo, + stringifyAsynStatus(status)); + break; + } + + if (status != asynSuccess) { + // Check if the axis aleady 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_"); + } + + if (motorStatusProblem == 0) { + pl_status = axis->setStringParam(this->messageText_, user_message); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "messageText_"); + } + } + } + + // Log the overall status (communication successfull or not) + if (status == asynSuccess) { + asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, + "%s: device response: %s\n", functionName, response); + pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0); + } else { + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s: asynOctetSyncIO->writeRead failed for command %s on " + "axis %d (%s)\n", + functionName, command, axisNo, stringifyAsynStatus(status)); + pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 1); + } + + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + } + return asynSuccess; +} + +asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, + epicsInt32 value) { + + int function = pasynUser->reason; + asynStatus status = asynSuccess; + char command[MAXBUF_] = {0}; + char response[MAXBUF_] = {0}; + static const char *functionName = "newPmacV3Controller::writeInt32"; + int nvals = 0; + + // ========================================================================= + + newPmacV3Axis *axis = getAxis(pasynUser); + if (axis == nullptr) { + // We already did the error logging directly in getAxis + return asynError; + } + + // Handle custom PVs + if (function == enableMotor_) { + + int ax_status = 0; + asynStatus pl_status = asynSuccess; + int timeout_enable_disable = 2; + char message[MAXBUF_] = {0}; + + // ===================================================================== + + // Check if the axis is currently enabled + snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); + status = writeRead(axis->axisNo_, command, response); + nvals = sscanf(response, "%d", &ax_status); + if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != + asynSuccess) { + return asynError; + } + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((ax_status != -3) == value) { + return asynSuccess; + } + + // Enable / disable the axis + snprintf(command, sizeof(command), "M%2.2d14=%d", axis->axisNo_, value); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %s axis %d on controller %s\n", functionName, + value ? "Enable" : "Disable", axis->axisNo_, portName); + if (value == 0) { + pl_status = setStringParam(messageText_, "Disabling ..."); + } else { + pl_status = setStringParam(messageText_, "Enabling ..."); + } + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "messageText_"); + } + status = writeRead(axis->axisNo_, command, response); + if (status != asynSuccess) { + return status; + } + + // Query the axis status every few milliseconds until the axis has been + // enabled or until the timeout has been reached + snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); + int startTime = time(NULL); + while (time(NULL) < startTime + timeout_enable_disable) { + + // Read the axis status + usleep(100); + status = writeRead(axis->axisNo_, command, response); + if (status != asynSuccess) { + return status; + } + nvals = sscanf(response, "%d", &ax_status); + if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != + asynSuccess) { + return asynError; + } + + if ((ax_status != -3) == value) { + bool moving = false; + // Perform a poll to update the parameter library + axis->poll(&moving); + return asynSuccess; + } + } + + // Failed to change axis status within timeout_enable_disable => Send a + // corresponding message + asynPrint( + this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Failed to %s axis %d on controller %s within %d seconds\n", + functionName, value ? "enable" : "disable", axis->axisNo_, portName, + timeout_enable_disable); + + // Output message to user + snprintf(command, sizeof(command), "Failed to %s within %d seconds", + value ? "enable" : "disable", timeout_enable_disable); + pl_status = setStringParam(messageText_, "Enabling ..."); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "messageText_"); + } + return asynError; + + } else if (function == rereadEncoderPosition_) { + + /* + This is not a command that can always be run when enabling a + motor as it also causes relative encoders to reread a position + necessitating recalibration. We only want it to run on absolute + encoders. We also want it to be clear to instrument scientists, that + power has to be cut to the motor, in order to reread the encoder as not + all motors have breaks and they may start to move when disabled. For + that reason, we don't automatically disable the motors to run the + command and instead require that the scientists first disable the motor. + */ + + if (!value) { + return asynSuccess; + } + + // Poll the current status of the axis + bool moving = false; + axis->poll(&moving); + + // Check if this is an absolute encoder + snprintf(command, sizeof(command), "I%2.2X04", axis->axisNo_); + status = writeRead(axis->axisNo_, command, response); + if (status != asynSuccess) { + return status; + } + + int reponse_length = strlen(response); + if (reponse_length < 3) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Unexpected reponse '%s' from axis %d on " + "controller %s while rereading encoder. Aborting...\n", + functionName, response, axis->axisNo_, portName); + return asynError; + } + + // We are only interested in the last two digits and the last value in + // the string before the terminator is \r + int encoder_id = 0; + nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id); + if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != + asynSuccess) { + return asynError; + } + + snprintf(command, sizeof(command), "P46"); + status = writeRead(axis->axisNo_, command, response); + if (status != asynSuccess) { + return status; + } + + int number_of_axes = strtol(response, NULL, 10); + if (encoder_id <= number_of_axes) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Trying to reread absolute encoder of axis %d on " + "controller %s, but it is a relative encoder. Aborting...\n", + functionName, axis->axisNo_, portName); + return asynError; + } + + // Check if the axis is disabled. If not, inform the user that this is + // necessary + int enabled = 0; + status = getIntegerParam(axis->axisNo_, motorEnabled_, &enabled); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "motorEnabled_"); + } + + if (enabled == 1) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Trying to reread absolute encoder of axis %d on " + "controller %s, but it is a relative encoder. Aborting...\n", + functionName, axis->axisNo_, portName); + status = setStringParam( + messageText_, + "Axis must be disabled before rereading the encoder."); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "messageText_"); + } + return asynError; + } + + // Switching on the axis while the rereading process is still ongoing + // causes it to fail. We currently have no way to check if it is + // actually finished, so we instead wait for 0.5 seconds. + usleep(500000); + + // turn off parameter as finished rereading + // this will only be immediately noticed in the read back variable + // though + status = axis->setIntegerParam(rereadEncoderPosition_, 0); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "rereadEncoderPosition_"); + } + return asynSuccess; + + } else { + + return asynMotorController::writeInt32(pasynUser, value); + } +} + +/* +Overloaded from asynMotorController because the special cases "motor enabling" +and "rereading the encoder" must be covered. +*/ +asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser, + epicsInt32 *value) { + + int function = pasynUser->reason, axStat; + asynStatus status = asynError; + static const char *functionName = "newPmacV3Controller::readInt32"; + char command[MAXBUF_]; + char response[MAXBUF_]; + + // ===================================================================== + + newPmacV3Axis *axis = getAxis(pasynUser); + if (axis == nullptr) { + // We already did the error logging directly in getAxis + return asynError; + } + + if (function == motorEnabled_) { + // Readback value for motorEnabled + + snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); + status = this->writeRead(axis->axisNo_, command, response); + if (status != asynSuccess) { + return status; + } + + int nvals = sscanf(response, "%d", value); + if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != + asynSuccess) { + return asynError; + } + status = setIntegerParam(motorEnabled_, (*value != -3)); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "motorEnabled_"); + } + return callParamCallbacks(); // Update the PVs from the parameter + // library + + } else if (function == rereadEncoderPositionRBV_) { + // Readback value for rereadEncoderPosition + + status = getIntegerParam(axis->axisNo_, rereadEncoderPosition_, value); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "HERE\n"); + return paramLibAccessFailed(status, "rereadEncoderPosition_"); + } + status = + setIntegerParam(axis->axisNo_, rereadEncoderPositionRBV_, *value); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "rereadEncoderPositionRBV_"); + } + return callParamCallbacks(); // Update the PVs from the parameter + // library + } else { + return asynMotorController::readInt32(pasynUser, value); + } +} + +const char *newPmacV3Controller::stringifyAsynStatus(asynStatus status) { + // See + // https://github.com/epics-modules/asyn/blob/master/asyn/asynDriver/asynDriver.h + // and + // https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/paramErrors.h + // for the definition of the error codes + switch (status) { + case asynSuccess: + return asynSuccessStringified; + case asynTimeout: + return asynTimeoutStringified; + case asynOverflow: + return asynOverflowStringified; + case asynError: + return asynErrorStringified; + case asynDisconnected: + return asynDisconnectedStringified; + case asynDisabled: + return asynDisabledStringified; + case asynParamAlreadyExists: + return asynParamAlreadyExistsStringified; + case asynParamNotFound: + return asynParamNotFoundStringified; + case asynParamWrongType: + return asynParamWrongTypeStringified; + case asynParamBadIndex: + return asynParamBadIndexStringified; + case asynParamUndefined: + return asynParamUndefinedStringified; + case asynParamInvalidList: + return asynParamInvalidListStringified; + } + + asynPrint(this->lowLevelPortUser_, ASYN_TRACE_ERROR, + "newPmacV3Controller::stringifyAsynStatus: FATAL error: Reached " + "unreachable code. Terminating\n"); + exit(-1); +} + +asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status, + const char *parameter) { + char message[MAXBUF_] = {0}; + snprintf(message, sizeof(message), + "Accessing the parameter library failed for parameter %s (%s). " + "This is a bug, please inform the software support.\n", + parameter, stringifyAsynStatus(status)); + + // Log the error message and try to propagate it + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + setStringParam(messageText_, message); + return status; +} + +asynStatus newPmacV3Controller::checkNumExpectedReads(int expected, int read, + const char *functionName, + int axisNo_) { + if (expected == read) { + return asynSuccess; + } else { + char message[MAXBUF_] = {0}; + snprintf(message, sizeof(message), + "Could not interpret controller response in function %s for " + "axis %d. This is a bug, please inform the software support.", + functionName, axisNo_); + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + setStringParam(messageText_, message); + setIntegerParam(motorStatusCommsError_, 1); + return asynError; + } +} + +/*************************************************************************************/ +/** The following functions are C-wrappers, and can be called directly from + * iocsh */ + +extern "C" { + +/* +C wrapper for the Controller constructor. +*/ +asynStatus newPmacV3CreateController(const char *portName, + const char *lowLevelPortName, int numAxes, + double movingPollPeriod, + double idlePollPeriod) { + /* + We create a new instance of CreateController, using the "new" keyword to + allocate it on the heap while avoiding RAII. TBD: Where is the pointer to + the controller stored? + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp + https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp + + Setting the pointer to nullptr / NULL immediately after construction is + simply done to avoid compiler warnings, see page 7 of this document: + https://subversion.xray.aps.anl.gov/synApps/measComp/trunk/documentation/measCompTutorial.pdf + */ + newPmacV3Controller *pController = new newPmacV3Controller( + portName, lowLevelPortName, numAxes, movingPollPeriod, idlePollPeriod); + pController = NULL; + + return asynSuccess; +} + +/* +C wrapper for the Axis constructor. +See Axis::Axis. +*/ +asynStatus newPmacV3CreateAxis(const char *port, int axis) { + newPmacV3Axis *pAxis; + + static const char *functionName = "newPmacV3CreateAxis"; + + /* + 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(port); + 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 + */ + printf("%s:%s: Error port %s not found\n", driverName, functionName, + port); + return asynError; + } + // Unsafe cast of the pointer to an asynPortDriver + asynPortDriver *apd = (asynPortDriver *)(ptr); + + // Safe downcast + newPmacV3Controller *pC = dynamic_cast(apd); + if (pC == nullptr) { + printf("%s: controller on port %s is not a Controller\n", functionName, + port); + return asynError; + } + + // Prevent manipulation of the controller from other threads while we create + // the new axis. + pC->lock(); + + /* + We create a new instance of Axis, using the "new" keyword to allocate it + on the heap while avoiding RAII. In the constructor, a pointer to the new + object is stored in the controller object "pC". Therefore, the axis instance + can still be reached later by quering "pC". + + Setting the pointer to nullptr / NULL immediately after construction is + simply done to avoid compiler warnings, see page 7 of this document: + https://subversion.xray.aps.anl.gov/synApps/measComp/trunk/documentation/measCompTutorial.pdf + */ + pAxis = new newPmacV3Axis(pC, axis); + pAxis = nullptr; + + // 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). +TBD: If the code is compiled for running on vxWorks, this registration is +apparently not necessary? +*/ + +#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 port name", + iocshArgString}; +static const iocshArg CreateControllerArg1 = {"Low level port name", + 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 *const CreateControllerArgs[] = { + &CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2, + &CreateControllerArg3, &CreateControllerArg4}; +static const iocshFuncDef configNewPmacV3CreateController = { + "newPmacV3CreateController", 5, CreateControllerArgs}; +static void configNewPmacV3CreateControllerCallFunc(const iocshArgBuf *args) { + newPmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, + args[3].dval, args[4].dval); +} + +/* +Same procedure as for the CreateController function, but for the axis +itself. +*/ +static const iocshArg CreateAxisArg0 = {"Controller port name", iocshArgString}; +static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0, + &CreateAxisArg1}; +static const iocshFuncDef configNewPmacV3CreateAxis = {"newPmacV3CreateAxis", 2, + CreateAxisArgs}; +static void configNewPmacV3CreateAxisCallFunc(const iocshArgBuf *args) { + newPmacV3CreateAxis(args[0].sval, args[1].ival); +} + +// This function is made known to EPICS in sinq.dbd and is called by EPICS +// in order to register both functions in the IOC shell +// TBD: Does this happen during compilation? +static void newPmacV3ControllerRegister(void) { + iocshRegister(&configNewPmacV3CreateController, + configNewPmacV3CreateControllerCallFunc); + iocshRegister(&configNewPmacV3CreateAxis, + configNewPmacV3CreateAxisCallFunc); +} +epicsExportRegistrar(newPmacV3ControllerRegister); + +#endif + +} // extern "C" \ No newline at end of file diff --git a/sinqEPICSApp/src/newPmacV3Controller.h b/sinqEPICSApp/src/newPmacV3Controller.h new file mode 100644 index 0000000..4f2ffe3 --- /dev/null +++ b/sinqEPICSApp/src/newPmacV3Controller.h @@ -0,0 +1,71 @@ +/******************************************** + * pmacV3Controller.h + * + * PMAC V3 controller driver based on the asynMotorController class + * + * Stefan Mathis, September 2024 + ********************************************/ + +#ifndef pmacV3Controller_H +#define pmacV3Controller_H +#include "asynMotorAxis.h" +#include "asynMotorController.h" +#include "newPmacV3Axis.h" + +class newPmacV3Controller : public asynMotorController { + + public: + newPmacV3Controller(const char *portName, const char *lowLevelPortName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, const int &extraParams = 2); + + virtual ~newPmacV3Controller(); + + /* Overloaded methods methods */ + newPmacV3Axis *getAxis(asynUser *pasynUser); + newPmacV3Axis *getAxis(int axisNo); + + // overloaded because we want to enable/disable the motor + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + // overloaded because we want to read the axis state + asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + + protected: + asynUser *lowLevelPortUser_; + + asynStatus writeRead(int axisNo, const char *command, char *response); + + // Create a descriptive string out of an asynStatus which can be used for + // logging or communicating with the user + const char *stringifyAsynStatus(asynStatus status); + asynStatus paramLibAccessFailed(asynStatus status, const char *parameter); + asynStatus checkNumExpectedReads(int expected, int read, + const char *function, int axisNo_); + newPmacV3Axis *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; + + /* + 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. This variable has to be specified in the + .cpp-file. + */ + static const double TIMEOUT_; + + int messageText_; + int enableMotor_; + int motorEnabled_; + int rereadEncoderPosition_; + int rereadEncoderPositionRBV_; + int readConfig_; + + friend class newPmacV3Axis; +}; + +#endif /* pmacV3Controller_H */ \ No newline at end of file diff --git a/sinqEPICSApp/src/sinq.dbd b/sinqEPICSApp/src/sinq.dbd index 4bccb8a..36dcd73 100644 --- a/sinqEPICSApp/src/sinq.dbd +++ b/sinqEPICSApp/src/sinq.dbd @@ -10,6 +10,7 @@ registrar(C804ControllerRegister) registrar(pmacAsynIPPortRegister) registrar(MasterMACSRegister) registrar(SINQControllerRegister) +registrar(newPmacV3ControllerRegister) #-------------------------------------------------------- # With the PSI module build system, including these items actually