diff --git a/site_ansto/safetyplc.c b/site_ansto/safetyplc.c new file mode 100644 index 00000000..a6da0cae --- /dev/null +++ b/site_ansto/safetyplc.c @@ -0,0 +1,299 @@ +/* + * S A F E T Y P L C + * + * Douglas Clowes, February 2007 + * + */ + +#include +#include +#include "network.h" +#include "multichan.h" +#include "nwatch.h" +#include "safetyplc.h" + +extern int DMC2280MotionControl; + +typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController; + +struct __SafetyPLCController { + pObjectDescriptor pDes; + pMultiChan mcc; /* associated MultiChan object */ + int iGetOut; + int iValue; + pNWTimer nw_tmr; /* NetWait timer handle */ +}; + +typedef struct __command Command, *pCommand; +typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len); + +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; +}; + +static int PLC_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 1; +} + +static int PLC_Rx(void* ctx, int rxchar) +{ + int iRet = 1; + pCommand myCmd = (pCommand) ctx; + + switch (myCmd->cstate) { + case 0: /* first character */ + /* 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) { + myCmd->cstate = 99; + /* end of line */ + } + else + myCmd->cstate = 1; + break; + } + if (myCmd->cstate == 99) { + myCmd->inp_buf[myCmd->inp_idx] = '\0'; + if (myCmd->func) + iRet = myCmd->func(myCmd->cntx, myCmd->inp_buf, myCmd->inp_idx); + 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 iRet; +} + +int PLC_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, PLC_Tx, PLC_Rx); +} + +static void PLC_Notify(void* context, int event) +{ + pSafetyPLCController self = (pSafetyPLCController) context; + + switch (event) { + case MCC_RECONNECT: + do { + mkChannel* sock = MultiChanGetSocket(self->mcc); + int flag = 1; + setsockopt(sock->sockid, /* socket affected */ + IPPROTO_TCP, /* set option at TCP level */ + TCP_NODELAY, /* name of option */ + (char *) &flag, /* the cast is historical cruft */ + sizeof(int)); /* length of option value */ + return; + } while (0); + } + return; +} + +/* + * \brief GetCallback is the callback for the read command. + */ +static int GetCallback(void* ctx, const char* resp, int resp_len) +{ + int iRet; + int iRead; + pSafetyPLCController self = (pSafetyPLCController) ctx; + iRet = sscanf(resp,"READ %0xd",&iRead); + if(iRet != 1) { // Not a number, probably an error response + self->iValue = 0; + } + else { + if (iRead < 0) + iRead = -iRead; + self->iValue = iRead; + } + if ((self->iValue & (1 << 6))) + DMC2280MotionControl = 1; + else + DMC2280MotionControl = 0; + + self->iGetOut = 0; + return 0; +} + +static int MyTimerCallback(void* context, int mode) +{ + pSafetyPLCController self = (pSafetyPLCController) context; + if (self->iGetOut) { + /* TODO error handling */ + DMC2280MotionControl = 0; + } + PLC_SendCmd(self->mcc, "READ", 4, + GetCallback, self, 132); + return 1; +} + +static int PLC_Action(SConnection *pCon, SicsInterp *pSics, + void *pData, int argc, char *argv[]) +{ + char line[132]; + pSafetyPLCController self = (pSafetyPLCController) pData; + if (argc == 1) { + snprintf(line, 132, "%s.iValue = %006x", argv[0], self->iValue & 0xffffff); + SCWrite(pCon, line, eStatus); + return OKOK; + } else if (argc == 2) { + if (strcmp(argv[1], "motioncontrol") == 0) { + snprintf(line, 132, "%s.MotionControl = %d", argv[0], DMC2280MotionControl); + SCWrite(pCon, line, eStatus); + return OKOK; + } + } + /* TODO: handle private stuff */ + return MultiChanAction(pCon, pSics, self->mcc, argc, argv); +} + +static pSafetyPLCController PLC_Create(const char* pName, int port) +{ + int i; + pSafetyPLCController self = NULL; + pMultiChan ctlr = NULL; + + self = (pSafetyPLCController) malloc(sizeof(SafetyPLCController)); + if (self == NULL) + return NULL; + memset(self, 0, sizeof(SafetyPLCController)); + if (MultiChanCreate(pName, &self->mcc) == 0) { + free(self); + return NULL; + } + MultiChanSetNotify(self->mcc, self, PLC_Notify); + + self->pDes = CreateDescriptor("SafetyPLC"); + return self; +} + +static int PLC_Init(pSafetyPLCController self) +{ + /* TODO: Init the controller */ + if (self->nw_tmr == NULL) + NetWatchRegisterTimerPeriodic(&self->nw_tmr, + 1000, 1000, + MyTimerCallback, + self); + return 1; +} + +static void PLC_Kill(pSafetyPLCController self) +{ + if (self->nw_tmr) + NetWatchRemoveTimer(self->nw_tmr); + free(self); + return; +} + +int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics, + void *pData, int argc, char *argv[]) +{ + pSafetyPLCController pNew = NULL; + int iRet, status; + char pError[256]; + + if(argc < 4) + { + SCWrite(pCon,"ERROR: insufficient no of arguments to SafetyPLCFactory", + eError); + return 0; + } + + /* + create data structure and open port + */ + pNew = PLC_Create(argv[2], atoi(argv[3])); + + if(!pNew) + { + SCWrite(pCon,"ERROR: failed to create SafetyPLC in SafetyPLCFactory",eError); + return 0; + } + + status = PLC_Init(pNew); + if(status != 1) + { + sprintf(pError,"ERROR: failed to connect to %s",argv[2]); + SCWrite(pCon,pError,eError); + } + + /* + create the command + */ + iRet = AddCommand(pSics, argv[1], PLC_Action, PLC_Kill, pNew); + if(!iRet) + { + sprintf(pError,"ERROR: duplicate command %s not created", argv[1]); + SCWrite(pCon,pError,eError); + PLC_Kill(pNew); + return 0; + } + return 1; +} diff --git a/site_ansto/safetyplc.h b/site_ansto/safetyplc.h new file mode 100644 index 00000000..fd3c028b --- /dev/null +++ b/site_ansto/safetyplc.h @@ -0,0 +1,13 @@ +/* + * S A F E T Y P L C + * + * Douglas Clowes, February 2007 + * + */ +#ifndef SICSSAFETYPLC +#define SICSSAFETYPLC + +int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics, + void *pData, int argc, char *argv[]); + +#endif /* SICSSAFETYPLC */