lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First draft for the new pmacV3 controller and axis. This draft is still

Browse Source 
buggy, please don't use it in production. The commit is only done to
save the progress made so far.
The changes to the C804 controller and axis are caused by applying clang-format.
This commit is contained in:
mathis_s
2024-10-04 13:29:05 +02:00
parent d44fdbf736
commit fd763a26ae
9 changed files with 2035 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

325
sinqEPICSApp/src/C804Axis.cpp
View File

@ -1,14 +1,14 @@
#include "C804Axis.h"
#include "C804Controller.h"
#include <errlog.h>
#include <string.h>
#include <math.h>
#include <cmath>
#include <unistd.h>
#include <errlog.h>
#include <limits>
#include <math.h>
#include <string.h>
#include <unistd.h>
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<int>::max() < position_c_units || std::numeric_limits<int>::min() > position_c_units)
{
// Steps can only be integer values => cast to integer while checking for
// overflow
if (std::numeric_limits<int>::max() < position_c_units ||
std::numeric_limits<int>::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<int>(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;
}

4
sinqEPICSApp/src/C804Controller.cpp
View File

@ -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_);
}

2
sinqEPICSApp/src/C804Controller.h
View File

@ -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:

246
sinqEPICSApp/src/clang-format Normal file
View File

@ -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
...

640
sinqEPICSApp/src/newPmacV3Axis.cpp Normal file
View File

@ -0,0 +1,640 @@
#include "newPmacV3Axis.h"
#include "newPmacV3Controller.h"
#include <cmath>
#include <errlog.h>
#include <limits>
#include <math.h>
#include <string.h>
#include <unistd.h>
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, &current_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;
}

40
sinqEPICSApp/src/newPmacV3Axis.h Normal file
View File

@ -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

871
sinqEPICSApp/src/newPmacV3Controller.cpp Normal file
View File

@ -0,0 +1,871 @@
#include "newPmacV3Controller.h"
#include "asynMotorController.h"
#include "asynOctetSyncIO.h"
#include "newPmacV3Axis.h"
#include <epicsExport.h>
#include <errlog.h>
#include <iocsh.h>
#include <netinet/in.h>
#include <registryFunction.h>
#include <string.h>
#include <unistd.h>
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<newPmacV3Axis *>(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<newPmacV3Controller *>(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"

71
sinqEPICSApp/src/newPmacV3Controller.h Normal file
View File

@ -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 */

1
sinqEPICSApp/src/sinq.dbd
View File

@ -10,6 +10,7 @@ registrar(C804ControllerRegister)
registrar(pmacAsynIPPortRegister)
registrar(MasterMACSRegister)
registrar(SINQControllerRegister)
registrar(newPmacV3ControllerRegister)
#--------------------------------------------------------
# With the PSI module build system, including these items actually