linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Change from RS232Controller to MultiChan

Browse Source 
r1711 | dcl | 2007-03-23 18:51:15 +1100 (Fri, 23 Mar 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-03-23 18:51:15 +11:00
parent 96b8a1aee6
commit f283ec6968
1 changed files with 368 additions and 194 deletions
Download Patch File Download Diff File Expand all files Collapse all files

562
site_ansto/motor_dmc2280.c
View File

@ -22,7 +22,7 @@
#include <sys/time.h>
#include <fortify.h>
#include <sics.h>
#include <rs232controller.h>
#include <multichan.h>
#include <nwatch.h>
#include <modriv.h>
#include <motor.h>
@ -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; i<retries; i++) {
status=readRS232(self->controller, 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; i<retries; i++) {
status=readRS232TillTerm(self->controller, 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);