523 lines
15 KiB
C
523 lines
15 KiB
C
/*
|
|
* S A F E T Y P L C
|
|
*
|
|
* Douglas Clowes, February 2007
|
|
*
|
|
*/
|
|
|
|
#include <netinet/tcp.h>
|
|
#include <sys/time.h>
|
|
#include <sics.h>
|
|
#include "network.h"
|
|
#include "asyncqueue.h"
|
|
#include "nwatch.h"
|
|
#include "safetyplc.h"
|
|
#include "sicsvar.h"
|
|
|
|
extern int DMC2280MotionControl;
|
|
|
|
#define KEY_ENABLED_BIT (1 << 0)
|
|
#define KEY_DISABLED_BIT (1 << 1)
|
|
#define SEC_OPENED_BIT (1 << 2)
|
|
#define SEC_CLOSED_BIT (1 << 3)
|
|
#define TER_OPENED_BIT (1 << 4)
|
|
#define TER_CLOSED_BIT (1 << 5)
|
|
#define MOTOR_ENABLED_BIT (1 << 6)
|
|
#define MOTOR_DISABLED_BIT (1 << 7)
|
|
#define ACCESS_LOCKED_BIT (1 << 8)
|
|
#define ACCESS_UNLOCKED_BIT (1 << 9)
|
|
#define DC_POWEROK_BIT (1 << 10)
|
|
#define EXIT_INPROGRESS_BIT (1 << 11)
|
|
#define SAFETY_TRIPPED_BIT (1 << 12)
|
|
#define SAFETY_MALFUNCTION_BIT (1 << 13)
|
|
#define TER_OPERATE_BIT (1 << 14)
|
|
#define RELAY_ENABLED_BIT (1 << 15)
|
|
#define INST_READY_BIT (1 << 16)
|
|
#define LAMP_TEST_BIT (1 << 17)
|
|
|
|
#define KEY_BOTH_BITS (KEY_ENABLED_BIT | KEY_DISABLED_BIT)
|
|
#define SEC_BOTH_BITS (SEC_OPENED_BIT | SEC_CLOSED_BIT)
|
|
#define TER_BOTH_BITS (TER_OPENED_BIT | TER_CLOSED_BIT)
|
|
#define MOTOR_BOTH_BITS (MOTOR_ENABLED_BIT | MOTOR_DISABLED_BIT)
|
|
#define ACCESS_BOTH_BITS (ACCESS_LOCKED_BIT | ACCESS_UNLOCKED_BIT)
|
|
|
|
static pAsyncProtocol PLC_Protocol = NULL;
|
|
|
|
int PLC_UserPriv = 0; /* Internal */
|
|
typedef enum {
|
|
Unknown_low, Invalid_high, Enabled, Disabled,
|
|
Opened, Closed, Locked, Unlocked, True, False,}PLC_STATUS;
|
|
|
|
char *plc_states[] = {
|
|
"Unknown_low", "Invalid_high", "Enabled",
|
|
"Disabled", "Opened", "Closed",
|
|
"Locked", "Unlocked", "True", "False"};
|
|
|
|
typedef enum {
|
|
Key,Secondary,Tertiary,MotionControl,Access,
|
|
DC,Exit,Trip,Fault,Operate,Relay,Ready,}PLC_PARAM;
|
|
|
|
char *plc_parname[] = {
|
|
"plc_key","plc_secondary","plc_tertiary","plc_motioncontrol",
|
|
"plc_access","plc_dc","plc_exit","plc_trip",
|
|
"plc_fault","plc_operate","plc_relay","plc_ready"};
|
|
|
|
typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;
|
|
|
|
struct __SafetyPLCController {
|
|
pObjectDescriptor pDes;
|
|
pAsyncUnit unit; /* associated AsyncUnit object */
|
|
int iGetOut;
|
|
int iValue;
|
|
int oldValue;
|
|
pNWTimer nw_tmr; /* periodic timer handle */
|
|
pNWTimer oneshot; /* oneshot timer handle */
|
|
int timeout;
|
|
};
|
|
|
|
static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState);
|
|
|
|
static int PLC_Tx(pAsyncProtocol p, pAsyncTxn myCmd)
|
|
{
|
|
int iRet = 1;
|
|
|
|
if (myCmd) {
|
|
myCmd->txn_status = ATX_ACTIVE;
|
|
iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
|
|
/* TODO handle errors */
|
|
if (iRet < 0) { /* TODO: EOF */
|
|
iRet = AsyncUnitReconnect(myCmd->unit);
|
|
if (iRet == 0)
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
static int PLC_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar)
|
|
{
|
|
int iRet = 1;
|
|
|
|
switch (myCmd->txn_state) {
|
|
case 0: /* first character */
|
|
/* normal data */
|
|
myCmd->txn_state = 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->txn_state = 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->txn_state = 99;
|
|
/* end of line */
|
|
}
|
|
else
|
|
myCmd->txn_state = 1;
|
|
break;
|
|
}
|
|
if (myCmd->txn_state == 99) {
|
|
myCmd->inp_buf[myCmd->inp_idx] = '\0';
|
|
iRet = 0;
|
|
myCmd->txn_state = 0;
|
|
myCmd->txn_status = ATX_COMPLETE;
|
|
}
|
|
if (iRet == 0) { /* end of command */
|
|
return AQU_POP_CMD;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
static int PLC_Ev(pAsyncProtocol p, pAsyncTxn myCmd, int event)
|
|
{
|
|
if (event == AQU_TIMEOUT) {
|
|
/* TODO: handle command timeout */
|
|
myCmd->txn_status = ATX_TIMEOUT;
|
|
return AQU_POP_CMD;
|
|
}
|
|
return AQU_POP_CMD;
|
|
}
|
|
|
|
static void PLC_Notify(void* context, int event)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) context;
|
|
|
|
switch (event) {
|
|
case AQU_RECONNECT:
|
|
do {
|
|
mkChannel* sock = AsyncUnitGetSocket(self->unit);
|
|
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(pAsyncTxn txn)
|
|
{
|
|
int iRet;
|
|
unsigned int iRead;
|
|
char* resp = txn->inp_buf;
|
|
int resp_len = txn->inp_idx;
|
|
PLC_STATUS plcState;
|
|
pSicsVariable plcVar=NULL;
|
|
|
|
pSafetyPLCController self = (pSafetyPLCController) txn->cntx;
|
|
if (resp_len < 0) {
|
|
DMC2280MotionControl = -1;
|
|
}
|
|
else {
|
|
iRet = sscanf(resp,"READ %x", &iRead);
|
|
if(iRet != 1) { // Not a number, probably an error response
|
|
self->iValue = 0;
|
|
}
|
|
else {
|
|
if ((iRead & LAMP_TEST_BIT) == 0)
|
|
self->iValue = iRead;
|
|
}
|
|
if ((self->iValue & MOTOR_BOTH_BITS) == 0) /* neither */
|
|
DMC2280MotionControl = -1;
|
|
else if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS) /* both */
|
|
DMC2280MotionControl = -1;
|
|
else if ((self->iValue & MOTOR_ENABLED_BIT)) /* enabled */
|
|
DMC2280MotionControl = 1;
|
|
else /* disabled */
|
|
DMC2280MotionControl = 0;
|
|
}
|
|
if (self->oldValue != self->iValue) {
|
|
unsigned int i;
|
|
for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
|
|
plcVar = (pSicsVariable)FindCommandData(pServ->pSics,plc_parname[i],"SicsVariable");
|
|
PLC_GetState(self,plc_parname[i],&plcState);
|
|
VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
|
|
}
|
|
}
|
|
|
|
self->oldValue = self->iValue;
|
|
self->iGetOut = 0;
|
|
return 0;
|
|
}
|
|
|
|
static int MyTimerCallback(void* context, int mode)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) context;
|
|
if (self->iGetOut) {
|
|
/* TODO error handling */
|
|
}
|
|
self->iGetOut = 1;
|
|
AsyncUnitSendTxn(self->unit, "READ", 4, GetCallback, self, 132);
|
|
return 1;
|
|
}
|
|
|
|
static int MyOneShotCallback(void* context, int mode)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) context;
|
|
self->oneshot = 0;
|
|
AsyncUnitSendTxn(self->unit, "WRITE 0", 7, NULL, NULL, 132);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
* \brief PutCallback is the callback for the write command.
|
|
*/
|
|
static int PutCallback(pAsyncTxn txn)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) txn->cntx;
|
|
if (self->oneshot)
|
|
NetWatchRemoveTimer(self->oneshot);
|
|
NetWatchRegisterTimer(&self->oneshot, 500, MyOneShotCallback, self);
|
|
return 0;
|
|
}
|
|
|
|
static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (strcasecmp(param, plc_parname[Key]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & KEY_BOTH_BITS) == KEY_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & KEY_ENABLED_BIT)
|
|
*retState = Enabled;
|
|
else if (self->iValue & KEY_DISABLED_BIT)
|
|
*retState = Disabled;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Secondary]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & SEC_BOTH_BITS) == SEC_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
if (self->iValue & SEC_OPENED_BIT)
|
|
*retState = Opened;
|
|
else if (self->iValue & SEC_CLOSED_BIT)
|
|
*retState = Closed;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Tertiary]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & TER_BOTH_BITS) == TER_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
if (self->iValue & TER_OPENED_BIT)
|
|
*retState = Opened;
|
|
else if (self->iValue & TER_CLOSED_BIT)
|
|
*retState = Closed;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[MotionControl]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & MOTOR_ENABLED_BIT)
|
|
*retState = Enabled;
|
|
else if (self->iValue & MOTOR_DISABLED_BIT)
|
|
*retState = Disabled;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Access]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & ACCESS_BOTH_BITS) == ACCESS_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & ACCESS_LOCKED_BIT)
|
|
*retState = Locked;
|
|
else if (self->iValue & ACCESS_UNLOCKED_BIT)
|
|
*retState = Unlocked;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[DC]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & DC_POWEROK_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Exit]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & EXIT_INPROGRESS_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Trip]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & SAFETY_TRIPPED_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Fault]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & SAFETY_MALFUNCTION_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Operate]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & TER_OPERATE_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Relay]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & RELAY_ENABLED_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Ready]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & INST_READY_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int PLC_Print(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, char *name, char *param)
|
|
{
|
|
char line[132];
|
|
PLC_STATUS state;
|
|
|
|
if (PLC_GetState(pData, param, &state) != OKOK) {
|
|
return 0;
|
|
} else {
|
|
snprintf(line, 132, "%s.%s = %s", name, param, plc_states[state]);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
}
|
|
|
|
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 = %06X", argv[0], self->iValue & 0xffffff);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
else if (argc == 2) {
|
|
if (strcasecmp(argv[1], "list") == 0) {
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Key]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Secondary]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Tertiary]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[MotionControl]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Access]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[DC]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Exit]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Trip]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Fault]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Operate]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Relay]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Ready]);
|
|
return OKOK;
|
|
}
|
|
if (PLC_Print(pCon, pSics, pData, argv[0], argv[1]))
|
|
return OKOK;
|
|
} else if (argc == 3) {
|
|
if (strcasecmp(argv[1], "hattach") == 0) {
|
|
}
|
|
else if (strcasecmp(argv[1], "shutter") == 0) {
|
|
if (strcasecmp(argv[2], "open") == 0) {
|
|
/* open shutter */
|
|
AsyncUnitSendTxn(self->unit, "WRITE 1", 4, PutCallback, self, 132);
|
|
return OKOK;
|
|
}
|
|
else if (strcasecmp(argv[2], "close") == 0 ||
|
|
strcasecmp(argv[2], "shut") == 0) {
|
|
/* close shutter */
|
|
AsyncUnitSendTxn(self->unit, "WRITE 2", 4, PutCallback, self, 132);
|
|
return OKOK;
|
|
}
|
|
else {
|
|
snprintf(line, 132, "%s %s does not understand %s",
|
|
argv[0], argv[1], argv[2]);
|
|
SCWrite(pCon, line, eError);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
snprintf(line, 132, "%s does not understand %s", argv[0], argv[1]);
|
|
SCWrite(pCon, line, eError);
|
|
return 0;
|
|
}
|
|
|
|
static pSafetyPLCController PLC_Create(const char* pName)
|
|
{
|
|
pSafetyPLCController self = NULL;
|
|
|
|
self = (pSafetyPLCController) malloc(sizeof(SafetyPLCController));
|
|
if (self == NULL)
|
|
return NULL;
|
|
memset(self, 0, sizeof(SafetyPLCController));
|
|
if (AsyncUnitCreate(pName, &self->unit) == 0) {
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
AsyncUnitSetNotify(self->unit, self, PLC_Notify);
|
|
|
|
self->pDes = CreateDescriptor("SafetyPLC");
|
|
return self;
|
|
}
|
|
|
|
static int PLC_Init(pSafetyPLCController self)
|
|
{
|
|
/* TODO: Init the controller */
|
|
if (self->nw_tmr != NULL)
|
|
NetWatchRemoveTimer(self->nw_tmr);
|
|
NetWatchRegisterTimerPeriodic(&self->nw_tmr,
|
|
1000, 1000,
|
|
MyTimerCallback,
|
|
self);
|
|
self->timeout=120000; /* huge */
|
|
return 1;
|
|
}
|
|
|
|
static void PLC_Kill(void* pData)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (self->nw_tmr)
|
|
NetWatchRemoveTimer(self->nw_tmr);
|
|
if (self->pDes) {
|
|
DeleteDescriptor(self->pDes);
|
|
self->pDes = NULL;
|
|
}
|
|
free(self);
|
|
return;
|
|
}
|
|
|
|
void SafetyPLCInitProtocol(SicsInterp *pSics) {
|
|
if (PLC_Protocol == NULL) {
|
|
PLC_Protocol = AsyncProtocolCreate(pSics, "SafetyPLC", NULL, NULL);
|
|
PLC_Protocol->sendCommand = PLC_Tx;
|
|
PLC_Protocol->handleInput = PLC_Rx;
|
|
PLC_Protocol->handleEvent = PLC_Ev;
|
|
PLC_Protocol->prepareTxn = NULL;
|
|
PLC_Protocol->killPrivate = NULL;
|
|
}
|
|
}
|
|
|
|
int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, int argc, char *argv[])
|
|
{
|
|
pSafetyPLCController pNew = NULL;
|
|
int iRet, status;
|
|
unsigned int i;
|
|
char pError[256];
|
|
pSicsVariable plcVar=NULL;
|
|
PLC_STATUS plcState;
|
|
|
|
if(argc < 3)
|
|
{
|
|
SCWrite(pCon,"ERROR: insufficient no of arguments to SafetyPLCFactory",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
create data structure and open port
|
|
*/
|
|
pNew = PLC_Create(argv[2]);
|
|
|
|
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);
|
|
}
|
|
|
|
for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
|
|
plcVar = VarCreate(PLC_UserPriv,veText,plc_parname[i]);
|
|
PLC_GetState(pNew,plc_parname[i],&plcState);
|
|
VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
|
|
AddCommand(pSics,plc_parname[i],VarWrapper,(KillFunc)VarKill,plcVar);
|
|
}
|
|
/*
|
|
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;
|
|
}
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|