linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
c95a7bbd9f3bbba2f4dabedbb935acabfac2abcd
sics/docho.c
cvs 1e60f3be82 - Many fixes to the triple axis stuff 
* update after a1-a6 drive
  * intrduction of targets
- POLDI writing
- Moved HKL calculation 4 TRICS to fourlib
2002-01-25 14:48:50 +00:00

747 lines
20 KiB
C
Raw Blame History

/*--------------------------------------------------------------------------
D o C h o
A SICS driver for a Dornier Chopper Control System accessed through a
RS-232 interface connected to a Macintosh PC running the SerialPortServer
terminal server program. There are two choppers which ususally run at fixed
speed ratios against each other. There ia also a phase difference between
the two choppers. And lots of machine surveillance parameters.
This driver is used by the generic chopper or device controller as described
in choco.tex.
Mark Koennecke, January 1999
Modified to support a single chopper only,
Uwe Filges, Mark Koennecke; November 2001
--------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include "fortify.h"
#include "sics.h"
#include "stringdict.h"
#include "hardsup/serialsinq.h"
#include "hardsup/el734_errcodes.h"
#include "hardsup/el734fix.h"
#include "codri.h"
/*-----------------------------------------------------------------------
A private data structure for this Dornier chopper
-------------------------------------------------------------------------*/
typedef struct {
char *pHost;
int iPort;
int iChannel;
void *pData;
int iRefreshIntervall;
pStringDict pPar;
time_t tRefresh;
int iStop;
long lTask;
int iError;
int iBusy;
float fRatio;
int iSingle;
char pError[80];
} DoCho, *pDoCho;
/*
pHost, iPort and iChannel combined are the adress of the chopper
controller at the Macintosh terminal server. pData is the serial
port connection data structure needed and managed by the SerialIO
functions.
As the communication with the Dornier Chopper System is very slow the
parameter list of this driver will only be updated a predefined time
intervalls. In between buffered values will be returned for requests.
The buffered parameters are held in the string dictioanry pPar.
iRefreshIntervall is the time between refreshs. tRefresh is the time for
the next refresh. iBusy is flag which indicates, that it was tried to
modify a variable. This will only be reflected with the next status update.
In between DoChoCheckPar might conclude, that the chopper is already
done. iBusy is meant to stop that. It is set when a parameter is changed
and cleared bu the status message code. DoChoCheckPar checks for it.
Refreshing will be performed by a special SICS task which will be
started when the driver is initialized. In order to stop this task when
need arises the parameter iStop can be set to true.
iError is the last error reported on this device. If no error: 0
fRatio is the target value for the chopper ratio. In contrast to the
other parameters, its target value cannot be extracted from the chopper
status message.
iSingle is a flag which is true if only a single chopper is controlled
through this driver. This supports the POLDI single choper case.
*/
/*----------------------------------------------------------------------
ERROR CODES:
*/
#define UNDRIVABLE -8002
#define UNKNOWNPAR -8003
#define PARERROR -8004
#define BADSYNC -8005
#define BADSTOP -8006
#define CHOPERROR -8007
extern char *trim(char *pTrim); /* trim.c */
/*----------------------------------------------------------------------*/
static void SplitChopperReply(pCodri self, char *prefix, char *pBueffel)
{
char pToken[30], pValue[20];
char *pPtr, *pTok, *pVal;
int iCount, iRet;
pDoCho pPriv = NULL;
pPriv = (pDoCho)self->pPrivate;
/* decompose pBueffel and store into string dictionary */
pPtr = strtok(pBueffel,";");
while(pPtr != NULL)
{
iCount = sscanf(pPtr,"%s %s",pToken,pValue);
if(iCount == 2)
{
pTok = trim(pToken);
pVal = trim(pValue);
sprintf(pToken,"%s.%s",prefix,pTok);
iRet = StringDictUpdate(pPriv->pPar,pToken,pVal);
if(!iRet)
{
StringDictAddPair(pPriv->pPar,pToken,pVal);
strcat(self->pParList,pToken);
strcat(self->pParList,",");
}
}
else
{
/* this fixes a bug with oversized messages in dphas */
if(strstr(pPtr,"dphas") != NULL)
{
sprintf(pToken,"%s.dphas",prefix);
iRet = StringDictUpdate(pPriv->pPar,
pToken,pPtr+5);
if(!iRet)
{
StringDictAddPair(pPriv->pPar,pToken,
pPtr+5);
strcat(self->pParList,pToken);
strcat(self->pParList,",");
}
}
}
pPtr = strtok(NULL,";");
}
}
/*-------------------------------------------------------------------------
Well, DoChoStatus sends a status request to the Dornier chopper control
system. There is a gotcha, you need three reads to get the full information.
Then the answer is parsed and decomposed into parameter content for the
string dictionary. The single status components are separated by ;.
-------------------------------------------------------------------------*/
static int DoChoStatus(pCodri self)
{
int iRet, iCount, iCode;
char pBueffel[1024], pToken[30], pValue[20];
char *pPtr, *pTok, *pVal;
pDoCho pPriv = NULL;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
pPriv->iBusy = 0;
pPriv->iError = 0;
/* first send, command, returns the echo */
iRet = SerialWriteRead(&(pPriv->pData),"asyst 1",pBueffel,1023);
if(iRet < 0)
{
pPriv->iError = iRet;
return 0;
}
/* next send: reads first chopper line */
iRet = SerialWriteRead(&(pPriv->pData),"",pBueffel,1023);
if(iRet < 0)
{
pPriv->iError = iRet;
return 0;
}
SplitChopperReply(self,"chopper1",pBueffel);
if(!pPriv->iSingle)
{
/* second send: get next second chopper line */
iRet = SerialWriteRead(&(pPriv->pData),"",pBueffel,1023);
if(iRet < 0)
{
pPriv->iError = iRet;
return 0;
}
SplitChopperReply(self,"chopper2",pBueffel);
}
return 1;
}
/*-------------------------------------------------------------------------*/
static int DoChoTask(void *pData)
{
pCodri self = NULL;
pDoCho pPriv = NULL;
int iCode, iRet;
char pDummy[60];
self = (pCodri)pData;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
/* check for stop */
if(pPriv->iStop)
return 0;
/* check if it is time to run a status request */
if(time(NULL) > pPriv->tRefresh)
{
/* try, fix error */
if(pPriv->iError != 0)
{
self->GetError(self,&iCode,pDummy,59);
iRet = self->TryFixIt(self,iCode);
if(iRet == CHFAIL)
{
pPriv->tRefresh = time(NULL) + pPriv->iRefreshIntervall;
return 1;
}
}
/* do it */
DoChoStatus(self);
pPriv->tRefresh = time(NULL) + pPriv->iRefreshIntervall;
}
return 1;
}
/*------------------------------------------------------------------------*/
static void DoChoKill(void *pData)
{
pCodri self = NULL;
pDoCho pPriv = NULL;
self = (pCodri)pData;
if(!self)
return;
pPriv = (pDoCho)self->pPrivate;
if(!pPriv)
return;
if(pPriv->pData)
{
SerialClose(&(pPriv->pData));
pPriv->pData = NULL;
}
if(pPriv->pHost)
free(pPriv->pHost);
if(pPriv->pPar)
DeleteStringDict(pPriv->pPar);
free(pPriv);
}
/*-------------------------------------------------------------------------*/
static int DoChoInit(pCodri self)
{
pDoCho pPriv = NULL;
int iRet;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
pPriv->iError = 0;
/* first open the connection to the serial port server and channel */
iRet = SerialOpen(&(pPriv->pData),pPriv->pHost,pPriv->iPort,
pPriv->iChannel);
if(iRet <= 0)
{
pPriv->iError = iRet;
return 0;
}
/* configure the connection */
SerialConfig(&(pPriv->pData),10000);
SerialATerm(&(pPriv->pData),"1\r\n");
SerialSendTerm(&(pPriv->pData),"\r");
pPriv->iStop = 0;
pPriv->tRefresh = 0; /* force a status request when first run */
/* start the update task */
if(pPriv->lTask == 0)
{
pPriv->lTask = TaskRegister(pServ->pTasker,
DoChoTask,
NULL,
NULL,
self,
1);
}
return 1;
}
/*------------------------------------------------------------------------*/
static int DoChoClose(pCodri self)
{
pDoCho pPriv = NULL;
int iRet;
long lVal;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
if(pPriv->pData)
{
SerialClose(&(pPriv->pData));
pPriv->pData = NULL;
}
return 1;
}
/*------------------------------------------------------------------------*/
static int DoChoDelete(pCodri self)
{
pDoCho pPriv = NULL;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
if(pPriv->pData)
{
SerialClose(&(pPriv->pData));
pPriv->pData = NULL;
}
if(pPriv->pHost)
free(pPriv->pHost);
if(pPriv->pPar)
DeleteStringDict(pPriv->pPar);
free(pPriv);
return 1;
}
/*--------------------------------------------------------------------------*/
static int DoChoSetPar2(pCodri self, char *parname, char *pValue)
{
pDoCho pPriv = NULL;
char pCommand[80], pReply[132];
char pState[20];
int iRet;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
/* deal with our four parameters */
if(strcmp(parname,"chopper1.nspee") == 0)
{
sprintf(pCommand,"nspee 1 %s",pValue);
}
else if(strcmp(parname,"chopper2.nspee") == 0)
{
iRet = StringDictGet(pPriv->pPar,"chopper2.state",pState,19);
if(iRet && strstr(pState,"async") != NULL )
{
sprintf(pCommand,"nspee 2 %s",pValue);
}
else
{
pPriv->iError = BADSYNC;
return 0;
}
}
else if(strcmp(parname,"chopper2.nphas") == 0)
{
sprintf(pCommand,"nphas 2 %s",pValue);
}
else if(strcmp(parname,"chopper2.ratio") == 0)
{
sprintf(pCommand,"ratio 2 %s",pValue);
}
else
{
pPriv->iError = UNDRIVABLE;
return 0;
}
iRet = SerialWriteRead(&(pPriv->pData),pCommand,pReply,131);
if(iRet != 1)
{
pPriv->iError = iRet;
return 0;
}
if(strstr(pReply,"error") != NULL)
{
pPriv->iError = CHOPERROR;
strncpy(pPriv->pError,pReply,79);
return 0;
}
else
{
pPriv->iError = 0;
}
pPriv->iBusy = 1;
return 1;
}
/*-------------------------------------------------------------------------*/
static int DoChoHalt(pCodri self)
{
pDoCho pPriv = NULL;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
/*
there is no documented way to stop the Dornier chopper
system. This at least makes SICS happy.
*/
pPriv->iError = BADSTOP;
pPriv->iBusy = 0;
return 1;
}
/*---------------------------------------------------------------------------*/
static int DoChoSetPar(pCodri self, char *parname, float fValue)
{
char pValue[50];
pDoCho pPriv = NULL;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
if(strstr(parname,"nspee") != NULL)
{
sprintf(pValue,"%d",(int)fValue);
}
else if(strstr(parname,"ratio") != NULL)
{
sprintf(pValue,"%d",(int)fValue);
pPriv->fRatio = (int)fValue;
}
else if(strcmp(parname,"updateintervall") == 0)
{
sprintf(pValue,"%d",(int)fValue);
StringDictUpdate(pPriv->pPar,"updateintervall",pValue);
pPriv->iRefreshIntervall = (int)fValue;
return 1;
}
else
{
sprintf(pValue,"%f",fValue);
}
return DoChoSetPar2(self,parname, pValue);
}
/*----------------------------------------------------------------------*/
static int DoChoGetPar(pCodri self, char *parname,
char *pBuffer, int iBufLen)
{
pDoCho pPriv = NULL;
int iRet;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
if(pPriv->iError != 0)
{
self->GetError(self,&iRet,pBuffer,iBufLen);
return 0;
}
iRet = StringDictGet(pPriv->pPar,parname,pBuffer,iBufLen);
if(!iRet)
{
pPriv->iError = UNKNOWNPAR;
return 0;
}
return 1;
}
/*-----------------------------------------------------------------------*/
static int DoChoCheckPar(pCodri self, char *parname)
{
pDoCho pPriv = NULL;
char pVal1[20], pVal2[20];
float fTarget, fIst, fDelta;
int iRet;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
/* check the busy flag first */
if(pPriv->iBusy)
return HWBusy;
/* was there an error in the status show? */
if(pPriv->iError != 0)
{
return HWFault;
}
/* updateintervall is always HWIdle */
if(strcmp(parname,"updateintervall") == 0)
{
return HWIdle;
}
/* OK, got a new status let us check the parameter */
/* chopper 1 speed */
if(strcmp(parname,"chopper1.nspee") == 0)
{
iRet = StringDictGet(pPriv->pPar,"chopper1.nspee",pVal1,19);
iRet += StringDictGet(pPriv->pPar,"chopper1.aspee",pVal2,19);
if(iRet != 2)
{
pPriv->iError = PARERROR;
return HWFault;
}
sscanf(pVal1,"%f",&fTarget);
sscanf(pVal2,"%f",&fIst);
fDelta = fTarget - fIst;
if(fDelta < 0.0)
fDelta = -fDelta;
if(fDelta > 50)
{
return HWBusy;
}
else
{
return HWIdle;
}
}
/* chopper 2 speed */
if(strcmp(parname,"chopper2.nspee") == 0)
{
iRet = StringDictGet(pPriv->pPar,"chopper2.nspee",pVal1,19);
iRet += StringDictGet(pPriv->pPar,"chopper2.aspee",pVal2,19);
if(iRet != 2)
{
pPriv->iError = PARERROR;
return HWFault;
}
sscanf(pVal1,"%f",&fTarget);
sscanf(pVal2,"%f",&fIst);
fDelta = fTarget - fIst;
if(fDelta < 0.0)
fDelta = -fDelta;
if(fDelta > 5.)
{
return HWBusy;
}
else
{
return HWIdle;
}
}
/* phase */
if(strcmp(parname,"chopper2.nphas") == 0)
{
iRet = StringDictGet(pPriv->pPar,"chopper2.dphas",pVal1,19);
sscanf(pVal1,"%f",&fDelta);
if(fDelta < 0.)
fDelta = - fDelta;
if(fDelta > 0.3)
{
return HWBusy;
}
else
{
return HWIdle;
}
}
/* ratio */
if(strcmp(parname,"chopper2.ratio") == 0)
{
iRet = StringDictGet(pPriv->pPar,"chopper2.ratio",pVal1,19);
sscanf(pVal1,"%f",&fIst);
fDelta = fIst - pPriv->fRatio;
if(fDelta < 0.)
fDelta = - fDelta;
if(fDelta > 0.3)
{
return HWBusy;
}
else
{
return HWIdle;
}
}
pPriv->iError = UNKNOWNPAR;
return HWFault;
}
/*-------------------------------------------------------------------------*/
static int DoChoError(pCodri self, int *iCode, char *pError, int iLen)
{
pDoCho pPriv = NULL;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
*iCode = pPriv->iError;
switch(pPriv->iError)
{
case UNDRIVABLE:
strncpy(pError,"Parameter is not drivable",iLen);
break;
case UNKNOWNPAR:
strncpy(pError,"Parameter is unknown",iLen);
break;
case PARERROR:
strncpy(pError,"Internal parameter error",iLen);
break;
case BADSYNC:
strncpy(pError,"Cannot drive slave chopper",iLen);
break;
case CHOPERROR:
strncpy(pError,pPriv->pError,iLen);
break;
case BADSTOP:
strncpy(pError,
"User called STOP. WARNING: chopper is still untamed!",
iLen);
break;
default:
SerialError(pPriv->iError,pError,iLen);
break;
}
pPriv->iError = 0;
return 1;
}
/*------------------------------------------------------------------------*/
static int DoChoFix(pCodri self, int iCode)
{
pDoCho pPriv = NULL;
int iRet;
assert(self);
pPriv = (pDoCho)self->pPrivate;
assert(pPriv);
switch(iCode)
{
/* network errors */
case EL734__BAD_FLUSH:
case EL734__BAD_RECV:
case EL734__BAD_RECV_NET:
case EL734__BAD_RECV_UNKN:
case EL734__BAD_RECVLEN:
case EL734__BAD_RECV1:
case EL734__BAD_RECV1_PIPE:
case EL734__BAD_RNG:
case EL734__BAD_SEND:
case EL734__BAD_SEND_PIPE:
case EL734__BAD_SEND_NET:
case EL734__BAD_SEND_UNKN:
case EL734__BAD_SENDLEN:
case NOCONNECTION:
SerialForceClose(&(pPriv->pData));
pPriv->pData = NULL;
iRet = SerialOpen(&(pPriv->pData),pPriv->pHost,
pPriv->iPort,pPriv->iChannel);
if(iRet == 1 )
{
return CHREDO;
}
else
{
return CHFAIL;
}
break;
case EL734__FORCED_CLOSED:
iRet = DoChoInit(self);
if(iRet)
{
return CHREDO;
}
else
{
return CHFAIL;
}
break;
default:
return CHFAIL;
break;
}
return CHFAIL;
}
/*-------------------------------------------------------------------------*/
pCodri MakeDoChoDriver(char *pHost, int iPort, int iChannel, int iSingle)
{
pCodri pNew = NULL;
pDoCho pPriv = NULL;
char *pText;
/* allocate memory */
pText = (char *)malloc(4096*sizeof(char));
pNew = (pCodri)malloc(sizeof(Codri));
pPriv = (pDoCho)malloc(sizeof(DoCho));
if( !pText || !pNew || !pPriv)
{
return NULL;
}
memset(pText,0,4096);
memset(pNew,0,sizeof(Codri));
memset(pPriv,0,sizeof(DoCho));
/* initialize private data structure */
pPriv->pHost = strdup(pHost);
pPriv->iPort = iPort;
pPriv->iChannel = iChannel;
pPriv->pData = NULL;
pPriv->iRefreshIntervall = 60;
pPriv->pPar = CreateStringDict();
pPriv->tRefresh = time(NULL);
pPriv->iSingle = iSingle;
if(!pPriv->pPar)
{
free(pText);
free(pNew);
free(pPriv);
return NULL;
}
/* install codri */
pNew->Init = DoChoInit;
pNew->Close = DoChoClose;
pNew->Delete = DoChoDelete;
pNew->SetPar = DoChoSetPar;
pNew->SetPar2 = DoChoSetPar2;
pNew->GetPar = DoChoGetPar;
pNew->CheckPar = DoChoCheckPar;
pNew->GetError = DoChoError;
pNew->TryFixIt = DoChoFix;
pNew->Halt = DoChoHalt;
pNew->pParList = pText;
strcpy(pNew->pParList,"updateintervall,");
StringDictAddPair(pPriv->pPar,"updateintervall","60");
pNew->pPrivate = pPriv;
return pNew;
}
Reference in New Issue View Git Blame Copy Permalink