lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity
Files
master
motorBase/motorInit.c
T
zimoch 2c24f73705 fix unsafe string manipulation
2021-09-07 16:12:56 +02:00

675 lines
24 KiB
C
Raw Permalink Blame History

/*
** motorInit.c
** ===========
**
** Program to initialise a motor record from an encoder at ioc boot time.
**
** Note that, under EPICS 3.14, there are problems with ca_context_destroy
** if any of the channels could not be connected, e.g. if the encoder is
** on a remote IOC which is "down". In such a case, motorInit will not exit
** but go into an idle loop. For this reason, the program has been written
** so that it is reentrant, i.e. it has no global variables.
**
** Usage:
** On host:
** cc -c ... motorInit.c
** On ioc:
** ld < motorInit.o
** motorInit, "motor", "encoder"
**
** Repository:
** $Source: /cvs/G/DRV/motor/EPICS-R3-14/motorR6-2-2/motorApp/snl/motorInit.c,v $
**
** $Revision: 1.7 $ $Date: 2008/05/06 11:44:48 $
**
** +--------------------------------------------------------------+
** | Paul Scherrer Institute |
** | SLS Controls Software Group |
** | |
** | This software may be used freely by non-profit organizations.|
** | It may be copied provided that the name of PSI and of the |
** | author is included. Neither PSI nor the author assume any |
** | responsibility for the use of this software outside of PSI. |
** +--------------------------------------------------------------+
*/
#ifdef vxWorks
#include <vxWorks.h>
#else
#define OK 0
#define ERROR 1
#endif
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#ifdef vxWorks
#include <taskLib.h>
#include <sysLib.h>
static void epicsThreadSleep (double secs) {
taskDelay ((int)(sysClkRateGet () * secs));
}
#else
#include <epicsThread.h>
#endif
#include <cadef.h>
#include <errlog.h>
#define DESTROY {if (pVar->connectingCounter > 0) { \
printf ("motorInit: pending connection -- cannot exit!\n"); \
motorInitSuspend (pVar); \
} \
if (pVar->debug) printf ("Calling ca_context_destroy ...\n"); \
ca_context_destroy (); \
return ERROR;}
/* Function Prototypes
*/
struct UserVar {
int connectingCounter;
int debug;
};
static void caConnectCallback (
struct connection_handler_args args);
static void motorInitSuspend (
struct UserVar *pVar);
static int myGetw (
char *chanName,
chid chId,
chtype type,
void *pVal,
double secs,
int debug);
static int myPutw (
char *chanName,
chid chId,
chtype type,
void *pVal,
double secs,
int debug);
static int mySearchw (
char *chanName,
chid *pChId,
double secs,
struct UserVar *pVar);
static int startCaSearch (
char *chanName,
chid *pChId,
struct UserVar *pVar);
static int waitAllConnected (
double secs,
struct UserVar *pVar);
/*--------------------------------------------------------------------------------
** motorInitSuspend -- Simply do nothing forever, unless connectingCounter
** should happen to go to zero, in which case we can
** return.
*/
static void motorInitSuspend (
/* ================
*/ struct UserVar *pVar) {
while (pVar->connectingCounter > 0) {
epicsThreadSleep (1);
}
}
/*--------------------------------------------------------------------------------
*/
static void caConnectCallback (
/* =================
*/ struct connection_handler_args myArgs) { /* The arguments */
struct UserVar *pVar;
if (myArgs.op != CA_OP_CONN_UP) {
errlogPrintf ("caConnectCallback: Connection failed, probably!\n");
} else {
pVar = (struct UserVar *) ca_puser (myArgs.chid);
if (pVar->debug) errlogPrintf ("caConnectCallback: Connection up.\n");
pVar->connectingCounter--;
}
}
/*--------------------------------------------------------------------------------
*/
static int myGetw (
/* ======
*/ char *chanName, /* Name of channel to be read */
chid chId, /* Channel ID of channel */
chtype type, /* Type of variable to be returned */
void *pVal, /* Pointer to value to be returned */
double secs, /* Time-out in secs */
int debug) { /* If non-zero, be verbose */
/* Read a value from an EPICS channel and wait till we've got it.
*/
int status;
if (debug) printf ("Reading %s, type %s ...\n", chanName, dbr_type_to_text (type));
status = ca_get (type, chId, pVal);
if (status != ECA_NORMAL) {
errlogPrintf ("Error return from ca_get of %s: %d\n", chanName, status);
return ERROR;
}
status = ca_pend_io (secs);
switch (status) {
case ECA_NORMAL:
if (debug) printf ("ca_get OK.\n");
return OK;
case ECA_TIMEOUT:
errlogPrintf ("Time-out from ca_pend_io for ca_get from channel %s\n", chanName);
return ERROR;
default:
errlogPrintf ("Error return from ca_pend_io for ca_get from channel %s: %d\n", chanName, status);
return ERROR;
}
return OK;
}
/*--------------------------------------------------------------------------------
*/
static int myPutw (
/* ======
*/ char *chanName, /* Name of channel to be read */
chid chId, /* Channel ID of channel */
chtype type, /* Type of variable to be written */
void *pVal, /* Pointer to value to be written */
double secs, /* Time-out in secs */
int debug) { /* If non-zero, be verbose */
/* Read a value from an EPICS channel and wait till we've got it.
*/
int status, i;
char *pChar;
if (debug) {
printf ("Writing %s, type %s\n", chanName, dbr_type_to_text (type));
pChar = (char *) pVal;
for (i = 0; i < 8; i++) printf (" %d", pChar[i]);
printf ("\n");
}
status = ca_put (type, chId, pVal);
if (status != ECA_NORMAL) {
errlogPrintf ("Error return from ca_put of %s: %d\n", chanName, status);
return ERROR;
}
status = ca_pend_io (secs);
switch (status) {
case ECA_NORMAL:
if (debug) printf ("ca_put OK.\n");
return OK;
case ECA_TIMEOUT:
errlogPrintf ("Time-out from ca_pend_io for ca_put to channel %s\n", chanName);
return ERROR;
default:
errlogPrintf ("Error return from ca_pend_io for ca_put to channel %s: %d\n", chanName, status);
return ERROR;
}
return OK;
}
/*--------------------------------------------------------------------------------
*/
static int mySearchw (
/* =========
*/ char *chanName, /* Name of channel to be connected */
chid *pChId, /* Channel ID to be returned */
double secs, /* Time-out in secs */
struct UserVar *pVar) { /* User data structure */
/* Search for an EPICS channel and wait until we have
** connected to it.
*/
int status;
if (pVar->debug) printf ("mySearchw: %s ...\n", chanName);
status = startCaSearch (chanName, pChId, pVar);
if (status != OK) {
errlogPrintf ("Error return from startCaSearch for %s: %d\n", chanName, status);
return ERROR;
}
if (pVar->debug) printf ("mySearchw: waiting for connection to complete ...\n");
if (waitAllConnected (secs, pVar) != OK) return ERROR;
return OK;
}
/*--------------------------------------------------------------------------------
*/
static int startCaSearch (
/* =============
*/ char *chanName,
chid *pChId,
struct UserVar *pVar) {
int status;
if (pVar->debug) printf ("Connecting to %s\n", chanName);
status = ca_create_channel (chanName, caConnectCallback, pVar, 0, pChId);
if (status != ECA_NORMAL) {
errlogPrintf ("Error return from ca_search of %s: %d\n", chanName, status);
return ERROR;
}
pVar->connectingCounter++;
return OK;
}
/*--------------------------------------------------------------------------------
*/
static int waitAllConnected (
/* ================
*/ double secs,
struct UserVar *pVar) {
if (secs <= 0.0) secs = 5.0;
while (pVar->connectingCounter & (secs >= 0)) {
epicsThreadSleep (0.1);
secs = secs - 0.1;
}
if (pVar->connectingCounter) {
errlogPrintf ("Time-out waiting for connection(s) to be established.\n");
return ERROR;
}
return OK;
}
/*--------------------------------------------------------------------------------
*/
int motorInit (
/* =========
*/ char *motor,
char *encoder,
int debug) {
char *usage = "Usage: motorInit \"motor\", \"encoder\" [, debug]";
char myMotor[64], myEncoder[64], *pDot;
int status, cntr;
size_t rdblLen;
double tmo = 5.0;
epicsInt32 foff_save = 0, able_save = 0;
double setVal;
struct UserVar *pVar;
/*-------------------------------------------------------
** Here are the channel id's of all the channels we need.
*/
chid ca_ENC_UDF; /* != 0 if encoder value undefined */
epicsInt32 ENC_UDF = 0;
char ENC_UDF_name[64];
chid ca_ENC_VAL; /* The encoder value */
double ENC_VAL;
char ENC_VAL_name[64];
chid ca_MOT_RRES; /* Motor "Readback resolution" */
double MOT_RRES;
char MOT_RRES_name[64];
chid ca_MOT_DMOV; /* DMOV == 1 if motor finished moving */
epicsInt32 MOT_DMOV = 0;
char MOT_DMOV_name[64];
chid ca_MOT_URIP; /* Motor "Use Readout If Present" */
epicsInt32 MOT_URIP = 0;
char MOT_URIP_name[64];
chid ca_MOT_RDBL; /* Motor Readback Link */
char MOT_RDBL[64];
char MOT_RDBL_name[64];
chid ca_MOT_SSET; /* Motor "Set SET Mode" */
epicsInt32 MOT_SSET = 0;
char MOT_SSET_name[64];
chid ca_MOT_DVAL; /* Motor "Dial Value" */
double MOT_DVAL;
char MOT_DVAL_name[64];
chid ca_MOT_DLLM; /* Motor Dial Low Limit */
double MOT_DLLM;
char MOT_DLLM_name[64];
chid ca_MOT_DHLM; /* Motor Dial High Limit */
double MOT_DHLM;
char MOT_DHLM_name[64];
chid ca_MOT_SUSE; /* Motor "Set USE Mode" */
epicsInt32 MOT_SUSE = 0;
char MOT_SUSE_name[64];
chid ca_MOT_FOFF; /* Motor "Offset-Freeze" if == 1 */
epicsInt32 MOT_FOFF = 0;
char MOT_FOFF_name[64];
chid ca_MOT_ABLE; /* Motor "Enable/Disable" */
epicsInt32 MOT_ABLE = 0;
char MOT_ABLE_name[64];
/*-------------------------------------------------------
*/
pVar = calloc (1, sizeof (struct UserVar));
if (debug) printf ("Address of connectingCounter = %p\n", &pVar->connectingCounter);
pVar->debug = debug;
pVar->connectingCounter = 0;
if ((motor == NULL) ||
(encoder == NULL)) {
printf ("%s\n", usage);
return ERROR;
}
if (((strlen (motor) < 2) || (strlen (motor) > 40)) ||
((strlen (encoder) < 2) || (strlen (encoder) > 40))) {
printf ("%s\n", usage);
return ERROR;
}
strcpy (myMotor, motor);
pDot = strchr (myMotor, '.');
if (pDot) {
*pDot = '\000';
printf ("Motor name has been truncated to \"%s\".\n", myMotor);
}
strcpy (myEncoder, encoder);
pDot = strchr (myEncoder, '.');
if (pDot) {
*pDot = '\000';
printf ("Encoder name has been truncated to \"%s\".\n", myEncoder);
}
status = ca_context_create (ca_enable_preemptive_callback);
if (status != ECA_NORMAL) {
errlogPrintf ("Error return from ca_context_create: %d", status);
return ERROR;
}
printf ("Synchronising %s and %s ...\n", myMotor, encoder);
sprintf (ENC_UDF_name, "%.58s.UDF", encoder);
sprintf (ENC_VAL_name, "%.58s", encoder);
sprintf (MOT_RRES_name, "%.58s.RRES", myMotor);
sprintf (MOT_DMOV_name, "%.58s.DMOV", myMotor);
sprintf (MOT_URIP_name, "%.58s.URIP", myMotor);
sprintf (MOT_RDBL_name, "%.58s.RDBL", myMotor);
sprintf (MOT_SSET_name, "%.58s.SSET", myMotor);
sprintf (MOT_DVAL_name, "%.58s.DVAL", myMotor);
sprintf (MOT_DLLM_name, "%.58s.DLLM", myMotor);
sprintf (MOT_DHLM_name, "%.58s.DHLM", myMotor);
sprintf (MOT_SUSE_name, "%.58s.SUSE", myMotor);
sprintf (MOT_FOFF_name, "%.58s.FOFF", myMotor);
sprintf (MOT_ABLE_name, "%.58s_able", myMotor);
if (debug) {
printf ("Channels to be used are:\n");
printf (" %s\n", ENC_UDF_name);
printf (" %s\n", ENC_VAL_name);
printf (" %s\n", MOT_RRES_name);
printf (" %s\n", MOT_DMOV_name);
printf (" %s\n", MOT_URIP_name);
printf (" %s\n", MOT_RDBL_name);
printf (" %s\n", MOT_SSET_name);
printf (" %s\n", MOT_DVAL_name);
printf (" %s\n", MOT_DLLM_name);
printf (" %s\n", MOT_DHLM_name);
printf (" %s\n", MOT_SUSE_name);
printf (" %s\n", MOT_FOFF_name);
printf (" %s\n", MOT_ABLE_name);
}
/*---------------------------------------------------
** Start by getting the RDBL field of the motor and
** checking that it matches the given encoder name.
*/
status = mySearchw (MOT_RDBL_name, &ca_MOT_RDBL, tmo, pVar);
if (status != OK) {
ca_clear_channel (ca_MOT_RDBL); DESTROY;
}
status = myGetw (
MOT_RDBL_name, ca_MOT_RDBL, DBR_STRING, &MOT_RDBL, tmo, debug);
if (status != OK) {
ca_context_destroy ();
return ERROR;
}
rdblLen = strcspn (MOT_RDBL, " "); MOT_RDBL[rdblLen] = '\0';
if (strcmp (MOT_RDBL, encoder) != 0) {
errlogPrintf ("\007RDBL field of %s is not %s.\n", myMotor, encoder);
errlogPrintf ("Synchronisation abandoned!\n");
ca_context_destroy ();
return ERROR;
}
/*---------------------------------------------------
** Next, check that the encoder channel exists.
*/
if (mySearchw (ENC_VAL_name, &ca_ENC_VAL, tmo, pVar) != OK) {
errlogPrintf ("Synchronisation abandoned!\n");
DESTROY;
}
status = myGetw (
ENC_VAL_name, ca_ENC_VAL, DBR_DOUBLE, &ENC_VAL, tmo, debug);
if (status != OK) {
errlogPrintf ("Synchronisation abandoned!\n");
ca_context_destroy ();
return ERROR;
}
/*---------------------------------------------------
** Preliminary checks OK, so connect to all channels.
*/
if (startCaSearch (ENC_UDF_name, &ca_ENC_UDF, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_RRES_name, &ca_MOT_RRES, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_DMOV_name, &ca_MOT_DMOV, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_URIP_name, &ca_MOT_URIP, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_SSET_name, &ca_MOT_SSET, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_DVAL_name, &ca_MOT_DVAL, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_DLLM_name, &ca_MOT_DLLM, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_DHLM_name, &ca_MOT_DHLM, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_SUSE_name, &ca_MOT_SUSE, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_FOFF_name, &ca_MOT_FOFF, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (startCaSearch (MOT_ABLE_name, &ca_MOT_ABLE, pVar) != OK) {ca_context_destroy (); return ERROR;}
if (waitAllConnected (tmo, pVar) != OK) {
DESTROY;
}
if (pVar->debug) printf ("All channels connected.\n");
/*---------------------------------------------------
** Wait for encoder value to become valid. Immediately
** after iocInit, this could, in principle, take a
** bit of time.
*/
if (debug) printf ("Ensuring that %s is valid ...\n", myEncoder);
cntr = 50;
if (myGetw (
ENC_UDF_name, ca_ENC_UDF, DBR_INT, &ENC_UDF, tmo, debug) != OK) {
DESTROY;
}
while (ENC_UDF != 0) {
cntr--;
if (cntr <= 0) {
errlogPrintf ("\007Time-out waiting for encoder value to become valid.\n");
errlogPrintf ("Synchronisation abandoned!\n");
DESTROY;
}
epicsThreadSleep (0.1);
if (myGetw (
ENC_UDF_name, ca_ENC_UDF, DBR_INT, &ENC_UDF, tmo, debug) != OK) {
DESTROY;
}
}
if (debug) printf ("%s is OK\n", myEncoder);
/*---------------------------------------------------
** Get the enable/disable state of the motor and then
** ensure that it is enabled.
*/
if (myGetw (
MOT_ABLE_name, ca_MOT_ABLE, DBR_LONG, &able_save, tmo, debug) != OK) {
DESTROY;
}
if (debug) printf ("%-27s = %10d\n", MOT_ABLE_name, able_save);
if (able_save != 0) {
MOT_ABLE = 0;
if (myPutw (
MOT_ABLE_name, ca_MOT_ABLE, DBR_LONG, &MOT_ABLE, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("Motor enabled\n");
epicsThreadSleep (1.0);
}
}
/*---------------------------------------------------
** Get the values we need or which need saving.
*/
if (myGetw (
MOT_FOFF_name, ca_MOT_FOFF, DBR_LONG, &foff_save, tmo, debug) != OK) {
DESTROY;
}
if (myGetw (
MOT_DLLM_name, ca_MOT_DLLM, DBR_DOUBLE, &MOT_DLLM, tmo, debug) != OK) {
DESTROY;
}
if (myGetw (
MOT_DHLM_name, ca_MOT_DHLM, DBR_DOUBLE, &MOT_DHLM, tmo, debug) != OK) {
DESTROY;
}
if (myGetw (
MOT_RRES_name, ca_MOT_RRES, DBR_DOUBLE, &MOT_RRES, tmo, debug) != OK) {
DESTROY;
}
if (myGetw (
ENC_VAL_name, ca_ENC_VAL, DBR_DOUBLE, &ENC_VAL, tmo, debug) != OK) {
DESTROY;
}
if (debug) printf ("%-27s = %10d\n", MOT_FOFF_name, foff_save);
if (debug) printf ("%-27s = %10.5f\n", MOT_DLLM_name, MOT_DLLM);
if (debug) printf ("%-27s = %10.5f\n", MOT_DHLM_name, MOT_DHLM);
if (debug) printf ("%-27s = %10.5f\n", MOT_RRES_name, MOT_RRES);
if (debug) printf ("%-27s = %10.5f\n", ENC_VAL_name, ENC_VAL);
setVal = MOT_RRES * ENC_VAL;
printf ("Value of encoder = %10.5f\n", setVal);
/*---------------------------------------------------
** Freeze the offsets
*/
MOT_FOFF = 1;
if (myPutw (
MOT_FOFF_name, ca_MOT_FOFF, DBR_LONG, &MOT_FOFF, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("Fixed offset.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
** Tell motor to use encoder
*/
MOT_URIP = 1;
if (myPutw (
MOT_URIP_name, ca_MOT_URIP, DBR_LONG, &MOT_URIP, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("URIP set to 1.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
** Go to "Set" mode
*/
MOT_SSET = 1;
if (myPutw (
MOT_SSET_name, ca_MOT_SSET, DBR_LONG, &MOT_SSET, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("\"Set\" mode has been set.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
** If the motor soft limits are set (i.e. not both zero),
** See if encoder has a valid value.
** If so, use it, otherwise set a middle point.
*/
if ((MOT_DLLM != 0.0) || (MOT_DHLM != 0.0)) {
if ((setVal < MOT_DLLM) || (setVal > MOT_DHLM)) {
printf ("\007Warning -- motor seems to be outside its soft limits!\n");
setVal = 0.5 * (MOT_DLLM + MOT_DHLM);
}
}
/* Force motor record update */
MOT_DVAL = setVal;
if (myPutw (
MOT_DVAL_name, ca_MOT_DVAL, DBR_DOUBLE, &MOT_DVAL, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("Dial value has been set to %f\n", MOT_DVAL);
}
/*---------------------------------------------------
** The update can take some time. Wait for the
** motor's DMOV field to get set.
*/
if (debug) printf ("Waiting for update to complete ...\n");
cntr = 50;
if (myGetw (
MOT_DMOV_name, ca_MOT_DMOV, DBR_INT, &MOT_DMOV, tmo, debug) != OK) {
DESTROY;
}
while (MOT_DMOV == 0) {
cntr--;
if (cntr <= 0) {
errlogPrintf ("\007Time-out waiting for update to complete.\n");
errlogPrintf ("Synchronisation abandoned!\n");
DESTROY;
}
epicsThreadSleep (0.1);
if (myGetw (
MOT_DMOV_name, ca_MOT_DMOV, DBR_INT, &MOT_DMOV, tmo, debug) != OK) {
DESTROY;
}
}
if (debug) printf ("Update completed.\n");
/*---------------------------------------------------
** Go back to "Use" mode.
*/
MOT_SUSE = 1;
if (myPutw (
MOT_SUSE_name, ca_MOT_SUSE, DBR_LONG, &MOT_SUSE, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("\"Use\" mode has been set.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
** Restore FOFF state.
*/
MOT_FOFF = foff_save;
if (myPutw (
MOT_FOFF_name, ca_MOT_FOFF, DBR_LONG, &MOT_FOFF, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("Offset state (fixed/variable) restored.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
** Restore "Enable" state.
*/
MOT_ABLE = able_save;
if (myPutw (
MOT_ABLE_name, ca_MOT_ABLE, DBR_LONG, &MOT_ABLE, tmo, debug) != OK) {
DESTROY;
}
if (debug) {
printf ("Motor enable/disable state restored.\n");
epicsThreadSleep (1.0);
}
/*---------------------------------------------------
*/
ca_context_destroy (); /* Close down channel access */
free (pVar);
printf ("%s and %s have been synchronised\n", myMotor, myEncoder);
return OK;
}
/*--------------------------------------------------*/
/* emacs setup - force text mode to prevent emacs */
/* from helping with the indentation */
/* and tell emacs to use spaces when */
/* tabbing. */
/* Local Variables: */
/* mode:text */
/* indent-tabs-mode:nil */
/* End: */
/*--------------------------------------------------*/
/*--------------------------------- End of $RCSfile: motorInit.c,v $ ----*/
View Git Blame Copy Permalink