From e8099767649192bdd7178ef57e1574e19af07e39 Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Thu, 17 Dec 2009 05:52:48 +0000 Subject: [PATCH] Changes to get callbacks to devAsynMotor working --- motorApp/MotorSrc/asynMotorDriver.cpp | 158 ++++++++++++++++++++++++-- 1 file changed, 149 insertions(+), 9 deletions(-) diff --git a/motorApp/MotorSrc/asynMotorDriver.cpp b/motorApp/MotorSrc/asynMotorDriver.cpp index c8536576..f5668283 100644 --- a/motorApp/MotorSrc/asynMotorDriver.cpp +++ b/motorApp/MotorSrc/asynMotorDriver.cpp @@ -1,5 +1,8 @@ +#include + #include "asynMotorDriver.h" + /** All of the arguments are simply passed to * the constructor for the asynPortDriver base class. After calling the base class * constructor this method sets reasonable default values for all of the parameters @@ -8,13 +11,13 @@ static const char *driverName = "asynMotorDriver"; -asynMotorDriver::asynMotorDriver(const char *portName, int maxAddr, int numParams, +asynMotorDriver::asynMotorDriver(const char *portName, int maxAxes, int numParams, int interfaceMask, int interruptMask, int asynFlags, int autoConnect, int priority, int stackSize) - : asynPortDriver(portName, maxAddr, NUM_MOTOR_DRIVER_PARAMS+numParams, - interfaceMask | asynInt32Mask | asynFloat64Mask, - interruptMask | asynInt32Mask | asynFloat64Mask, + : asynPortDriver(portName, maxAxes, NUM_MOTOR_DRIVER_PARAMS+numParams, + interfaceMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask | asynDrvUserMask, + interruptMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask, asynFlags, autoConnect, priority, stackSize) { @@ -58,20 +61,157 @@ asynMotorDriver::asynMotorDriver(const char *portName, int maxAddr, int numParam addParam(motorStatusLowLimitString, &motorStatusLowLimit); addParam(motorStatusHomedString, &motorStatusHomed); + this->axisStatus = (MotorStatus *)calloc(maxAxes, sizeof(MotorStatus)); + this->axisStatusChanged = (int *)calloc(maxAxes, sizeof(int)); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s:%s: constructor complete\n", driverName, functionName); +} +// We override the setIntegerParam, setDoubleParam, and callParamCallbacks methods so we can construct +// the aggregate status structure and do callbacks on it + +asynStatus asynMotorDriver::setIntegerParam(int axis, int function, int value) +{ + int mask; + + // This assumes the parameters defined above are in the same order as the bits the motor record expects! + if (function >= motorStatusDirection && + function <= motorStatusHomed) { + mask = 1 << (function - motorStatusDirection); + if (value) this->axisStatus[axis].status |= mask; + else this->axisStatus[axis].status &= ~mask; + this->axisStatusChanged[axis] = 1; + } + // Call the base class method + return asynPortDriver::setIntegerParam(axis, function, value); +} + +asynStatus asynMotorDriver::setDoubleParam(int axis, int function, double value) +{ + if (function == motorPosition) { + axisStatusChanged[axis] = 1; + this->axisStatus[axis].position = value; + } else if (function == motorEncoderPosition) { + axisStatusChanged[axis] = 1; + this->axisStatus[axis].encoder_posn = value; + } + // Call the base class method + return asynPortDriver::setDoubleParam(axis, function, value); +} + +asynStatus asynMotorDriver::callParamCallbacks(int axis, int addr) +{ + if (this->axisStatusChanged[axis]) { + this->axisStatusChanged[axis] = 0; + doCallbacksGenericPointer((void *)&this->axisStatus[axis], motorStatus, axis); + } + return asynPortDriver::callParamCallbacks(axis, addr); +} + +asynStatus asynMotorDriver::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + int axis; + int function = pasynUser->reason; + asynStatus status=asynSuccess; + double baseVelocity, velocity, accel; + static const char *functionName = "writeFloat64"; + + status = this->getAddress(pasynUser, functionName, &axis); + + /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the + * status at the end, but that's OK */ + status = setDoubleParam(axis, function, value); + getDoubleParam(axis, motorAccel, &accel); + + if (function == motorStop) { + status = this->stopAxis(pasynUser, accel); + + } else if (function == motorHome) { + getDoubleParam(axis, motorVelBase, &baseVelocity); + getDoubleParam(axis, motorVelocity, &velocity); + getDoubleParam(axis, motorAccel, &accel); + status = this->homeAxis(pasynUser, baseVelocity, velocity, accel, (value == 0) ? 0 : 1); + + } else if (function == motorUpdateStatus) { + // We don't implement this yet. Is it needed? + //status = this->forceCallback)(pasynUser); + } + + /* Do callbacks so higher layers see any changes */ + callParamCallbacks(axis, axis); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s error, status=%d axis=%d, function=%d, value=%d\n", + driverName, functionName, status, axis, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s:: axis=%d, function=%d, value=%d\n", + driverName, functionName, axis, function, value); + return status; +} + + +asynStatus asynMotorDriver::writeFloat64(asynUser *pasynUser, epicsFloat64 value) +{ + int axis; + int function = pasynUser->reason; + double baseVelocity, velocity, accel; + asynStatus status = asynError; + static const char *functionName = "writeFloat64"; + + status = this->getAddress(pasynUser, functionName, &axis); + + /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the + * status at the end, but that's OK */ + status = setDoubleParam(axis, function, value); + + getDoubleParam(axis, motorVelBase, &baseVelocity); + getDoubleParam(axis, motorVelocity, &velocity); + getDoubleParam(axis, motorAccel, &accel); + + + if (function == motorMoveRel) { + status = this->moveAxis(pasynUser, value, 1, baseVelocity, velocity, accel); + + } else if (function == motorMoveAbs) { + status = this->moveAxis(pasynUser, value, 0, baseVelocity, velocity, accel); + + } else if (function == motorMoveVel) { + status = this->moveVelocityAxis(pasynUser, baseVelocity, value, accel); + + } + + /* Do callbacks so higher layers see any changes */ + callParamCallbacks(axis, axis); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s error, status=%d axis=%d, function=%d, value=%f\n", + driverName, functionName, status, axis, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s:: axis=%d, function=%d, value=%f\n", + driverName, functionName, axis, function, value); + return status; + } asynStatus asynMotorDriver::readGenericPointer(asynUser *pasynUser, void *pointer) { static const char *functionName = "readGenericPointer"; - - asynPrint(pasynUser, ASYN_TRACE_ERROR, - "%s:%s: not implemented in this driver\n", - driverName, functionName); - return(asynError); + MotorStatus *pStatus = (MotorStatus *)pointer; + int axis; + + getAddress(pasynUser, functionName, &axis); + getIntegerParam(axis, motorStatus, (int *)&pStatus->status); + getDoubleParam(axis, motorPosition, &pStatus->position); + getDoubleParam(axis, motorEncoderPosition, &pStatus->encoder_posn); + getDoubleParam(axis, motorVelocity, &pStatus->velocity); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: MotorStatus = status%d, position=%f, encoder position=%f, velocity=%f\n", + driverName, functionName, pStatus->status, pStatus->position, pStatus->encoder_posn, pStatus->velocity); + return(asynSuccess); } asynStatus asynMotorDriver::moveAxis(asynUser *pasynUser, double position, int relative, double min_velocity, double max_velocity, double acceleration)