diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 592462de..dc6fd667 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -67,7 +67,7 @@ typedef struct __MoDriv { /* DMC-2280 specific fields */ - prs232 controller; + pMultiChan mcc; pMotor pMot; /**< Points to logical motor object */ int errorCode; char *errorMsg; /**< Points to memory for error messages */ @@ -165,6 +165,22 @@ static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); +typedef struct __command Command, *pCommand; +typedef int (*CommandCallback)(pCommand pCmd); + +struct __command { + pMultiChan unit; + int cstate; + int lstate; + char* out_buf; + int out_len; + int out_idx; + char* inp_buf; + int inp_len; + int inp_idx; + CommandCallback func; + void* cntx; +}; /** \brief Convert axis speed in physical units to * motor speed in steps/sec. @@ -202,66 +218,242 @@ static int motDecel(pDMC2280Driv self, float axisDecel) { return decel; } -/** \brief Reads a single character from the DMC2280 controller. - * - * On failure it sets the errorCode field in the motor's data structure - * \param self (rw) provides access to the motor's data structure - * \param *reply (w) the character read from the controller - * \return - * - SUCCESS - * - FAILURE - * \see SUCCESS FAILURE - */ -static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) { - int i, status, retries=1, dataLen=1; - reply[0] = '\0'; - for (i=0; icontroller, reply, &dataLen); - switch (status) { - case 1: - return SUCCESS; - case TIMEOUT: - self->errorCode = status; - continue; - default: - self->errorCode = status; - return FAILURE; +static int DMC_Tx(void* ctx) +{ + int iRet = 1; + pCommand myCmd = (pCommand) ctx; + + if (myCmd) { + iRet = MultiChanWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len); + /* TODO handle errors */ + if (iRet < 0) { /* TODO: EOF */ + /* + iRet = MultiChanReconnect(myCmd->unit); + if (iRet == 0) + */ + return 0; } } - return FAILURE; + return 1; } -/** \brief Gets output from the DMC2280, the abstract motor code should - * handle retries if the request times out. - * - * Note: The timeout for readRS232TillTerm is set by DMC2280Connect - * \param self (rw) provides access to the motor's data structure - * \param *reply (w) the data from the DMC2280. - * \return - * - SUCCESS - * - FAILURE - * \see SUCCESS FAILURE +static int DMC_Rx(void* ctx, int rxchar) { + int iRet = 1; + pCommand myCmd = (pCommand) ctx; + + if (rxchar == MCC_TIMEOUT) { + /* handle command timeout */ + myCmd->inp_idx = MCC_TIMEOUT; + if (myCmd->func) + iRet = myCmd->func(myCmd); + free(myCmd->out_buf); + free(myCmd->inp_buf); + free(myCmd); + return MCC_POP_CMD; + } + + switch (myCmd->cstate) { + case 0: /* first character */ + if (rxchar == ':') { + /* normal prompt */ + myCmd->cstate = 99; + } + else if (rxchar == '?') { + /* error prompt, send TC1 ahead of any queued commands */ + iRet = MultiChanWrite(myCmd->unit, "TC1\r\n", 5); + myCmd->cstate = 1; + } + else { + /* normal data */ + myCmd->cstate = 1; + } + /* note fallthrough */ + case 1: /* receiving reply */ + if (myCmd->inp_idx < myCmd->inp_len) + myCmd->inp_buf[myCmd->inp_idx++] = rxchar; + if (rxchar == 0x0D) + myCmd->cstate = 2; + break; + case 2: /* received CR and looking for LF */ + if (myCmd->inp_idx < myCmd->inp_len) + myCmd->inp_buf[myCmd->inp_idx++] = rxchar; + if (rxchar == 0x0A) { + /* end of line */ +/* + myCmd->inp_idx -= 2; + myCmd->inp_buf[myCmd->inp_idx++] = rxchar; */ -static int DMC2280Receive(pDMC2280Driv self, char *reply) { - int i, status, retries=1, dataLen=255; - reply[0] = '\0'; - for (i=0; icontroller, reply, &dataLen); - switch (status) { - case 1: - if (self->debug) - SICSLogWrite(reply, eStatus); - return dataLen; - case TIMEOUT: - self->errorCode = status; - continue; - /* TODO case INCOMPLETE: */ + myCmd->cstate = 0; + } + else + myCmd->cstate = 1; + break; + } + if (myCmd->cstate == 99) { + myCmd->inp_buf[myCmd->inp_idx] = '\0'; + if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) { + int i; + SICSLogWrite("Line echo detected", eStatus); + for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) { + myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i]; + } + } + if (myCmd->func) + iRet = myCmd->func(myCmd); + else + iRet = 0; + myCmd->cstate = 0; + myCmd->inp_idx = 0; + } + if (iRet == 0) { /* end of command */ + free(myCmd->out_buf); + free(myCmd->inp_buf); + free(myCmd); + return MCC_POP_CMD; + } + return iRet; +} + +static int DMC_SendCmd(pMultiChan unit, + char* command, int cmd_len, + CommandCallback callback, void* context, int rsp_len) +{ + pCommand myCmd = NULL; + + assert(unit); + myCmd = (pCommand) malloc(sizeof(Command)); + assert(myCmd); + memset(myCmd, 0, sizeof(Command)); + myCmd->out_buf = (char*) malloc(cmd_len + 5); + memcpy(myCmd->out_buf, command, cmd_len); + myCmd->out_len = cmd_len; + if (myCmd->out_len < 2 || + myCmd->out_buf[myCmd->out_len - 1] != 0x0A || + myCmd->out_buf[myCmd->out_len - 2] != 0x0D) { + myCmd->out_buf[myCmd->out_len++] = 0x0D; + myCmd->out_buf[myCmd->out_len++] = 0x0A; + } + myCmd->out_buf[myCmd->out_len] = '\0'; + myCmd->func = callback; + myCmd->cntx = context; + if (rsp_len == 0) + myCmd->inp_buf = NULL; + else { + myCmd->inp_buf = malloc(rsp_len + 1); + memset(myCmd->inp_buf, 0, rsp_len + 1); + } + myCmd->inp_len = rsp_len; + myCmd->unit = unit; + return MultiChanEnque(unit, myCmd, DMC_Tx, DMC_Rx); +} + +static void DMC_Notify(void* context, int event) +{ + pDMC2280Driv self = (pDMC2280Driv) context; + char line[132]; + + switch (event) { + case MCC_DISCONNECT: + snprintf(line, 132, "Disconnect on Motor '%s'", self->name); + SICSLogWrite(line, eStatus); + /* TODO: disconnect */ + break; + case MCC_RECONNECT: + snprintf(line, 132, "Reconnect on Motor '%s'", self->name); + SICSLogWrite(line, eStatus); + /* TODO: reconnect */ + break; + } + return; +} + +typedef struct txn_s { + char* transReply; + int transWait; +} TXN, *pTXN; + +/** + * \brief SendCallback is the callback for the general command. + */ +static int SendCallback(pCommand pCmd) +{ + char* cmnd = pCmd->out_buf; + char* resp = pCmd->inp_buf; + int resp_len = pCmd->inp_idx; + pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; + + if (resp_len > 0) { + switch (resp[0]) { + case ':': + case ' ': + if (self->debug) { + SICSLogWrite(cmnd, eStatus); + SICSLogWrite(resp, eStatus); + } + break; + case '?': + strncpy(self->lastCmd, pCmd->out_buf, CMDLEN); + strncpy(self->dmc2280Error, &resp[1], CMDLEN); + SICSLogWrite(cmnd, eError); + SICSLogWrite(resp, eError); + self->errorCode = BADCMD; + break; default: - self->errorCode = status; - return FAILURE; + SICSLogWrite(cmnd, eError); + SICSLogWrite(resp, eError); + self->errorCode = BADUNKNOWN; + break; } } - return FAILURE; + else { + /* TODO: timeout */ + } + + return 0; +} + +/** + * \brief TransCallback is the callback for the general command transaction. + */ +static int TransCallback(pCommand pCmd) { + char* resp = pCmd->inp_buf; + int resp_len = pCmd->inp_idx; + pTXN self = (pTXN) pCmd->cntx; + + if (resp_len < 0) { + self->transReply[0] = '\0'; + self->transWait = -1; + } + else { + memcpy(self->transReply, resp, resp_len); + self->transReply[resp_len] = '\0'; + self->transWait = 0; + } + return 0; +} + +/*------------------------------------------------------------------------*/ +static int DMC_transact(pDMC2280Driv self, void *send, int sendLen, + void *reply, int replyLen) +{ + TXN txn; + assert(self); + txn.transReply = reply; + txn.transWait = 1; + DMC_SendCmd(self->mcc, + send, sendLen, + TransCallback, &txn, replyLen); + while (txn.transWait == 1) + TaskYield(pServ->pTasker); + if (txn.transWait < 0) + return txn.transWait; + return 1; +} + +static int DMC2280Queue(pDMC2280Driv self, char *cmd, CommandCallback cb) { + if (cb == NULL) + cb = SendCallback; + return DMC_SendCmd(self->mcc, cmd, strlen(cmd), cb, self, CMDLEN); } /** \brief Sends a DMC2280 command to the motor controller. @@ -283,53 +475,7 @@ static int DMC2280Receive(pDMC2280Driv self, char *reply) { ' ' for a valid command with a response (':' follows) */ static int DMC2280Send(pDMC2280Driv self, char *command) { - char cmdValid, reply[256]; - char *GetEMsg = "TC 1"; - int status; - - if ((self->lastCmd) != command) { - /*@-mayaliasunique@ this won't happen unless they overlap */ - strncpy(self->lastCmd, command, CMDLEN); - /*@+mayaliasunique@*/ - } - - if (self->debug) - SICSLogWrite(command, eStatus); - - /*@+matchanyintegral@ let size_t from strlen match int */ - status = writeRS232(self->controller, command, strlen(command)); - /*@-matchanyintegral@*/ - if (status != 1) { - self->errorCode = status; - return FAILURE; - } - - if (FAILURE == (status = DMC2280ReadChar(self, &cmdValid))) { - return FAILURE; - } else { - switch (cmdValid) { - case ':': - case ' ': - return SUCCESS; - case '?': - /*@+matchanyintegral@ let size_t from strlen match int */ - status = writeRS232(self->controller, GetEMsg, strlen(GetEMsg)); - /*@-matchanyintegral@*/ - if (status != 1) { - self->errorCode = status; - return FAILURE; - } - if (FAILURE == DMC2280Receive(self, reply)) - return HWFault; - strncpy(self->dmc2280Error, reply, CMDLEN); - SICSLogWrite(reply, eError); - self->errorCode = BADCMD; - return FAILURE; - default: - self->errorCode = BADUNKNOWN; - return FAILURE; - } - } + return DMC2280Queue(self, command, SendCallback); } /** @@ -343,10 +489,46 @@ static int DMC2280Send(pDMC2280Driv self, char *command) { static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { int status; - if (FAILURE == DMC2280Send(self, cmd)) - return FAILURE; - if (FAILURE == DMC2280Receive(self, reply)) + status = DMC_transact(self, cmd, strlen(cmd), reply, CMDLEN); + + if (status != 1) { + if (self->debug) + SICSLogWrite(cmd, eStatus); + if (status == -1) + self->errorCode = MOTCMDTMO; + else + self->errorCode = BADUNKNOWN; return FAILURE; + } + + switch (reply[0]) { + case ':': + if (self->debug) { + SICSLogWrite(cmd, eStatus); + SICSLogWrite(reply, eStatus); + } + return SUCCESS; + case ' ': + if (self->debug) { + SICSLogWrite(cmd, eStatus); + SICSLogWrite(reply, eStatus); + } + return SUCCESS; + case '?': + strncpy(self->lastCmd, cmd, CMDLEN); + strncpy(self->dmc2280Error, &reply[1], CMDLEN); + SICSLogWrite(cmd, eError); + SICSLogWrite(reply, eError); + self->errorCode = BADCMD; + return FAILURE; + default: + strncpy(self->lastCmd, cmd, CMDLEN); + strncpy(self->dmc2280Error, &reply[0], CMDLEN); + SICSLogWrite(cmd, eError); + SICSLogWrite(reply, eError); + self->errorCode = BADUNKNOWN; + return FAILURE; + } return OKOK; } @@ -561,14 +743,43 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ return OKOK; } +/** + * \brief process the airpad status response + */ +static int airpad_callback(pCommand pCmd) { + char* resp = pCmd->inp_buf; + int resp_len = pCmd->inp_idx; + pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; + + if (resp_len > 0) { + float fReply; + if (self->debug) + SICSLogWrite(resp, eStatus); + fReply = (float) atof(resp); + if (self->airpad_state == AIRPADS_RAISE && fReply > 0) { + int iRet; + self->airpad_state = AIRPADS_UP; + iRet = DMC2280RunCommon(self, self->fTarget); + if (iRet != OKOK) + self->errorCode = BADCUSHION; + return 0; + } + if (self->airpad_state == AIRPADS_LOWER && fReply == 0) { + self->airpad_state = AIRPADS_DOWN; + return 0; + } + } + else { + /* TODO: timeout */ + } + return 0; +} /** * \brief request the airpad status periodically */ static int airpad_timeout(void* ctx, int mode) { pDMC2280Driv self = (pDMC2280Driv) ctx; - char reply[CMDLEN]; - float fReply; self->airpad_timer = NULL; @@ -576,29 +787,6 @@ static int airpad_timeout(void* ctx, int mode) { self->airpad_state == AIRPADS_DOWN) return 0; - if (self->airpad_counter <= 0) { - self->errorCode = BADCUSHION; - self->airpad_state = AIRPADS_DOWN; - return 0; - } - if (FAILURE == DMC2280SendReceive(self, "MG APDONE", reply)) { - self->errorCode = BADCUSHION; - self->airpad_state = AIRPADS_DOWN; - return 0; - } - fReply = (float) atof(reply); - if (self->airpad_state == AIRPADS_RAISE && fReply > 0) { - int iRet; - self->airpad_state = AIRPADS_UP; - iRet = DMC2280RunCommon(self, self->fTarget); - if (iRet != OKOK) - self->errorCode = BADCUSHION; - return 0; - } - if (self->airpad_state == AIRPADS_LOWER && fReply == 0) { - self->airpad_state = AIRPADS_DOWN; - return 0; - } if (self->airpad_counter <= 0) { self->errorCode = BADCUSHION; self->airpad_state = AIRPADS_DOWN; @@ -607,6 +795,12 @@ static int airpad_timeout(void* ctx, int mode) { --self->airpad_counter; NetWatchRegisterTimer(&self->airpad_timer, 1000, airpad_timeout, self); + + if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) { + self->errorCode = BADCUSHION; + self->airpad_state = AIRPADS_DOWN; + return 0; + } return 0; } @@ -707,7 +901,7 @@ static int DMC2280Run(void *pData,float fValue){ self->time_settle_done.tv_sec = 0; /* - * Note: this will read the current position + * Note: this will read the current position which will block */ do { #if 1 @@ -1001,12 +1195,14 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ } *iCode = self->errorCode; switch(*iCode){ +#ifdef HAS_RS232 case NOTCONNECTED: - case TIMEOUT: case BADSEND: case BADMEMORY: case INCOMPLETE: - getRS232Error(*iCode, error, errLen); + strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */ + break; +#endif case MOTCMDTMO: strncpy(error, "Command Timeout", (size_t)errLen); break; @@ -1090,7 +1286,6 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ case BADADR: return MOTFAIL; case BADCMD: - //case TIMEOUT: case BADPAR: case BLOCKED: case MOTIONCONTROLOFF: @@ -1098,13 +1293,14 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ case MOTCMDTMO: return MOTFAIL; case POSFAULT: +#if HAS_RS232 case BADSEND: case TIMEOUT: case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */ case INCOMPLETE: return MOTREDO; case NOTCONNECTED: - initRS232(self->controller); +#endif return MOTREDO; break; default: @@ -1564,45 +1760,15 @@ static void KillDMC2280(/*@only@*/void *pData){ free(self->errorMsg); self->errorMsg = NULL; } + if (self->mcc) { + MultiChanDestroy(self->mcc); + self->mcc = NULL; + } /* Not required as performed in caller * free(self); */ return; } -/*@only@*/ prs232 createRS232(char *host, int iPort); -/** \brief Open a connection to the motor controller - * \param *pCon (r) connection object. - * \param *host (r) DMC2280 host address or name. - * \param port DMC2280 port number - * \return controller structure - */ -/*@null@*/ /*@only@*/ static prs232 DMC2280Connect(/*@dependent@*/SConnection *pCon, char *host, int port) { - prs232 controller=NULL; - char pError[ERRLEN]; - int msecTimeout = 5000; - - controller=createRS232(host,port); - if (controller==NULL) { - snprintf(pError, ERRLEN, - "ERROR: failed to create controller for %s at port %d", - host, port); - SCWrite(pCon,pError,eError); - return NULL; - } - if ( initRS232(controller) != 1) { - snprintf(pError, ERRLEN, - "ERROR: failed to connect to %s at port %d", - controller->pHost, controller->iPort); - SCWrite(pCon,pError,eError); - KillRS232(controller); - return NULL; - } - setRS232ReplyTerminator(controller,"&\r\n:"); - setRS232SendTerminator(controller,"\r\n"); - setRS232Timeout(controller, msecTimeout); - return controller; -} - /** \brief Create a driver for the DMC2280 Galil controller. * @@ -1632,7 +1798,6 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { char buffer[132]; char pError[ERRLEN]; char cmd[CMDLEN]; - int port; Tcl_Interp *interp; buffer[0]='\0'; @@ -1644,35 +1809,44 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { */ pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv)); if(NULL == pNew){ - (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver", - eError); + snprintf(pError, ERRLEN, "ERROR: no memory when creating DMC2280 motor '%s'", motor); + SCWrite(pCon, pError, eError); return NULL; } memset(pNew, 0, sizeof(DMC2280Driv)); - pNew->controller = NULL; - pNew->name = NULL; - pNew->errorCode = 0; - pNew->errorMsg = NULL; - pNew->lastCmd[0] = '\0'; - pNew->dmc2280Error[0] = '\0'; - pNew->absEncHome = 0; - pNew->cntsPerX = 0; - /* Get hostname and port from the list of named parameters */ - if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) { - KillDMC2280(pNew); - return NULL; - } - if (sscanf(pPtr,"%d",&port)==0) - port = getPortNum(pCon, pPtr); - if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) { - KillDMC2280(pNew); - return NULL; - } - strncpy(buffer,pPtr, 131); - /* Connect to the controller */ - pNew->controller = DMC2280Connect(pCon, buffer,port); - if( pNew->controller == NULL ) { + /* Get multichan from the list of named parameters */ + if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL) { + /* MultiChan */ + if (!MultiChanCreate(pPtr, &pNew->mcc)) { + snprintf(pError, ERRLEN, "Cannot find MultiChan '%s' when creating DMC2280 motor '%s'", + pPtr, motor); + SCWrite(pCon,pError,eError); + KillDMC2280(pNew); + return NULL; + } + MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify); + } + else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) { + char* host = pPtr; + if ((pPtr=getParam(pCon, interp, params,"port",_REQUIRED)) == NULL) { + snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); + SCWrite(pCon,pError,eError); + KillDMC2280(pNew); + return NULL; + } + /* MultiChan */ + if (!MultiChanCreateHost(host, pPtr, &pNew->mcc)) { + snprintf(pError, ERRLEN, + "Cannot create MultiChan '%s:%s' for DMC2280 motor '%s'", + host, pPtr, motor); + SCWrite(pCon,pError,eError); + KillDMC2280(pNew); + return NULL; + } + MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify); + } + else { snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew);