sics/epicsmotor.c

/**
 * This is a very basic first generation SICS EPICS motor driver for testing. 
 * It should be replaced by a second generation version when SINQ gets serious 
 * about EPICS and SICS.
 *
 * Mark Koennecke, February 2012
 */
#include <stdlib.h>
#include <sics.h>
#include <modriv.h>

/* EPICS stuff */
#include <tsDefs.h> 
#include <cadef.h> 
#include <ezca.h>
#include <alarmString.h>
#include <menuAlarmStat.h>
#include <db_access.h>
#include <epicsThread.h>

typedef struct __epicsMoDriv{
              /* general motor driver interface 
                 fields. REQUIRED!
              */
              float fUpper; /* upper limit */
              float fLower; /* lower limit */
              char *name;
              int   (*GetPosition)(void *self, float *fPos);
              int   (*RunTo)(void *self,float fNewVal);
              int   (*GetStatus)(void *self);
              void  (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
              int   (*TryAndFixIt)(void *self, int iError,float fNew);
              int   (*Halt)(void *self);
              int   (*GetDriverPar)(void *self, char *name, 
                    float *value);
              int    (*SetDriverPar)(void *self,SConnection *pCon,
                                    char *name, float newValue);
              void   (*ListDriverPar)(void *self, char *motorName,
                                     SConnection *pCon);
              void   (*KillPrivate)(void *self);
              /* your drivers private fields follow below */
              char *motBaseName;
              char *ezcaError;
              int ezcaCode;
              int connectCount;
             } epicsMotorDriver;
 
/*================================================================
  GetPos returns OKOK on success, HWFault on failure 
------------------------------------------------------------------*/
static int epicsGetPos(void *data, float *fPos){
  epicsMotorDriver *self = NULL;
  char fullName[132];
  int status;
  double position;

  self = (epicsMotorDriver *)data;
  snprintf(fullName,sizeof(fullName),"%s.DRBV",self->motBaseName);
  status = ezcaGet(fullName,ezcaDouble,1, &position);
  if(status == EZCA_OK){
    *fPos =(float) position;
    self->connectCount = 0;
    return OKOK;
  } else {
    ezcaGetErrorString("ERROR: EPICS: ", &self->ezcaError);
    self->ezcaCode = status;
    return HWFault;
  }
}
/*-------------- dummy callback for runto -------------------------*/
static void RunToDummy(struct event_handler_args d)
{
}
/*----------------------------------------------------------------
  RunTo starts the motor running. Returns OKOK on success, HWfault
  on Errors
------------------------------------------------------------------*/
static int epicsRunTo(void *data, float newValue){
  epicsMotorDriver *self = NULL;
  char fullName[132];
  double position = newValue;  
  int status;
  chid *cid;

  self = (epicsMotorDriver *)data;
  snprintf(fullName,sizeof(fullName),"%s.DVAL",self->motBaseName);
  ezcaPvToChid(fullName,&cid);
  status = ca_put_callback(DBR_DOUBLE,*cid,&position, RunToDummy,NULL);
  if(status != ECA_NORMAL){
    snprintf(fullName, sizeof(fullName),"Bad CA status %d", status);
    self->ezcaError = strdup(fullName);
    self->ezcaCode = status;
    return HWFault;
  } else {
    ca_pend_event(.05);
    self->connectCount = 0;
    return OKOK;
  }
  
}
/*------------------------------------------------------------------
 CheckStatus queries the sattus of a running motor. Possible return
 values can be:
   HWBusy     : motor still running
   HWFault    : motor error detected
   HWPosFault : motor finished, but position not reached
   HWIdle     : motor finished OK
   HWWarn     : motor issued warning
--------------------------------------------------------------------*/
static int epicsCheckStatus(void *data){
  epicsMotorDriver *self = NULL;
  char fullName[132];
  short smov, stat;
  int status;

  self = (epicsMotorDriver *)data;
  snprintf(fullName,sizeof(fullName),"%s.DMOV",self->motBaseName);
  status = ezcaGet(fullName,ezcaShort,1, &smov);
  if(status == EZCA_OK){
    self->connectCount = 0;
    if(smov == 0){
      return HWBusy;
    } else {
      snprintf(fullName,sizeof(fullName),"%s.STAT",self->motBaseName);
      status = ezcaGet(fullName,ezcaShort,1, &stat);
      if(stat != menuAlarmStatNO_ALARM){
	snprintf(fullName,sizeof(fullName),"EPICS ALARM: %s", epicsAlarmConditionStrings[stat]);
	self->ezcaError = strdup(fullName);
	return HWFault;
      } else {
	return HWIdle;
      }
    }
  } else {
    ezcaGetErrorString("ERROR: EPICS: ", &self->ezcaError);
    self->ezcaCode = status;
    return HWFault;
  }
}
/*------------------------------------------------------------------
 GetError gets more information about error which occurred
  *iCode is an integer error code to be used in TryFixIt as indicator
  buffer is a buffer for a text description of the problem
  iBufLen is the length of buffer
--------------------------------------------------------------------*/
static void epicsGetError(void *data, int *iCode, char *buffer,
                              int iBufLen){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
  if(self->ezcaError != NULL){
    strncpy(buffer,self->ezcaError,iBufLen);
    ezcaFree(self->ezcaError);
    self->ezcaError = NULL;
  }
}
/*------------------------------------------------------------------
 TryAndFixIt tries everything which is possible in software to fix 
 a problem. iError is the error code from GetError, newValue is 
 the target value for the motor
 Possible retrun values are:
    MOTOK   : everything fixed
    MOTREDO : try again 
    MOTFAIL : cannot fix this
--------------------------------------------------------------------*/
static int epicsFixIt(void *data, int iError, float newValue){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
  if(self->ezcaCode == EZCA_NOTCONNECTED && self->connectCount < 2) {
    self->connectCount++;
    return MOTREDO;
  }
  return MOTFAIL;
}
/*-------------------------------------------------------------------
 Halt tries to stop the motor. Halt errors are ignored.

 For some as yet unknown reasons, I cannot send the stop on the 
 same CA line then the rest. But with a new thread which just sends 
 the stop, it works. This is a straightforward use of a thread:
 do your thing and terminate. Do not interact with other parts 
 of the program.    
---------------------------------------------------------------------*/
static void HaltThreadFunc(void *param)
{
  char *pvName = (char *)param;
  short stop = 1;
  chid cid;

  ca_context_create(ca_disable_preemptive_callback);
  ca_create_channel(pvName,NULL,NULL,10,&cid);
  ca_pend_io(5.);
  ca_put(DBR_SHORT,cid,&stop);
  ca_pend_io(5.0);
  free(pvName);
  printf("HaltThread ends\n");
  
}
/*-------------------------------------------------------------------*/
static int epicsHalt(void *data){
  epicsMotorDriver *self = NULL;
  char fullName[132];
  short stop =1;
  chid *cid;
  int status;

  self = (epicsMotorDriver *)data;
  snprintf(fullName,sizeof(fullName),"%s.STOP",self->motBaseName);
  /* ezcaStartGroup(); */
  /* ezcaPut(fullName,ezcaShort,1,&stop); */
  /* ezcaEndGroup(); */
  /* ezcaPvToChid(fullName,&cid); */
  /* status = ca_put_callback(DBR_SHORT,*cid,&stop, RunToDummy,NULL); */
  /* printf("Halt status %d\n", status); */
  /* ca_flush_io(); */
  /* printf("Halt after poll\n"); */
  epicsThreadCreate("Hugo", 
		    epicsThreadPriorityHigh,
		    epicsThreadStackMedium,
		    HaltThreadFunc,
		    (void *)strdup(fullName));

  return 1;
}
/*--------------------------------------------------------------------
 GetDriverPar retrieves the value of a driver parameter.
 Name is the name of the parameter, fValue the value when found.
 Returns 0 on success, 0 else
-----------------------------------------------------------------------*/
static int epicsGetDriverPar(void *data, char *name, float *value){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
  return 0;
}
/*----------------------------------------------------------------------
 SetDriverPar sets a driver parameter. Returns 0 on failure, 1 on 
  success. Name is the parameter name, pCon the connection to report
  errors too, value the new value
------------------------------------------------------------------------*/
static int epicsSetDriverPar(void *data, SConnection *pCon,
                                 char *name, float value){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
   return 0;
}
/*-----------------------------------------------------------------------
   ListDriverPar lists the names and values of driver parameters to 
   pCon. Motorname is the name of the motor ro prefix to the listing.
-------------------------------------------------------------------------*/
static void epicsListDriverPar(void *data, char *motorname, 
                                   SConnection *pCon){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
}
/*-----------------------------------------------------------------------
 KillPrivate has the task to delete possibly dynamically allocated 
 memory in the private part of the driver structure
------------------------------------------------------------------------*/
static void epicsKillPrivate(void *data){
  epicsMotorDriver *self = NULL;
 
  self = (epicsMotorDriver *)data;
  if(self->ezcaError != NULL){
    ezcaFree(self->ezcaError);
  }
  if(self->motBaseName != NULL){
    free(self->motBaseName);
  }
}
/*=======================================================================*/
MotorDriver *epicsMakeMotorDriver(char *baseName) {
   epicsMotorDriver *pNew = NULL;
   char fullName[132];
   double limit;

   pNew = malloc(sizeof(epicsMotorDriver));
   if(pNew == NULL){
     return NULL;
   }
   memset(pNew,0,sizeof(epicsMotorDriver));
   pNew->motBaseName = strdup(baseName);
   snprintf(fullName,sizeof(fullName),"%s.DRBV",pNew->motBaseName);
   ezcaSetMonitor(fullName,ezcaDouble,1);
   snprintf(fullName,sizeof(fullName),"%s.DMOV",pNew->motBaseName);
   ezcaSetMonitor(fullName,ezcaShort,1);
   snprintf(fullName,sizeof(fullName),"%s.STAT",pNew->motBaseName);
   ezcaSetMonitor(fullName,ezcaShort,1); 
 
   snprintf(fullName,sizeof(fullName),"%s.HLM",pNew->motBaseName);
   ezcaGet(fullName,ezcaDouble,1, &limit);
   pNew->fUpper = limit;
   snprintf(fullName,sizeof(fullName),"%s.LLM",pNew->motBaseName);
   ezcaGet(fullName,ezcaDouble,1, &limit);
   pNew->fLower = limit;

   pNew->GetPosition    = epicsGetPos;
   pNew->RunTo          = epicsRunTo;
   pNew->Halt           = epicsHalt;
   pNew->GetStatus      = epicsCheckStatus;
   pNew->GetError       = epicsGetError;
   pNew->TryAndFixIt    = epicsFixIt;
   pNew->GetDriverPar   = epicsGetDriverPar;
   pNew->SetDriverPar   = epicsSetDriverPar;
   pNew->ListDriverPar  = epicsListDriverPar;
   pNew->KillPrivate    = epicsKillPrivate;

   /* ezcaTraceOn(); */
   return (MotorDriver *)pNew;
}