- Adapted indenation to new agreed upon system
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
407
lmd200.c
407
lmd200.c
@ -14,13 +14,13 @@
|
||||
*
|
||||
* Mark Koennecke, October 2007
|
||||
*/
|
||||
#include <sics.h>
|
||||
#include <sicsobj.h>
|
||||
#include <sicshipadaba.h>
|
||||
#include <network.h>
|
||||
#include <nwatch.h>
|
||||
#include <commandlog.h>
|
||||
#include <stptok.h>
|
||||
#include <sics.h>
|
||||
#include <sicsobj.h>
|
||||
#include <sicshipadaba.h>
|
||||
#include <network.h>
|
||||
#include <nwatch.h>
|
||||
#include <commandlog.h>
|
||||
#include <stptok.h>
|
||||
/*
|
||||
* possible states
|
||||
*/
|
||||
@ -29,222 +29,235 @@
|
||||
#define WAITDATA2 2
|
||||
/*
|
||||
* private data structure
|
||||
*/
|
||||
*/
|
||||
#define BUFLEN 1024
|
||||
typedef struct {
|
||||
int state;
|
||||
char host[256];
|
||||
int port;
|
||||
mkChannel *pSock;
|
||||
pNWContext watchContext;
|
||||
char lineBuffer[BUFLEN];
|
||||
} LMD200, *pLMD200;
|
||||
typedef struct {
|
||||
int state;
|
||||
char host[256];
|
||||
int port;
|
||||
mkChannel *pSock;
|
||||
pNWContext watchContext;
|
||||
char lineBuffer[BUFLEN];
|
||||
} LMD200, *pLMD200;
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static void killLMD200(void *data){
|
||||
pLMD200 priv = (pLMD200)data;
|
||||
if(priv == NULL){
|
||||
return;
|
||||
}
|
||||
if(priv->watchContext != NULL){
|
||||
NetWatchRemoveCallback(priv->watchContext);
|
||||
}
|
||||
if(priv->pSock != NULL){
|
||||
NETClosePort(priv->pSock);
|
||||
free(priv->pSock);
|
||||
}
|
||||
free(priv);
|
||||
}
|
||||
static void killLMD200(void *data)
|
||||
{
|
||||
pLMD200 priv = (pLMD200) data;
|
||||
if (priv == NULL) {
|
||||
return;
|
||||
}
|
||||
if (priv->watchContext != NULL) {
|
||||
NetWatchRemoveCallback(priv->watchContext);
|
||||
}
|
||||
if (priv->pSock != NULL) {
|
||||
NETClosePort(priv->pSock);
|
||||
free(priv->pSock);
|
||||
}
|
||||
free(priv);
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static void setAlarm(pSICSOBJ self, char *text){
|
||||
hdbValue alarmVal;
|
||||
pHdb node = NULL;
|
||||
|
||||
alarmVal = MakeHdbText(text);
|
||||
node = GetHipadabaNode(self->objectNode,"alarm");
|
||||
assert(node != NULL);
|
||||
UpdateHipadabaPar(node,alarmVal, NULL);
|
||||
static void setAlarm(pSICSOBJ self, char *text)
|
||||
{
|
||||
hdbValue alarmVal;
|
||||
pHdb node = NULL;
|
||||
|
||||
alarmVal = MakeHdbText(text);
|
||||
node = GetHipadabaNode(self->objectNode, "alarm");
|
||||
assert(node != NULL);
|
||||
UpdateHipadabaPar(node, alarmVal, NULL);
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static void doAlarm(pSICSOBJ self, char *lineBuffer){
|
||||
|
||||
setAlarm(self,lineBuffer);
|
||||
WriteToCommandLog("CERCA>> ", lineBuffer);
|
||||
ServerWriteGlobal(lineBuffer,eError);
|
||||
static void doAlarm(pSICSOBJ self, char *lineBuffer)
|
||||
{
|
||||
|
||||
setAlarm(self, lineBuffer);
|
||||
WriteToCommandLog("CERCA>> ", lineBuffer);
|
||||
ServerWriteGlobal(lineBuffer, eError);
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
extern char *trim(char *);
|
||||
|
||||
static void storeData(pSICSOBJ self, int start, char *lineBuffer){
|
||||
pHdb node = NULL;
|
||||
char number[80], *pPtr = NULL, *pKomma;
|
||||
static void storeData(pSICSOBJ self, int start, char *lineBuffer)
|
||||
{
|
||||
pHdb node = NULL;
|
||||
char number[80], *pPtr = NULL, *pKomma;
|
||||
|
||||
node = GetHipadabaNode(self->objectNode,"data");
|
||||
assert(node != NULL);
|
||||
/*
|
||||
* throw first away
|
||||
*/
|
||||
pPtr = stptok(lineBuffer,number,80," ");
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr,number,80," ");
|
||||
while(pPtr != NULL){
|
||||
if(strstr(number,"noinp") != NULL){
|
||||
node->value.v.floatArray[start] = -9999.99;
|
||||
start++;
|
||||
} else {
|
||||
pKomma = strchr(number,',');
|
||||
if(pKomma != NULL){
|
||||
*pKomma = '.';
|
||||
}
|
||||
pKomma = strchr(number,'(');
|
||||
if(pKomma != NULL){
|
||||
*pKomma = ' ';
|
||||
}
|
||||
pKomma = strchr(number,')');
|
||||
if(pKomma != NULL){
|
||||
*pKomma = ' ';
|
||||
}
|
||||
node->value.v.floatArray[start] = atof(trim(number));
|
||||
start++;
|
||||
}
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr,number,80," ");
|
||||
}
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static int countTokens(char *lineBuffer){
|
||||
int count = 0;
|
||||
char myBuffer[BUFLEN], token[60];
|
||||
char *pPtr = NULL;
|
||||
|
||||
|
||||
memset(myBuffer,0,BUFLEN);
|
||||
strcpy(myBuffer, lineBuffer);
|
||||
pPtr = myBuffer;
|
||||
pPtr = stptok(pPtr,token, 60," \r\n");
|
||||
while(pPtr != NULL){
|
||||
count++;
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr,token,60," \r\n");
|
||||
node = GetHipadabaNode(self->objectNode, "data");
|
||||
assert(node != NULL);
|
||||
/*
|
||||
* throw first away
|
||||
*/
|
||||
pPtr = stptok(lineBuffer, number, 80, " ");
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr, number, 80, " ");
|
||||
while (pPtr != NULL) {
|
||||
if (strstr(number, "noinp") != NULL) {
|
||||
node->value.v.floatArray[start] = -9999.99;
|
||||
start++;
|
||||
} else {
|
||||
pKomma = strchr(number, ',');
|
||||
if (pKomma != NULL) {
|
||||
*pKomma = '.';
|
||||
}
|
||||
pKomma = strchr(number, '(');
|
||||
if (pKomma != NULL) {
|
||||
*pKomma = ' ';
|
||||
}
|
||||
pKomma = strchr(number, ')');
|
||||
if (pKomma != NULL) {
|
||||
*pKomma = ' ';
|
||||
}
|
||||
node->value.v.floatArray[start] = atof(trim(number));
|
||||
start++;
|
||||
}
|
||||
return count;
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr, number, 80, " ");
|
||||
}
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static void interpretLine(pSICSOBJ self, pLMD200 priv){
|
||||
static int countTokens(char *lineBuffer)
|
||||
{
|
||||
int count = 0;
|
||||
char myBuffer[BUFLEN], token[60];
|
||||
char *pPtr = NULL;
|
||||
|
||||
|
||||
memset(myBuffer, 0, BUFLEN);
|
||||
strcpy(myBuffer, lineBuffer);
|
||||
pPtr = myBuffer;
|
||||
pPtr = stptok(pPtr, token, 60, " \r\n");
|
||||
while (pPtr != NULL) {
|
||||
count++;
|
||||
pPtr = trim(pPtr);
|
||||
pPtr = stptok(pPtr, token, 60, " \r\n");
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static void interpretLine(pSICSOBJ self, pLMD200 priv)
|
||||
{
|
||||
pHdb node = NULL;
|
||||
|
||||
switch(priv->state){
|
||||
case IDLE:
|
||||
if(strstr(priv->lineBuffer, "Alarm") != NULL){
|
||||
doAlarm(self, priv->lineBuffer);
|
||||
return;
|
||||
} else if(strstr(priv->lineBuffer,"Pre") != NULL){
|
||||
setAlarm(self,priv->lineBuffer);
|
||||
} else {
|
||||
priv->state = WAITDATA1;
|
||||
setAlarm(self,"I do not feel very alarmed");
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case WAITDATA1:
|
||||
if(strstr(priv->lineBuffer,"...0") == NULL){
|
||||
/* this data is out of order, recover...*/
|
||||
priv->state == IDLE;
|
||||
return;
|
||||
}
|
||||
storeData(self, 0, priv->lineBuffer);
|
||||
priv->state = WAITDATA2;
|
||||
break;
|
||||
case WAITDATA2:
|
||||
if(strstr(priv->lineBuffer,"...0") == NULL){
|
||||
/* this data is out of order, recover...*/
|
||||
priv->state == IDLE;
|
||||
return;
|
||||
}
|
||||
storeData(self, 8, priv->lineBuffer);
|
||||
priv->state = IDLE;
|
||||
node = GetHipadabaNode(self->objectNode,"data");
|
||||
assert(node != NULL);
|
||||
NotifyHipadabaPar(node,NULL);
|
||||
break;
|
||||
switch (priv->state) {
|
||||
case IDLE:
|
||||
if (strstr(priv->lineBuffer, "Alarm") != NULL) {
|
||||
doAlarm(self, priv->lineBuffer);
|
||||
return;
|
||||
} else if (strstr(priv->lineBuffer, "Pre") != NULL) {
|
||||
setAlarm(self, priv->lineBuffer);
|
||||
} else {
|
||||
priv->state = WAITDATA1;
|
||||
setAlarm(self, "I do not feel very alarmed");
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case WAITDATA1:
|
||||
if (strstr(priv->lineBuffer, "...0") == NULL) {
|
||||
/* this data is out of order, recover... */
|
||||
priv->state == IDLE;
|
||||
return;
|
||||
}
|
||||
storeData(self, 0, priv->lineBuffer);
|
||||
priv->state = WAITDATA2;
|
||||
break;
|
||||
case WAITDATA2:
|
||||
if (strstr(priv->lineBuffer, "...0") == NULL) {
|
||||
/* this data is out of order, recover... */
|
||||
priv->state == IDLE;
|
||||
return;
|
||||
}
|
||||
storeData(self, 8, priv->lineBuffer);
|
||||
priv->state = IDLE;
|
||||
node = GetHipadabaNode(self->objectNode, "data");
|
||||
assert(node != NULL);
|
||||
NotifyHipadabaPar(node, NULL);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
* This handles the terminator discovery and causes the evaluation of complete
|
||||
* lines.
|
||||
* --------------------------------------------------------------------------*/
|
||||
static int LMD200Callback(void *context, int mode){
|
||||
pSICSOBJ self = (pSICSOBJ)context;
|
||||
pLMD200 priv = NULL;
|
||||
char buffer[512], *pPtr = NULL, line[132];
|
||||
static int LMD200Callback(void *context, int mode)
|
||||
{
|
||||
pSICSOBJ self = (pSICSOBJ) context;
|
||||
pLMD200 priv = NULL;
|
||||
char buffer[512], *pPtr = NULL, line[132];
|
||||
|
||||
assert(self != NULL);
|
||||
priv = (pLMD200)self->pPrivate;
|
||||
if(mode == nwatch_read){
|
||||
memset(buffer,0,512);
|
||||
memset(line,0,132);
|
||||
NETRead(priv->pSock, buffer, 512, 0);
|
||||
pPtr = stptok(buffer,line,132,"\r");
|
||||
while(pPtr != NULL){
|
||||
if(strlen(line) > 3){
|
||||
strncpy(priv->lineBuffer,line,BUFLEN);
|
||||
interpretLine(self, priv);
|
||||
priv->lineBuffer[0] = '\0';
|
||||
}
|
||||
pPtr = stptok(pPtr,line,132,"\r");
|
||||
}
|
||||
assert(self != NULL);
|
||||
priv = (pLMD200) self->pPrivate;
|
||||
if (mode == nwatch_read) {
|
||||
memset(buffer, 0, 512);
|
||||
memset(line, 0, 132);
|
||||
NETRead(priv->pSock, buffer, 512, 0);
|
||||
pPtr = stptok(buffer, line, 132, "\r");
|
||||
while (pPtr != NULL) {
|
||||
if (strlen(line) > 3) {
|
||||
strncpy(priv->lineBuffer, line, BUFLEN);
|
||||
interpretLine(self, priv);
|
||||
priv->lineBuffer[0] = '\0';
|
||||
}
|
||||
pPtr = stptok(pPtr, line, 132, "\r");
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int MakeLMD200(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]){
|
||||
pSICSOBJ self = NULL;
|
||||
pLMD200 priv = NULL;
|
||||
hdbValue dataValue, textValue;
|
||||
int status;
|
||||
int MakeLMD200(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pSICSOBJ self = NULL;
|
||||
pLMD200 priv = NULL;
|
||||
hdbValue dataValue, textValue;
|
||||
int status;
|
||||
|
||||
if(argc < 4){
|
||||
SCWrite(pCon,
|
||||
"ERROR: require name host and port parameters for initialization",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
priv = (pLMD200)malloc(sizeof(LMD200));
|
||||
memset(priv,0,sizeof(LMD200));
|
||||
strncpy(priv->host,argv[2], 256);
|
||||
priv->port = atoi(argv[3]);
|
||||
priv->pSock = NETConnect(priv->host,priv->port);
|
||||
if(priv->pSock == NULL){
|
||||
SCWrite(pCon,"ERROR: failed to connect to LMD200",eError);
|
||||
killLMD200(priv);
|
||||
return 0;
|
||||
}
|
||||
if (argc < 4) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: require name host and port parameters for initialization",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
priv = (pLMD200) malloc(sizeof(LMD200));
|
||||
memset(priv, 0, sizeof(LMD200));
|
||||
strncpy(priv->host, argv[2], 256);
|
||||
priv->port = atoi(argv[3]);
|
||||
priv->pSock = NETConnect(priv->host, priv->port);
|
||||
if (priv->pSock == NULL) {
|
||||
SCWrite(pCon, "ERROR: failed to connect to LMD200", eError);
|
||||
killLMD200(priv);
|
||||
return 0;
|
||||
}
|
||||
|
||||
self = MakeSICSOBJv(argv[1],"LMD400",HIPNONE, 0);
|
||||
if(self == NULL || priv == NULL){
|
||||
return 0;
|
||||
}
|
||||
textValue = MakeHdbText("I do not feel very alarmed");
|
||||
dataValue = makeHdbValue(HIPFLOATAR,16);
|
||||
AddSICSHdbPar(self->objectNode,"alarm", usInternal,textValue);
|
||||
AddSICSHdbPar(self->objectNode,"data", usInternal,dataValue);
|
||||
self->pPrivate = priv;
|
||||
self->KillPrivate = killLMD200;
|
||||
ReleaseHdbValue(&dataValue);
|
||||
NetWatchRegisterCallback(&priv->watchContext, priv->pSock->sockid,
|
||||
LMD200Callback, self);
|
||||
self = MakeSICSOBJv(argv[1], "LMD400", HIPNONE, 0);
|
||||
if (self == NULL || priv == NULL) {
|
||||
return 0;
|
||||
}
|
||||
textValue = MakeHdbText("I do not feel very alarmed");
|
||||
dataValue = makeHdbValue(HIPFLOATAR, 16);
|
||||
AddSICSHdbPar(self->objectNode, "alarm", usInternal, textValue);
|
||||
AddSICSHdbPar(self->objectNode, "data", usInternal, dataValue);
|
||||
self->pPrivate = priv;
|
||||
self->KillPrivate = killLMD200;
|
||||
ReleaseHdbValue(&dataValue);
|
||||
NetWatchRegisterCallback(&priv->watchContext, priv->pSock->sockid,
|
||||
LMD200Callback, self);
|
||||
|
||||
status = AddCommand(pSics,
|
||||
argv[1],
|
||||
InterInvokeSICSOBJ,
|
||||
KillSICSOBJ,
|
||||
self);
|
||||
if(status != 1){
|
||||
KillSICSOBJ(self);
|
||||
SCPrintf(pCon,eError,"ERROR: failed create duplicate command %s", argv[1]);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics,
|
||||
argv[1], InterInvokeSICSOBJ, KillSICSOBJ, self);
|
||||
if (status != 1) {
|
||||
KillSICSOBJ(self);
|
||||
SCPrintf(pCon, eError, "ERROR: failed create duplicate command %s",
|
||||
argv[1]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user