Major rewrite using asynMotorAxis and asynMotorController classes

2011-03-29 17:51:27 +00:00
parent daa353490d
commit 99f98547a1
2 changed files with 240 additions and 117 deletions
@@ -1,25 +1,46 @@
 #include <stdlib.h>

+#include <epicsThread.h>
+
 #include <asynPortDriver.h>
 #define epicsExportSharedSymbols
 #include <shareLib.h>
 #include "asynMotorDriver.h"

+/* Number of fast polls when poller first wakes up.  This allows for fact
+ * that the status may not be moving on the first few polls */
+ #define FORCED_FAST_POLLS 10
+
 /** All of the arguments are simply passed to the constructor for the asynPortDriver base class. 
  * After calling the base class constructor this method creates the motor parameters
  * defined in asynMotorDriver.h.
  */

 static const char *driverName = "asynMotorDriver";
+static void asynMotorPollerC(void *drvPvt);

-asynMotorController::asynMotorController(const char *portName, int maxAxes, int numParams,
+  asynMotorAxis::asynMotorAxis(class asynMotorController *pController, int axisNo)
+    : pController_(pController), axisNo_(axisNo), statusChanged_(1)
+{
+  if (!pController) return;
+  if ((axisNo < 0) || (axisNo >= pController->numAxes_)) return;
+  pController->pAxes_[axisNo] = this;
+  status_.status = 0;
+  
+  // Create the asynUser, connect to this axis
+  pasynUser_ = pasynManager->createAsynUser(NULL, NULL);
+  pasynManager->connectDevice(pasynUser_, pController->portName, axisNo);
+}
+
+asynMotorController::asynMotorController(const char *portName, int numAxes, int numParams,
                                         int interfaceMask, int interruptMask,
                                         int asynFlags, int autoConnect, int priority, int stackSize)

-  : asynPortDriver(portName, maxAxes, NUM_MOTOR_DRIVER_PARAMS+numParams,
+  : asynPortDriver(portName, numAxes, NUM_MOTOR_DRIVER_PARAMS+numParams,
      interfaceMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask | asynDrvUserMask,
      interruptMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask,
-      asynFlags, autoConnect, priority, stackSize)
+      asynFlags, autoConnect, priority, stackSize),
+    numAxes_(numAxes)

 {
  static const char *functionName = "asynMotorDriver";
@@ -62,53 +83,12 @@ asynMotorController::asynMotorController(const char *portName, int maxAxes, int
  createParam(motorStatusLowLimitString,         asynParamInt32,      &motorStatusLowLimit_);
  createParam(motorStatusHomedString,            asynParamInt32,      &motorStatusHomed_);

-  axisStatus_ = (MotorStatus *)calloc(maxAxes, sizeof(MotorStatus));
-  axisStatusChanged_ = (int *)calloc(maxAxes, sizeof(int));
-
+  pAxes_ = (asynMotorAxis**) calloc(numAxes, sizeof(asynMotorAxis*));
+  pollEventId_ = epicsEventMustCreate(epicsEventEmpty);
+  
  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 asynMotorController::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) axisStatus_[axis].status |= mask;
-  else       axisStatus_[axis].status &= ~mask;
-  axisStatusChanged_[axis] = 1;
-  }
-  // Call the base class method
-  return asynPortDriver::setIntegerParam(axis, function, value);
-}
-
-asynStatus asynMotorController::setDoubleParam(int axis, int function, double value)
-{
-  if (function == motorPosition_) {
-    axisStatusChanged_[axis] = 1;
-    axisStatus_[axis].position = value;
-  } else if (function == motorEncoderPosition_) {
-    axisStatusChanged_[axis] = 1;
-    axisStatus_[axis].encoderPosition = value;
-  }  
-  // Call the base class method
-  return asynPortDriver::setDoubleParam(axis, function, value);
-}   
-
-asynStatus asynMotorController::callParamCallbacks(int axis)
-{
-  if (axisStatusChanged_[axis]) {
-    axisStatusChanged_[axis] = 0;
-    doCallbacksGenericPointer((void *)&axisStatus_[axis], motorStatus_, axis);
-  }
-  return asynPortDriver::callParamCallbacks(axis);
 }

 asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value)
@@ -116,10 +96,12 @@ asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value
  int axis;
  int function = pasynUser->reason;
  asynStatus status=asynSuccess;
+  asynMotorAxis *pAxis;
  double accel;
  static const char *functionName = "writeFloat64";

  status = getAddress(pasynUser, &axis);
+  pAxis = pAxes_[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 */
@@ -127,7 +109,7 @@ asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value

  if (function == motorStop_) {
    getDoubleParam(axis, motorAccel_, &accel);
-    status = stopAxis(pasynUser, accel);
+    status = pAxis->stop(accel);
  
  } else if (function == motorUpdateStatus_) {
    // We don't implement this yet.  Is it needed?
@@ -147,16 +129,18 @@ asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value
  return status;
 }    
  
-
 asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
 {
-  int axis;
  int function = pasynUser->reason;
-  double baseVelocity, velocity, accel;
+  double baseVelocity, velocity, acceleration;
+  asynMotorAxis *pAxis;
+  int axis;
+  int forwards;
  asynStatus status = asynError;
  static const char *functionName = "writeFloat64";

-  status = getAddress(pasynUser, &axis);
+  pAxis = getAxis(pasynUser);
+  axis = pAxis->axisNo_;

  /* 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 */
@@ -164,25 +148,49 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v

  getDoubleParam(axis, motorVelBase_, &baseVelocity);
  getDoubleParam(axis, motorVelocity_, &velocity);
-  getDoubleParam(axis, motorAccel_, &accel);
+  getDoubleParam(axis, motorAccel_, &acceleration);

  if (function == motorMoveRel_) {
-    status = moveAxis(pasynUser, value, 1, baseVelocity, velocity, accel);
+    status = pAxis->move(value, 1, baseVelocity, velocity, acceleration);
+    pAxis->setIntegerParam(motorStatusDone_, 0);
+    pAxis->callParamCallbacks();
+    wakeupPoller();
+    asynPrint(pasynUser, ASYN_TRACE_FLOW, 
+      "%s:%s: Set driver %s, axis %d move relative by %f, base velocity=%f, velocity=%f, acceleration=%f\n",
+      driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration );
  
  } else if (function == motorMoveAbs_) {
-    status = moveAxis(pasynUser, value, 0, baseVelocity, velocity, accel);
+    status = pAxis->move(value, 0, baseVelocity, velocity, acceleration);
+    pAxis->setIntegerParam(motorStatusDone_, 0);
+    pAxis->callParamCallbacks();
+    wakeupPoller();
+    asynPrint(pasynUser, ASYN_TRACE_FLOW, 
+      "%s:%s: Set driver %s, axis %d move absolute to %f, base velocity=%f, velocity=%f, acceleration=%f\n",
+      driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration );

  } else if (function == motorMoveVel_) {
-    status = moveVelocityAxis(pasynUser, baseVelocity, value, accel);
+    status = pAxis->moveVelocity(baseVelocity, value, acceleration);
+    pAxis->setIntegerParam(motorStatusDone_, 0);
+    pAxis->callParamCallbacks();
+    wakeupPoller();
+    asynPrint(pasynUser, ASYN_TRACE_FLOW, 
+      "%s:%s: Set port %s, axis %d move with velocity of %f, acceleration=%f\n",
+      driverName, functionName, portName, pAxis->axisNo_, value, acceleration);

  // Note, the motorHome command happens on the asynFloat64 interface, even though the value (direction) is really integer 
  } else if (function == motorHome_) {
-    status = homeAxis(pasynUser, baseVelocity, velocity, accel, (value == 0) ? 0 : 1);
+    forwards = (value == 0) ? 0 : 1;
+    status = pAxis->home(baseVelocity, velocity, acceleration, forwards);
+    pAxis->setIntegerParam(motorStatusDone_, 0);
+    pAxis->callParamCallbacks();
+    wakeupPoller();
+    asynPrint(pasynUser, ASYN_TRACE_FLOW, 
+      "%s:%s: Set driver %s, axis %d to home %s, base velocity=%f, velocity=%f, acceleration=%f\n",
+      driverName, functionName, portName, pAxis->axisNo_, (forwards?"FORWARDS":"REVERSE"), baseVelocity, velocity, acceleration);

  }
  
  /* Do callbacks so higher layers see any changes */
-  callParamCallbacks(axis);
  if (status) 
    asynPrint(pasynUser, ASYN_TRACE_ERROR, 
      "%s:%s error, status=%d axis=%d, function=%d, value=%f\n", 
@@ -212,47 +220,6 @@ asynStatus asynMotorController::readGenericPointer(asynUser *pasynUser, void *po
  return(asynSuccess);
 }  

-asynStatus asynMotorController::moveAxis(asynUser *pasynUser, double position, int relative, double minVelocity, double maxVelocity, double acceleration)
-{
-  static const char *functionName = "moveAxis";
-
-  asynPrint(pasynUser, ASYN_TRACE_ERROR,
-    "%s:%s: not implemented in this driver\n", 
-    driverName, functionName);
-  return(asynError);
-}
-
-asynStatus asynMotorController::moveVelocityAxis(asynUser *pasynUser, double minVelocity, double maxVelocity, double acceleration)
-{
-  static const char *functionName = "moveVelocityAxis";
-
-  asynPrint(pasynUser, ASYN_TRACE_ERROR,
-    "%s:%s: not implemented in this driver\n", 
-    driverName, functionName);
-  return(asynError);
-}
-
-asynStatus asynMotorController::homeAxis(asynUser *pasynUser, double minVelocity, double maxVelocity, double acceleration, int forwards)
-{
-  static const char *functionName = "homeAxis";
-
-  asynPrint(pasynUser, ASYN_TRACE_ERROR,
-    "%s:%s: not implemented in this driver\n", 
-    driverName, functionName);
-  return(asynError);
-}
-
-
-asynStatus asynMotorController::stopAxis(asynUser *pasynUser, double acceleration)
-{
-  static const char *functionName = "stopAxis";
-
-  asynPrint(pasynUser, ASYN_TRACE_ERROR,
-    "%s:%s: not implemented in this driver\n", 
-    driverName, functionName);
-  return(asynError);
-}
-
 asynStatus asynMotorController::profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger)
 {
  static const char *functionName = "profileMove";
@@ -263,6 +230,20 @@ asynStatus asynMotorController::profileMove(asynUser *pasynUser, int npoints, do
  return(asynError);
 }

+asynMotorAxis* asynMotorController::getAxis(asynUser *pasynUser)
+{
+    int axisNo;
+    
+    getAddress(pasynUser, &axisNo);
+    return getAxis(axisNo);
+}
+
+asynMotorAxis* asynMotorController::getAxis(int axisNo)
+{
+    if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL;
+    return pAxes_[axisNo];
+}
+
 asynStatus asynMotorController::triggerProfile(asynUser *pasynUser)
 {
  static const char *functionName = "triggerProfile";
@@ -273,3 +254,127 @@ asynStatus asynMotorController::triggerProfile(asynUser *pasynUser)
  return(asynError);
 }

+asynStatus asynMotorController::startPoller(double movingPollPeriod, double idlePollPeriod)
+{
+  movingPollPeriod_ = movingPollPeriod;
+  idlePollPeriod_   = idlePollPeriod;
+  epicsThreadCreate("motorPoller", 
+                    epicsThreadPriorityLow,
+                    epicsThreadGetStackSize(epicsThreadStackMedium),
+                    (EPICSTHREADFUNC)asynMotorPollerC, (void *)this);
+  return asynSuccess;
+}
+
+asynStatus asynMotorController::wakeupPoller()
+{
+  epicsEventSignal(pollEventId_);
+  return asynSuccess;
+}
+
+asynStatus asynMotorController::poll()
+{
+  return asynSuccess;
+}
+
+static void asynMotorPollerC(void *drvPvt)
+{
+  asynMotorController *pController = (asynMotorController*)drvPvt;
+  pController->asynMotorPoller();
+}
+  
+
+void asynMotorController::asynMotorPoller()
+{
+  double timeout;
+  int i;
+  int forcedFastPolls=0;
+  int anyMoving;
+  int moving;
+  asynMotorAxis *pAxis;
+  int status;
+
+  timeout = idlePollPeriod_;
+  wakeupPoller();  /* Force on poll at startup */
+
+  while(1) {
+    if (timeout != 0.) status = epicsEventWaitWithTimeout(pollEventId_, timeout);
+    else               status = epicsEventWait(pollEventId_);
+    if (status == epicsEventWaitOK) {
+      /* We got an event, rather than a timeout.  This is because other software
+       * knows that an axis should have changed state (started moving, etc.).
+       * Force a minimum number of fast polls, because the controller status
+       * might not have changed the first few polls
+       */
+      forcedFastPolls = FORCED_FAST_POLLS;
+    }
+    anyMoving = 0;
+    lock();
+    this->poll();
+    for (i=0; i<numAxes_; i++) {
+        pAxis=getAxis(i);
+        pAxis->poll(&moving);
+        if (moving) anyMoving=1;
+    }
+    unlock();
+    if (forcedFastPolls > 0) {
+      timeout = movingPollPeriod_;
+      forcedFastPolls--;
+    } else if (anyMoving) {
+      timeout = movingPollPeriod_;
+    } else {
+      timeout = idlePollPeriod_;
+    }
+  }
+}
+
+
+// We implement the setIntegerParam, setDoubleParam, and callParamCallbacks methods so we can construct 
+// the aggregate status structure and do callbacks on it
+
+asynStatus asynMotorAxis::setIntegerParam(int function, int value)
+{
+  int mask;
+  asynMotorController *pC = getController();
+  
+  // This assumes the parameters defined above are in the same order as the bits the motor record expects!
+  if (function >= pC->motorStatusDirection_ && 
+      function <= pC->motorStatusHomed_) {
+    mask = 1 << (function - pC->motorStatusDirection_);
+    if (value) status_.status |= mask;
+    else       status_.status &= ~mask;
+    statusChanged_ = 1;
+  }
+  // Call the base class method
+  return pC->setIntegerParam(axisNo_, function, value);
+}
+
+asynStatus asynMotorAxis::setDoubleParam(int function, double value)
+{
+  asynMotorController *pC = getController();
+
+  if (function == pC->motorPosition_) {
+    statusChanged_ = 1;
+    status_.position = value;
+  } else if (function == pC->motorEncoderPosition_) {
+    statusChanged_ = 1;
+    status_.encoderPosition = value;
+  }  
+  // Call the base class method
+  return pC->setDoubleParam(axisNo_, function, value);
+}   
+
+asynStatus asynMotorAxis::callParamCallbacks()
+{
+  asynMotorController *pC = getController();
+  
+  if (statusChanged_) {
+    statusChanged_ = 0;
+    pC->doCallbacksGenericPointer((void *)&status_, pC->motorStatus_, axisNo_);
+  }
+  return pC->callParamCallbacks(axisNo_);
+}
+
+asynMotorController* asynMotorAxis::getController()
+{
+  return pController_;
+}
@@ -8,6 +8,7 @@
 #ifndef asynMotorDriver_H
 #define asynMotorDriver_H

+#include <epicsEvent.h>
 #include <epicsTypes.h>

 #define motorMoveRelString              "MOTOR_MOVE_REL"
@@ -61,23 +62,37 @@ typedef struct MotorStatus {
 class epicsShareFunc asynMotorAxis {
 public:
  /* This is the constructor for the class. */
-  asynMotorAxis(int axisNumber, class asynMotorController *pController);
+  asynMotorAxis(class asynMotorController *pController, int axisNumber);

-  virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration);
-  virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration);
-  virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards);
-  virtual asynStatus stop(double acceleration);
+  virtual asynStatus setIntegerParam(int index, int value);
+  virtual asynStatus setDoubleParam(int index, double value);
+  virtual asynStatus callParamCallbacks();
+  asynMotorController* getController();
+
+  // These are pure virtual functions which derived classes must implement
+  virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) = 0;
+  virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration) = 0;
+  virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards) = 0;
+  virtual asynStatus stop(double acceleration) = 0;
+  virtual asynStatus poll(int *moving) = 0;
+
+protected:
+  class asynMotorController *pController_;
+  int axisNo_;
+  asynUser *pasynUser_;

 private:
-  int axisNumber_;
-  class asynMotorController *pController_;
+  MotorStatus status_;
+  int statusChanged_;
+  
+friend class asynMotorController;
 };

 /** Class from which motor drivers are directly derived. */
 class epicsShareFunc asynMotorController : public asynPortDriver {
 public:
  /* This is the constructor for the class. */
-  asynMotorController(const char *portName, int maxAxes, int numParams,
+  asynMotorController(const char *portName, int numAxes, int numParams,
                      int interfaceMask, int interruptMask,
                      int asynFlags, int autoConnect, int priority, int stackSize);

@@ -85,17 +100,16 @@ public:
  virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
  virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
  virtual asynStatus readGenericPointer(asynUser *pasynUser, void *pointer);
-  virtual asynStatus setIntegerParam(int list, int index, int value);
-  virtual asynStatus setDoubleParam(int list, int index, double value);
-  virtual asynStatus callParamCallbacks(int addr);

  /* These are the methods that are new to this class */
-  virtual asynStatus moveAxis(asynUser *pasynUser, double position, int relative, double minVelocity, double maxVelocity, double acceleration);
-  virtual asynStatus moveVelocityAxis(asynUser *pasynUser, double minVelocity, double maxVelocity, double acceleration);
-  virtual asynStatus homeAxis(asynUser *pasynUser, double minVelocity, double maxVelocity, double acceleration, int forwards);
-  virtual asynStatus stopAxis(asynUser *pasynUser, double acceleration);
+  virtual asynMotorAxis* getAxis(asynUser *pasynUser);
+  virtual asynMotorAxis* getAxis(int axisNo);
+  virtual asynStatus startPoller(double movingPollPeriod, double idlePollPeriod);
+  virtual asynStatus wakeupPoller();
  virtual asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger);
  virtual asynStatus triggerProfile(asynUser *pasynUser);
+  virtual asynStatus poll();
+  void asynMotorPoller();  // This should be private but is called from C function

 protected:
  int motorMoveRel_;
@@ -137,9 +151,13 @@ protected:
  int motorStatusHomed_;
  #define LAST_MOTOR_PARAM motorStatusHomed_

-private:
-  MotorStatus *axisStatus_;
-  int *axisStatusChanged_;
+  int numAxes_;
+  asynMotorAxis **pAxes_;
+  epicsEventId pollEventId_;
+  double idlePollPeriod_;
+  double movingPollPeriod_;
+  
+friend class asynMotorAxis;
 };
 #define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1)