- Added messagepipe.c
- Added initial version of sicsget. This is a more generalised way of reading and writing SICS data wherever it is. The thing is extendable if reading something the current way is to slow. This has both a C interface and an interpreter interface.
This commit is contained in:
koennecke
548
sicsget.c
Normal file
548
sicsget.c
Normal file
@ -0,0 +1,548 @@
|
||||
/**
|
||||
* This is a generalized SICS get/put routine. It tries to hard to match a name
|
||||
* with the data.
|
||||
*
|
||||
* The other difference is that this uses the message pipe system for implementation.
|
||||
* See messsagepipe.c for a pointer to documentation.
|
||||
*
|
||||
* copyright: see file COPYRIGHT
|
||||
*
|
||||
*/
|
||||
#include <ctype.h>
|
||||
#include <sics.h>
|
||||
#include <sicsget.h>
|
||||
#include <messagepipe.h>
|
||||
#include <sicshipadaba.h>
|
||||
#include <splitter.h>
|
||||
#include <stptok.h>
|
||||
#include <sicsdata.h>
|
||||
|
||||
static pMP getPipe, setPipe, sicsPipe, tclPipe;
|
||||
|
||||
|
||||
typedef struct {
|
||||
int success;
|
||||
char *name;
|
||||
hdbValue *v;
|
||||
} SSGMessage, *pSSGMessage;
|
||||
/*---------------------------------------------------------------------------*/
|
||||
typedef struct {
|
||||
int success;
|
||||
char *command;
|
||||
char *response;
|
||||
hdbValue *v;
|
||||
} ParseMessage, *pParseMessage;
|
||||
|
||||
extern char *trim(char *txt);
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int sget(char *name, hdbValue *v)
|
||||
{
|
||||
SSGMessage ssg;
|
||||
int status;
|
||||
|
||||
ssg.success = 0;
|
||||
ssg.name = strdup(name);
|
||||
ssg.v = v;
|
||||
|
||||
MPprocess(getPipe,&ssg);
|
||||
free(ssg.name);
|
||||
|
||||
return ssg.success;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
int sput(char *name, hdbValue v)
|
||||
{
|
||||
SSGMessage ssg;
|
||||
int status;
|
||||
|
||||
ssg.success = 0;
|
||||
ssg.name = strdup(name);
|
||||
ssg.v = &v;
|
||||
|
||||
MPprocess(setPipe,&ssg);
|
||||
free(ssg.name);
|
||||
|
||||
return ssg.success;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int SICSGetCommand(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
int status;
|
||||
hdbValue v;
|
||||
pDynString data;
|
||||
|
||||
if(argc < 2) {
|
||||
SCWrite(pCon,"ERROR: need at least address for sicsget",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = sget(argv[1], &v);
|
||||
if(status){
|
||||
data = formatValue(v,NULL);
|
||||
if(data != NULL){
|
||||
SCPrintf(pCon,eValue,"%s",GetCharArray(data));
|
||||
DeleteDynString(data);
|
||||
} else {
|
||||
SCPrintf(pCon,eError,"ERROR: formatting value for %s failed", argv[1]);
|
||||
ReleaseHdbValue(&v);
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
SCPrintf(pCon,eError,"ERROR: value for %s not found", argv[1]);
|
||||
return 0;
|
||||
}
|
||||
ReleaseHdbValue(&v);
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int SICSPutCommand(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
int status;
|
||||
hdbValue v;
|
||||
char buffer[2048];
|
||||
char error[132];
|
||||
|
||||
if(argc < 2) {
|
||||
SCWrite(pCon,"ERROR: need at least address for sicsget",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = sget(argv[1],&v);
|
||||
if(!status){
|
||||
SCPrintf(pCon,eError,"ERROR: %s not found",argv[1]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
Arg2Text(argc-2, &argv[2], buffer,sizeof(buffer));
|
||||
status = readHdbValue(&v,buffer,error,sizeof(error));
|
||||
if(status == 0){
|
||||
SCPrintf(pCon,eError,"ERROR: failed to parse data for %s, error %s",
|
||||
argv[1], error);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = sput(argv[1],v);
|
||||
if(status == 0){
|
||||
SCPrintf(pCon,eError,"ERROR: error setting %s, value invalid?",
|
||||
argv[1]);
|
||||
} else {
|
||||
SCSendOK(pCon);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int InvokeSICSFunc(void *ms, void *userData)
|
||||
{
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
int status;
|
||||
|
||||
SCsetMacro(pServ->dummyCon,1);
|
||||
status = InterpExecute(pServ->pSics, pServ->dummyCon, self->command);
|
||||
SCsetMacro(pServ->dummyCon,0);
|
||||
if(!status){
|
||||
self->success = 0;
|
||||
return MPSTOP;
|
||||
}
|
||||
self->response = strdup(Tcl_GetStringResult(InterpGetTcl(pServ->pSics)));
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int CheckSICSError(void *ms, void *userData)
|
||||
{
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
|
||||
if(strstr(self->response,"ERROR") != NULL){
|
||||
self->success = 0;
|
||||
return MPSTOP;
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int SplitOffEqual(void *ms, void *userData)
|
||||
{
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
char *pPtr, *pEQ;
|
||||
|
||||
pPtr = self->response;
|
||||
if((pEQ = strstr(pPtr, "=")) != NULL){
|
||||
self->response = strdup(trim(pEQ+1));
|
||||
free(pPtr);
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int isNumber(char *txt)
|
||||
{
|
||||
if(*txt == '\0'){
|
||||
return 1;
|
||||
}
|
||||
if(isalpha(*txt)){
|
||||
return 0;
|
||||
} else {
|
||||
return isNumber(txt+1);
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int countWords(char *txt)
|
||||
{
|
||||
char number[80];
|
||||
|
||||
if(txt == NULL){
|
||||
return 0;
|
||||
} else {
|
||||
return 1 + countWords(stptok(txt,number,sizeof(number)," "));
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static hdbValue makeArray(char *txt, int type, int count)
|
||||
{
|
||||
hdbValue ar;
|
||||
char *pPtr;
|
||||
char number[80];
|
||||
int i = 0;
|
||||
|
||||
if(type == HIPINT){
|
||||
ar = makeHdbValue(HIPINTAR,count);
|
||||
} else {
|
||||
ar = makeHdbValue(HIPFLOATAR,count);
|
||||
}
|
||||
pPtr = txt;
|
||||
while((pPtr = stptok(pPtr,number,sizeof(number)," ")) != NULL){
|
||||
if(type == HIPINT){
|
||||
ar.v.intArray[i] = atoi(number);
|
||||
} else {
|
||||
ar.v.floatArray[i] = atof(number);
|
||||
}
|
||||
}
|
||||
return ar;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int ReturnAsNumber(void *ms, void *userData)
|
||||
{
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
int type, count;
|
||||
hdbValue v;
|
||||
|
||||
if(isNumber(self->response)){
|
||||
if(strstr(self->response,".") != NULL){
|
||||
type = HIPFLOAT;
|
||||
} else {
|
||||
type = HIPINT;
|
||||
}
|
||||
count = countWords(self->response);
|
||||
if(count > 1) {
|
||||
v = makeArray(self->response, type, count);
|
||||
} else {
|
||||
if(type == HIPINT){
|
||||
v = MakeHdbInt(atoi(self->response));
|
||||
} else {
|
||||
v = MakeHdbFloat(atof(self->response));
|
||||
}
|
||||
}
|
||||
cloneHdbValue(&v,self->v);
|
||||
ReleaseHdbValue(&v);
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
} else {
|
||||
return MPCONTINUE;
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int ReturnAsText(void *ms, void *userData)
|
||||
{
|
||||
hdbValue v;
|
||||
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
self->success = 1;
|
||||
v = MakeHdbText(strdup(self->response));
|
||||
cloneHdbValue(&v,self->v);
|
||||
ReleaseHdbValue(&v);
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static void configureSICSPipe()
|
||||
{
|
||||
sicsPipe = MakeMP();
|
||||
AppendMPFilter(sicsPipe, InvokeSICSFunc, NULL, NULL);
|
||||
AppendMPFilter(sicsPipe, CheckSICSError, NULL, NULL);
|
||||
AppendMPFilter(sicsPipe, SplitOffEqual, NULL, NULL);
|
||||
AppendMPFilter(sicsPipe, ReturnAsNumber, NULL, NULL);
|
||||
AppendMPFilter(sicsPipe, ReturnAsText, NULL, NULL);
|
||||
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int FindTclVar(void *ms, void *userData)
|
||||
{
|
||||
pParseMessage self = (pParseMessage)ms;
|
||||
|
||||
self->response = Tcl_GetVar(InterpGetTcl(pServ->pSics),self->command, TCL_GLOBAL_ONLY);
|
||||
if(self->response == NULL){
|
||||
return MPSTOP;
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static void configureTclPipe()
|
||||
{
|
||||
tclPipe = MakeMP();
|
||||
AppendMPFilter(tclPipe, FindTclVar, NULL, NULL);
|
||||
AppendMPFilter(tclPipe, SplitOffEqual, NULL, NULL);
|
||||
AppendMPFilter(tclPipe, ReturnAsNumber, NULL, NULL);
|
||||
AppendMPFilter(tclPipe, ReturnAsText, NULL, NULL);
|
||||
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int GetHdbFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pHdb node = NULL;
|
||||
|
||||
node = FindHdbNode(NULL,self->name,NULL);
|
||||
if(node != NULL){
|
||||
cloneHdbValue(&node->value, self->v);
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
} else {
|
||||
return MPCONTINUE;
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int GetSICSFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
ParseMessage ps;
|
||||
|
||||
memset(&ps,0,sizeof(ParseMessage));
|
||||
ps.command = strdup(self->name);
|
||||
ps.v = self->v;
|
||||
MPprocess(sicsPipe,&ps);
|
||||
self->success = ps.success;
|
||||
free(ps.command);
|
||||
if(ps.response != NULL){
|
||||
free(ps.response);
|
||||
}
|
||||
if(self->success){
|
||||
return MPSTOP;
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int GetTclFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
ParseMessage ps;
|
||||
|
||||
memset(&ps,0,sizeof(ParseMessage));
|
||||
ps.command = strdup(self->name);
|
||||
ps.v = self->v;
|
||||
MPprocess(tclPipe,&ps);
|
||||
self->success = ps.success;
|
||||
free(ps.command);
|
||||
if(self->success){
|
||||
return MPSTOP;
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int GetDrivableFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
void *data = NULL;
|
||||
pIDrivable pDriv = NULL;
|
||||
float fVal;
|
||||
hdbValue v;
|
||||
|
||||
data = FindCommandData(pServ->pSics, self->name,NULL);
|
||||
if(data != NULL){
|
||||
pDriv = GetDrivableInterface(data);
|
||||
if(pDriv != NULL){
|
||||
fVal = pDriv->GetValue(data,pServ->dummyCon);
|
||||
v = MakeHdbFloat(fVal);
|
||||
self->success = 1;
|
||||
cloneHdbValue(&v,self->v);
|
||||
return MPSTOP;
|
||||
}
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int GetSicsdataFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pSICSData data = NULL;
|
||||
hdbValue v;
|
||||
int i;
|
||||
|
||||
data = FindCommandData(pServ->pSics,self->name, "SICSData");
|
||||
if(data != NULL){
|
||||
if(data->dataType[0] == INTTYPE){
|
||||
v = makeHdbValue(HIPINTVARAR, data->dataUsed);
|
||||
for(i = 0; i < data->dataUsed; i++){
|
||||
v.v.intArray[i] = data->data[i];
|
||||
}
|
||||
} else {
|
||||
v = makeHdbValue(HIPFLOATVARAR, data->dataUsed);
|
||||
for(i = 0; i < data->dataUsed; i++){
|
||||
v.v.floatArray[i] = (double)data->data[i];
|
||||
}
|
||||
}
|
||||
cloneHdbValue(&v, self->v);
|
||||
ReleaseHdbValue(&v);
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
}
|
||||
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static void configureGetPipe()
|
||||
{
|
||||
getPipe = MakeMP();
|
||||
AppendMPFilter(getPipe, GetHdbFunc, NULL, NULL);
|
||||
AppendMPFilter(getPipe, GetDrivableFunc, NULL, NULL);
|
||||
AppendMPFilter(getPipe, GetSicsdataFunc, NULL, NULL);
|
||||
AppendMPFilter(getPipe, GetSICSFunc, NULL, NULL);
|
||||
AppendMPFilter(getPipe, GetTclFunc, NULL, NULL);
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int PutHdbFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pHdb node = NULL;
|
||||
int status;
|
||||
|
||||
node = FindHdbNode(NULL,self->name,NULL);
|
||||
if(node != NULL){
|
||||
status = UpdateHipadabaPar(node,*(self->v),NULL);
|
||||
self->success = status;
|
||||
if(status == 1){
|
||||
return MPSTOP;
|
||||
}
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int PutSICSFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pDynString data = NULL;
|
||||
int status;
|
||||
char sicsobj[80];
|
||||
void *cdata;
|
||||
|
||||
|
||||
data = formatValue(*(self->v),NULL);
|
||||
if(data != NULL){
|
||||
DynStringInsert(data," ", 0);
|
||||
DynStringInsert(data,self->name, 0);
|
||||
status = InterpExecute(pServ->pSics, pServ->dummyCon, GetCharArray(data));
|
||||
DeleteDynString(data);
|
||||
if(status == 1) {
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
} else {
|
||||
/*
|
||||
Without this test bad sets on SICS commands may fall
|
||||
through to Tcl where they succedd
|
||||
*/
|
||||
stptok(self->name,sicsobj,sizeof(sicsobj)," ");
|
||||
cdata = FindCommandData(pServ->pSics,sicsobj,NULL);
|
||||
if(cdata != NULL){
|
||||
self->success = 0;
|
||||
return MPSTOP;
|
||||
}
|
||||
}
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int PutTclFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pDynString data = NULL;
|
||||
int status;
|
||||
char *pPtr = NULL;
|
||||
|
||||
data = formatValue(*(self->v),NULL);
|
||||
if(data != NULL){
|
||||
pPtr = Tcl_SetVar(InterpGetTcl(pServ->pSics),self->name,GetCharArray(data),TCL_GLOBAL_ONLY);
|
||||
DeleteDynString(data);
|
||||
if(pPtr != NULL) {
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
}
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int PutDrivableFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
void *data = NULL;
|
||||
pIDrivable pDriv = NULL;
|
||||
float fVal;
|
||||
hdbValue v;
|
||||
|
||||
data = FindCommandData(pServ->pSics, self->name,NULL);
|
||||
if(data != NULL){
|
||||
pDriv = GetDrivableInterface(data);
|
||||
if(pDriv != NULL){
|
||||
StartMotor(pServ->pExecutor,pServ->pSics, pServ->dummyCon,
|
||||
self->name, RUNRUN, (float)self->v->v.doubleValue);
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
}
|
||||
}
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int PutSicsdataFunc(void *ms, void *userData)
|
||||
{
|
||||
pSSGMessage self = (pSSGMessage)ms;
|
||||
pSICSData data = NULL;
|
||||
int i;
|
||||
|
||||
data = FindCommandData(pServ->pSics,self->name, "SICSData");
|
||||
if(data != NULL){
|
||||
if(self->v->dataType == HIPINTAR || self->v->dataType == HIPINTVARAR){
|
||||
for(i = 0; i < self->v->arrayLength; i++){
|
||||
setSICSDataInt(data,i,self->v->v.intArray[i]);
|
||||
}
|
||||
} else if(self->v->dataType == HIPFLOATAR || self->v->dataType == HIPFLOATVARAR){
|
||||
for(i = 0; i < self->v->arrayLength; i++){
|
||||
setSICSDataFloat(data,i,(float)self->v->v.floatArray[i]);
|
||||
}
|
||||
} else {
|
||||
return MPSTOP;
|
||||
}
|
||||
self->success = 1;
|
||||
return MPSTOP;
|
||||
}
|
||||
|
||||
return MPCONTINUE;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static void configureSetPipe()
|
||||
{
|
||||
setPipe = MakeMP();
|
||||
AppendMPFilter(setPipe, PutHdbFunc, NULL, NULL);
|
||||
AppendMPFilter(setPipe, PutDrivableFunc, NULL, NULL);
|
||||
AppendMPFilter(setPipe, PutSicsdataFunc, NULL, NULL);
|
||||
AppendMPFilter(setPipe, PutSICSFunc, NULL, NULL);
|
||||
AppendMPFilter(setPipe, PutTclFunc, NULL, NULL);
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
void SICSGetInit(void)
|
||||
{
|
||||
if (getPipe == NULL) {
|
||||
configureGetPipe();
|
||||
configureSetPipe();
|
||||
configureSICSPipe();
|
||||
configureTclPipe();
|
||||
AddCmd("sget", SICSGetCommand);
|
||||
AddCmd("sput", SICSPutCommand);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user