diff --git a/amor2t.c b/amor2t.c index 41bbb61..047b3a0 100644 --- a/amor2t.c +++ b/amor2t.c @@ -674,7 +674,7 @@ pAmor2T pNew, pAOM = NULL; int i, iRet; char pBueffel[512]; - char *pMot = NULL; + const char *pMot = NULL; if(argc < 4) { @@ -710,7 +710,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTMOM] = FindMotor(pSics,pMot); + pNew->aEngine[MOTMOM] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTMOM]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -726,7 +726,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTSOM] = FindMotor(pSics,pMot); + pNew->aEngine[MOTSOM] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTSOM]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -742,7 +742,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTCOZ] = FindMotor(pSics,pMot); + pNew->aEngine[MOTCOZ] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTCOZ]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -758,7 +758,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTCOX] = FindMotor(pSics,pMot); + pNew->aEngine[MOTCOX] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTCOX]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -774,7 +774,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTSTZ] = FindMotor(pSics,pMot); + pNew->aEngine[MOTSTZ] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTSTZ]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -790,7 +790,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTSOZ] = FindMotor(pSics,pMot); + pNew->aEngine[MOTSOZ] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTSOZ]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -806,7 +806,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTD4B] = FindMotor(pSics,pMot); + pNew->aEngine[MOTD4B] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTD4B]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -822,7 +822,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTD5B] = FindMotor(pSics,pMot); + pNew->aEngine[MOTD5B] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTD5B]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -838,7 +838,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTCOM] = FindMotor(pSics,pMot); + pNew->aEngine[MOTCOM] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTCOM]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -854,7 +854,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTAOZ] = FindMotor(pSics,pMot); + pNew->aEngine[MOTAOZ] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTAOZ]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -870,7 +870,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTAOM] = FindMotor(pSics,pMot); + pNew->aEngine[MOTAOM] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTAOM]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); @@ -886,7 +886,7 @@ A2TKill(pNew); return 0; } - pNew->aEngine[MOTC3Z] = FindMotor(pSics,pMot); + pNew->aEngine[MOTC3Z] = FindMotor(pSics,(char *)pMot); if(!pNew->aEngine[MOTC3Z]) { sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot); diff --git a/amorstat.c b/amorstat.c index 4c75328..7260f89 100644 --- a/amorstat.c +++ b/amorstat.c @@ -50,7 +50,8 @@ static int iTOF = 0; static pHistMem pHMHM = NULL; /*-------------------------------------------------------------------------*/ - static int HMCountStartCallback(int iEvent, void *pEvent, void *pUser) + static int HMCountStartCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { SConnection *pCon = (SConnection *)pUser; const float *fTime = NULL; @@ -75,15 +76,18 @@ iTime[i+1] = htonl((int)((fTime[i]/10.)*65536.)); } /* send new time binning to all clients */ + SCPushContext2(pCon,cc); SCWrite(pCon,"TOFClear",eError); SCWriteUUencoded(pCon,"arrowaxis_time",iTime, (iLength+1)*sizeof(int)); + SCPopContext(pCon); free(iTime); } return 1; } /*-------------------------------------------------------------------------*/ - static int ScanStartCallback(int iEvent, void *pEvent, void *pUser) + static int ScanStartCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { float *fAxis = NULL; int *iAxis = NULL; @@ -116,16 +120,19 @@ iAxis[i+1] = htonl((int)(fAxis[i]*65536.)); } /* send new axis to client */ + SCPushContext2(pCon,cc); SCWrite(pCon,"SCANClear",eError); SCWriteUUencoded(pCon,pBueffel,iAxis, (iLength+1)*sizeof(int)); + SCPopContext(pCon); free(iAxis); free(fAxis); } return 1; } /*------------------------------------------------------------------------*/ - static int ScanPointCallback(int iEvent, void *pEvent, void *pUser) + static int ScanPointCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { long *lData = NULL; int *iData = NULL; @@ -155,6 +162,7 @@ iData[i+1] = htonl((int)(lData[i])); } /* send counts to client */ + SCPushContext2(pCon,cc); SCWriteUUencoded(pCon,"arrow_spinupup",iData, (iLength+1)*sizeof(int)); /* send counts for other detector */ @@ -166,6 +174,7 @@ SCWriteUUencoded(pCon,"arrow_spinuplo",iData, (iLength+1)*sizeof(int)); /* to do: check for polarization and send spinlo */ + SCPopContext(pCon); free(iData); free(lData); } @@ -201,7 +210,8 @@ return 0; } /*------------------------------------------------------------------------*/ - static int LoadCallback(int iEvent, void *pEvent, void *pUser) + static int LoadCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { pAmorStat pAS = NULL; SConnection *pCon = NULL; @@ -212,7 +222,9 @@ pCon = (SConnection *)pUser; assert(pAS); assert(pCon); - SendLoadedData(pAS,pCon); + SCPushContext2(pCon,cc); + SendLoadedData(pAS,pCon); + SCPopContext(pCon); } return 1; } @@ -378,6 +390,7 @@ long lID; pDummy pDum = NULL; pICallBack pCall = NULL; + commandContext comCon; assert(self); assert(pCon); @@ -386,8 +399,9 @@ iTOF invoke the apropriate callbacks in order to force an initial update. */ + comCon = SCGetContext(pCon); /* file load callback */ - lID = RegisterCallback(self->pCall, FILELOADED, LoadCallback, + lID = RegisterCallback(self->pCall, comCon,FILELOADED, LoadCallback, pCon, NULL); SCRegister(pCon,pServ->pSics, self->pCall,lID); SendLoadedData(self,pCon); @@ -397,31 +411,31 @@ pCall = pDum->pDescriptor->GetInterface(pDum,CALLBACKINTERFACE); if(pCall) { - lID = RegisterCallback(pCall,SCANSTART,ScanStartCallback, + lID = RegisterCallback(pCall,comCon,SCANSTART,ScanStartCallback, pCon, NULL); SCRegister(pCon,pServ->pSics,pCall,lID); - lID = RegisterCallback(pCall,SCANPOINT,ScanPointCallback, + lID = RegisterCallback(pCall,comCon,SCANPOINT,ScanPointCallback, pCon, NULL); SCRegister(pCon,pServ->pSics,pCall,lID); - lID = RegisterCallback(pCall,SCANEND,ScanPointCallback, + lID = RegisterCallback(pCall,comCon,SCANEND,ScanPointCallback, pCon, NULL); SCRegister(pCon,pServ->pSics,pCall,lID); if(iTOF == 0) { - ScanStartCallback(SCANSTART,pDum,pCon); - ScanPointCallback(SCANPOINT,pDum,pCon); + ScanStartCallback(SCANSTART,pDum,pCon,SCGetContext(pCon)); + ScanPointCallback(SCANPOINT,pDum,pCon,SCGetContext(pCon)); } } pDum = (pDummy)self->pHM; pCall = pDum->pDescriptor->GetInterface(pDum,CALLBACKINTERFACE); if(pCall) { - lID = RegisterCallback(pCall,COUNTSTART,HMCountStartCallback, + lID = RegisterCallback(pCall,comCon,COUNTSTART,HMCountStartCallback, pCon, NULL); SCRegister(pCon,pServ->pSics,pCall,lID); if(iTOF == 1) { - HMCountStartCallback(COUNTSTART,pDum,pCon); + HMCountStartCallback(COUNTSTART,pDum,pCon,SCGetContext(pCon)); } } return 1; @@ -1001,7 +1015,7 @@ } else if(strcmp(argv[1],"tofmode") == 0) { - HMCountStartCallback(COUNTSTART,NULL,pCon); + HMCountStartCallback(COUNTSTART,NULL,pCon,SCGetContext(pCon)); return 1; } else diff --git a/dornier2.c b/dornier2.c index 9e3293c..0c1c749 100644 --- a/dornier2.c +++ b/dornier2.c @@ -595,7 +595,7 @@ static int DornierStatNew(pVelSelDriv self, int *iCode, float *fCur){ { pVelSelDriv pNew = NULL; pDornier pDorn = NULL; - char *pPtr = NULL; + const char *pPtr = NULL; int iVal, iRet, iPort; char pHost[132]; diff --git a/ecb.c b/ecb.c index 8fbbb8e..f66b0cb 100644 --- a/ecb.c +++ b/ecb.c @@ -24,7 +24,7 @@ #define READ_BYTES 3 #define WRITE_BYTES 4 #define DMAREAD 5 -#define ECB_BYTES 65536L +#define ECB_BYTES 65535 typedef union /* Used to swap bytes in 'address' and 'byte_count' */ { @@ -112,7 +112,7 @@ static int ecbPrepareIO(pECB self, int func, unsigned short address, Swap save, adr, count; int status, bytes; - if(byteCount > ECB_BYTES){ + if(byteCount >= ECB_BYTES){ self->lastError = ECBOVERFLOW; return 0; } diff --git a/el734hp.c b/el734hp.c index f8b87af..90a9cd0 100644 --- a/el734hp.c +++ b/el734hp.c @@ -392,6 +392,27 @@ static void EL734Error(void *pData, int *iCode, char *error, int errLen){ } } /*----------------------------------------------------------------------*/ +static int EL734Halt(void *pData){ + pEL734Driv self = NULL; + int status; + char pCommand[50],pReply[80]; + + self = (pEL734Driv)pData; + assert(self); + + snprintf(pCommand,79,"s %d\r",self->iMotor); + status = transactEL734(self->controller,pCommand,strlen(pCommand), + pReply,79); + if(status != 1){ + self->errorCode = status; + return 0; + } + if(!checkResponse(self,pReply)){ + return 0; + } + return 1; +} +/*----------------------------------------------------------------------*/ static int EL734Fix(void *pData, int iCode, float fValue){ pEL734Driv self = NULL; int status, msr, i, len = 49; @@ -404,7 +425,10 @@ static int EL734Fix(void *pData, int iCode, float fValue){ case BADADR: case BADCMD: case BADPAR: + return MOTREDO; case BADBSY: + EL734Halt(pData); + SicsWait(1); return MOTREDO; case TIMEOUT: for(i = 0; i < 3; i++){ @@ -466,27 +490,6 @@ static int EL734Fix(void *pData, int iCode, float fValue){ } return MOTFAIL; } -/*----------------------------------------------------------------------*/ -static int EL734Halt(void *pData){ - pEL734Driv self = NULL; - int status; - char pCommand[50],pReply[80]; - - self = (pEL734Driv)pData; - assert(self); - - snprintf(pCommand,79,"s %d\r",self->iMotor); - status = transactEL734(self->controller,pCommand,strlen(pCommand), - pReply,79); - if(status != 1){ - self->errorCode = status; - return 0; - } - if(!checkResponse(self,pReply)){ - return 0; - } - return 1; -} /*--------------------------------------------------------------------*/ static int EL734GetPar(void *pData, char *name, float *fValue){ diff --git a/fowrite.c b/fowrite.c index 8582197..1a8b352 100644 --- a/fowrite.c +++ b/fowrite.c @@ -67,7 +67,8 @@ /*------------------ The Countstart Callback Function ----------------------*/ - static int Countstartcallback(int iEvent, void *pEventData, void *pUser) + static int Countstartcallback(int iEvent, void *pEventData, void *pUser, + commandContext cc) { pFoWrite self = NULL; @@ -85,7 +86,8 @@ return 1; } /*------------------ The Countend Callback Function ----------------------*/ - static int Countendcallback(int iEvent, void *pEventData, void *pUser) + static int Countendcallback(int iEvent, void *pEventData, void *pUser, + commandContext cc) { pFoWrite self = NULL; @@ -941,6 +943,7 @@ pICallBack pCall = NULL; pDummy pDum; pHMcontrol pHMC = NULL; + commandContext comCon; /* check arguments */ if(argc < 4 ) @@ -1025,9 +1028,11 @@ { pNew->pCount = (pCounter)pCom->pData; } - - RegisterCallback(pHMC->pCall,COUNTSTART,Countstartcallback,pNew,NULL); - RegisterCallback(pHMC->pCall,COUNTEND,Countendcallback,pNew,NULL); + + comCon.transID = 0; + strncpy(comCon.deviceID,"internal",SCDEVIDLEN); + RegisterCallback(pHMC->pCall,comCon,COUNTSTART,Countstartcallback,pNew,NULL); + RegisterCallback(pHMC->pCall,comCon,COUNTEND,Countendcallback,pNew,NULL); /* install command */ AddCommand(pSics,"StoreFocus",FoAction,KillFoWrite,pNew); diff --git a/libpsi.a b/libpsi.a index d9cb397..9264a7b 100644 Binary files a/libpsi.a and b/libpsi.a differ diff --git a/make_gen b/make_gen index 131276f..3d8d103 100644 --- a/make_gen +++ b/make_gen @@ -16,10 +16,10 @@ OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \ el755driv.o amorscan.o serial.o scontroller.o t_update.o \ t_rlp.o t_conv.o el737hpdriv.o dornier2.o el734hp.o \ el737hpv2driv.o swmotor2.o tricssupport.o amorcomp.o \ - $(MZOBJ) amordrive.o amorset.o \ - dgrambroadcast.o sinq.o tabledrive.o + $(MZOBJ) amordrive.o amorset.o tcpdornier.o\ + dgrambroadcast.o sinq.o tabledrive.o tcpdocho.o -MZOBJ=fsm.o logger.o sugar.o pardef.o ease.o strobj.o oxinst.o logreader.o \ +MZOBJ=fsm.o logger.o sugar.o pardef.o ease.o strobj.o oxinst.o \ ipsdriv.o ilmdriv.o itcdriv.o ighdriv.o euro2kdriv.o modbus.o libpsi.a: $(OBJ) diff --git a/nextrics.c b/nextrics.c index 0a5ce10..d0c51fe 100644 --- a/nextrics.c +++ b/nextrics.c @@ -1339,7 +1339,7 @@ name of hkl object holding crystallographic information return iRet; } /*-------------------------------------------------------------------------*/ - static int FrameInterest(int iEvent, void *pEvent, void *pUser) + static int FrameInterest(int iEvent, void *pEvent, void *pUser, commandContext cc) { SConnection *pCon = NULL; int *iFrame; @@ -1354,7 +1354,9 @@ name of hkl object holding crystallographic information iFrame = (int *)pEvent; assert(pCon); sprintf(pBueffel,"framenumber = %d",*iFrame); + SCPushContext2(pCon,cc); SCWrite(pCon,pBueffel,eWarning); + SCPopContext(pCon); return 1; } /*------------------------------------------------------------------------- @@ -1454,6 +1456,7 @@ name of hkl object holding crystallographic information char pBueffel[1024]; int iRet, iDet, iFrame; long lID; + commandContext comCon; self = (pNexTrics)pData; assert(self); @@ -1470,6 +1473,7 @@ name of hkl object holding crystallographic information /* install an error handler */ NXMSetError((void *)pCon,SNError); + comCon = SCGetContext(pCon); strtolower(argv[1]); if(strcmp(argv[1],"start") == 0) { @@ -1505,7 +1509,7 @@ name of hkl object holding crystallographic information } else if(strcmp(argv[1],"interest") == 0) { - lID = RegisterCallback(self->pCall, NEWFRAME, FrameInterest, + lID = RegisterCallback(self->pCall, comCon, NEWFRAME, FrameInterest, pCon, NULL); SCRegister(pCon,pSics, self->pCall,lID); SCSendOK(pCon); diff --git a/pardef.c b/pardef.c index 033d4eb..2735efe 100644 --- a/pardef.c +++ b/pardef.c @@ -239,12 +239,15 @@ int ParLog(void *object) { return next; } /*-------------------------------------------------------------------------*/ -static int ParCallBack(int event, void *eventData, void *userData) { +static int ParCallBack(int event, void *eventData, void *userData, + commandContext cc) { char *pBuf = (char *)eventData; SConnection *con = (SConnection *)userData; if (event == VALUECHANGE) { + SCPushContext2(con,cc); SCWrite(con,pBuf,eValue); + SCPopContext(con); return 1; } return 1; @@ -356,7 +359,7 @@ static int ParExecute(SConnection *con, SicsInterp *sics, void *object, int argc o->pCall = CreateCallBackInterface(); } assert(o->pCall); - id = RegisterCallback(o->pCall, VALUECHANGE, ParCallBack, con, NULL); + id = RegisterCallback(o->pCall, SCGetContext(con),VALUECHANGE, ParCallBack, con, NULL); SCRegister(con, pServ->pSics, o->pCall, id); SCSendOK(con); return 1; diff --git a/pimotor.c b/pimotor.c index d162237..94d6b87 100644 --- a/pimotor.c +++ b/pimotor.c @@ -297,7 +297,7 @@ pC804Driv pNew = NULL; int iRet, iVal, iTmo; double dVal; - char *pPar = NULL; + const char *pPar = NULL; char pCommand[20], pReply[40]; /* allocate space: the final frontier */ diff --git a/pipiezo.c b/pipiezo.c index 6c9ef39..80d0d2c 100644 --- a/pipiezo.c +++ b/pipiezo.c @@ -284,7 +284,7 @@ pPiPiezo pNew = NULL; int iRet, iVal, iTmo; double dVal; - char *pPar = NULL; + const char *pPar = NULL; char pCommand[20], pReply[40]; /* allocate space: the final frontier */ diff --git a/polterwrite.c b/polterwrite.c index 061a3c1..f2ae394 100644 --- a/polterwrite.c +++ b/polterwrite.c @@ -53,7 +53,8 @@ typedef struct { static void PoldiUpdate(pPolterdi self, SConnection *pCon); /*------------------ The Countstart Callback Function ----------------------*/ - static int Countstartcallback(int iEvent, void *pEventData, void *pUser) + static int Countstartcallback(int iEvent, void *pEventData, void *pUser, + commandContext cc) { pPolterdi self = NULL; @@ -71,7 +72,8 @@ typedef struct { return 1; } /*------------------ The Countend Callback Function ----------------------*/ - static int Countendcallback(int iEvent, void *pEventData, void *pUser) + static int Countendcallback(int iEvent, void *pEventData, void *pUser, + commandContext cc) { pPolterdi self = NULL; @@ -665,6 +667,7 @@ int PolterInstall(SConnection *pCon, SicsInterp *pSics, pICallBack pCall = NULL; pDummy pDum; pHistMem pHist = NULL; + commandContext comCon; /* configure fortify */ /* @@ -711,8 +714,10 @@ int PolterInstall(SConnection *pCon, SicsInterp *pSics, KillPolterdi(pNew); return 0; } - RegisterCallback(pCall,COUNTSTART,Countstartcallback,pNew,NULL); - RegisterCallback(pCall,COUNTEND,Countendcallback,pNew,NULL); + comCon.transID = 0; + strncpy(comCon.deviceID,"internal",SCDEVIDLEN); + RegisterCallback(pCall,comCon,COUNTSTART,Countstartcallback,pNew,NULL); + RegisterCallback(pCall,comCon,COUNTEND,Countendcallback,pNew,NULL); AddCommand(pSics,"storedata",PolterAction,KillPolterdi,pNew); return 1; diff --git a/psi.c b/psi.c index 1aab88f..eaa9a0b 100644 --- a/psi.c +++ b/psi.c @@ -55,6 +55,12 @@ #include "tabledrive.h" #include "amorset.h" +/* + from tcpdornier.c +*/ +extern int VelSelTcpFactory(SConnection *pCon, SicsInterp *pSics, void *pData, + int argc, char *argv[]); + /*--------------------------------------------------------------------------*/ void SiteInit(void) { @@ -62,8 +68,7 @@ void SiteInit(void) { /* insert here initialization routines ... */ - INIT(LogReaderInit); - INIT(IlmStartup); + INIT(IlmStartup); INIT(IpsStartup); INIT(ItcStartup); INIT(IghStartup); @@ -102,6 +107,7 @@ static void AddPsiCommands(SicsInterp *pInter){ AddCommand(pInter,"MakeSinq",SinqFactory,NULL,NULL); AddCommand(pInter,"MakeTableDrive",TableDriveFactory,NULL,NULL); AddCommand(pInter,"MakeAmorSet",AmorSetFactory,NULL,NULL); + AddCommand(pInter,"MakeTCPSelector",VelSelTcpFactory,NULL,NULL); /* AddCommand(pInter,"MakeDifrac",MakeDifrac,NULL,NULL); */ @@ -121,6 +127,7 @@ static void RemovePsiCommands(SicsInterp *pSics){ RemoveCommand(pSics,"MakeAmor2T"); RemoveCommand(pSics,"MakeStoreAmor"); RemoveCommand(pSics,"MakeAmorStatus"); + RemoveCommand(pSics,"MakeTCPSelector"); /* RemoveCommand(pSics,"MakeDifrac"); */ @@ -273,6 +280,8 @@ static pVelSelDriv CreatePsiVelSelDriv(char *name, char *array, extern pCodri MakeDoChoDriver(char *pHost, int iPort, int iChannel, int iSingle); extern pCodri MakeCookerDriver(char *pHost, int iPort, int iChannel); +extern pCodri MakeTcpDoChoDriver(char *tclArray, SConnection *pCon); + /*-------------------------------------------------------------------*/ static pCodri CreatePsiController(SConnection *pCon,int argc, char *argv[]){ pCodri pNew = NULL; @@ -312,6 +321,13 @@ static pCodri CreatePsiController(SConnection *pCon,int argc, char *argv[]){ } } pNew = MakeDoChoDriver(argv[1],iPort,iChannel,iSingle); + } else if(strcmp(argv[0],"tcpdocho") == 0){ + if(argc < 2){ + SCWrite(pCon,"ERROR: insufficient number of argumets for creating TcpDoCho", + eError); + return NULL; + } + return MakeTcpDoChoDriver(argv[1], pCon); }else if(strcmp(argv[0],"sanscook") == 0) { if(argc < 4){ SCWrite(pCon, diff --git a/scontroller.c b/scontroller.c index 141a59a..7ec31f4 100644 --- a/scontroller.c +++ b/scontroller.c @@ -43,7 +43,7 @@ EXTERN void SerialMurder(ClientData pData) } /*------------------ a forward declaration -----------------------------*/ EXTERN int SurielSend(ClientData clientData, Tcl_Interp *interp, - int argc, char *argv[]); + int argc, const char *argv[]); /*---------------------------------------------------------------------------- Controller is the main entry point for this stuff. It connects to a motor @@ -143,7 +143,7 @@ int Controller(ClientData clientData, Tcl_Interp *interp, ----------------------------------------------------------------------------*/ EXTERN int SurielSend(ClientData clientData, Tcl_Interp *interp, - int argc, char *argv[]) + int argc, const char *argv[]) { char pBueffel[256]; char pAnswer[256]; @@ -183,7 +183,7 @@ EXTERN int SurielSend(ClientData clientData, Tcl_Interp *interp, Tcl_AppendResult(interp, "Expected parameter after -tmo",NULL); return TCL_ERROR; } - iRet = SerialSendTerm(&(pData),argv[2]); + iRet = SerialSendTerm(&(pData),(char *)argv[2]); if(iRet != 1) { Tcl_AppendResult(interp,"To many characters for terminator",NULL); @@ -198,7 +198,7 @@ EXTERN int SurielSend(ClientData clientData, Tcl_Interp *interp, Tcl_AppendResult(interp, "Expected parameter after -tmo",NULL); return TCL_ERROR; } - iRet = SerialATerm(&(pData),argv[2]); + iRet = SerialATerm(&(pData),(char *)argv[2]); if(!iRet) { Tcl_AppendResult(interp,"To many characters for terminator",NULL); diff --git a/serial.c b/serial.c index 285d325..bdaf701 100644 --- a/serial.c +++ b/serial.c @@ -12,7 +12,7 @@ #include "sics.h" extern int Controller(ClientData pData, Tcl_Interp *pInter, - int argc, char *argv[]); + int argc, const char *argv[]); int SerialInit(SConnection *pCon,SicsInterp *pSics, void *pData, int argc, char *argv[]) diff --git a/tasinit.c b/tasinit.c index 166529d..b2dbf9b 100644 --- a/tasinit.c +++ b/tasinit.c @@ -261,7 +261,8 @@ static int TasSaveStatus(void *self, char *name, FILE *fd) used on the variables which switch the counter box into the appropriate mode. -----------------------------------------------------------------------*/ -static int MonitorCallback(int iEvent, void *pEvent, void *pUser) +static int MonitorCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { pTASdata self = (pTASdata)pUser; assert(self); @@ -274,7 +275,8 @@ static int MonitorCallback(int iEvent, void *pEvent, void *pUser) return 1; } /*---------------------------------------------------------------------*/ -static int TimerCallback(int iEvent, void *pEvent, void *pUser) +static int TimerCallback(int iEvent, void *pEvent, void *pUser, + commandContext cc) { pTASdata self = (pTASdata)pUser; assert(self); @@ -309,7 +311,8 @@ static int RecalcAction(SConnection *pCon, SicsInterp *pSics, void *pData, to allow for the analyzer shielding to settle down. This is done through this callback function ---------------------------------------------------------------------------*/ -static int A6WaitCallback(int iEvent, void *pEventData, void *pUserData) +static int A6WaitCallback(int iEvent, void *pEventData, void *pUserData, + commandContext cc) { if(iEvent == MOTEND) { @@ -340,6 +343,7 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData, pSicsVariable pVar = NULL; CommandList *pCom = NULL; pMotor pMot = NULL; + commandContext comCon; /* check arguments*/ if(argc < 2) @@ -404,14 +408,16 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData, the variables should have been accessed earlier on. */ pVar = FindVariable(pSics,"MN"); + comCon.transID = 0; + strncpy(comCon.deviceID,"internal",SCDEVIDLEN); if(pVar) { - RegisterCallback(pVar->pCall,VALUECHANGE,MonitorCallback,pNew,NULL); + RegisterCallback(pVar->pCall,comCon,VALUECHANGE,MonitorCallback,pNew,NULL); } pVar = FindVariable(pSics,"TI"); if(pVar) { - RegisterCallback(pVar->pCall,VALUECHANGE,TimerCallback,pNew,NULL); + RegisterCallback(pVar->pCall,comCon,VALUECHANGE,TimerCallback,pNew,NULL); } /* @@ -420,7 +426,7 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData, pMot = FindMotor(pSics,"a6"); if(pMot != NULL) { - RegisterCallback(pMot->pCall,MOTEND,A6WaitCallback,NULL,NULL); + RegisterCallback(pMot->pCall,comCon,MOTEND,A6WaitCallback,NULL,NULL); } diff --git a/tasscan.c b/tasscan.c index 6bfb1bf..b4f8d1a 100644 --- a/tasscan.c +++ b/tasscan.c @@ -599,7 +599,7 @@ static int TASScanPoint(pScanData self, int iPoint) static void fixPowder(unsigned char tasTargetMask[20]){ char *pPtr = NULL; - pPtr = Tcl_GetVar(pServ->pSics->pTcl,"powder",TCL_GLOBAL_ONLY); + pPtr = (char *)Tcl_GetVar(pServ->pSics->pTcl,"powder",TCL_GLOBAL_ONLY); if(pPtr){ if(strstr(pPtr,"1") != NULL){ tasTargetMask[2] = 0; diff --git a/tcpdocho.c b/tcpdocho.c new file mode 100644 index 0000000..a49c06f --- /dev/null +++ b/tcpdocho.c @@ -0,0 +1,929 @@ +/*-------------------------------------------------------------- + This is a driver for the newer Astrium == Dornier chopper + systems which use a TCP/IP server for communication. + This driver has to take care of some ugliness: + - As of december 2005, the communication is in unicode! + To go from ASCII to unicode and back one has to + add a 0x00 before each character or to remove it. + - The controller is slow in responding and the controller + must be watched in the environment monitor. This is taken + care of by a special SICS task which updates the status + regularly and returning cached values anytime else. + + + Inititial Implementation: Mark Koennecke, December 2005 + ------------------------------------------------------------*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +/*======================================================================== + Our private data structure + ========================================================================*/ +#define ASYNMODE 0 +#define SYNCMODE 1 + +typedef struct{ + prs232 controller; + int iRefreshIntervall; + time_t nextRefresh; + long lTask; + pStringDict parameters; + int lastError; + int numChoppers; + int mode; + int timeout; + char user[132]; + char pword[132]; + int stop; + int busy; + char *config; +}TcpDoCho, *pTcpDoCho; +/*----------------------------------------------------------------------- +Error codes: +-----------------------------------------------------------------------*/ +#define WRONGMODE -8301 +#define BADCONVERSION -8302 +#define FAILEDCOMMAND -8303 +#define BADWRITE -8304 +#define BADRESPONSE -8306 +#define UNDRIVABLE -8307 +#define BADPAR -8308 +#define ESTOP -8309 + +extern char *trim(char *str); +#define ABS(x) (x < 0 ? -(x) : (x)) + +#define SPEEDTOL 2 +#define PHASETOL .5 +/*=============== support functions ===================================*/ +static int asciiToUnicode(char *input, char **output){ + int len, i; + char *result = NULL; + + len = strlen(input); + result = (char *)malloc(2*len*sizeof(char)); + if(result == NULL){ + return 0; + } + memset(result,0,2*len*sizeof(char)); + for(i = 0; i < len; i++){ + result[i*2] = input[i]; + } + *output = result; + return 2*len; +} +/*-------------------------------------------------------------------*/ +static int unicodeToAscii(char *input, int len, char **output){ + int i, alen; + char *result = NULL; + + alen = len/2; + + result = (char *)malloc((alen+1)*sizeof(char)); + if(result == NULL){ + return 0; + } + memset(result,0,(alen+1)*sizeof(char)); + for(i = 0; i < alen; i++){ + result[i] = input[i*2]; + } + *output = result; + return alen; +} +/*----------------------------------------------------------------*/ +static int tcpDoChoSend(pTcpDoCho self, char *command, int choNum, + char *value){ + char buffer[1024]; + char *converted; + int sendlen, status; + + /* + format and send command + */ + self->lastError = 0; + if(choNum < 0){ + snprintf(buffer,1023,"#SOS#%s \r\n",command); + } else if(value != NULL){ + snprintf(buffer,1023,"#SOS#%s%1.1d: %s\r\n",command, choNum, + value); + } else { + snprintf(buffer,1023,"#SOS#%s%1.1d:\r\n",command, choNum); + } + sendlen = asciiToUnicode(buffer,&converted); + if(sendlen == 0){ + self->lastError = BADCONVERSION; + return 0; + } + status = send(self->controller->pSock->sockid,converted,sendlen,0); + free(converted); + if(status < 0){ + self->lastError = BADWRITE; + return 0; + } + return 1; +} +/*----------------------------------------------------------------*/ +static int statusComplete(char *statusMessage){ + char *pPtr = NULL; + + /* + currently the status message has no terminator. I try to find + the last component of the message which happens to be TIME and + the last # after that + */ + pPtr = strstr(statusMessage,"TIME"); + if(pPtr != NULL){ + pPtr = strstr(pPtr+6,"#"); + if(pPtr != NULL){ + return 1; + } else { + return 0; + } + } else { + return 0; + } +} +/*---------------------------------------------------------------*/ +static int readChopperNum(char *entry){ + int num; + char *pPtr = NULL; + + pPtr = strstr(entry,"CH"); + if(pPtr == NULL){ + return -1; + } + sscanf(pPtr+3,"%d",&num); + return num; +} +/*---------------------------------------------------------------*/ +static void addEntry(pTcpDoCho self, int choNum, char *entry){ + char name[80], value[80], *pPtr = NULL; + char num[5]; + + memset(name,0,80); + memset(value,0,80); + snprintf(num,5,"_%1.1d",choNum); + pPtr = strstr(entry,"="); + if(pPtr != NULL){ + strncpy(name,entry,pPtr -entry); + strncat(trim(name),num,80-strlen(trim(name))); + strcpy(value,pPtr+1); + if(StringDictExists(self->parameters,name)){ + StringDictUpdate(self->parameters,name,trim(value)); + } else { + StringDictAddPair(self->parameters,name,trim(value)); + } + } +} +/*----------------------------------------------------------------*/ +static int parseStatus(pTcpDoCho self, char *statusMessage){ + int choNum; + char entry[80], *pPtr; + + pPtr = statusMessage; + + /* skip over SOS */ + pPtr = stptok(pPtr,entry,79,"#"); + pPtr = stptok(pPtr,entry,79,"#"); + pPtr = stptok(pPtr,entry,79,"#"); + + choNum = readChopperNum(entry); + if(choNum < 0){ + self->lastError = BADRESPONSE; + return 0; + } + while((pPtr = stptok(pPtr,entry,79,"#")) != NULL){ + addEntry(self,choNum,entry); + memset(entry,0,80); + } + return 1; +} +/*-----------------------------------------------------------------*/ +static int tcpDoChoReceive(pTcpDoCho self){ + char buffer[1024]; + int len, bytesRead, bytesConverted, bufferStart, status; + char *converted = NULL; + time_t endTime; + + endTime = time(NULL) + self->timeout; + bufferStart = 0; + bytesRead = 0; + memset(buffer,0,1024*sizeof(char)); + while(time(NULL) < endTime){ + if(availableRS232(self->controller)){ + bytesRead += recv(self->controller->pSock->sockid,buffer+bufferStart, + 1024 - bytesRead,0); + if(bytesRead < 0){ + self->lastError = BADREAD; + return 0; + } + bytesConverted = unicodeToAscii(buffer,bytesRead,&converted); + if(bytesConverted == 0){ + self->lastError = BADCONVERSION; + return 0; + } + if(strstr(converted,"State") != NULL){ + if(statusComplete(converted) == 0) { + continue; + } else { + status = parseStatus(self,converted); + free(converted); + return status; + } + } + if(strstr(converted,"ACCEPT") != NULL){ + free(converted); + return 1; + } else if(strstr(converted,"NCCEPT") != NULL){ + free(converted); + self->lastError = FAILEDCOMMAND; + return 0; + } else { + self->lastError = BADRESPONSE; + return 0; + } + } else { + SicsWait(1); + } + } + self->lastError = TIMEOUT; + return 0; +} +/*-----------------------------------------------------------------*/ +static int tcpDoChoCommand(pTcpDoCho self, char *command, int choNum, + char *value){ + int status; + + status = tcpDoChoSend(self,command,choNum,value); + if(status == 0){ + return 0; + } + return tcpDoChoReceive(self); +} +/*---------------------------------------------------------------*/ +static int TcpDoChoConnect(pTcpDoCho self){ + int status, sendLen, readLen; + char *converted = NULL; + char buffer[256]; + + status = initRS232(self->controller); + if(status != 1){ + self->lastError = status; + return 0; + } + setRS232Timeout(self->controller,5); + + readLen = 255; + readRS232(self->controller,(void *)buffer,&readLen); + unicodeToAscii(buffer,readLen,&converted); + free(converted); + + /* + user name + */ + snprintf(buffer,255,"user:%s\r\n",self->user); + sendLen = asciiToUnicode(buffer,&converted); + if(sendLen == 0){ + self->lastError = BADCONVERSION; + return 0; + } + status = send(self->controller->pSock->sockid,converted,sendLen,0); + free(converted); + if(status < 0){ + self->lastError = BADSEND; + return 0; + } + readLen = 255; + readRS232(self->controller,(void *)buffer,&readLen); + unicodeToAscii(buffer,readLen,&converted); + free(converted); + + /* + password + */ + snprintf(buffer,255,"password:%s\r\n",self->pword); + sendLen = asciiToUnicode(buffer,&converted); + if(sendLen == 0){ + self->lastError = BADCONVERSION; + return 0; + } + status = send(self->controller->pSock->sockid,converted,sendLen,0); + free(converted); + if(status < 0){ + self->lastError = BADSEND; + return 0; + } + readLen = 255; + readRS232(self->controller,(void *)buffer,&readLen); + unicodeToAscii(buffer,readLen,&converted); + free(converted); + + /* + TODO: responses should be checked to test for a valid login. + I do not know at this time how the controller reacts upon a + bad login. + */ + + return 1; +} +/*==================== actual driver implementation code ==================*/ +static int TcpChopperTask(void *pData){ + pCodri self = NULL; + pTcpDoCho pPriv = NULL; + int status, code, i; + char buffer[80]; + char error[512]; + + self = (pCodri)pData; + assert(self); + pPriv = (pTcpDoCho)self->pPrivate; + assert(pPriv); + + if(pPriv->stop == 1){ + return 0; + } + + if(time(NULL) > pPriv->nextRefresh){ + if(pPriv->lastError != 0){ + self->GetError(self,&code,buffer,79); + snprintf(error,511,"WARNING: chopper tries to fix: %s",buffer); + WriteToCommandLog("Chopper-task:>>", error); + status = self->TryFixIt(self,code); + if(status == CHFAIL){ + pPriv->nextRefresh = time(NULL) + pPriv->iRefreshIntervall; + return 1; + } + } else { + pPriv->busy = 1; + for(i = 0; i < pPriv->numChoppers; i++){ + status = tcpDoChoCommand(pPriv,"STATE ", i+1,NULL); + if(status != 1){ + /* + force error correction + */ + return 1; + } + } + pPriv->nextRefresh = time(NULL) + pPriv->iRefreshIntervall; + pPriv->busy = 0; + } + } + return 1; +} +/*------------------------------------------------------------------------------*/ +static int TcpDoChoKill(pCodri self){ + pTcpDoCho pPriv = NULL; + + pPriv = (pTcpDoCho)self->pPrivate; + if(!pPriv) + return 1; + + if(pPriv->controller != NULL){ + KillRS232(pPriv->controller); + } + if(pPriv->parameters != NULL){ + DeleteStringDict(pPriv->parameters); + } + if(pPriv->config != NULL){ + free(pPriv->config); + } + free(pPriv); + return 1; +} +/*-------------------------------------------------------------------*/ +static int TcpDoChoConfigure(pCodri pDriv){ + pTcpDoCho self = NULL; + int status; + char *pPtr = NULL; + char command[80]; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + if(self->config != NULL){ + pPtr = self->config; + while( (pPtr = stptok(pPtr,command,79,"\n")) != NULL){ + status = tcpDoChoCommand(self,command,-1,NULL); + if(status != 1){ + return 0; + } + } + } + return 1; +} +/*---------------------------------------------------------------------*/ +static int TcpDoChoInit(pCodri pDriv){ + pTcpDoCho self = NULL; + int status; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + self->lastError = 0; + self->stop = 0; + self->nextRefresh = 0; + + status = TcpDoChoConnect(self); + if(status != 1){ + return 0; + } + + status = TcpDoChoConfigure(pDriv); + if(status != 1){ + return 0; + } + + /* start the update task */ + if(self->lTask == 0){ + self->lTask = TaskRegister(pServ->pTasker, + TcpChopperTask, + NULL, + NULL, + pDriv, + 1); + } + return 1; +} +/*-------------------------------------------------------------------*/ +static void waitForBusy(pTcpDoCho self){ + time_t endTime; + + endTime = time(NULL) + 10 *60; /* max 10 min */ + while(time(NULL) < endTime){ + if(self->busy == 1){ + SicsWait(3); + } else { + return; + } + } + WriteToCommandLog("Chopper-task>> ","WARNING: timeout on busy flag, flag forced"); + self->busy = 0; +} +/*----------------------------------------------------------------------*/ +static int TcpDoChoClose(pCodri pDriv){ + pTcpDoCho self = NULL; + int status; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + self->stop = 1; + if(self->controller != NULL){ + KillRS232(self->controller); + self->controller = NULL; + } + return 1; +} +/*----------------------------------------------------------------------*/ +static int dissectName(char *name, char par[80], int *num){ + char *pPtr = NULL; + + pPtr = strrchr(name,(int)'_'); + if(pPtr == NULL){ + return 0; + } + memset(par,0,80*sizeof(char)); + strncpy(par,name,pPtr - name); + if(sscanf(pPtr+1,"%d",num) != 1){ + return 0; + } + return 1; +} +/*----------------------------------------------------------------------*/ +static int TcpDoChoSetPar2(pCodri pDriv, char *parname, char *value){ + + pTcpDoCho self = NULL; + int status, choNum; + char par[80], buffer[80], state[80]; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + + /* + force status requests right after setting something in order + to make the stored status represent the new target values + */ + if(dissectName(parname,par,&choNum)){ + /* + check for emergency stop + */ + snprintf(buffer,79,"State_%1.1d",choNum); + memset(state,0,80*sizeof(char)); + StringDictGet(self->parameters,buffer,state,79); + if(strstr(state,"E-Stop") != NULL){ + self->lastError = ESTOP; + return 0; + } + + if(strcmp(par,"speed") == 0){ + waitForBusy(self); + status = tcpDoChoCommand(self,"SPEED ",choNum,trim(value)); + tcpDoChoCommand(self,"STATE ",choNum,NULL); + if(status != 1){ + return 0; + } else { + return 1; + } + } else if(strcmp(par,"phase") == 0){ + waitForBusy(self); + status = tcpDoChoCommand(self,"PHASE ",choNum,trim(value)); + tcpDoChoCommand(self,"STATE ",choNum,NULL); + if(status != 1){ + return 0; + } else { + return 1; + } + } + } + self->lastError = UNDRIVABLE; + return 0; +} +/*-----------------------------------------------------------------------*/ +static int TcpDoChoHalt(pCodri pDriv){ + + pTcpDoCho self = NULL; + int status; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + waitForBusy(self); + tcpDoChoCommand(self,"ESTOP :",-1,NULL); + return 1; +} +/*-----------------------------------------------------------------------*/ +static int TcpDoChoSetPar(pCodri pDriv, char *parname, float fValue){ + + pTcpDoCho self = NULL; + int status, choNum; + char value[80]; + char par[80]; + + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + if(dissectName(parname,par,&choNum)){ + if(strcmp(par,"speed") == 0){ + snprintf(value,79,"%5.1f", fValue); + return TcpDoChoSetPar2(pDriv,parname,value); + } else if(strcmp(par,"phase") == 0){ + snprintf(value,79,"%6.2f",fValue); + return TcpDoChoSetPar2(pDriv,parname,value); + } + } + if(strcmp(parname,"updateintervall") == 0){ + sprintf(value,"%d",(int)fValue); + StringDictUpdate(self->parameters,"updateintervall",value); + self->iRefreshIntervall = (int)fValue; + return 1; + } else { + snprintf(value,79,"%f",fValue); + return TcpDoChoSetPar2(pDriv,parname,value); + } +} +/*---------------------------------------------------------------------*/ +static int TcpDoChoGetPar(pCodri pDriv, char *parname, + char *pBuffer, int iBuflen){ + pTcpDoCho self = NULL; + int status = 0, choNum; + char par[80], buffer[80]; + float val; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + memset(par,0,80); + dissectName(parname,par,&choNum); + if(strcmp(par,"speed") == 0){ + snprintf(buffer,80,"ASPEED_%1.1d", choNum); + status = StringDictGet(self->parameters,buffer,pBuffer,iBuflen); + } else if(strcmp(par,"phase") == 0){ + snprintf(buffer,80,"APHASE_%1.1d", choNum); + status = StringDictGet(self->parameters,buffer,pBuffer,iBuflen); + } else { + status = StringDictGet(self->parameters,parname,pBuffer,iBuflen); + } + return status; +} +/*----------------------------------------------------------------------*/ +static int TcpDoChoCheckPar(pCodri pDriv, char *parname){ + pTcpDoCho self = NULL; + int status = 0, choNum; + float val, soll, delta; + char value[80], csoll[80], par[80], buffer[80], state[80]; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + /* + check for flags first + */ + if(self->busy){ + return HWBusy; + } + if(self->lastError != 0) { + return HWFault; + } + + /* + updateintervall is always Idle + */ + if(strcmp(parname,"updateintervall") == 0){ + return HWIdle; + } + + /* + check for emergency stop + */ + snprintf(buffer,79,"State_%1.1d",choNum); + memset(state,0,80*sizeof(char)); + StringDictGet(self->parameters,buffer,state,79); + if(strstr(state,"E-Stop") != NULL){ + self->lastError = HWFault; + return 0; + } + + memset(par,0,80); + dissectName(parname,par,&choNum); + if(strcmp(par,"speed") == 0){ + snprintf(buffer,79,"RSPEED_%1.1d", choNum); + StringDictGet(self->parameters,buffer,csoll,79); + sscanf(csoll,"%f",&soll); + snprintf(buffer,79,"ASPEED_%1.1d", choNum); + StringDictGet(self->parameters,buffer,value,79); + sscanf(value,"%f",&val); + delta = ABS(soll - val); + if(delta > SPEEDTOL){ + return HWBusy; + } else { + return HWIdle; + } + } else if(strcmp(par,"phase") == 0){ + snprintf(buffer,79,"RPHASE_%1.1d", choNum); + StringDictGet(self->parameters,buffer,csoll,79); + sscanf(value,"%f",&soll); + snprintf(buffer,79,"APHASE_%1.1d", choNum); + StringDictGet(self->parameters,buffer,value,79); + sscanf(value,"%f",&val); + delta = ABS(soll - val); + if(delta > PHASETOL){ + return HWBusy; + } else { + return HWIdle; + } + } + self->lastError = BADPAR; + return HWFault; +} +/*---------------------------------------------------------------------------*/ +static int TcpDoChoError(pCodri pDriv, int *iCode, char *pError, int iLen){ + pTcpDoCho self = NULL; + int status = 0; + float val, soll, delta; + char value[80]; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + *iCode = self->lastError; + switch(self->lastError){ + case WRONGMODE: + strncpy(pError,"Chopper in wrong mode",iLen); + break; + case BADCONVERSION: + strncpy(pError,"Bad ASCII to unicode conversion",iLen); + break; + case FAILEDCOMMAND: + strncpy(pError,"Command not accepted",iLen); + break; + case BADWRITE: + strncpy(pError,"Failed to write to chopper controller",iLen); + break; + case BADRESPONSE: + strncpy(pError,"Chopper controller send invalid command",iLen); + break; + case UNDRIVABLE: + strncpy(pError,"Parameter cannot be changed",iLen); + break; + case BADPAR: + strncpy(pError,"No such parameter",iLen); + break; + case ESTOP: + strncpy(pError,"Emergency stop is engaged",iLen); + break; + default: + getRS232Error(self->lastError,pError,iLen); + break; + } + return 1; +} +/*---------------------------------------------------------------------*/ +static int TcpDoChoFix(pCodri pDriv, int iCode){ + pTcpDoCho self = NULL; + int status = 0; + float val, soll, delta; + char value[80]; + + assert(pDriv != NULL); + self = (pTcpDoCho)pDriv->pPrivate; + assert(self != NULL); + + self->lastError = 0; + switch(iCode){ + case BADCONVERSION: + case BADRESPONSE: + return CHREDO; + break; + case WRONGMODE: + case FAILEDCOMMAND: + case UNDRIVABLE: + case BADPAR: + case ESTOP: + return CHFAIL; + break; + default: + closeRS232(self->controller); + status = TcpDoChoConnect(self); + if(status == 1){ + return CHREDO; + } else { + return CHFAIL; + } + break; + } + return CHFAIL; +} +/*-------------------------------------------------------------------*/ +pCodri MakeTcpDoChoDriver(char *tclArray, SConnection *pCon){ + pCodri pNew = NULL; + pTcpDoCho self = NULL; + const char *pPtr = NULL; + char buffer[132]; + int port, i, count; + Tcl_DString pars; + char *parnames[] = {"State", + "ASPEED", + "RSPEED", + "APHASE", + "RPHASE", + "AVETO", + "DIR", + "MONIT", + "FLOWR", + "WTEMP", + "MTEMP", + "MVIBR", + "MVACU", + "speed", + "phase", + NULL, + }; + + /* + allocate memory + */ + pNew = (pCodri)malloc(sizeof(Codri)); + self = (pTcpDoCho)malloc(sizeof(TcpDoCho)); + if(!pNew || !self){ + return NULL; + } + memset(pNew,0,sizeof(Codri)); + memset(self,0,sizeof(TcpDoCho)); + + /* port and host name */ + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"port",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: port not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + sscanf(pPtr,"%d",&port); + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"host",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: host not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + memset(buffer,0,132); + strncpy(buffer,pPtr, 131); + self->controller = createRS232(buffer,port); + + /* number of choppers */ + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"nchopper",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: nchopper not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + sscanf(pPtr,"%d",&port); + if(port < 0 || port > 8){ + SCWrite(pCon,"ERROR: number of choppers not in range 1 - 8",eError); + free(pNew); + free(self); + } + self->numChoppers = port; + + /* timeout */ + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"timeout",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: timeout not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + sscanf(pPtr,"%d",&port); + self->timeout = port; + + /* username and password */ + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"user",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: user not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + strncpy(self->user,pPtr, 131); + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"password",TCL_GLOBAL_ONLY); + if(!pPtr){ + SCWrite(pCon,"ERROR: password not found in configuration array for TCP Dornier Chopper", + eError); + free(pNew); + free(self); + return NULL; + } + strncpy(self->pword,pPtr, 131); + + /* + chopper configuration + */ + pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"config",TCL_GLOBAL_ONLY); + if(pPtr != NULL){ + self->config = strdup(pPtr); + } + + + /* initialize some more */ + self->parameters = CreateStringDict(); + if(self->parameters == NULL || self->controller == NULL){ + SCWrite(pCon,"ERROR: out of memory in MakeTcpDoCho",eError); + free(pNew); + free(self); + return NULL; + } + self->iRefreshIntervall = 60; + pNew->Init = TcpDoChoInit; + pNew->Close = TcpDoChoClose; + pNew->Delete = TcpDoChoKill; + pNew->SetPar = TcpDoChoSetPar; + pNew->SetPar2 = TcpDoChoSetPar2; + pNew->GetPar = TcpDoChoGetPar; + pNew->CheckPar = TcpDoChoCheckPar; + pNew->GetError = TcpDoChoError; + pNew->TryFixIt = TcpDoChoFix; + pNew->Halt = TcpDoChoHalt; + StringDictAddPair(self->parameters,"updateintervall","60"); + pNew->pPrivate = self; + + /* + create parameter list + */ + Tcl_DStringInit(&pars); + count = 0; + Tcl_DStringAppend(&pars,"updateintervall",15); + while(parnames[count] != NULL){ + for(i = 0; i < self->numChoppers; i++){ + snprintf(buffer,131,",%s_%1.1d", parnames[count], i + 1); + Tcl_DStringAppend(&pars,buffer,strlen(buffer)); + } + count++; + } + pNew->pParList = strdup(Tcl_DStringValue(&pars)); + Tcl_DStringFree(&pars); + return pNew; +} diff --git a/tcpdornier.c b/tcpdornier.c new file mode 100644 index 0000000..4ee022b --- /dev/null +++ b/tcpdornier.c @@ -0,0 +1,1146 @@ +/*-------------------------------------------------------------------------- + Yet another driver for a Astrium == Dornier velocity selector. This one + is for the new control software with the TCP-server installed. Actually + this is only valid for the modified protocoll as implemented at ANSTO. + + As the motor for the tilt is included with the Astrium package, this code + also has to implement a motor driver for that motor. + + As even the Tcp version of the Astrium Control Software is slow in responding, + a state machine has beem implemented for status request. There are only two + states: status request sent (WAITING) or status message processed (READY). + The idea is that getStatus sends a request and returns VSACCEL. It then + tests for data availability. If no data is available, VSACCEL is returned, + else the data is processed. This is done in order not to make SICS + unresponsive for seconds while driving the velocity selector. This scheme + implies that all other functions must take care of the state the connection + is in and possibly read the connection free before doing their work. + + There is another efficiency feauture in place which causes status messages + younger then three times timeout to be reused. The VS does everything very + slowly, thus this is good enough. + + The new command set for the VS hides this, but it is a known fact that the + VS has two modes of operation: above a certain threshold he can be normally + driven. Below that threshold (~3000 rpm) it must be started. Starting does + sometimes fail. This code allows the VS half an hour to start before flagging + an error. + + copyright: see file COPYRIGHT + + Mark Koennecke, December 2005 +----------------------------------------------------------------------------*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define RPMALIFE 3100 + +/* defines for the communication state*/ +#define READY 0 +#define WAITING 1 + + +/* error codes */ +#define BADREPLY -17501 +#define BADACCEPT -17502 +#define SELTOFAST -17503 +#define FAILEDSTART -17504 + +extern char *trim(char *str); +#define ABS(x) (x < 0 ? -(x) : (x)) +/*----------------------------- The private data structure ---------------*/ + typedef struct{ + prs232 controller; + int iLastError; + float fTarget; + int comState; + int timeout; + pStringDict status; + time_t requestTimeout; + time_t statusAge; + time_t driveStart; + int debug; + char user[132]; + char pword[132]; + } TcpDornier, *pTcpDornier; +/*------------------------------------------------------------------------ + Some utility stuff to parse the response from the velocity selector. + Any response contains the whole status message. Thus we can use any + response to update the status. +----------------------------------------------------------------------------*/ +static void addAstriumPar(char *token, pStringDict target){ + char name[80], value[80]; + char *pos = NULL; + + pos = strstr(token,"="); + if(pos != NULL){ + memset(name,0,79); + strncpy(name,token,pos-token); + strcpy(value,pos+1); + } + if(StringDictExists(target,trim(name))){ + StringDictUpdate(target,trim(name),trim(value)); + } else { + StringDictAddPair(target,trim(name), trim(value)); + } +} +/*------------------------------------------------------------------------*/ +static int parseTcpDornierStatus(char *statusText, pStringDict target){ + char *pos = NULL; + char token[80]; + + /* + test if this is a valid message + */ + if(strstr(statusText,"#SOS#") == NULL) { + return 0; + } + + /* + extract the command status code + */ + pos = stptok(statusText+5,token,79,"#"); + if(pos == NULL){ + return 0; + } + if(StringDictExists(target,"commandstatus") == 1) { + StringDictUpdate(target,"commandstatus",trim(token)); + } else { + StringDictAddPair(target,"commandstatus",trim(token)); + } + + /* + extract all the others + */ + while( (pos = stptok(pos,token,79,"#")) != NULL) { + addAstriumPar(token,target); + } + return 1; +} +/*---------------------------------------------------------------------------*/ +static int readAstriumReply(prs232 controller, char *buffer, int buflen, + int timeout){ + int status; + int bytesRead = 0; + time_t endTime; + char *pos = NULL; + + endTime = time(NULL) + timeout; + memset(buffer,0,buflen*sizeof(char)); + while(time(NULL) < endTime) { + if(availableRS232(controller)) { + bytesRead = recv(controller->pSock->sockid, buffer + bytesRead, + buflen - bytesRead, 0); + if(bytesRead < 0){ + return BADREAD; + } + /* + as of december-9-2005 the Astrium protocoll has no proper + terminator. I check here for the last data entry which + happens to be BCUNN. The # after that terminates the + message. This algoritjm may need to be modified when + the protocoll changes. + */ + + pos = strstr(buffer,"BCUUN"); + if(pos != NULL){ + pos = strstr(pos+1,"#"); + if(pos != NULL) { + return 1; + } + } + } else { + SicsWait(1); + } + } + return TIMEOUT; +} +/*---------------------------------------------------------------------------*/ +static int readAndDecodeReply(pTcpDornier pDorn){ + int status; + char buffer[1024]; + + status = readAstriumReply(pDorn->controller, buffer, 1023, pDorn->timeout); + if(pDorn->debug > 0){ + printf("Read status = %d, Read data = %s\n", status, buffer); + } + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + pDorn->comState = READY; + pDorn->statusAge = time(NULL); + status = parseTcpDornierStatus(buffer,pDorn->status); + if(status != 1) { + pDorn->iLastError = BADREPLY; + return 0; + } + return 1; +} +/*-------------------------------------------------------------------------*/ +static int readAstriumValue(pTcpDornier pDorn, char *key, float *value){ + int status; + char sValue[80]; + + status = readAndDecodeReply(pDorn); + if(status != 1) { + return status; + } + status = StringDictGet(pDorn->status, key, sValue, 79); + if(status != 1){ + pDorn->iLastError = BADREPLY; + return 0; + } + sscanf(sValue,"%f",value); + return 1; +} +/*-------------------------------------------------------------------------*/ +static int AstriumConnect(pTcpDornier pDorn){ + int status; + char buffer[256]; + + status = initRS232(pDorn->controller); + if(status != 1) { + return 0; + } + status = readRS232UntilWord(pDorn->controller,buffer,255,"ID"); + if(status != 1) { + return 0; + } + snprintf(buffer,255,"user:%s",pDorn->user); + status = writeRS232(pDorn->controller,buffer,strlen(buffer)); + if(status != 1) { + return 0; + } + status = readRS232UntilWord(pDorn->controller,buffer,255,"password"); + if(status != 1) { + return 0; + } + snprintf(buffer,255,"password:%s",pDorn->pword); + status = writeRS232(pDorn->controller,buffer,strlen(buffer)); + if(status != 1) { + return 0; + } + status = readRS232UntilWord(pDorn->controller,buffer,255,"Hello"); + if(status != 1) { + return 0; + } + pDorn->comState = READY; + return 1; +} +/*-------------------------------------------------------------------------*/ +static float readSpeed(pStringDict dict){ + char value[80]; + float fPos; + + /* + The dornier has two speed ranges: Normally it is ASPEED but under + 100 it is SSPEED + */ + StringDictGet(dict,"ASPEED",value, 79); + sscanf(value,"%f",&fPos); + if(fPos < 20.) { + StringDictGet(dict,"SSPEED",value, 79); + sscanf(value,"%f",&fPos); + + } + return fPos; +} +/*----------------------------------------------------------------------------*/ +static int GetTcpDornierPos(pVelSelDriv self, float *fPos) + { + pTcpDornier pDorn = NULL; + int status; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + /* + with astrium replying so slowly, we should not be bothered to get + the latest and greatest but rather be efficient in the case of frequent + requests in a short time, such as after driving. statusAge is set in + readAndDecode when succcessful. + */ + if(time(NULL) > pDorn->statusAge + 3 * pDorn->timeout){ + if(pDorn->comState == READY) { + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + pDorn->comState = WAITING; + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + } + if(readAndDecodeReply(pDorn) != 1) { + return 0; + } + } + *fPos = readSpeed(pDorn->status); + return 1; + } +/*--------------------------------------------------------------------------*/ + static int TcpDornierHalt(pVelSelDriv self) + { + pTcpDornier pDorn = NULL; + int status; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + pDorn->fTarget = .0; + + if(pDorn->comState == WAITING){ + readAndDecodeReply(pDorn); + } + status = writeRS232(pDorn->controller,"#SOS#BRAKE ",11); + pDorn->comState = WAITING; + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + readAndDecodeReply(pDorn); + + return 1; + } +/*----------------------------------------------------------------------------*/ + static int TcpDornierRun(pVelSelDriv self, float fVal) + { + int status; + char pCommand[50], pAnswer[50]; + pTcpDornier pDorn = NULL; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + if(pDorn->comState == WAITING){ + readAndDecodeReply(pDorn); + } + pDorn->comState = READY; + /* + a requested value of 0 or very little means to stop + */ + if(fVal < self->fTolerance){ + pDorn->driveStart = time(NULL); + return TcpDornierHalt(self); + } + /* This is the normal logic: new value */ + snprintf(pCommand,49,"#SOS#SPEED %5d",(int)fVal); + status = writeRS232(pDorn->controller,pCommand,strlen(pCommand)); + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + pDorn->comState = WAITING; + pDorn->driveStart = time(NULL); + status = readAndDecodeReply(pDorn); + if(status != 1) { + return 0; + } + + StringDictGet(pDorn->status,"commandstatus",pAnswer,49); + if(strstr(pAnswer,"ACCEPT") == NULL) { + pDorn->iLastError = BADACCEPT; + return 0; + } + pDorn->fTarget = fVal; + return 1; + } +/*--------------------------------------------------------------------------*/ + static int TcpDornierError(pVelSelDriv self, int *iCode, + char *error, int iErrLen) + { + pTcpDornier pDorn = NULL; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + *iCode = pDorn->iLastError; + switch(pDorn->iLastError){ + case BADREPLY: + strncpy(error,"Velocity Selector sent invalid reply",iErrLen); + break; + case BADACCEPT: + strncpy(error,"VS refused command or speed out of range", + iErrLen); + break; + case SELTOFAST: + strncpy(error,"Cannot drive tilt angle while selector is running", + iErrLen); + break; + case FAILEDSTART: + strncpy(error,"Failed to start velocitty selector",iErrLen); + break; + default: + getRS232Error(pDorn->iLastError,error, iErrLen); + break; + } + + return 1; + } +/*---------------------------------------------------------------------------*/ + static int TcpDornierFixIt(pVelSelDriv self, int iError) + { + pTcpDornier pDorn = NULL; + int status; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + switch(iError){ + case BADREPLY: + case TIMEOUT: + pDorn->comState = READY; + return VELOREDO; + break; + case BADACCEPT: + pDorn->comState = READY; + case FAILEDCONNECT: + case SELTOFAST: + case FAILEDSTART: + return VELOFAIL; + break; + default: + /* + these are mostly connection errors + Try to reconnect + */ + closeRS232(pDorn->controller); + status = AstriumConnect(pDorn); + pDorn->comState = READY; + if(status){ + return VELOREDO; + } else { + return VELOFAIL; + } + break; + } + return VELOFAIL; + } +/*--------------------------------------------------------------------------*/ + static int TcpDornierStat(pVelSelDriv self, int *iCode, float *fCur) + { + pTcpDornier pDorn = NULL; + float fDelta; + static int count = 0; + int status; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + *iCode = ROTMOVE; + if(pDorn->comState == READY){ + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + if(status != 1) { + pDorn->iLastError = status; + return VSFAIL; + } + pDorn->comState = WAITING; + pDorn->requestTimeout = time(NULL) + pDorn->timeout; + } + + status = availableRS232(pDorn->controller); + if(status == 1) { + status = readAndDecodeReply(pDorn); + if(status != 1) { + return VSFAIL; + } + pDorn->comState = READY; + *fCur = readSpeed(pDorn->status); + fDelta = *fCur - pDorn->fTarget; + if(ABS(fDelta) < self->fTolerance) { + count++; + /* + we want at least three readings of the selector within + tolerance before we believe it arrived + */ + if(count > 3){ + return VSOK; + } else { + return VSACCEL; + } + } else { + count = 0; + /* + if the VS is still at low speed after half an hour we must + assume that it failed to start and has already had its three + times worth of start tries. We flag this now... + */ + if(time(NULL) > pDorn->driveStart + 30 * 60 && *fCur < RPMALIFE){ + pDorn->iLastError = FAILEDSTART; + return VSFAIL; + } + } + return VSACCEL; + } else { + if(time(NULL) > pDorn->requestTimeout) { + pDorn->iLastError = TIMEOUT; + return VSFAIL; + } else { + return VSACCEL; + } + } + + return VELOOK; + } +/*-------------------------------------------------------------------------*/ + static int TcpDornierText(pVelSelDriv self, char *pText, int iTextLen) + { + pTcpDornier pDorn = NULL; + char buffer[1024]; + const char *name = NULL; + char value[80], entry[132]; + int charUsed = 0, status; + + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + memset(pText,0,iTextLen*sizeof(char)); + if(time(NULL) > pDorn->statusAge + 3. * pDorn->timeout){ + if(pDorn->comState == READY) { + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + pDorn->comState = WAITING; + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + } + if(readAndDecodeReply(pDorn) != 1) { + pDorn->comState = READY; + return 0; + } + } + strcpy(buffer,""); + while((name = StringDictGetNext(pDorn->status,value,79)) != NULL){ + if(strstr(name,"commandstatus") == NULL){ + snprintf(entry,131,"%s = %s\n", name,value); + if(charUsed + 132 < 1023){ + strcat(buffer,entry); + charUsed += strlen(entry); + } + } + } + strncpy(pText,buffer,iTextLen); + return 1; + } +/*------------------------------------------------------------------------*/ + static int TcpDornierLoss(pVelSelDriv self, float *fLoss) + { + pTcpDornier pDorn = NULL; + int status; + time_t endTime; + float speed, soll; + static int count = 0; + char value[80]; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + + if(pDorn->comState == WAITING){ + readAndDecodeReply(pDorn); + } + status = writeRS232(pDorn->controller,"#SOS#PLOSS ",11); + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + pDorn->comState = WAITING; + status = readAndDecodeReply(pDorn); + if(status != 1) { + return 0; + } + count = 0; + + /* + loop until at speed again + */ + endTime = time(NULL) + 10 * 60; + while(time(NULL) < endTime){ + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + if(status != 1) { + pDorn->iLastError = status; + return 0; + } + status = readAndDecodeReply(pDorn); + if(status != 1) { + return 0; + } + speed = readSpeed(pDorn->status); + StringDictGet(pDorn->status,"RSPEED",value,79); + sscanf(value,"%f",&soll); + if(ABS(soll - speed) < self->fTolerance){ + count++; + if(count > 3){ + StringDictGet(pDorn->status,"PLOSS",value,79); + sscanf(value,"%f",fLoss); + return 1; + } + } else { + count = 0; + } + } + pDorn->iLastError = TIMEOUT; + return 0; + } +/*-------------------------------------------------------------------------*/ + static void TcpDornierKill(void *pData) + { + pTcpDornier pDorn = NULL; + + pDorn = (pTcpDornier)pData; + assert(pDorn); + + if(pDorn->controller != NULL){ + KillRS232(pDorn->controller); + } + if(pDorn->status != NULL){ + DeleteStringDict(pDorn->status); + } + + free(pDorn); + } +/*------------------------------------------------------------------------*/ + static int TcpDornierInit(pVelSelDriv self, SConnection *pCon) + { + pTcpDornier pDorn = NULL; + int status; + + assert(self); + pDorn = (pTcpDornier)self->pPrivate; + assert(pDorn); + + status = AstriumConnect(pDorn); + if(status != 1){ + SCWrite(pCon, + "ERROR: failed to connect or login to Astrium velocity selector controller", + eError); + return 1; + } + + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + pDorn->comState = WAITING; + if(status != 1) { + SCWrite(pCon, + "ERROR: failed to write status request to controller", + eError); + return 0; + } + status = readAndDecodeReply(pDorn); + if(status != 1) { + SCWrite(pCon, + "ERROR: failed to read and decode status request", + eError); + return 0; + } + + return 1; + } +/*========================================================================= + Section with the motor driver code + ==========================================================================*/ + typedef struct __TcpAsMoDriv { + /* general motor driver interface + fields. REQUIRED! + */ + float fUpper; /* upper limit */ + float fLower; /* lower limit */ + char *name; + int (*GetPosition)(void *self, float *fPos); + int (*RunTo)(void *self,float fNewVal); + int (*GetStatus)(void *self); + void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen); + int (*TryAndFixIt)(void *self, int iError,float fNew); + int (*Halt)(void *self); + int (*GetDriverPar)(void *self, char *name, + float *value); + int (*SetDriverPar)(void *self,SConnection *pCon, + char *name, float newValue); + void (*ListDriverPar)(void *self, char *motorName, + SConnection *pCon); + void (*KillPrivate)(void *self); + pTcpDornier master; + pVelSelDriv selector; + float tiltTarget; + float fTolerance; + } TcpAsMotorDriver, *pTcpAsMotorDriver; +/*-------------------------------------------------------------------------*/ +static int TcpMotGetPosition(void *pData, float *fPos){ + int status; + pTcpAsMotorDriver self = NULL; + char pAnswer[80]; + + self =(pTcpAsMotorDriver)pData; + assert(self != NULL); + + /* + same as above for slow status reponses + */ + if(time(NULL) > self->master->statusAge + 3 * self->master->timeout){ + if(self->master->comState == READY) { + status = writeRS232(self->master->controller,"#SOS#STATE ",11); + if(status != 1) { + self->master->comState = WAITING; + self->master->iLastError = status; + return 0; + } + } + status = readAstriumValue(self->master,"TTANG", fPos); + if(status != 1){ + return 0; + } + } else { + StringDictGet(self->master->status,"TTANG",pAnswer,79); + sscanf(pAnswer,"%f", fPos); + return 1; + } + return 1; +} +/*------------------------------------------------------------------------*/ +static int TcpMotRunTo(void *pData, float newValue){ + int status; + pTcpAsMotorDriver self = NULL; + float speed; + char command[80], pAnswer[80]; + + self =(pTcpAsMotorDriver)pData; + assert(self != NULL); + + + if(self->master->comState == WAITING){ + readAndDecodeReply(self->master); + } + + /* + just another test to make sure that the selector is stopped before + driving that motor + */ + speed = readSpeed(self->master->status); + if(speed > 10){ + self->master->iLastError = SELTOFAST; + return 0; + } + + /* + send a command + */ + snprintf(command,79,"#SOS#TTANGL %6.3f", newValue); + status = writeRS232(self->master->controller,command,strlen(command)); + if(status != 1) { + self->master->iLastError = status; + return 0; + } + /* + check the reply + */ + status = readAndDecodeReply(self->master); + if(status != 1) { + return 0; + } + + StringDictGet(self->master->status,"commandstatus",pAnswer,49); + if(strstr(pAnswer,"ACCEPT") == NULL) { + self->master->iLastError = BADACCEPT; + return 0; + } + self->tiltTarget = newValue; + return 1; +} +/*------------------------------------------------------------------------*/ +static int TcpAsMotStat(void *pData){ + pTcpAsMotorDriver self = NULL; + pTcpDornier pDorn = NULL; + float fDelta, value; + int status; + + self =(pTcpAsMotorDriver)pData; + assert(self != NULL); + + assert(self); + pDorn = self->master; + + if(pDorn->comState == READY){ + status = writeRS232(pDorn->controller,"#SOS#STATE ",11); + if(status != 1) { + pDorn->iLastError = status; + return HWFault; + } + pDorn->comState = WAITING; + pDorn->requestTimeout = time(NULL) + pDorn->timeout; + } + + status = availableRS232(pDorn->controller); + if(status == 1) { + status = readAstriumValue(pDorn,"TTANG",&value); + if(status != 1) { + return HWFault; + } + pDorn->comState = READY; + fDelta = value - self->tiltTarget; + if(ABS(fDelta) < self->fTolerance) { + return HWIdle; + } else { + return HWBusy; + } + } else { + if(time(NULL) > pDorn->requestTimeout) { + pDorn->iLastError = TIMEOUT; + return HWFault; + } else { + return HWBusy; + } + } + return HWIdle; + } +/*------------------------------------------------------------------------*/ +static void TcpAsGetError(void *pData, int *iCode, char *error, int errLen){ + pTcpAsMotorDriver self = NULL; + + self =(pTcpAsMotorDriver)pData; + assert(self != NULL); + TcpDornierError(self->selector, iCode, error, errLen); +} +/*-----------------------------------------------------------------------*/ +static int TcpAsFixit(void *pData, int code, float fnew){ + pTcpAsMotorDriver self = NULL; + int status; + + self =(pTcpAsMotorDriver)pData; + assert(self != NULL); + + if(code == BADACCEPT){ + return MOTREDO; + } + + status = TcpDornierFixIt(self->selector, code); + switch(status){ + case VELOFAIL: + return MOTFAIL; + break; + case VELOREDO: + return MOTREDO; + break; + default: + assert(0); + break; + } + return MOTFAIL; +} +/*-----------------------------------------------------------------------*/ +static int TcpAsMotHalt(void *pData){ + /* + There is no command to halt the tilt motor! + */ + return 1; +} +/*================= creation code ========================================*/ + pVelSelDriv VSCreateTcpDornierANSTO(char *name, Tcl_Interp *pTcl) + { + pVelSelDriv pNew = NULL; + pTcpDornier pDorn = NULL; + MotorDriver *pAstDriv = NULL; + const char *pPtr = NULL; + char host[132]; + int iVal, iRet, port; + + /* the most likely error is the parameters specified are wrong! + So check this first. We''ll use Tcl's result for error reporting. + name is the name of an Tcl array which should hold the info + necessary + */ + + /* allocate a TcpDornier structure */ + pDorn = (pTcpDornier)malloc(sizeof(TcpDornier)); + if(!pDorn) + { + return NULL; + } + memset(pDorn,0,sizeof(TcpDornier)); + + + /* host name */ + pPtr = Tcl_GetVar2(pTcl,name,"Host",TCL_GLOBAL_ONLY); + if(!pPtr) + { + Tcl_AppendResult(pTcl,"ERROR: no hostname found in",name,NULL); + free(pDorn); + return NULL; + } + strncpy(host,pPtr,131); + + /* port number */ + pPtr = Tcl_GetVar2(pTcl,name,"Port",TCL_GLOBAL_ONLY); + if(!pPtr) + { + Tcl_AppendResult(pTcl,"ERROR: no port number found in",name,NULL); + free(pDorn); + return NULL; + } + iRet = Tcl_GetInt(pTcl,pPtr,&iVal); + if(iRet != TCL_OK) + { + free(pDorn); + return NULL; + } + port = iVal; + pDorn->controller = createRS232(host,port); + pDorn->status = CreateStringDict(); + if(pDorn->controller == NULL || pDorn->status == NULL){ + free(pDorn); + return NULL; + } + setRS232SendTerminator(pDorn->controller,"\r\n"); + + /* + username and password + */ + pPtr = Tcl_GetVar2(pTcl,name,"User",TCL_GLOBAL_ONLY); + if(!pPtr){ + strncpy(pDorn->user,"NVS",131); + } else { + strncpy(pDorn->user,pPtr,131); + } + pPtr = Tcl_GetVar2(pTcl,name,"Password",TCL_GLOBAL_ONLY); + if(!pPtr){ + strncpy(pDorn->pword,"NVS",131); + } else { + strncpy(pDorn->pword,pPtr,131); + } + + /* time out. This one gets defaulted when not specified */ + pPtr = Tcl_GetVar2(pTcl,name,"Timeout",TCL_GLOBAL_ONLY); + if(!pPtr) + { + pDorn->timeout = 5; + } + else + { + iRet = Tcl_GetInt(pTcl,pPtr,&iVal); + if(iRet == TCL_OK) + { + pDorn->timeout = iVal; + } + } + + pPtr = Tcl_GetVar2(pTcl,name,"Debug",TCL_GLOBAL_ONLY); + if(pPtr != NULL) + { + setRS232Debug(pDorn->controller, 10); + pDorn->debug = 1; + } + + + /* business as usual: allocate memory */ + pNew = (pVelSelDriv)malloc(sizeof(VelSelDriv)); + if(!pNew) + { + return NULL; + } + + /* zero the world */ + memset(pNew,0,sizeof(VelSelDriv)); + pNew->pPrivate = pDorn; + + /* initialise function pointers */ + pNew->DeletePrivate = TcpDornierKill; + pNew->Halt = TcpDornierHalt; + pNew->GetError = TcpDornierError; + pNew->TryAndFixIt = TcpDornierFixIt; + pNew->GetRotation = GetTcpDornierPos; + pNew->SetRotation = TcpDornierRun; + pNew->GetStatus = TcpDornierStat; + pNew->GetDriverText = TcpDornierText; + pNew->GetLossCurrent = TcpDornierLoss; + pNew->Init = TcpDornierInit; + + /* tolerance This one gets defaulted when not specified */ + pPtr = Tcl_GetVar2(pTcl,name,"Tolerance",TCL_GLOBAL_ONLY); + if(!pPtr) + { + pNew->fTolerance = 10.; + } + else + { + iRet = Tcl_GetInt(pTcl,pPtr,&iVal); + if(iRet != TCL_OK) + { + pNew->fTolerance = (float)iVal; + } + } + + + /* done it */ + return pNew; + } +/*----------------------------------------------------------------------*/ +static pTcpAsMotorDriver MakeAstriumMotor(pVelSelDriv sel, pTcpDornier pDorn ){ + pTcpAsMotorDriver pNew = NULL; + + pNew = (pTcpAsMotorDriver)malloc(sizeof(TcpAsMotorDriver)); + if(pNew == NULL){ + return NULL; + } + memset(pNew,0,sizeof(TcpAsMotorDriver)); + pNew->GetPosition = TcpMotGetPosition; + pNew->RunTo = TcpMotRunTo; + pNew->GetStatus = TcpAsMotStat; + pNew->GetError = TcpAsGetError; + pNew->TryAndFixIt = TcpAsFixit; + pNew->Halt = TcpAsMotHalt; + pNew->selector = sel; + pNew->master = pDorn; + pNew->fTolerance = .1; + return (pTcpAsMotorDriver)pNew; +} +/*----------------------------------------------------------------------*/ +extern pEVDriver MakeDummyVel(pVelSel pVel); /* in velo.c */ + +int VelSelTcpFactory(SConnection *pCon, SicsInterp *pSics, void *pData, + int argc, char *argv[]){ + pVelSelDriv pDriv = NULL; + pVelSel pNew = NULL; + pMotor pTilt = NULL; + char pBueffel[256]; + Tcl_Interp *pT = NULL; + int iRet; + pEVDriver pMonDriv = NULL; + pTcpAsMotorDriver pAstDriv = NULL; + const char *pPtr = NULL; + double d; + float limit; + + assert(pCon); + assert(pSics); + + /* minimum 3 arguments! */ + if(argc < 3) + { + SCWrite(pCon,"ERROR: Insufficient number of arguments to VelSelFactory", + eError); + return 0; + } + + /* first one is name */ + strtolower(argv[1]); + + /* second is the Tcl-array with the parameters + Create the velocity selector driver + */ + pDriv = VSCreateTcpDornierANSTO(argv[2], pSics->pTcl); + if(pDriv == NULL){ + SCWrite(pCon,"ERROR: failed to create velocity selector driver",eError); + return 0; + } + pAstDriv = MakeAstriumMotor(pDriv,(pTcpDornier)pDriv->pPrivate); + if(pAstDriv == NULL){ + SCWrite(pCon,"ERROR: failed to create velocity selector motor driver",eError); + return 0; + } + pTilt = MotorInit("astriumdriver","selectortilt",(MotorDriver *)pAstDriv); + if(pTilt == NULL){ + SCWrite(pCon,"ERROR: failed to create velocity selector motor",eError); + return 0; + } + + /* + now initialize additional parameters for the motor + */ + pPtr = Tcl_GetVar2(pSics->pTcl,argv[2],"TiltTolerance",TCL_GLOBAL_ONLY); + pAstDriv->fTolerance = .1; + if(pPtr != NULL){ + iRet = Tcl_GetDouble(pSics->pTcl,pPtr,&d); + if(iRet == TCL_OK){ + pAstDriv->fTolerance = (float)d; + } + } + + limit = 10.; + pPtr = Tcl_GetVar2(pSics->pTcl,argv[2],"TiltUpper",TCL_GLOBAL_ONLY); + if(pPtr != NULL){ + iRet = Tcl_GetDouble(pSics->pTcl,pPtr,&d); + if(iRet == TCL_OK){ + limit = (float)d; + } + } + pAstDriv->fUpper = limit; + MotorSetPar(pTilt,pCon,"softupperlim",limit); + + pPtr = Tcl_GetVar2(pSics->pTcl,argv[2],"TiltLower",TCL_GLOBAL_ONLY); + limit = -10.; + if(pPtr != NULL){ + iRet = Tcl_GetDouble(pSics->pTcl,pPtr,&d); + if(iRet == TCL_OK){ + limit = (float)d; + } + } + pAstDriv->fLower = limit; + MotorSetPar(pTilt,pCon,"softlowerlim",limit); + + + + /* now initialise this and install it as command */ + pNew = VSCreate(pTilt,pDriv); + if(!pNew) + { + SCWrite(pCon,"ERROR: creating velocity selector, no memory",eError); + return 0; + } + iRet = pDriv->Init(pDriv,pCon); + if(!iRet) + { + SCWrite(pCon,"ERROR: failed to initialize velocity selector", + eError); + VSDestroy(pNew); + return 0; + } + pNew->pName = strdup(argv[1]); + iRet = AddCommand(pSics,argv[1],VelSelAction, + VSDestroy,pNew); + if(!iRet) + { + sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[2]); + SCWrite(pCon,pBueffel,eError); + VSDestroy((void *)pNew); + return 0; + } + + /* install the evcontroller bit of the velocity selector */ + pMonDriv = MakeDummyVel(pNew); + if(!pMonDriv) + { + RemoveCommand(pSics,argv[1]); + SCWrite(pCon,"ERROR: failed to create monitor for nvs",eError); + return 0; + } + pBueffel[0] = '\0'; + strcpy(pBueffel,argv[1]); + strcat(pBueffel,"watch"); + pNew->pMonitor = CreateEVController(pMonDriv,pBueffel,&iRet); + if(!pNew->pMonitor) + { + DeleteEVDriver(pMonDriv); /* was missing M.Z. Jul 04 */ + SCWrite(pCon,"ERROR: failed to create monitor for nvs",eError); + return 0; + } + iRet = AddCommand(pSics,pBueffel,EVControlWrapper, + NULL,pNew->pMonitor); + if(!iRet) + { + sprintf(pBueffel,"ERROR: duplicate command %s not created",pBueffel); + RemoveCommand(pSics,argv[1]); + return 0; + } + EVRegisterController(FindEMON(pSics),pBueffel,pNew->pMonitor,pCon); + return iRet; + } + diff --git a/tricssupport.c b/tricssupport.c index e4e854a..aecda52 100644 --- a/tricssupport.c +++ b/tricssupport.c @@ -55,7 +55,8 @@ static void KillTricsSupport(void *pData){ } } /*=====================================================================*/ -static int FrameSupInterest(int iEvent, void *pEvent, void *pUser){ +static int FrameSupInterest(int iEvent, void *pEvent, void *pUser, + commandContext cc){ SConnection *pCon = NULL; int *iFrame; char pBueffel[512]; @@ -68,7 +69,9 @@ static int FrameSupInterest(int iEvent, void *pEvent, void *pUser){ iFrame = (int *)pEvent; assert(pCon); sprintf(pBueffel,"framenumber = %d",*iFrame); + SCPushContext2(pCon,cc); SCWrite(pCon,pBueffel,eWarning); + SCPopContext(pCon); return 1; } /*====================================================================== @@ -182,8 +185,9 @@ int TricsSupportAction(SConnection *pCon, SicsInterp *pSics, void *pData, return 1; } else if(strcmp(argv[1],"interest") == 0){ - lID = RegisterCallback(self->pCall, NEWFRAME, FrameSupInterest, - pCon, NULL); + lID = RegisterCallback(self->pCall, SCGetContext(pCon), + NEWFRAME, FrameSupInterest, + pCon, NULL); SCRegister(pCon,pSics, self->pCall,lID); SCSendOK(pCon); return 1; diff --git a/velodornier.c b/velodornier.c index f167ab0..b5cc384 100644 --- a/velodornier.c +++ b/velodornier.c @@ -69,7 +69,7 @@ typedef struct __VelSelDriv *pVelSelDriv; #define VSACCEL -7 #define VSFAIL -2 -typedef enum {eStart, eRegel, eHalted} eVeloMode; +typedef enum {vStart, eRegel, eHalted} eVeloMode; #define RPMALIFE 3090 /*----------------------------- The private data structure ---------------*/ @@ -165,7 +165,7 @@ typedef enum {eStart, eRegel, eHalted} eVeloMode; case eHalted: strcpy(pCommand,"SST"); pDorn->fTarget = fVal; - pDorn->eVelo = eStart; + pDorn->eVelo = vStart; pDorn->t_End = time(NULL) + 1800; /* start time + 30 min */ break; case eRegel: @@ -323,7 +323,7 @@ typedef enum {eStart, eRegel, eHalted} eVeloMode; /* some serious logic because of multi - modes */ switch(pDorn->eVelo) { - case eStart: + case vStart: *iCode = ROTSTART; *fCur = 0.; if(sStatus.cur_rpm >= RPMALIFE) @@ -577,7 +577,7 @@ typedef enum {eStart, eRegel, eHalted} eVeloMode; { pVelSelDriv pNew = NULL; pDornier pDorn = NULL; - char *pPtr = NULL; + const char *pPtr = NULL; int iVal, iRet; /* the most likely error is the parameters specified are wrong!