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

multichan becomes AsyncQueue and AsyncProtocol

Browse Source 
r1957 | dcl | 2007-05-11 17:28:31 +1000 (Fri, 11 May 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-05-11 17:28:31 +10:00
parent 6d9120f796
commit a18500cbf0
13 changed files with 1785 additions and 503 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
site_ansto/hardsup/nhq200util.c
View File

@@ -51,67 +51,39 @@
#include <sics.h>
#include <modriv.h>
#include <nwatch.h>
#include <multichan.h>
#include <asyncqueue.h>
#include "nhq200util.h"
/* -------------------------------------------------------------------*/
typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len);
static pAsyncProtocol NHQ_Protocol = NULL;
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 NHQ_Tx1(void* ctx)
static int NHQ_Tx1(pAsyncProtocol p, void* ctx)
{
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
assert(myCmd);
iRet = MultiChanWrite(myCmd->unit, &myCmd->out_buf[myCmd->out_idx], 1);
iRet = AsyncUnitWrite(myCmd->unit, &myCmd->out_buf[myCmd->out_idx], 1);
return iRet;
}
static int NHQ_Tx(void* ctx)
static int NHQ_Tx(pAsyncProtocol p, pAsyncTxn myCmd)
{
pCommand myCmd = (pCommand) ctx;
/*
* Set/reset command states for send/resend of command
*/
myCmd->cstate = 0;
myCmd->lstate = 0;
myCmd->txn_state = 0;
myCmd->out_idx = 0;
myCmd->inp_idx = 0;
return NHQ_Tx1(myCmd);
myCmd->txn_status = ATX_ACTIVE;
return NHQ_Tx1(p, myCmd);
}
static int NHQ_Rx(void* ctx, int rxchar)
static int NHQ_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar)
{
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
if (rxchar == MCC_TIMEOUT) {
/* TODO: handle command timeout */
if (myCmd->func)
iRet = myCmd->func(myCmd->cntx, NULL, MCC_TIMEOUT);
free(myCmd->out_buf);
free(myCmd->inp_buf);
free(myCmd);
return MCC_POP_CMD;
}
switch (myCmd->cstate) {
switch (myCmd->txn_state) {
case 0: /* send with echo */
if (rxchar != myCmd->out_buf[myCmd->out_idx]) {
/* TODO: bad echo */
@@ -120,12 +92,12 @@ static int NHQ_Rx(void* ctx, int rxchar)
myCmd->out_idx > 0 &&
myCmd->out_buf[myCmd->out_idx - 1] == 0x0D) {
myCmd->inp_idx = 0;
myCmd->cstate = 1;
myCmd->txn_state = 1;
/* TODO: end of line */
}
else if (myCmd->out_idx < myCmd->out_len) {
myCmd->out_idx++;
iRet = NHQ_Tx1(myCmd);
iRet = NHQ_Tx1(p, myCmd);
}
else {
/* TODO: out of data */
@@ -135,65 +107,37 @@ static int NHQ_Rx(void* ctx, int rxchar)
if (myCmd->inp_idx < myCmd->inp_len)
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
if (rxchar == 0x0D)
myCmd->cstate = 2;
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) {
/* end of line */
myCmd->cstate = 3;
myCmd->txn_state = 3;
myCmd->inp_idx -= 2;
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->txn_status = ATX_COMPLETE;
iRet = 0;
}
else
myCmd->cstate = 1;
myCmd->txn_state = 1;
break;
}
if (iRet == 0) { /* end of command */
free(myCmd->out_buf);
free(myCmd->inp_buf);
free(myCmd);
return MCC_POP_CMD;
return AQU_POP_CMD;
}
return iRet;
}
int NHQ_SendCmd(pMultiChan unit,
char* command, int cmd_len,
CommandCallback callback, void* context, int rsp_len)
static int NHQ_Ev(pAsyncProtocol p, pAsyncTxn myCmd, int event)
{
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;
if (event == AQU_TIMEOUT) {
/* TODO: handle command timeout */
myCmd->txn_status = ATX_TIMEOUT;
return AQU_POP_CMD;
}
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, NHQ_Tx, NHQ_Rx);
return AQU_POP_CMD;
}
static void NHQ_Notify(void* context, int event)
@@ -201,14 +145,14 @@ static void NHQ_Notify(void* context, int event)
pNHQ200 self = (pNHQ200) context;
switch (event) {
case MCC_DISCONNECT:
case AQU_DISCONNECT:
if (self->transWait == 1) {
self->transWait = NHQ200__FAULT;
strcpy(self->pAns, "DISCONNECTED");
}
case MCC_RECONNECT:
case AQU_RECONNECT:
do {
mkChannel* sock = MultiChanGetSocket(self->mcc);
mkChannel* sock = AsyncUnitGetSocket(self->unit);
int flag = 1;
setsockopt(sock->sockid, /* socket affected */
IPPROTO_TCP, /* set option at TCP level */
@@ -302,13 +246,15 @@ static void parse_Vx(pNHQ200 self, const char* resp, int resp_len)
#define STATE_NX 5
#define STATE_VX 6
#define STATE_END 9
static int InitCallback(void* ctx, const char* resp, int resp_len)
static int InitCallback(pAsyncTxn pTxn)
{
char cmd[20];
int cmd_len;
pNHQ200 self = (pNHQ200) ctx;
const char* resp = pTxn->inp_buf;
int resp_len = pTxn->inp_idx;
pNHQ200 self = (pNHQ200) pTxn->cntx;
/* TODO: FIXME finish initialisation */
if (resp_len < 0) {
if (pTxn->txn_status == ATX_TIMEOUT) {
self->iError = NHQ200__BADSET;
self->iState = 0;
}
@@ -316,37 +262,37 @@ static int InitCallback(void* ctx, const char* resp, int resp_len)
switch (self->iState) {
case 0: /* Initial */
cmd_len = snprintf(cmd, sizeof(cmd), "#");
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_HASH;
break;
case STATE_HASH: /* # */
parse_hash(self, resp, resp_len);
cmd_len = snprintf(cmd, sizeof(cmd), "S%d", self->iControl);
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_SX;
break;
case STATE_SX: /* Sx */
parse_Sx(self, resp, resp_len);
cmd_len = snprintf(cmd, sizeof(cmd), "T%d", self->iControl);
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_TX;
break;
case STATE_TX: /* Tx */
parse_Tx(self, resp, resp_len);
cmd_len = snprintf(cmd, sizeof(cmd), "M%d", self->iControl);
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_MX;
break;
case STATE_MX: /* Mx */
parse_Mx(self, resp, resp_len);
cmd_len = snprintf(cmd, sizeof(cmd), "N%d", self->iControl);
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_NX;
break;
case STATE_NX: /* Nx */
parse_Nx(self, resp, resp_len);
cmd_len = snprintf(cmd, sizeof(cmd), "V%d", self->iControl);
NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, cmd, cmd_len, InitCallback, self, 80);
self->iState = STATE_VX;
break;
case STATE_VX: /* Vx */
@@ -363,19 +309,20 @@ static int InitCallback(void* ctx, const char* resp, int resp_len)
static void NHQ_Init(pNHQ200 self)
{
self->iState = 0;
NHQ_SendCmd(self->mcc, "", 0, InitCallback, self, 80);
AsyncUnitSendTxn(self->unit, "", 0, InitCallback, self, 80);
}
/*
* \brief GetCallback is the callback for the get position/value command.
*/
static int GetCallback(void* ctx, const char* resp, int resp_len)
static int GetCallback(pAsyncTxn pTxn)
{
int iRet;
float fRead;
pNHQ200 self = (pNHQ200) ctx;
const char* resp = pTxn->inp_buf;
pNHQ200 self = (pNHQ200) pTxn->cntx;
if (resp_len < 0) {
if (pTxn->txn_status == ATX_TIMEOUT) {
self->iError = NHQ200__BADREAD;
}
else {
@@ -396,11 +343,13 @@ static int GetCallback(void* ctx, const char* resp, int resp_len)
/*
* \brief TransCallback is the callback for the general command transaction.
*/
static int TransCallback(void* ctx, const char* resp, int resp_len)
static int TransCallback(pAsyncTxn pTxn)
{
pNHQ200 self = (pNHQ200) ctx;
const char* resp = pTxn->inp_buf;
int resp_len = pTxn->inp_idx;
pNHQ200 self = (pNHQ200) pTxn->cntx;
if (resp_len < 0) {
if (pTxn->txn_status == ATX_TIMEOUT) {
self->transReply[0] = '\0';
self->transWait = -1;
}
@@ -419,7 +368,7 @@ int transactNHQ200(pNHQ200 self, void *send, int sendLen,
assert(self);
self->transReply = reply;
self->transWait = 1;
NHQ_SendCmd(self->mcc,
AsyncUnitSendTxn(self->unit,
send, sendLen,
TransCallback, self, replyLen);
while (self->transWait == 1)
@@ -448,7 +397,8 @@ int NHQ200_Check_Status(pNHQ200 self)
do
{
sprintf(pCommand,"S%d", self->iControl);
if ((iRet=transactNHQ200(self,pCommand,strlen(pCommand),pReply,79))<=0)
iRet=AsyncUnitTransact(self->unit, pCommand, strlen(pCommand), pReply, 79);
if (iRet <= 0)
{
printf("Comms error!\n");
return iRet; // Comms problem
@@ -472,10 +422,6 @@ int NHQ200_Check_Status(pNHQ200 self)
/* Operations common to both Open and Config functions */
static int NHQ200_Setup(pNHQ200 self, int iControl)
{
int iRet;
char pCommand[20];
char pReply[132];
if (!self)
return NHQ200__BADCOM;
@@ -505,12 +451,12 @@ int NHQ200_Open(pNHQ200 *pData, char *pName, int iSensor, int iCTRL, int iMode)
self->fDiv = 1.0;
self->fMult = 1.0;
if (MultiChanCreate(pName, &self->mcc) == 0) {
if (AsyncUnitCreate(pName, &self->unit) == 0) {
return NHQ200__NONHQ200;
}
MultiChanSetNotify(self->mcc, self, NHQ_Notify);
AsyncUnitSetNotify(self->unit, self, NHQ_Notify);
sock = MultiChanGetSocket(self->mcc);
sock = AsyncUnitGetSocket(self->unit);
if (sock) {
int flag = 1;
iRet = setsockopt(sock->sockid, /* socket affected */
@@ -560,7 +506,7 @@ int NHQ200_Send(pNHQ200 *pData, char *pCommand, char *pReply, int iLen)
/* Send command direct to the NHQ200 */
commandlen=strlen(pCommand);
iRet=transactNHQ200(self,pCommand,commandlen,pReply,iLen);
iRet=AsyncUnitTransact(self->unit, pCommand, commandlen, pReply, iLen);
return iRet;
}
@@ -581,13 +527,20 @@ int NHQ200_Read(pNHQ200 *pData, float *fVal)
struct timeval tv_this;
gettimeofday(&tv_this, NULL);
if ((tv_this.tv_sec - self->tv_last.tv_sec) > 0) {
NHQ_SendCmd(self->mcc,
AsyncUnitSendTxn(self->unit,
pCommand, 2,
GetCallback, self, 132);
self->iGetOut = 1;
self->tv_last = tv_this;
}
}
while (self->iGetOut) {
struct timeval tv_this;
gettimeofday(&tv_this, NULL);
if ((tv_this.tv_sec - self->tv_last.tv_sec) > 1)
break;
TaskYield(pServ->pTasker);
}
*fVal = self->fValue;
iRet = 1;
@@ -597,7 +550,7 @@ int NHQ200_Read(pNHQ200 *pData, float *fVal)
int NHQ200_Set(pNHQ200 *pData, float fVal)
{
char pCommand[20], pCommandRead[20], pReply[132], pCommandGo[20];
int iRet, i;
int iRet;
const float fPrecision = 0.1;
float fDelta, fRead;
pNHQ200 self;
@@ -615,11 +568,11 @@ int NHQ200_Set(pNHQ200 *pData, float fVal)
sprintf(pCommandRead,"D%d", self->iControl);
/* send Dn=nnn command, we get a blank line response */
iRet = transactNHQ200(self,pCommand,strlen(pCommand),pReply,131);
iRet = AsyncUnitTransact(self->unit,pCommand,strlen(pCommand),pReply,131);
if (iRet <= 0)
return iRet;
/* read the set value again using the Dn command */
iRet = transactNHQ200(self,pCommandRead,strlen(pCommandRead),pReply,131);
iRet = AsyncUnitTransact(self->unit,pCommandRead,strlen(pCommandRead),pReply,131);
if (iRet <= 0)
return iRet;
printf("D%d: Response %d chars: '%s'\n",self->iControl, iRet, pReply);
@@ -636,7 +589,7 @@ int NHQ200_Set(pNHQ200 *pData, float fVal)
if(fDelta < fPrecision)
{
sprintf(pCommandGo, "G%d", self->iControl);
iRet = transactNHQ200(self,pCommandGo,strlen(pCommandGo),pReply,131);
iRet = AsyncUnitTransact(self->unit,pCommandGo,strlen(pCommandGo),pReply,131);
if (iRet <= 0)
return iRet;
printf("G%d: Response %d chars: '%s'\n",self->iControl, iRet, pReply);
@@ -686,3 +639,14 @@ void NHQ200_ErrorTxt(pNHQ200 *pData,int iCode, char *pError, int iLen)
break;
}
}
void NHQ200InitProtocol(SicsInterp *pSics) {
if (NHQ_Protocol == NULL) {
NHQ_Protocol = AsyncProtocolCreate(pSics, "NHQ200", NULL, NULL);
NHQ_Protocol->sendCommand = NHQ_Tx;
NHQ_Protocol->handleInput = NHQ_Rx;
NHQ_Protocol->handleEvent = NHQ_Ev;
NHQ_Protocol->prepareTxn = NULL;
NHQ_Protocol->killPrivate = NULL;
}
}

2
site_ansto/hardsup/nhq200util.h
View File

@@ -29,7 +29,7 @@
/*------------------------------------------------------------------------*/
typedef struct __NHQ200 {
pMultiChan mcc;
pAsyncUnit unit;
int iRead;
int iControl;
void *pData;

3
site_ansto/make_gen_variables
View File

@@ -26,7 +26,8 @@ SOBJ = network.o ifile.o conman.o SCinter.o splitter.o passwd.o \
mcstashm.o initializer.o remob.o tclmotdriv.o protocol.o \
sinfox.o sicslist.o cone.o hipadaba.o sicshipadaba.o statistics.o \
moregress.o hdbcommand.o multicounter.o regresscter.o histregress.o \
sicshdbadapter.o polldriv.o sicspoll.o statemon.o multichan.o
sicshdbadapter.o polldriv.o sicspoll.o statemon.o \
asyncqueue.o asyncprotocol.o
# These are intermediate files generated from .tc files, marking
# them as SECONDARY prevents make from removing them.

446
site_ansto/motor_dmc2280.c
View File

@@ -22,7 +22,7 @@
#include <sys/time.h>
#include <fortify.h>
#include <sics.h>
#include <multichan.h>
#include <asyncqueue.h>
#include <nwatch.h>
#include <modriv.h>
#include <motor.h>
@@ -40,12 +40,8 @@
enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration};
enum commandtype {CMD_RUN=1, CMD_HALT=2};
typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv;
typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(pCommand pCmd);
enum eventtype {eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent};
typedef struct EvtEvent_s EvtEvent, *pEvtEvent;
@@ -54,7 +50,7 @@ typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event);
typedef struct EvtTimer_s { } EvtTimer;
typedef struct EvtMessage_s {
pCommand cmd;
pAsyncTxn cmd;
} EvtMessage;
typedef struct EvtCommand_s {
@@ -73,6 +69,8 @@ struct EvtEvent_s {
} event;
};
static pAsyncProtocol DMC2280_Protocol = NULL;
/*-----------------------------------------------------------------------
The motor driver structure. Please note that the first set of fields has
be identical with the fields of AbstractModriv in ../modriv.h
@@ -102,7 +100,7 @@ struct __MoDriv {
/* DMC-2280 specific fields */
pMultiChan mcc;
pAsyncUnit asyncUnit;
pMotor pMot; /**< Points to logical motor object */
int errorCode;
char *errorMsg; /**< Points to memory for error messages */
@@ -226,20 +224,6 @@ static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
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.
* \param self (r) provides access to the motor's data structure
@@ -397,17 +381,18 @@ static int motCreep(pDMC2280Driv self, float target) {
return target_steps;
}
static int DMC_Tx(void* ctx)
static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx)
{
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
if (myCmd) {
iRet = MultiChanWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
myCmd->txn_status = ATX_ACTIVE;
iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
/* TODO handle errors */
if (iRet < 0) { /* TODO: EOF */
/*
iRet = MultiChanReconnect(myCmd->unit);
iRet = AsyncUnitReconnect(myCmd->unit);
if (iRet == 0)
*/
return 0;
@@ -416,42 +401,32 @@ static int DMC_Tx(void* ctx)
return 1;
}
static int DMC_Rx(void* ctx, int rxchar) {
static int DMC_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
pAsyncTxn myCmd = (pAsyncTxn) 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) {
switch (myCmd->txn_state) {
case 0: /* first character */
if (rxchar == ':') {
/* normal prompt */
myCmd->cstate = 99;
myCmd->txn_state = 99;
myCmd->txn_status = ATX_COMPLETE;
}
else if (rxchar == '?') {
/* error prompt, send TC1 ahead of any queued commands */
iRet = MultiChanWrite(myCmd->unit, "TC1\r\n", 5);
myCmd->cstate = 1;
iRet = AsyncUnitWrite(myCmd->unit, "TC1\r\n", 5);
myCmd->txn_state = 1;
}
else {
/* normal data */
myCmd->cstate = 1;
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->cstate = 2;
myCmd->txn_state = 2;
break;
case 2: /* received CR and looking for LF */
if (myCmd->inp_idx < myCmd->inp_len)
@@ -462,13 +437,13 @@ static int DMC_Rx(void* ctx, int rxchar) {
myCmd->inp_idx -= 2;
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
*/
myCmd->cstate = 0;
myCmd->txn_state = 0;
}
else
myCmd->cstate = 1;
myCmd->txn_state = 1;
break;
}
if (myCmd->cstate == 99) {
if (myCmd->txn_state == 99) {
myCmd->inp_buf[myCmd->inp_idx] = '\0';
if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) {
int i;
@@ -477,60 +452,28 @@ static int DMC_Rx(void* ctx, int rxchar) {
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;
iRet = 0;
}
if (iRet == 0) { /* end of command */
free(myCmd->out_buf);
free(myCmd->inp_buf);
free(myCmd);
return MCC_POP_CMD;
return AQU_POP_CMD;
}
return iRet;
}
static int DMC_SendCommand(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;
static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
if (event == AQU_TIMEOUT) {
/* handle command timeout */
pTxn->txn_status = ATX_TIMEOUT;
return AQU_POP_CMD;
}
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);
return AQU_POP_CMD;
}
static int DMC_SendCmd(pDMC2280Driv self,
char* command,
CommandCallback callback)
AsyncTxnHandler callback)
{
return DMC_SendCommand(self->mcc,
return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
callback, self, CMDLEN);
}
@@ -541,12 +484,12 @@ static void DMC_Notify(void* context, int event)
char line[132];
switch (event) {
case MCC_DISCONNECT:
case AQU_DISCONNECT:
snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: disconnect */
break;
case MCC_RECONNECT:
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: reconnect */
@@ -555,22 +498,24 @@ static void DMC_Notify(void* context, int event)
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)
static int SendCallback(pAsyncTxn 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) {
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
}
else {
switch (resp[0]) {
case ':':
case ' ':
@@ -593,52 +538,11 @@ static int SendCallback(pCommand pCmd)
break;
}
}
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_SendCommand(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) {
static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) {
if (cb == NULL)
cb = SendCallback;
return DMC_SendCmd(self, cmd, cb);
@@ -677,7 +581,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
int status;
status = DMC_transact(self, cmd, strlen(cmd), reply, CMDLEN);
status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, CMDLEN);
if (status != 1) {
if (self->debug)
@@ -985,12 +889,19 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
/**
* \brief process the airpad status response
*/
static int airpad_callback(pCommand pCmd) {
static int airpad_callback(pAsyncTxn pCmd) {
char* resp = pCmd->inp_buf;
int resp_len = pCmd->inp_idx;
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
if (resp_len > 0) {
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
}
else {
float fReply;
if (self->debug) {
SICSLogWrite(pCmd->inp_buf, eStatus);
@@ -1010,9 +921,6 @@ static int airpad_callback(pCommand pCmd) {
return 0;
}
}
else {
/* TODO: timeout */
}
return 0;
}
@@ -1228,15 +1136,36 @@ static char* state_name(StateFunc func) {
return "<unknown_state>";
}
void str_n_cat(char* s1, int len, const char* s2) {
int i = strlen(s1);
const char* p = s2;
while (i < len - 3 && *p) {
if (*p == '\r') {
s1[i++] = '\\';
s1[i++] = 'r';
++p;
}
else if (*p == '\n') {
s1[i++] = '\\';
s1[i++] = 'n';
++p;
}
else
s1[i++] = *p++;
}
s1[i] = '\0';
}
static char* event_name(pEvtEvent event, char* text, int length) {
switch (event->event_type) {
case eTimerEvent:
snprintf(text, length, "eTimerEvent");
return text;
case eMessageEvent:
snprintf(text, length, "eMessageEvent:%s:%s",
event->event.msg.cmd->out_buf,
event->event.msg.cmd->inp_buf);
snprintf(text, length, "eMessageEvent:");
str_n_cat(text, length, event->event.msg.cmd->out_buf);
str_n_cat(text, length, "|");
str_n_cat(text, length, event->event.msg.cmd->inp_buf);
return text;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
@@ -1285,19 +1214,12 @@ static void change_state(pDMC2280Driv self, StateFunc func) {
self->subState = 0;
}
static int state_msg_callback(pCommand pCmd)
static int state_msg_callback(pAsyncTxn pCmd)
{
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
EvtEvent event;
if (pCmd->inp_idx > 0) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite(pCmd->inp_buf, eStatus);
}
event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd;
}
else {
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
@@ -1305,6 +1227,14 @@ static int state_msg_callback(pCommand pCmd)
event.event_type = eTimeoutEvent;
event.event.msg.cmd = pCmd;
}
else {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite(pCmd->inp_buf, eStatus);
}
event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd;
}
if (self->debug || self->trace)
report_event(self, &event);
self->myState(self, &event);
@@ -1336,7 +1266,6 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) {
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
int value;
float steps, counts;
switch (event->event_type) {
case eTimerEvent:
@@ -1363,7 +1292,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet;
iRet = set_currMotion(self, pCmd->inp_buf);
@@ -1374,7 +1303,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_Idle);
return;
}
value = ((counts - self->absEncHome)/ self->cntsPerX) * self->stepsPerX;
value = ((self->currCounts - self->absEncHome) / self->cntsPerX) * self->stepsPerX;
self->currSteps = value;
snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value);
DMC_SendCmd(self, cmd, state_msg_callback);
@@ -1399,7 +1328,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG _XQ0,_TSx */
float fReply;
int iRet, iFlags;
@@ -1479,7 +1408,7 @@ static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) {
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F') { /* FTUBE */
NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER,
@@ -1523,7 +1452,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* SH */
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
@@ -1532,7 +1461,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
}
else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, absolute;
float steps, counts, target;
float target;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
@@ -1602,7 +1531,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'B') { /* BG */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
@@ -1779,6 +1708,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
}
break;
case eTimeoutEvent:
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
state_cmd_execute(self, CMD_HALT);
@@ -1800,7 +1730,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* ST */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
@@ -1881,14 +1811,13 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
}
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
DMC_SendCmd(self, "MG APDONE", state_msg_callback);
return;
case eMessageEvent:
do {
pCommand pCmd = event->event.msg.cmd;
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F') { /* FTUBE */
}
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE */
@@ -2534,13 +2463,15 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->absEncHome;
return 1;
}
if(strcasecmp(name,"creep_offset") == 0) {
*fValue = self->creep_offset;
return 1;
}
if(strcasecmp(name,"creep_precision") == 0) {
*fValue = self->creep_precision;
return 1;
if (self->has_fsm) {
if(strcasecmp(name,"creep_offset") == 0) {
*fValue = self->creep_offset;
return 1;
}
if(strcasecmp(name,"creep_precision") == 0) {
*fValue = self->creep_precision;
return 1;
}
}
}
else {
@@ -2717,40 +2648,43 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
}
}
/* Set creep offset,
* managers only */
if(self->abs_encoder && strcasecmp(name,"creep_offset") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->creep_offset = fabs(newValue);
return 1;
}
}
/* Set creep_precision,
* managers only */
if(self->abs_encoder && strcasecmp(name,"creep_precision") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->creep_precision = fabs(newValue);
return 1;
}
}
/* Invoke Home Run routine in controller,
* managers only */
if(self->abs_encoder == 0 && strcasecmp(name,"homerun") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
if (DMC2280MotionControl != 1 && newValue > 0.5) {
snprintf(pError, ERRLEN,"ERROR: Motion Control must be on");
SCWrite(pCon, pError, eError);
if (self->abs_encoder) { /* If we DO have an absolute encoder */
if (self->has_fsm) { /* If we DO have a finite state machine */
/* Set creep offset */
if (strcasecmp(name,"creep_offset") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_offset = fabs(newValue);
return 1;
}
}
/* Set creep_precision */
if (strcasecmp(name,"creep_precision") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_precision = fabs(newValue);
return 1;
}
}
}
}
else { /* If we do NOT have an absolute encoder */
/* Invoke Home Run routine in controller */
if(strcasecmp(name,"homerun") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
if (DMC2280MotionControl != 1 && newValue > 0.5) {
snprintf(pError, ERRLEN,"ERROR: Motion Control must be on");
SCWrite(pCon, pError, eError);
}
RunHomeRoutine(self, newValue);
return 1;
}
RunHomeRoutine(self, newValue);
return 1;
}
}
@@ -2877,12 +2811,14 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
if (self->abs_encoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
SCWrite(pCon, buffer, eStatus);
if (self->has_fsm) {
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
SCWrite(pCon, buffer, eStatus);
}
}
snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX);
SCWrite(pCon, buffer, eStatus);
@@ -2903,9 +2839,9 @@ static void KillDMC2280(/*@only@*/void *pData){
free(self->errorMsg);
self->errorMsg = NULL;
}
if (self->mcc) {
MultiChanDestroy(self->mcc);
self->mcc = NULL;
if (self->asyncUnit) {
AsyncUnitDestroy(self->asyncUnit);
self->asyncUnit = NULL;
}
/* Not required as performed in caller
* free(self);
@@ -2957,17 +2893,18 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
}
memset(pNew, 0, sizeof(DMC2280Driv));
/* 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'",
/* Get AsyncQueue from the list of named parameters */
if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL ||
(pPtr=getParam(pCon, interp, params, "asyncqueue", _OPTIONAL)) != NULL ||
(pPtr=getParam(pCon, interp, params, "asyncunit", _OPTIONAL)) != NULL) {
if (!AsyncUnitCreate(pPtr, &pNew->asyncUnit)) {
snprintf(pError, ERRLEN, "Cannot find AsyncQueue '%s' when creating DMC2280 motor '%s'",
pPtr, motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
return NULL;
}
MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify);
AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
}
else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) {
char* host = pPtr;
@@ -2977,16 +2914,16 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
KillDMC2280(pNew);
return NULL;
}
/* MultiChan */
if (!MultiChanCreateHost(host, pPtr, &pNew->mcc)) {
/* AsyncUnit */
if (!AsyncUnitCreateHost(host, pPtr, &pNew->asyncUnit)) {
snprintf(pError, ERRLEN,
"Cannot create MultiChan '%s:%s' for DMC2280 motor '%s'",
"Cannot create AsyncUnit '%s:%s' for DMC2280 motor '%s'",
host, pPtr, motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
return NULL;
}
MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify);
AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
}
else {
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
@@ -3002,6 +2939,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
KillDMC2280(pNew);
return NULL;
}
pNew->pMot = NULL;
strcpy(pNew->name, motor);
pNew->home = 0.0;
@@ -3135,24 +3073,25 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->cntsPerX = 1.0;
else
sscanf(pPtr,"%f",&(pNew->cntsPerX));
/* CREEP_OFFSET: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
pNew->creep_offset = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_offset));
if (pNew->creep_offset < 0)
pNew->creep_offset = -pNew->creep_offset;
}
if (pNew->has_fsm) {
/* CREEP_OFFSET: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
pNew->creep_offset = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_offset));
if (pNew->creep_offset < 0)
pNew->creep_offset = -pNew->creep_offset;
}
/* CREEP_PRECISION: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL)
pNew->creep_precision = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_precision));
if (pNew->creep_precision < 0)
pNew->creep_precision = -pNew->creep_precision;
/* CREEP_PRECISION: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL)
pNew->creep_precision = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_precision));
if (pNew->creep_precision < 0)
pNew->creep_precision = -pNew->creep_precision;
}
}
}
if (pNew->has_fsm) {
@@ -3297,7 +3236,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
return 1;
}
else if(strcasecmp("trace", argv[1]) == 0) {
if (strcasecmp("on", argv[2]) == 0) {
if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
self->trace = pCon;
SCWrite(pCon, "TRACE ON", eValue);
}
@@ -3310,3 +3249,14 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
}
return MotorAction(pCon, pSics, pData, argc, argv);
}
void DMC2280InitProtocol(SicsInterp *pSics) {
if (DMC2280_Protocol == NULL) {
DMC2280_Protocol = AsyncProtocolCreate(pSics, "DMC2280", NULL, NULL);
DMC2280_Protocol->sendCommand = DMC_Tx;
DMC2280_Protocol->handleInput = DMC_Rx;
DMC2280_Protocol->handleEvent = DMC_Ev;
DMC2280_Protocol->prepareTxn = NULL;
DMC2280_Protocol->killPrivate = NULL;
}
}

4
site_ansto/motor_dmc2280.h
View File

@@ -11,10 +11,14 @@
#ifdef __cplusplus
extern "C" {
#endif
void DMC2280InitProtocol(SicsInterp *pSics);
MotorDriver *CreateDMC2280(SConnection *pCon, char* motor, char *params);
int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[]);
#ifdef __cplusplus
}
#endif

3
site_ansto/nhq200.h
View File

@@ -44,6 +44,9 @@ int NHQ200SetPar(pEVControl self, char *name, float fNew,
int NHQ200GetPar(pEVControl self, char *name, float *fVal);
int NHQ200List(pEVControl self, SConnection *pCon);
/*------------------------- The NHQ200 protocol ------------------------------*/
void NHQ200InitProtocol(SicsInterp *pSics);
#endif

196
site_ansto/safetyplc.c
View File

@@ -9,7 +9,7 @@
#include <sys/time.h>
#include <sics.h>
#include "network.h"
#include "multichan.h"
#include "asyncqueue.h"
#include "nwatch.h"
#include "safetyplc.h"
#include "sicsvar.h"
@@ -41,6 +41,8 @@ extern int DMC2280MotionControl;
#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,
@@ -64,45 +66,27 @@ typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;
struct __SafetyPLCController {
pObjectDescriptor pDes;
pMultiChan mcc; /* associated MultiChan object */
pAsyncUnit unit; /* associated AsyncUnit object */
int iGetOut;
int iValue;
int oldValue;
pNWTimer nw_tmr; /* NetWait timer handle */
pNWTimer nw_tmr; /* periodic timer handle */
pNWTimer oneshot; /* oneshot timer handle */
int timeout;
struct timeval tvSend;
};
typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len);
struct __command {
pSafetyPLCController plc;
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_GetState(void *pData, char *param, PLC_STATUS *retState);
static int PLC_Tx(void* ctx)
static int PLC_Tx(pAsyncProtocol p, pAsyncTxn myCmd)
{
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
if (myCmd) {
gettimeofday(&myCmd->plc->tvSend, NULL); /* refresh */
iRet = MultiChanWrite(myCmd->plc->mcc, myCmd->out_buf, myCmd->out_len);
myCmd->txn_status = ATX_ACTIVE;
iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
/* TODO handle errors */
if (iRet < 0) { /* TODO: EOF */
iRet = MultiChanReconnect(myCmd->plc->mcc);
iRet = AsyncUnitReconnect(myCmd->unit);
if (iRet == 0)
return 0;
}
@@ -110,94 +94,52 @@ static int PLC_Tx(void* ctx)
return 1;
}
static int PLC_Rx(void* ctx, int rxchar)
static int PLC_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar)
{
int iRet = 1;
pCommand myCmd = (pCommand) ctx;
if (rxchar == MCC_TIMEOUT) {
/* TODO: handle command timeout */
if (myCmd->func)
iRet = myCmd->func(myCmd->cntx, NULL, MCC_TIMEOUT);
free(myCmd->out_buf);
free(myCmd->inp_buf);
free(myCmd);
return MCC_POP_CMD;
}
switch (myCmd->cstate) {
switch (myCmd->txn_state) {
case 0: /* first character */
/* normal data */
myCmd->cstate = 1;
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->cstate = 2;
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->cstate = 99;
myCmd->txn_state = 99;
/* end of line */
}
else
myCmd->cstate = 1;
myCmd->txn_state = 1;
break;
}
if (myCmd->cstate == 99) {
if (myCmd->txn_state == 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;
iRet = 0;
myCmd->txn_state = 0;
myCmd->txn_status = ATX_COMPLETE;
}
if (iRet == 0) { /* end of command */
free(myCmd->out_buf);
free(myCmd->inp_buf);
free(myCmd);
return MCC_POP_CMD;
return AQU_POP_CMD;
}
return iRet;
}
static int PLC_SendCmd(pSafetyPLCController self,
char* command, int cmd_len,
CommandCallback callback, void* context, int rsp_len)
static int PLC_Ev(pAsyncProtocol p, pAsyncTxn myCmd, int event)
{
pCommand myCmd = NULL;
assert(self);
assert(self->mcc);
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;
if (event == AQU_TIMEOUT) {
/* TODO: handle command timeout */
myCmd->txn_status = ATX_TIMEOUT;
return AQU_POP_CMD;
}
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->plc = self;
gettimeofday(&self->tvSend, NULL); /* refresh */
return MultiChanEnque(self->mcc, myCmd, PLC_Tx, PLC_Rx);
return AQU_POP_CMD;
}
static void PLC_Notify(void* context, int event)
@@ -205,9 +147,9 @@ static void PLC_Notify(void* context, int event)
pSafetyPLCController self = (pSafetyPLCController) context;
switch (event) {
case MCC_RECONNECT:
case AQU_RECONNECT:
do {
mkChannel* sock = MultiChanGetSocket(self->mcc);
mkChannel* sock = AsyncUnitGetSocket(self->unit);
int flag = 1;
setsockopt(sock->sockid, /* socket affected */
IPPROTO_TCP, /* set option at TCP level */
@@ -223,14 +165,16 @@ static void PLC_Notify(void* context, int event)
/*
* \brief GetCallback is the callback for the read command.
*/
static int GetCallback(void* ctx, const char* resp, int resp_len)
static int GetCallback(pAsyncTxn txn)
{
int iRet,i;
unsigned int iRead;
char* resp = txn->inp_buf;
int resp_len = txn->inp_idx;
PLC_STATUS plcState;
pSicsVariable plcVar=NULL;
pSafetyPLCController self = (pSafetyPLCController) ctx;
pSafetyPLCController self = (pSafetyPLCController) txn->cntx;
if (resp_len < 0) {
DMC2280MotionControl = -1;
}
@@ -269,20 +213,33 @@ static int MyTimerCallback(void* context, int mode)
{
pSafetyPLCController self = (pSafetyPLCController) context;
if (self->iGetOut) {
struct timeval now;
gettimeofday(&now, NULL);
/* TODO error handling */
if (((now.tv_sec - self->tvSend.tv_sec) * 1000
+ (now.tv_usec / 1000)
- (self->tvSend.tv_usec / 1000)) < self->timeout)
return 1;
DMC2280MotionControl = -1;
}
self->iGetOut = 1;
PLC_SendCmd(self, "READ", 4, GetCallback, self, 132);
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;
@@ -427,13 +384,32 @@ static int PLC_Action(SConnection *pCon, SicsInterp *pSics,
} 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, int port)
static pSafetyPLCController PLC_Create(const char* pName)
{
pSafetyPLCController self = NULL;
@@ -441,11 +417,11 @@ static pSafetyPLCController PLC_Create(const char* pName, int port)
if (self == NULL)
return NULL;
memset(self, 0, sizeof(SafetyPLCController));
if (MultiChanCreate(pName, &self->mcc) == 0) {
if (AsyncUnitCreate(pName, &self->unit) == 0) {
free(self);
return NULL;
}
MultiChanSetNotify(self->mcc, self, PLC_Notify);
AsyncUnitSetNotify(self->unit, self, PLC_Notify);
self->pDes = CreateDescriptor("SafetyPLC");
return self;
@@ -457,7 +433,7 @@ static int PLC_Init(pSafetyPLCController self)
if (self->nw_tmr != NULL)
NetWatchRemoveTimer(self->nw_tmr);
NetWatchRegisterTimerPeriodic(&self->nw_tmr,
1000, 100,
1000, 1000,
MyTimerCallback,
self);
self->timeout=120000; /* huge */
@@ -473,6 +449,17 @@ static void PLC_Kill(void* pData)
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[])
{
@@ -482,7 +469,7 @@ int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
pSicsVariable plcVar=NULL;
PLC_STATUS plcState;
if(argc < 4)
if(argc < 3)
{
SCWrite(pCon,"ERROR: insufficient no of arguments to SafetyPLCFactory",
eError);
@@ -492,7 +479,7 @@ int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
/*
create data structure and open port
*/
pNew = PLC_Create(argv[2], atoi(argv[3]));
pNew = PLC_Create(argv[2]);
if(!pNew)
{
@@ -524,5 +511,6 @@ int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
PLC_Kill(pNew);
return 0;
}
return 1;
SCSendOK(pCon);
return 1;
}

2
site_ansto/safetyplc.h
View File

@@ -7,6 +7,8 @@
#ifndef SICSSAFETYPLC
#define SICSSAFETYPLC
void SafetyPLCInitProtocol(SicsInterp *pSics);
int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
void *pData, int argc, char *argv[]);