First iteration of SICS object to handle the Safety PLC digital I/O
r1561 | dcl | 2007-03-01 09:07:00 +1100 (Thu, 01 Mar 2007) | 2 lines
This commit is contained in:
Douglas Clowes
299
site_ansto/safetyplc.c
Normal file
299
site_ansto/safetyplc.c
Normal file
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
* S A F E T Y P L C
|
||||
*
|
||||
* Douglas Clowes, February 2007
|
||||
*
|
||||
*/
|
||||
|
||||
#include <netinet/tcp.h>
|
||||
#include <sics.h>
|
||||
#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;
|
||||
}
|
||||
13
site_ansto/safetyplc.h
Normal file
13
site_ansto/safetyplc.h
Normal file
@@ -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 */
|
||||
Reference in New Issue
Block a user