diff --git a/motorApp/OmsSrc/drvMAXv_trajectory.cc b/motorApp/OmsSrc/drvMAXv_trajectory.cc new file mode 100644 index 00000000..53dea842 --- /dev/null +++ b/motorApp/OmsSrc/drvMAXv_trajectory.cc @@ -0,0 +1,1429 @@ +/* +FILENAME... drvMAXv.cc +USAGE... Motor record driver level support for OMS model MAXv. + +Version: $Revision: 11147 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2010-06-09 13:43:20 -0500 (Wed, 09 Jun 2010) $ +HeadURL: $URL: https://subversion.xor.aps.anl.gov/synApps/motor/trunk/motorApp/OmsSrc/drvMAXv.cc $ +*/ + +/* + * Original Author: Ron Sluiter + * Date: 04/05/04 + * Current Author: Ron Sluiter + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * NOTES + * ----- + * Verified with firmware: + * - MAXv ver:1.25 + * - MAXv ver:1.29 (has ECO #1432; fixes initialization problem). + * - MAXv ver:1.31 (fixes DPRAM encoder position data problem when using + * mixed motor types.) + * - MAXv ver:1.33, FPGA:B2:A6 BOOT:1.2 (Watchdog Timeout Counter added) + * + * Modification Log: + * ----------------- + * 01 04-05-04 rls Copied from drvOms58.cc + * 02 09-20-04 rls - support for 32axes/controller. + * - added MAXvConfig() with initilization string. Axis type + * MUST be set before iocInit is called. + * 03 12-14-04 rls - MS Visual C compiler support. + * - eliminate calls to devConnectInterrupt() due to C++ + * problems with devLib.h; i.e. "sorry, not implemented: + * `tree_list' not supported..." compiler error message. + * 04 03-21-05 rls - Make MAXv OSI. + * 05 05-02-05 rls - Bug fix for stale data delay; set delay = 10ms. + * 06 05-17-06 rls - Allow polling rate up to 1/epicsThreadSleepQuantum(). + * - Protect against multiple MAXvSetup() calls. + * 07 06-05-07 rls - Added Jens Eden (BESSY) modifications; + * - register iocsh commands. + * - added USE_DEVLIB with RTEMS conditionial. + * - replaced errlogPrintf calls in ISR with + * epicsInterruptContextMessage calls. + * 08 08-20-07 rls - Make send_mess() and recv_mess() non-global. + * - removed unneeded stub start_status(). + * 09 02-26-08 rls - set "update delay" to zero. + * 10 05-14-08 rls - read the commanded velocity. + * 11 05-20-08 rls - A24/A32 address mode bug fix. + * 12 01-05-09 rls - Dirk Zimoch's (PSI) bug fix for set_status() overwriting + * the home switch status in the response string. + * 13 06-18-09 rls - Make MAXvSetup() error messages more prominent. + * 14 07-02-09 rls - backwards compatibility with ver:1.29 and earlier + * firmware. OMS changed from '' to '' for + * RA, QA, EA and RL command with ver:1.30 + * 15 09-09-09 rls - board "running" error check added. + * 16 03-08-10 rls - sprintf() not callable from RTEMS interrupt context. + * 17 03-09-10 rls - sprintf() not callable from any OS ISR. + * 18 06-01-10 rls - Save firmware version in static float array. + * - For firmware ver:1.33 and above, read Watchdog Timeout + * Counter. If Counter is nonzero, print error message and + * clear Counter. + * 19 06-07-10 rls - Disable board if WDT CTR is nonzero; don't clear CTR. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "motorRecord.h" /* For Driver Power Monitor feature only. */ +#include "motor.h" +#include "motordevCom.h" /* For Driver Power Monitor feature only. */ +#include "drvMAXv.h" + +#include "epicsExport.h" + +struct MAXv_DevicePrivate { + epicsMutexId message_mutex; +}; +#define MUTEX(card) ((struct MAXv_DevicePrivate *)(motor_state[card]->DevicePrivate))->message_mutex + +/* Define for return test on devNoResponseProbe() */ +#define PROBE_SUCCESS(STATUS) ((STATUS)==S_dev_addressOverlap) + +/* Are we using VME-Bus/devLib */ +#if (defined(vxWorks) || defined(__rtems__)) + #define USE_DEVLIB +#endif + +/* jps: INFO messages - add RV and move QA to top */ +#define ALL_INFO "QA EA" +#define AXIS_INFO "QA" +#define ENCODER_QUERY "EA ID" +#define AXIS_CLEAR "CA" /* Clear done of addressed axis */ +#define DONE_QUERY "RA" /* ?? Is this needed?? */ +#define PID_QUERY "?KA ID" + + +/*----------------debugging-----------------*/ +#ifdef __GNUG__ + #ifdef DEBUG + #define Debug(l, f, args...) {if (l <= drvMAXvdebug) \ + errlogPrintf(f, ## args);} + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug +#endif +volatile int drvMAXvdebug = 0; +extern "C" {epicsExportAddress(int, drvMAXvdebug);} + +#define pack2x16(p) ((epicsUInt32)(((p[0])<<16)|(p[1]))) +#define INITSTR_SIZE 300 /* 300 byte configuration string. */ + +/* Global data. */ +int MAXv_num_cards = 0; + +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + +/* --- Local data common to all OMS drivers. --- */ +static char *MAXv_addrs = 0x0; +static epicsAddressType MAXv_ADDRS_TYPE; +static volatile unsigned MAXvInterruptVector = 0; +static volatile epicsUInt8 omsInterruptLevel = OMS_INT_LEVEL; +static volatile int motionTO = 10; +static char *MAXv_axis[] = {"X", "Y", "Z", "T", "U", "V", "R", "S"}; +static double quantum; +static char **initstring = 0; +static epicsUInt32 MAXv_brd_size; /* card address boundary */ + +/*----------------functions-----------------*/ + +/* Common local function declarations. */ +extern "C" { +RTN_STATUS MAXvSetup(int, int, unsigned int, unsigned int, int, int); +RTN_VALUES MAXvConfig(int, const char *); +} + +static long report(int); +static long init(); +static void query_done(int, int, struct mess_node *); +static int set_status(int, int); +static RTN_STATUS send_mess(int, char const *, char *); +static int recv_mess(int, char *, int); +static int getPositions(int card, epicsInt32 *positions, int nPositions); +static int send_recv_mess(int card, char const * command, char *axis, char *buf, int nMessages); + +extern "C" { + +int MAXV_send_mess(int card, char const * command, char *axis) { + return (int)send_mess(card, command, axis); +} + +int MAXV_recv_mess(int card, char *buf, int nMessages) { + return recv_mess(card, buf, nMessages); +} + +int MAXV_send_recv_mess(int card, char const * command, char *axis, char *buf, int nMessages) { + return send_recv_mess(card, command, axis, buf, nMessages); +} + +int MAXV_getPositions(int card, epicsInt32 *positions, int nPositions) { + return getPositions(card, positions, nPositions); +} + +} + +static int getPositions(int card, epicsInt32 *positions, int nPositions) { + volatile struct MAXv_motor *pmotor; + pmotor = (struct MAXv_motor *) motor_state[card]->localaddr; + int i; + for (i=0; icmndPos[i]; + Debug(5, "getPositions: motor %d position=%d\n", i, positions[i]); + } + return(0); +} + +static void motorIsr(int); +static int motor_init(); +static void MAXv_reset(void *); +static char *readbuf(volatile struct MAXv_motor *, char *); + +static int motorIsrSetup(int card); + +struct driver_table MAXv_access = +{ + NULL, + motor_send, + motor_free, + motor_card_info, + motor_axis_info, + &mess_queue, + &queue_lock, + &free_list, + &freelist_lock, + &motor_sem, + &motor_state, + &total_cards, + &any_motor_in_motion, + send_mess, + recv_mess, + set_status, + query_done, + NULL, + &initialized, + MAXv_axis +}; + +struct drvMAXv_drvet +{ + long number; + long (*report) (int); + long (*init) (void); +} drvMAXv = {2, report, init}; + +extern "C" {epicsExportAddress(drvet, drvMAXv);} + +static struct thread_args targs = {SCAN_RATE, &MAXv_access, 0.000}; + +static struct MAXvbrdinfo /* MAXv board info. */ +{ + float fwver[MAXv_NUM_CARDS]; /* firmware version */ +} MAXvdata; + +#define DISABLE_ON_WD_CTR 0 +#if DISABLE_ON_WD_CTR +static char wdctrmsg[] = "\n***MAXv card #%d Disabled*** Watchdog Timeout CTR %s\n\n"; +#else +static char wdctrmsg[] = "\n***MAXv card #%d Error*** Watchdog Timeout CTR %s\n\n"; +#endif +static char norunmsg[] = "\n*** MAXv card #%d is NOT running *** status = 0x%x\n"; + +/*----------------functions-----------------*/ + +static long report(int level) +{ + int card; + + if (MAXv_num_cards <= 0) + printf(" No MAXv controllers configured.\n"); + else + { + for (card = 0; card < MAXv_num_cards; card++) + { + struct controller *brdptr = motor_state[card]; + + if (brdptr == NULL) + printf(" Oms MAXv motor card #%d not found.\n", card); + else + printf(" Oms MAXv motor card #%d @ %p, id: %s \n", card, + motor_state[card]->localaddr, motor_state[card]->ident); + } + } + return (0); +} + +static long init() +{ + initialized = true; /* Indicate that driver is initialized. */ + (void) motor_init(); + return ((long) 0); +} + + +static void query_done(int card, int axis, struct mess_node *nodeptr) +{ + char buffer[MAX_IDENT_LEN]; + + /*send_mess(card, DONE_QUERY, MAXv_axis[axis]); recv_mess(card, buffer, 1);*/ + send_recv_mess(card, DONE_QUERY, MAXv_axis[axis], buffer, 1); + + if (nodeptr->status.Bits.RA_PROBLEM) + send_mess(card, AXIS_STOP, MAXv_axis[axis]); +} + + +/****************************************************************************** +* FUNCTION NAME: set_status +* +* ARGUMETS Type I/O Description +* -------- ---- --- ----------- +* +* card int I Controller card index #. +* signal int I Motor index. +* +* LOGIC +* Initialize. +* IF encoder present. +* Get axis and encoder information. +* ELSE +* Get axis information. +* ENDIF +* +* Process controller response strings. +* +* ... +* ... +* IF "motor-in-motion" (i.e., nodeptr != 0), AND, no limit switch error. +* IF drive power monitoring enabled. +* ... +* ENDIF +* ENDIF +* +* IF no motion, OR, status indicates limit switch error, OR, motor done, OR, +* controller problem. +* Set return state to "callback record". +* ELSE +* Set return state to skip "record callback". +* ENDIF +* IF status indicates DONE/LIMIT, AND, "motor-in-motion" (nodeptr != 0), AND, +* post-move message is not null. +* Send post-move message to controller. +* Clear post-move message pointer. +* ENDIF +* EXIT with return state indicator. +******************************************************************************/ + +static int set_status(int card, int signal) +{ + struct mess_info *motor_info; + struct mess_node *nodeptr; + volatile struct MAXv_motor *pmotor; + epicsInt32 motorData; + /* Message parsing variables */ + char *p, *tok_save; + struct axis_status *ax_stat; + struct encoder_status *en_stat; + char q_buf[MAX_IDENT_LEN], outbuf[50]; + int index; + bool ls_active = false; + bool got_encoder; + msta_field status; + + int rtn_state; + + motor_info = &(motor_state[card]->motor_info[signal]); + nodeptr = motor_info->motor_motion; + pmotor = (struct MAXv_motor *) motor_state[card]->localaddr; + status.All = motor_info->status.All; + + if (MAXvdata.fwver[card] >= 1.33) + { + /*send_mess(card, "#WS", (char) NULL); recv_mess(card, q_buf, 1);*/ + send_recv_mess(card, "#WS", (char) NULL, q_buf, 1); + if (strcmp(q_buf, "=0") != 0) + { + errlogPrintf(wdctrmsg, card, q_buf); +#if DISABLE_ON_WD_CTR + status.Bits.RA_PROBLEM = 1; + motor_info->status.All = status.All; + send_mess(card, STOP_ALL, (char) NULL); + /* Disable board. */ + motor_state[card] = (struct controller *) NULL; + return(rtn_state = 1); /* End move. */ +#else + send_mess(card, "#WC", (char) NULL); +#endif + } + } + + if (motor_info->encoder_present == YES) + { + /* get 4 pieces of info from axis */ + /* send_mess(card, ALL_INFO, MAXv_axis[signal]); recv_mess(card, q_buf, 2);*/ + send_recv_mess(card, ALL_INFO, MAXv_axis[signal], q_buf, 2); + got_encoder = true; + } + else + { + /* get 2 pieces of info from axis */ + /* send_mess(card, AXIS_INFO, MAXv_axis[signal]); recv_mess(card, q_buf, 1);*/ + send_recv_mess(card, AXIS_INFO, MAXv_axis[signal], q_buf, 1); + got_encoder = false; + } + + for (index = 0, p = epicsStrtok_r(q_buf, ",", &tok_save); p; + p = epicsStrtok_r(NULL, ",", &tok_save), index++) + { + switch (index) + { + case 0: /* axis status */ + ax_stat = (struct axis_status *) p; + + status.Bits.RA_DIRECTION = (ax_stat->direction == 'P') ? 1 : 0; + status.Bits.RA_HOME = (ax_stat->home == 'H') ? 1 : 0; + status.Bits.RA_DONE = (ax_stat->done == 'D') ? 1 : 0; + + if (ax_stat->overtravel == 'L') + { + ls_active = true; + if (status.Bits.RA_DIRECTION) + status.Bits.RA_PLUS_LS = 1; + else + status.Bits.RA_MINUS_LS = 1; + } + else + { + ls_active = false; + status.Bits.RA_PLUS_LS = 0; + status.Bits.RA_MINUS_LS = 0; + } + break; + + case 1: /* encoder status */ + en_stat = (struct encoder_status *) p; + + status.Bits.EA_SLIP = (en_stat->slip_enable == 'E') ? 1 : 0; + status.Bits.EA_POSITION = (en_stat->pos_enable == 'E') ? 1 : 0; + status.Bits.EA_SLIP_STALL = (en_stat->slip_detect == 'S') ? 1 : 0; + status.Bits.EA_HOME = (en_stat->axis_home == 'H') ? 1 : 0; + break; + + default: + break; + } + } + + /* motor pulse count (position) */ + motorData = pmotor->cmndPos[signal]; + + if (motorData == motor_info->position) + { + if (nodeptr != 0) /* Increment counter only if motor is moving. */ + motor_info->no_motion_count++; + } + else + { + motor_info->no_motion_count = 0; + motor_info->position = motorData; + } + + if (motor_info->no_motion_count > motionTO) + { + status.Bits.RA_PROBLEM = 1; + send_mess(card, AXIS_STOP, MAXv_axis[signal]); + motor_info->no_motion_count = 0; + errlogSevPrintf(errlogMinor, "Motor motion timeout ERROR on card: %d, signal: %d\n", + card, signal); + } + else if (pmotor->firmware_status.Bits.running == 0) + { + status.Bits.RA_PROBLEM = 1; + errlogPrintf(norunmsg, card, (unsigned int) pmotor->firmware_status.All); + } + else + status.Bits.RA_PROBLEM = 0; + + /* get command velocity */ + /*send_mess(card, "RV", MAXv_axis[signal]); recv_mess(card, q_buf, 1);*/ + send_recv_mess(card, "RV", MAXv_axis[signal], q_buf, 1); + motor_info->velocity = atoi(q_buf); + + /* Get encoder position */ + motorData = pmotor->encPos[signal]; + + motor_info->encoder_position = motorData; + + if (nodeptr != NULL && ls_active == false) + { + struct motor_trans *trans = (struct motor_trans *) nodeptr->mrecord->dpvt; + if (trans->dpm == true) + { + errlogPrintf("Drive power failure at MAXv card#%d motor#%d\n", + card, signal); + } + } + + rtn_state = (!motor_info->no_motion_count || ls_active == true || + status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; + + /* Test for post-move string. */ + if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && + nodeptr->postmsgptr != 0) + { + char buffer[40]; + + /* Test for a "device directive" in the POST string. */ + if (nodeptr->postmsgptr[0] == '@') + { + bool errind = false; + char *end = strchr(&nodeptr->postmsgptr[1], '@'); + if (end == NULL) + errind = true; + else + { + DBADDR addr; + char *start, *tail; + int size = (end - &nodeptr->postmsgptr[0]) + 1; + + /* Copy device directive to buffer. */ + strncpy(buffer, nodeptr->postmsgptr, size); + buffer[size] = (char) NULL; + + if (strncmp(buffer, "@PUT(", 5) != 0) + goto errorexit; + + /* Point "start" to PV name argument. */ + tail = NULL; + start = epicsStrtok_r(&buffer[5], ",", &tail); + if (tail == NULL) + goto errorexit; + + if (dbNameToAddr(start, &addr)) /* Get address of PV. */ + { + errPrintf(-1, __FILE__, __LINE__, "Invalid PV name: %s", start); + goto errorexit; + } + + /* Point "start" to PV value argument. */ + start = epicsStrtok_r(NULL, ")", &tail); + if (dbPutField(&addr, DBR_STRING, start, 1L)) + { + errPrintf(-1, __FILE__, __LINE__, "invalid value: %s", start); + goto errorexit; + } + } + + if (errind == true) +errorexit: errMessage(-1, "Invalid device directive"); + end++; + strcpy(buffer, end); + } + else + strcpy(buffer, nodeptr->postmsgptr); + + strcpy(outbuf, buffer); + send_mess(card, outbuf, MAXv_axis[signal]); + nodeptr->postmsgptr = NULL; + } + + /* Bug fix for DC servo moving away from limit switch, but move is not far enough to + * get off limit switch; resulting in limit error. Fix is to force CDIR to match + * MSTA.RA_DIRECTION. + */ + if (ls_active == true && status.Bits.GAIN_SUPPORT && + status.Bits.EA_POSITION == 0 && nodeptr != 0) + { + struct motorRecord *mr = (struct motorRecord *) nodeptr->mrecord; + + if (mr->cdir != (short) status.Bits.RA_DIRECTION) + mr->cdir = status.Bits.RA_DIRECTION; + } + + motor_info->status.All = status.All; /* Update status from local copy. */ + return (rtn_state); +} + +/************************************************** + * send a message to the OMS board and get the reply + **************************************************/ +static int send_recv_mess(int card, char const * command, char *axis, char *buf, int nMessages) { + int retval; + + if (!epicsMutexTryLock(MUTEX(card))) { + Debug(1, "send_recv_mess: waiting for mutex\n"); + epicsMutexLock(MUTEX(card)); + } + retval = (int)send_mess(card, command, axis); + retval |= recv_mess(card, buf, nMessages); + epicsMutexUnlock(MUTEX(card)); + return(retval); +} + +/*****************************************************/ +/* send a message to the OMS board */ +/* send_mess() */ +/*****************************************************/ +static RTN_STATUS send_mess(int card, char const *com, char *name) +{ + volatile struct MAXv_motor *pmotor; + epicsInt16 putIndex; + char outbuf[MAX_MSG_SIZE], *p; + RTN_STATUS return_code; + int count; + + if (strlen(com) > MAX_MSG_SIZE) + { + errlogPrintf("drvMAXv.cc:send_mess(); message size violation.\n"); + return (ERROR); + } + + /* Check that card exists */ + if (!motor_state[card]) + { + errlogPrintf("drvMAXv.cc:send_mess() - invalid card #%d\n", card); + return (ERROR); + } + + pmotor = (struct MAXv_motor *) motor_state[card]->localaddr; + Debug(9, "send_mess: pmotor = %p\n", pmotor); + + if (!epicsMutexTryLock(MUTEX(card))) { + Debug(1, "send_mess: waiting for mutex\n"); + epicsMutexLock(MUTEX(card)); + } + + return_code = OK; + + Debug(9, "send_mess: checking card %d status\n", card); + + /* see if junk at input port - should not be any data available */ + if (pmotor->inGetIndex != pmotor->inPutIndex) + { + Debug(1, "send_mess - clearing data in buffer\n"); + recv_mess(card, outbuf, FLUSH); + /*recv_mess(card, NULL, -1);*/ + } + + + if (name == NULL) + strcpy(outbuf, com); + else + { + strcpy(outbuf, "A"); + strcat(outbuf, name); + strcat(outbuf, " "); + strcat(outbuf, com); + } + + Debug(9, "send_mess: ready to send message.\n"); + putIndex = pmotor->outPutIndex; + for (p = outbuf; *p != '\0'; p++) + { + pmotor->outBuffer[putIndex++] = *p; + if (putIndex >= BUFFER_SIZE) + putIndex = 0; + } + + Debug(4, "send_mess: sent card %d message:", card); + Debug(4, "%s\n", outbuf); + + pmotor->outPutIndex = putIndex; /* Message Sent */ + + for (count = 0; (pmotor->outPutIndex != pmotor->outGetIndex) && + (count < 1000); count++) + { +#ifdef DEBUG + epicsInt16 deltaIndex, delta; + + deltaIndex = pmotor->outPutIndex - pmotor->outGetIndex; + delta = (deltaIndex < 0) ? BUFFER_SIZE + deltaIndex : deltaIndex; + Debug(5, "send_mess: Waiting for ack: index delta=%d\n", delta); +#endif + epicsThreadSleep(epicsThreadSleepQuantum()); + }; + + if (count >= 1000) + { + errlogPrintf("\n*** MAXv card #%d communication timeout ***\n", card); + return_code = ERROR; + } + + epicsMutexUnlock(MUTEX(card)); + return (return_code); +} + +/* + * FUNCTION... recv_mess(int card, char *com, int amount) + * + * INPUT ARGUMENTS... + * card - + * *com - + * amount - + * + * LOGIC... + * IF controller card does not exist. + * ERROR Exit. + * ENDIF + * IF "amount" indicates buffer flush. + * WHILE characters left in input buffer. + * Remove characters from controller's input buffer. + * ENDWHILE + * NORMAL RETURN. + * ENDIF + * + * FOR each message requested (i.e. "amount"). + * Initialize head and tail pointers. + * Initialize local buffer "get" index. + * FOR + * IF characters left in controller's input buffer. + * + * ENDIF + * ENDFOR + * ENDFOR + * + */ +static int recv_mess(int card, char *com, int amount) +{ + volatile struct MAXv_motor *pmotor; + int itera; + char *bufptr; + + /* Check that card exists */ + if (!motor_state[card]) + { + Debug(1, "recv_mess - invalid card #%d\n", card); + return(-1); + } + + pmotor = (struct MAXv_motor *) motor_state[card]->localaddr; + + if (!epicsMutexTryLock(MUTEX(card))) { + Debug(1, "recv_mess: waiting for mutex\n"); + epicsMutexLock(MUTEX(card)); + } + + if (amount == -1) + { + if (pmotor->inGetIndex != pmotor->inPutIndex) + { + char junk[MAX_IDENT_LEN]; + + readbuf(pmotor, junk); + + Debug(1, "recv_mess(): flushed - %s\n", junk); + } + epicsMutexUnlock(MUTEX(card)); + return(0); + } + + bufptr = com; + *bufptr = (char) NULL; + + do + { + itera = 1; + double time = 0.0; + + while (pmotor->status1_flag.Bits.text_response == 0 && time < 0.100) + { + Debug(1, "recv_mess(): response wait - %d\n", itera); + time += quantum * itera; + epicsThreadSleep(quantum * itera); + itera++; + } + + if (pmotor->status1_flag.Bits.text_response == 0) + { + Debug(1, "Timeout occurred in recv_mess\n"); + *bufptr = '\0'; + epicsMutexUnlock(MUTEX(card)); + return(-1); + } + + bufptr = readbuf(pmotor, bufptr); + if (--amount > 0) + { + if (*(bufptr-1) == '\n') /* For ver:1.29 and before firmware, */ + *(bufptr-1) = ','; /* replace <0> with <,><0>. */ + else /* For ver:1.30 and after firmware, */ + *(bufptr++) = ','; /* replace with <,>. */ + } + } while (amount > 0); + + epicsMutexUnlock(MUTEX(card)); + Debug(4, "recv_mess(): card#%d - %s\n", card, com); + return(0); +} + +static char *readbuf(volatile struct MAXv_motor *pmotor, char *bufptr) +{ + STATUS1 flag1; + epicsUInt32 getIndex, putIndex; + int bufsize; + char *start, *end, *bufend; + + getIndex = pmotor->inGetIndex; + putIndex = pmotor->inPutIndex; + bufsize = putIndex - getIndex; + + start = (char *) &pmotor->inBuffer[getIndex]; + end = (char *) &pmotor->inBuffer[putIndex]; + + if (start < end) /* Test for message wraparound in buffer. */ + memcpy(bufptr, start, bufsize); + else if (start == end) /* Test for empty input buffer. */ + { + static char emptymsg[] = + "MAXv DPRAM input buffer empty; inGetIndex = %d inPutIndex = %d\n"; + + errlogPrintf(emptymsg, getIndex, putIndex); + return(bufptr); + } + else + { + int size; + + bufend = (char *) &pmotor->inBuffer[BUFFER_SIZE]; + size = bufend - start; + bufsize += BUFFER_SIZE; + + if (bufsize > MAX_IDENT_LEN) + { + errlogPrintf("\n*** MAXv readbuf() overrun ***; bufsize = %d\n\n", bufsize); + return(bufptr); + } + else + { + memcpy(bufptr, start, size); + memcpy((bufptr + size), (const char *) &pmotor->inBuffer[0], (bufsize - size)); + } + } + + getIndex += bufsize; + if (getIndex >= BUFFER_SIZE) + getIndex -= BUFFER_SIZE; + + bufptr += (bufsize - 1); + *bufptr = (char) NULL; + + while (getIndex != pmotor->inPutIndex) + { + Debug(1, "readbuf(): flushed - %d\n", pmotor->inBuffer[getIndex]); + if (++getIndex > BUFFER_SIZE) + getIndex = 0; + } + + pmotor->inGetIndex = getIndex; + flag1.All = pmotor->status1_flag.All; + pmotor->status1_flag.All = flag1.All; + pmotor->msg_semaphore = 0; + return(bufptr); +} + +/*****************************************************/ +/* Configuration function for module_types data */ +/* areas. MAXvSetup() */ +/*****************************************************/ +RTN_STATUS +MAXvSetup(int num_cards, /* maximum number of cards in rack */ + int addrs_type, /* VME address type; 16 - A16, 24 - A24 or 32 - A32. */ + unsigned int addrs, /* Base Address. */ + unsigned int vector, /* noninterrupting(0), valid vectors(64-255) */ + int int_level, /* interrupt level (1-6) */ + int scan_rate) /* 1 <= polling rate <= (1/epicsThreadSleepQuantum) */ +{ + int itera; + char **strptr; + RTN_STATUS rtncode = OK; + double frequency; + char errbase[] = "\nMAXvSetup: *** invalid "; + + if (initstring == NULL) + initstring = (char **) callocMustSucceed(1, + sizeof(char *) * MAXv_NUM_CARDS, "MAXvSetup() initstring"); + else + { + /* Deallocate memory for initialization strings. */ + for (itera = 0, strptr = &initstring[0]; itera < MAXv_num_cards; itera++, strptr++) + free(*strptr); + } + + if (num_cards < 1 || num_cards > MAXv_NUM_CARDS) + { + char format[] = "%snumber of cards specified = %d ***\n"; + MAXv_num_cards = MAXv_NUM_CARDS; + errlogPrintf(format, errbase, num_cards); + errlogPrintf(" *** using maximum number = %d ***\n", MAXv_NUM_CARDS); + epicsThreadSleep(5.0); + rtncode = ERROR; + } + else + MAXv_num_cards = num_cards; + + { + char addmsg[] = "%sA%d address *** = 0x%X.\n"; + switch (addrs_type) + { + case 16: + MAXv_ADDRS_TYPE = atVMEA16; + if ((epicsUInt32) addrs & 0xFFFF0FFF) + { + errlogPrintf(addmsg, errbase, 16, (epicsUInt32) addrs); + rtncode = ERROR; + } + else + { + MAXv_addrs = (char *) addrs; + MAXv_brd_size = 0x1000; + } + break; + case 24: + MAXv_ADDRS_TYPE = atVMEA24; + if ((epicsUInt32) addrs & 0xFF00FFFF) + { + errlogPrintf(addmsg, errbase, 24, (epicsUInt32) addrs); + rtncode = ERROR; + } + else + { + MAXv_addrs = (char *) addrs; + MAXv_brd_size = 0x10000; + } + break; + case 32: + MAXv_ADDRS_TYPE = atVMEA32; + if ((epicsUInt32) addrs & 0x00FFFFFF) + { + errlogPrintf(addmsg, errbase, 32, (epicsUInt32) addrs); + rtncode = ERROR; + } + else + { + MAXv_addrs = (char *) addrs; + MAXv_brd_size = 0x1000000; + } + break; + default: + { + char format[] = "%sVME address type = %d ***\n"; + errlogPrintf(format, errbase, addrs_type); + } + rtncode = ERROR; + break; + } + } + + if (rtncode == ERROR) + epicsThreadSleep(5.0); + + MAXvInterruptVector = vector; + if (vector < 64 || vector > 255) + { + if (vector != 0) + { + char format[] = "%sinterrupt vector = %d ***\n"; + MAXvInterruptVector = (unsigned) OMS_INT_VECTOR; + errlogPrintf(format, errbase, vector); + epicsThreadSleep(5.0); + rtncode = ERROR; + } + } + + if (int_level < 1 || int_level > 6) + { + char format[] = "%sinterrupt level = %d ***\n"; + omsInterruptLevel = OMS_INT_LEVEL; + errlogPrintf(format, errbase, int_level); + epicsThreadSleep(5.0); + rtncode = ERROR; + } + else + omsInterruptLevel = int_level; + + quantum = epicsThreadSleepQuantum(); + frequency = 1.0 / quantum; + + /* Set motor polling task rate */ + if (scan_rate >= 1 && scan_rate <= frequency) + targs.motor_scan_rate = scan_rate; + else + { + char format[] = "%spolling rate = %d ***\n"; + targs.motor_scan_rate = (int) frequency; + errlogPrintf(format, errbase, scan_rate); + epicsThreadSleep(5.0); + rtncode = ERROR; + } + + /* Allocate memory for initialization strings. */ + for (itera = 0, strptr = &initstring[0]; itera < MAXv_num_cards; itera++, strptr++) + { + *strptr = (char *) malloc(INITSTR_SIZE); + **strptr = (char) NULL; + } + + return(rtncode); +} + +RTN_VALUES MAXvConfig(int card, /* number of card being configured */ + const char *initstr) /* configuration string */ +{ + if (card < 0 || card >= MAXv_num_cards) + { + errlogPrintf("MAXvConfig: invalid card %d\n", card); + epicsThreadSleep(5.0); + return(ERROR); + } + if (strlen(initstr) > INITSTR_SIZE) + { + errlogPrintf("\n*** MAXvConfig ERROR ***\n"); + errlogPrintf("Configuration string: %d bytes > %d maximum.\n\n", strlen(initstr), INITSTR_SIZE); + epicsThreadSleep(5.0); + return(ERROR); + } + + strcpy(initstring[card], initstr); + return(OK); +} + + +/*****************************************************/ +/* Interrupt service routine. */ +/* motorIsr() */ +/*****************************************************/ +static void motorIsr(int card) +{ + volatile struct controller *pmotorState; + volatile struct MAXv_motor *pmotor; + STATUS1 status1_flag; + static char errmsg1[] = "\ndrvMAXv.cc:motorIsr: Invalid entry - card xx\n"; + static char errmsg2[] = "\ndrvMAXv.cc:motorIsr: command error - card xx\n"; + + if (card >= total_cards || (pmotorState = motor_state[card]) == NULL) + { + errmsg1[46-2] = '0' + card%10; + errmsg1[46-3] = '0' + (card/10)%10; + epicsInterruptContextMessage(errmsg1); + return; + } + + pmotor = (struct MAXv_motor *) (pmotorState->localaddr); + status1_flag.All = pmotor->status1_flag.All; + + /* Motion done handling */ + if (status1_flag.Bits.done != 0) + motor_sem.signal(); /* Wake up 'motor_task()' to issue callbacks */ + + if (status1_flag.Bits.cmndError) + { + errmsg2[46-2] = '0' + card%10; + errmsg2[46-3] = '0' + (card/10)%10; + epicsInterruptContextMessage(errmsg2); + } + + if (status1_flag.Bits.text_response != 0) /* Don't clear this. */ + status1_flag.Bits.text_response = 0; + + pmotor->status1_flag.All = status1_flag.All; /* Release IRQ's. */ +} + +static int motorIsrSetup(int card) +{ + volatile struct MAXv_motor *pmotor; + STATUS1 status1_irq; +#ifdef USE_DEVLIB + long status; +#endif + + Debug(5, "motorIsrSetup: Entry card#%d\n", card); + + pmotor = (struct MAXv_motor *) (motor_state[card]->localaddr); + +#ifdef USE_DEVLIB + + status = pdevLibVirtualOS->pDevConnectInterruptVME( + MAXvInterruptVector + card, +#if LT_EPICSBASE(3,14,8) + (void (*)()) motorIsr, +#else + (void (*)(void *)) motorIsr, +#endif + (void *) card); + + if (!RTN_SUCCESS(status)) + { + errPrintf(status, __FILE__, __LINE__, "Can't connect to vector %d\n", MAXvInterruptVector + card); + return (ERROR); + } + + status = devEnableInterruptLevel(OMS_INTERRUPT_TYPE, + omsInterruptLevel); + if (!RTN_SUCCESS(status)) + { + errPrintf(status, __FILE__, __LINE__, "Can't enable enterrupt level %d\n", omsInterruptLevel); + return (ERROR); + } + +#endif + + /* Setup card for interrupt-on-done */ + status1_irq.All = 0; + status1_irq.Bits.done = 0xFF; + status1_irq.Bits.cmndError = 1; + + pmotor->status1_irq_enable.All = status1_irq.All; /* Enable interrupts. */ + pmotor->status2_irq_enable = 0x0; + return (OK); +} + +/*****************************************************/ +/* initialize all software and hardware */ +/* motor_init() */ +/*****************************************************/ +static int motor_init() +{ + struct mess_info *motor_info; + volatile struct controller *pmotorState; + volatile struct MAXv_motor *pmotor; + long status=0; + int card_index, motor_index, itera; + char axis_pos[MAX_IDENT_LEN], encoder_pos[MAX_IDENT_LEN], **strptr; + char *tok_save, *pos_ptr; + int total_encoders = 0, total_axis = 0, total_pidcnt = 0; + volatile void *localaddr=0; + void *probeAddr; + + tok_save = NULL; + + /* Check for setup */ + if (MAXv_num_cards <= 0) + { + Debug(1, "\nmotor_init: MAXv driver disabled\nMAXvSetup() is missing from startup script.\n"); + return (ERROR); + } + + /* allocate space for total number of motors */ + motor_state = (struct controller **) malloc(MAXv_num_cards * + sizeof(struct controller *)); + + /* allocate structure space for each motor present */ + + total_cards = MAXv_num_cards; + + if (epicsAtExit(MAXv_reset, NULL) == ERROR) + Debug(1, "MAXv motor_init: MAXv_reset disabled\n"); + + for (card_index = 0; card_index < MAXv_num_cards; card_index++) + { + epicsInt8 *startAddr; + epicsInt8 *endAddr; + bool wdtrip; + + Debug(2, "motor_init: card %d\n", card_index); + + probeAddr = MAXv_addrs + (card_index * MAXv_brd_size); + startAddr = (epicsInt8 *) probeAddr; + endAddr = startAddr + MAXv_brd_size; + + Debug(9, "motor_init: devNoResponseProbe() on addr %p\n", probeAddr); + /* Scan memory space to assure card id */ +#ifdef USE_DEVLIB + do + { + status = devNoResponseProbe(MAXv_ADDRS_TYPE, (unsigned int) startAddr, 2); + startAddr += (MAXv_brd_size / 10); + } while (PROBE_SUCCESS(status) && startAddr < endAddr); +#endif + if (!PROBE_SUCCESS(status)) + { + Debug(3, "motor_init: Card NOT found!\n"); + motor_state[card_index] = (struct controller *) NULL; + goto loopend; + } + +#ifdef USE_DEVLIB + status = devRegisterAddress(__FILE__, MAXv_ADDRS_TYPE, + (size_t) probeAddr, MAXv_brd_size, + (volatile void **) &localaddr); + Debug(9, "motor_init: devRegisterAddress() status = %d\n", (int) status); + if (!RTN_SUCCESS(status)) + { + errPrintf(status, __FILE__, __LINE__, "Can't register address 0x%x\n", + (unsigned int) probeAddr); + motor_state[card_index] = (struct controller *) NULL; + goto loopend; + } +#endif + + Debug(9, "motor_init: localaddr = %p\n", localaddr); + pmotor = (struct MAXv_motor *) localaddr; + + if (pmotor->firmware_status.Bits.running == 0) + { + errlogPrintf(norunmsg, card_index, (unsigned int) pmotor->firmware_status.All); + motor_state[card_index] = (struct controller *) NULL; + goto loopend; + } + + Debug(9, "motor_init: malloc'ing motor_state\n"); + motor_state[card_index] = (struct controller *) malloc(sizeof(struct controller)); + pmotorState = motor_state[card_index]; + pmotorState->localaddr = (char *) localaddr; + pmotorState->motor_in_motion = 0; + pmotorState->cmnd_response = false; + + pmotorState->DevicePrivate = (void *) malloc(sizeof(struct MAXv_DevicePrivate)); + ((struct MAXv_DevicePrivate *)(pmotorState->DevicePrivate))->message_mutex = epicsMutexMustCreate(); + + if (MAXvInterruptVector == 0) + pmotor->IACK_vector = 0; + else + pmotor->IACK_vector = MAXvInterruptVector + card_index; + + pmotor->status1_flag.All = 0xFFFFFFFF; + pmotor->status2_flag = 0xFFFFFFFF; + /* Disable all interrupts */ + pmotor->status1_irq_enable.All = 0; + pmotor->status2_irq_enable = 0; + + send_mess(card_index, ERROR_CLEAR, (char) NULL); + send_mess(card_index, STOP_ALL, (char) NULL); + + /* send_mess(card_index, GET_IDENT, (char) NULL); recv_mess(card_index, (char *) pmotorState->ident, 1);*/ + send_recv_mess(card_index, GET_IDENT, (char) NULL, (char *) pmotorState->ident, 1); + Debug(3, "Identification = %s\n", pmotorState->ident); + + /* Save firmware version to static float array. */ + pos_ptr = strchr((char *)pmotorState->ident, ':'); + sscanf(++pos_ptr, "%f", &MAXvdata.fwver[card_index]); + + wdtrip = false; + + if (MAXvdata.fwver[card_index] >= 1.33) + { + /* send_mess(card_index, "#WS", (char) NULL); recv_mess(card_index, axis_pos, 1);*/ + send_recv_mess(card_index, "#WS", (char) NULL, axis_pos, 1); + if (strcmp(axis_pos, "=0") != 0) + { + errlogPrintf(wdctrmsg, card_index, axis_pos); + epicsThreadSleep(2.0); + motor_state[card_index] = (struct controller *) NULL; + wdtrip = true; + } + } + + if (wdtrip == false) + { + send_mess(card_index, initstring[card_index], (char) NULL); + + /*send_mess(card_index, ALL_POS, (char) NULL); recv_mess(card_index, axis_pos, 1);*/ + send_recv_mess(card_index, ALL_POS, (char) NULL, axis_pos, 1); + + for (total_axis = 0, pos_ptr = epicsStrtok_r(axis_pos, ",", &tok_save); + pos_ptr; pos_ptr = epicsStrtok_r(NULL, ",", &tok_save), total_axis++) + { + pmotorState->motor_info[total_axis].motor_motion = NULL; + pmotorState->motor_info[total_axis].status.All = 0; + } + + Debug(3, "motor_init: Total axis = %d\n", total_axis); + pmotorState->total_axis = total_axis; + + for (total_encoders = total_pidcnt = 0, motor_index = 0; motor_index < total_axis; motor_index++) + { + STATUS1 flag1; + + /* Test if motor has an encoder. */ + send_mess(card_index, ENCODER_QUERY, MAXv_axis[motor_index]); + while (!pmotor->status1_flag.Bits.done) /* Wait for command to complete. */ + epicsThreadSleep(quantum); + + if (pmotor->status1_flag.Bits.cmndError) + { + Debug(2, "motor_init: No encoder on axis %d\n", motor_index); + pmotorState->motor_info[motor_index].encoder_present = NO; + flag1.All = pmotor->status1_flag.All; /* Clear command error. */ + pmotor->status1_flag.All = flag1.All; + } + else + { + total_encoders++; + pmotorState->motor_info[motor_index].encoder_present = YES; + recv_mess(card_index, encoder_pos, 1); + } + + /* Test if motor has PID parameters. */ + send_mess(card_index, PID_QUERY, MAXv_axis[motor_index]); + while (!pmotor->status1_flag.Bits.done) /* Wait for command to complete. */ + epicsThreadSleep(quantum); + if (pmotor->status1_flag.Bits.cmndError) + { + Debug(2, "motor_init: No PID parameters on axis %d\n", motor_index); + pmotorState->motor_info[motor_index].pid_present = NO; + flag1.All = pmotor->status1_flag.All; /* Clear command error. */ + pmotor->status1_flag.All = flag1.All; + } + else + { + total_pidcnt++; + pmotorState->motor_info[motor_index].pid_present = YES; + recv_mess(card_index, encoder_pos, FLUSH); /* Flush response. */ + } + } + + /* Enable interrupt-when-done if selected */ + if (MAXvInterruptVector) + { + if (motorIsrSetup(card_index) == ERROR) + errMessage(-1, "Interrupts Disabled!\n"); + } + + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + motor_info = (struct mess_info *) &pmotorState->motor_info[motor_index]; + + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + + if (motor_info->encoder_present == YES) + motor_info->status.Bits.EA_PRESENT = 1; + if (motor_info->pid_present == YES) + motor_info->status.Bits.GAIN_SUPPORT = 1; + + set_status(card_index, motor_index); + /* Is this needed??? */ + /* send_mess(card_index, DONE_QUERY, MAXv_axis[motor_index]); + recv_mess(card_index, axis_pos, 1);*/ + send_recv_mess(card_index, DONE_QUERY, MAXv_axis[motor_index], axis_pos, 1); + } + + Debug(2, "motor_init: Init Address=%p\n", localaddr); + Debug(3, "motor_init: Total encoders = %d\n", total_encoders); + Debug(3, "motor_init: Total with PID = %d\n", total_pidcnt); + } +loopend:; + } + + any_motor_in_motion = 0; + + mess_queue.head = (struct mess_node *) NULL; + mess_queue.tail = (struct mess_node *) NULL; + + free_list.head = (struct mess_node *) NULL; + free_list.tail = (struct mess_node *) NULL; + + Debug(3, "Motors initialized\n"); + + epicsThreadCreate((char *) "MAXv_motor", epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); + + Debug(3, "Started motor_task\n"); + + /* Deallocate memory for configuration strings. */ + for (itera = 0, strptr = &initstring[0]; itera < MAXv_num_cards; itera++, strptr++) + free(*strptr); + free(initstring); + initstring = NULL; + + return (0); +} + + +/* Disables interrupts. Called on CTL X reboot. */ + +static void MAXv_reset(void *arg) +{ + short card; + volatile struct MAXv_motor *pmotor; + + for (card = 0; card < total_cards; card++) + { + if (motor_state[card] != NULL) + { + pmotor = (struct MAXv_motor *) motor_state[card]->localaddr; + pmotor->status1_irq_enable.All = 0; + } + } +} + +extern "C" +{ + +// Oms Setup arguments + static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; + static const iocshArg setupArg1 = {"VME address type", iocshArgInt}; + static const iocshArg setupArg2 = {"Base Address on 4K (0x1000) boundary", iocshArgInt}; + static const iocshArg setupArg3 = {"noninterrupting(0), valid vectors(64-255)", iocshArgInt}; + static const iocshArg setupArg4 = {"interrupt level (1-6)", iocshArgInt}; + static const iocshArg setupArg5 = {"polling rate - 1/60 sec units", iocshArgInt}; +// Oms Config arguments + static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; + static const iocshArg configArg1 = {"configuration string", iocshArgString}; + + static const iocshArg * const OmsSetupArgs[6] = {&setupArg0, &setupArg1, + &setupArg2, &setupArg3, &setupArg4, &setupArg5}; + static const iocshArg * const OmsConfigArgs[2] = {&configArg0, &configArg1}; + + static const iocshFuncDef setupMAXv = {"MAXvSetup", 6, OmsSetupArgs}; + + static const iocshFuncDef configMAXv = {"MAXvConfig", 2, OmsConfigArgs}; + + static void setupMAXvCallFunc(const iocshArgBuf *args) + { + MAXvSetup(args[0].ival, args[1].ival, args[2].ival, args[3].ival, args[4].ival, args[5].ival); + } + + static void configMAXvCallFunc(const iocshArgBuf *args) + { + MAXvConfig(args[0].ival, args[1].sval); + } + + static void OmsMAXvRegister(void) + { + iocshRegister(&setupMAXv, setupMAXvCallFunc); + iocshRegister(&configMAXv, configMAXvCallFunc); + } + + epicsExportRegistrar(OmsMAXvRegister); + +} // extern "C"