diff --git a/amorcomp.c b/amorcomp.c index a86a512..210f3a7 100644 --- a/amorcomp.c +++ b/amorcomp.c @@ -92,6 +92,11 @@ int handleCompCommand(pamorComp comp, SConnection *pCon, SCWrite(pCon,pBueffel,eValue); return 1; } + } else if (strcmp(argv[2], "calc") == 0) { + snprintf(pBueffel,511," %s %s calc = %f", + argv[0], argv[1], calcCompPosition(comp)); + SCWrite(pCon,pBueffel,eValue); + return 1; } else { snprintf(pBueffel,511,"ERROR: subcommand %s to %s %s not understood", argv[2], argv[0], argv[1]); diff --git a/amordrive.h b/amordrive.h index d1a53c5..dd192d0 100644 --- a/amordrive.h +++ b/amordrive.h @@ -22,4 +22,3 @@ int AmorDriveAction(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]); #endif - diff --git a/amorset.c b/amorset.c index 8b5b9ae..d49057d 100644 --- a/amorset.c +++ b/amorset.c @@ -79,8 +79,8 @@ static int calcAmorSettings(pamorSet self,SConnection *pCon){ /* * soz */ - dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M)); - soz = dist*Tand(-self->targetm2t); + dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M)); + soz = dist*Tand(-self->targetm2t); addMotorToList(self->driveList,"soz",soz); /* @@ -357,8 +357,10 @@ static int amorSetSave(void *data, char *name,FILE *fd){ return 0; } fprintf(fd,"%s dspar %f\n", name, self->dspar); + fprintf(fd,"%s detectoroffset %f\n", name, self->detectoroffset); fprintf(fd,"%s verbose %d\n", name, self->verbose); - saveAmorComp(fd,name,"mono",&self->M); + saveAmorComp(fd,name,"chopper",&self->chopper); + saveAmorComp(fd,name,"mono",&self->M); saveAmorComp(fd,name,"ds",&self->DS); saveAmorComp(fd,name,"slit2",&self->D2); saveAmorComp(fd,name,"slit3",&self->D3); @@ -519,6 +521,8 @@ static pamorComp locateComponent(pamorSet self, char *name){ return &self->D; }else if(strcmp(name,"ana") == 0){ return &self->A; + } else if(strcmp(name,"chopper") == 0){ + return &self->chopper; } else { return NULL; } @@ -527,6 +531,7 @@ static pamorComp locateComponent(pamorSet self, char *name){ static double calcCD(pamorSet self){ double soz, cmh, smh, sdh, cd, dist; + dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M)); soz = dist*Cotd(self->targetm2t); cmh = calcCompPosition(&self->M); smh = calcCompPosition(&self->S) - calcCompPosition(&self->M); @@ -535,6 +540,20 @@ static double calcCD(pamorSet self){ return cd; } /*-----------------------------------------------------------------------*/ +static double calcChopperDetectorDistance(pamorSet self){ + double dist, diff, soz; + + dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M)); + soz = dist*Tand(-self->targetm2t); + + dist = ABS(calcCompPosition(&self->M) - calcCompPosition(&self->chopper)); + diff = calcCompPosition(&self->M) - calcCompPosition(&self->S); + dist += sqrt(diff*diff + soz*soz); + dist += ABS(calcCompPosition(&self->S) - calcCompPosition(&self->D)); + dist += self->detectoroffset; + return dist; +} +/*-----------------------------------------------------------------------*/ int AmorSetAction(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]){ pamorSet self = NULL; @@ -587,12 +606,33 @@ int AmorSetAction(SConnection *pCon, SicsInterp *pSics, void *pData, SCWrite(pCon,pBueffel,eValue); return 1; } + }else if(strcmp(argv[1],"detectoroffset") == 0){ + if(argc > 2){ + if(!SCMatchRights(pCon,usUser)){ + return 0; + } + self->detectoroffset = atof(argv[2]); + SCSendOK(pCon); + return 1; + } else { + snprintf(pBueffel,131,"%s detectoroffset = %f", argv[0], + self->detectoroffset); + SCWrite(pCon,pBueffel,eValue); + return 1; + } } else if(strcmp(argv[1],"cd") == 0){ snprintf(pBueffel,131,"%s cd = %f", argv[0], calcCD(self)); SCWrite(pCon,pBueffel,eValue); return 1; + } else if(strcmp(argv[1],"cdd") == 0){ + snprintf(pBueffel,131,"%s cdd = %f", argv[0], + calcChopperDetectorDistance(self)); + SCWrite(pCon,pBueffel,eValue); + return 1; } else { - SCWrite(pCon,"ERROR: unknown subcommand to amorset",eError); + snprintf(pBueffel,131,"ERROR: unknown subcommand %s to amorset", + argv[1]); + SCWrite(pCon,pBueffel,eError); return 0; } diff --git a/amorset.h b/amorset.h index 6b0cbd7..f483d59 100644 --- a/amorset.h +++ b/amorset.h @@ -16,6 +16,7 @@ typedef struct { pObjectDescriptor pDes; pIDrivable pDriv; pIDrivable listDrive; + amorComp chopper; amorComp M; amorComp DS; amorComp D2; @@ -35,6 +36,7 @@ typedef struct { int mustRecalculate; int driveList; double dspar; + double detectoroffset; int verbose; }amorSet, *pamorSet; /*--------------------------------------------------------------------*/ diff --git a/amorset.tex b/amorset.tex index 61203d8..ca91dd8 100644 --- a/amorset.tex +++ b/amorset.tex @@ -67,6 +67,7 @@ $\langle$amorsetint {\footnotesize ?}$\rangle\equiv$ \mbox{}\verb@ pObjectDescriptor pDes;@\\ \mbox{}\verb@ pIDrivable pDriv;@\\ \mbox{}\verb@ pIDrivable listDrive;@\\ +\mbox{}\verb@ amorComp chopper;@\\ \mbox{}\verb@ amorComp M;@\\ \mbox{}\verb@ amorComp DS;@\\ \mbox{}\verb@ amorComp D2;@\\ @@ -86,6 +87,7 @@ $\langle$amorsetint {\footnotesize ?}$\rangle\equiv$ \mbox{}\verb@ int mustRecalculate;@\\ \mbox{}\verb@ int driveList;@\\ \mbox{}\verb@ double dspar;@\\ +\mbox{}\verb@ double detectoroffset;@\\ \mbox{}\verb@ int verbose;@\\ \mbox{}\verb@}amorSet, *pamorSet;@\\ \mbox{}\verb@/*--------------------------------------------------------------------*/@\\ diff --git a/amorset.w b/amorset.w index 8d9098d..2a7140a 100644 --- a/amorset.w +++ b/amorset.w @@ -50,6 +50,7 @@ typedef struct { pObjectDescriptor pDes; pIDrivable pDriv; pIDrivable listDrive; + amorComp chopper; amorComp M; amorComp DS; amorComp D2; @@ -69,6 +70,7 @@ typedef struct { int mustRecalculate; int driveList; double dspar; + double detectoroffset; int verbose; }amorSet, *pamorSet; /*--------------------------------------------------------------------*/ diff --git a/julcho.c b/julcho.c new file mode 100644 index 0000000..db7a9a1 --- /dev/null +++ b/julcho.c @@ -0,0 +1,1247 @@ +/** +* This is the implementation of a chopper driver for the MARS Chopper cascade +* as provided by the Forschungszentrum Juelich. The original host protocol +* from Juelich has been changed on our request; Gerd Theidel did some of that +* Turbo Pascal coding. +* +* copyright: see file COPYRIGHT +* +* Mark Koennecke, June-July 2006 +*/ +#include +#include +#include +#include +#define CHOCOINTERNAL +#include +#include +#include +#include +#include +#include +/*----------------------------- error codes -----------------------------*/ +#define FRAMEERROR -801 +#define CKERROR -802 +#define BADCOMMAND -803 +#define WRONGNOPAR -804 +#define ILLPAR -805 +#define PARRANGE -806 +#define BADERR -807 +#define BADREPLY -808 +#define NOTPAR -809 +#define BADTEXT -810 +#define ROPAR -811 +#define NOMEM -812 +#define HALT -813 +/*------------------------- chopper internal names -----------------------*/ +#define CH1N "snail" +#define CH2N "master" +#define CH3N "rabbit" +#define CH4N "four" +#define CH5N "five" +/*=============== Juelich chopper private data structure ================== */ +typedef struct { + prs232 controller; + pHdb parNode; + int errorCode; + time_t lastUpdate; + int updateIntervall; + int halt; +}JulCho, *pJulCho; +/*------------------------------------------------------------------------*/ +typedef struct { + char prefix[10]; + char postfix[10]; + char comCode[10]; + pJulCho pDriv; +} julCBData, *pJulCBData; +/*================= internal support functions ============================*/ +static int calculateJulCheckSum(char *realCommand){ + int i, checkSum = 0; + + for(i = 1; i < strlen(realCommand); i++){ + checkSum += (int)realCommand[i]; + } + return checkSum; +} +/*------------------------------------------------------------------------*/ +static int testJulError(char *reply){ + char *pPtr = NULL, *pEnd = NULL; + int code, status; + + if(strstr(reply,"ERR") == NULL){ + return 1; + } + pPtr = &reply[9]; /* #ERR:CCC: */ + pEnd = strchr(pPtr,'{'); + if(pEnd == NULL){ + return BADERR; + } + *pEnd = '\0'; + code = atoi(pPtr); + switch(code){ + case 1: + status = FRAMEERROR; + break; + case 2: + status = CKERROR; + break; + case 3: + status = BADCOMMAND; + break; + case 4: + status = WRONGNOPAR; + break; + case 5: + status = ILLPAR; + break; + case 6: + status = PARRANGE; + break; + default: + status = BADERR; + break; + } + return status; +} +/*------------------------------------------------------------------------*/ +static void readClean(prs232 controller){ + char buffer[1024]; + int count = 0, bufSize; + + while(availableRS232(controller) == 1 && count < 20){ + bufSize = 1024; + readRS232(controller,buffer, &bufSize); + } +} +/*-------------------------------------------------------------------------*/ +static int JulChoTransact(pJulCho self, char *command, + char *reply, int replyLen){ + int status, length, checkSum; + char realCommand[1024], checkString[30]; + + strcpy(realCommand,"#"); + strcat(realCommand,command); + + checkSum = calculateJulCheckSum(realCommand); + snprintf(checkString,30,"{%d}$", checkSum); + strcat(realCommand,checkString); + + /* + * clean the socket: If someone whacked the keyboard and the + * controller did not repsond, the line might be full of shit + */ + readClean(self->controller); + + status = transactRS232(self->controller, realCommand,strlen(realCommand), + (void *)reply, replyLen); + if(status <= 0){ + return status; + } + + status = testJulError(reply); + + return status; +} +/*---------------------------------------------------------------------------*/ +static int JulChoSetCallback(void *userData, void *callData, + pHdb currentNode, hdbValue v){ + pJulCBData cbData = NULL; + char command[256], reply[256]; + int status; + + cbData = (pJulCBData)userData; + if(cbData == NULL){ + return 0; + } + + if(v.dataType == HIPINT){ + snprintf(command,255,"%s %s%d%s",cbData->comCode, cbData->prefix, + (int)v.v.intValue,cbData->postfix); + } else if(v.dataType == HIPFLOAT){ + snprintf(command,255,"%s %s%f%s",cbData->comCode, cbData->prefix, + (float)v.v.doubleValue,cbData->postfix); + } else { + assert(0); /* this is a programming error */ + } + + cbData->pDriv->halt = 0; + status = JulChoTransact(cbData->pDriv, command, reply, 255); + if(status < 0) { + cbData->pDriv->errorCode = status; + return 0; + } + return 1; +} +/*------------------------------------------------------------------------------*/ +static pHdbCallback MakeJulChoSetCallback(pJulCho driv, char *command, + char *prefix, char *postfix){ + pJulCBData cbData = NULL; + pHdbCallback hdbCB = NULL; + + hdbCB = malloc(sizeof(hdbCallback)); + cbData = malloc(sizeof(julCBData)); + if(cbData == NULL || hdbCB == NULL){ + return NULL; + } + cbData->pDriv = driv; + strncpy(cbData->comCode,command,9); + strncpy(cbData->prefix,prefix,9); + strncpy(cbData->postfix,postfix,9); + hdbCB->next = NULL; + hdbCB->previous = NULL; + hdbCB->id = 557; + hdbCB->internalID = -1; + hdbCB->killFunc = free; + hdbCB->userCallback = JulChoSetCallback; + hdbCB->userData = cbData; + return hdbCB; +} +/*--------------------------------------------------------------------------*/ +static int splitJulChoInt(char *reply, int data[5]){ + char number[10]; + char *pPtr = NULL; + int count = 0; + + pPtr = stptok(reply,number,10,":"); + pPtr = stptok(pPtr,number,10,":"); + while(pPtr != NULL && count < 5){ + data[count] = atoi(number); + count++; + pPtr = stptok(pPtr,number,10,":"); + } + if(count < 4){ + return 0; + } + return 1; +} +/*--------------------------------------------------------------------------*/ +static int splitJulChoDouble(char *reply, double data[5]){ + char number[10]; + char *pPtr = NULL; + int count = 0; + + pPtr = stptok(reply,number,10,":"); + pPtr = stptok(pPtr,number,10,":"); + while(pPtr != NULL && count < 5){ + data[count] = (double)atof(number); + count++; + pPtr = stptok(pPtr,number,10,":"); + } + if(count < 4){ + return 0; + } + return 1; +} +/*--------------------------------------------------------------------------*/ +static int setJulChoIntPar(pHdb root, char *par, int data[5]){ + char path[256]; + hdbValue v; + pHdb node = NULL; + + memset(&v,0,sizeof(hdbValue)); + v.dataType = HIPINT; + + snprintf(path,255,"%s/%s",CH1N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.intValue = (long)data[0]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH2N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.intValue = (long)data[1]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH3N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.intValue = (long)data[2]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH4N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.intValue = (long)data[3]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH5N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.intValue = (long)data[4]; + UpdateHipadabaPar(node,v,NULL); + + return 1; +} +/*--------------------------------------------------------------------------*/ +static int setJulChoDoublePar(pHdb root, char *par, double data[5]){ + char path[256]; + hdbValue v; + pHdb node = NULL; + + memset(&v,0,sizeof(hdbValue)); + v.dataType = HIPFLOAT; + + snprintf(path,255,"%s/%s",CH1N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.doubleValue = data[0]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH2N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.doubleValue = data[1]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH3N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.doubleValue = data[2]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH4N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.doubleValue = data[3]; + UpdateHipadabaPar(node,v,NULL); + + snprintf(path,255,"%s/%s",CH5N,par); + node = GetHipadabaNode(root,path); + assert(node != NULL); + v.v.doubleValue = data[4]; + UpdateHipadabaPar(node,v,NULL); + + return 1; +} +/*--------------------------------------------------------------------------*/ +static void updateJulChoFlag(pHdb node, char *choppername, char *parname, + int code, int mask){ + char path[256]; + hdbValue v; + pHdb target = NULL; + + v.dataType = HIPINT; + snprintf(path,255,"%s/%s",choppername,parname); + if((mask & code) > 0) { + v.v.intValue = 1; + } else { + v.v.intValue = 0; + } + target = GetHipadabaNode(node,path); + assert(target != NULL); + UpdateHipadabaPar(target,v,NULL); +} +/*---------------------------------------------------------------------------*/ +static int setJulChoFlags(pHdb node, int intData[5]){ + char *chNames[] = {CH1N, + CH2N, + CH3N, + CH4N, + CH5N, + NULL}; + char path[256]; + int i, code; + hdbValue v; + + memset(&v,0,sizeof(hdbValue)); + v.dataType = HIPINT; + for(i = 0; i < 5; i++){ + code = intData[i]; + updateJulChoFlag(node,chNames[i],"microok",code,1); + updateJulChoFlag(node,chNames[i],"atspeed",code,2); + updateJulChoFlag(node,chNames[i],"atphase",code,4); + updateJulChoFlag(node,chNames[i],"magneton",code,8); + updateJulChoFlag(node,chNames[i],"dcon",code,16); + updateJulChoFlag(node,chNames[i],"driveon",code,32); + updateJulChoFlag(node,chNames[i],"currentdc",code,64); + updateJulChoFlag(node,chNames[i],"lockopen",code,128); + updateJulChoFlag(node,chNames[i],"diskopen",code,256); + updateJulChoFlag(node,chNames[i],"diskclosed",code,512); + updateJulChoFlag(node,chNames[i],"speedoverflow",code,1024); + updateJulChoFlag(node,chNames[i],"bearingfailed",code,2048); + updateJulChoFlag(node,chNames[i],"voltagetohigh",code,4096); + } + + return 1; +} +/*--------------------------------------------------------------------------*/ +static int ReadJulChoFlags(pJulCho self){ + int status, intData[5]; + char reply[256]; + + status = JulChoTransact(self,"RSC",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoInt(reply,intData)){ + self->errorCode = BADREPLY; + } + setJulChoFlags(self->parNode, intData); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int UpdateJulChoParameters(pJulCho self){ + int status, intData[5]; + double doubleData[5]; + char reply[255]; + + if(time(NULL) < self->lastUpdate + self->updateIntervall){ + return 1; + } + + status = JulChoTransact(self,"RNS",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoInt(reply,intData)){ + self->errorCode = BADREPLY; + } + setJulChoIntPar(self->parNode,"nomspeed",intData); + + status = JulChoTransact(self,"RAS",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoInt(reply,intData)){ + self->errorCode = BADREPLY; + } + setJulChoIntPar(self->parNode,"actspeed",intData); + + status = JulChoTransact(self,"RNP",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"nomphase",doubleData); + + status = JulChoTransact(self,"RAP",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"actphase",doubleData); + + status = JulChoTransact(self,"RGW",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"gatewidth",doubleData); + + status = JulChoTransact(self,"RNC",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"nomcurrent",doubleData); + + + status = JulChoTransact(self,"RAC",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"actcurrent",doubleData); + + status = JulChoTransact(self,"RAV",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"voltage",doubleData); + + status = JulChoTransact(self,"RIT",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"inverter_temperature",doubleData); + + status = JulChoTransact(self,"RST",reply,255); + if(status < 0){ + self->errorCode = status; + return 0; + } + if(!splitJulChoDouble(reply,doubleData)){ + self->errorCode = BADREPLY; + } + setJulChoDoublePar(self->parNode,"stator_temperature",doubleData); + + status = ReadJulChoFlags(self); + + self->lastUpdate = time(NULL); + return status; +} +/*------------------------------------------------------------------------*/ +static void JulChoErrorcodeToString(int code, char *pError, int iLen){ + switch(code){ + case FRAMEERROR: + strncpy(pError,"Frame error",iLen); + break; + case CKERROR: + strncpy(pError,"Checksum error",iLen); + break; + case BADCOMMAND: + strncpy(pError,"Bad Command",iLen); + break; + case WRONGNOPAR: + strncpy(pError,"Wrong number of parameters",iLen); + break; + case ILLPAR: + strncpy(pError,"Illegal parameter",iLen); + break; + case PARRANGE: + strncpy(pError,"Parameter out of range",iLen); + break; + case BADERR: + strncpy(pError,"Controller error not recognised",iLen); + break; + case BADREPLY: + strncpy(pError,"Unexpected reply",iLen); + break; + case NOTPAR: + strncpy(pError,"Unsupported parameter",iLen); + break; + case BADTEXT: + strncpy(pError,"Failed to convert text to number",iLen); + break; + case ROPAR: + strncpy(pError,"Read only Parameter",iLen); + break; + case NOMEM: + strncpy(pError,"Out of memory formatting parameter",iLen); + break; + case HALT: + strncpy(pError,"User requested HALT; choppers status undefined ",iLen); + break; + case SICSCBRANGE: + strncpy(pError,"Parameter value out of range",iLen); + break; + case SICSCBRO: + strncpy(pError,"Parameter is READ-ONLY",iLen); + break; + default: + getRS232Error(code, pError, iLen); + break; + } +} +/*-------------------------------------------------------------------------*/ +static int JulChoGetCallback(void *userData, void *callData, + pHdb currentNode, hdbValue v){ + pJulCho self = NULL; + SConnection *pCon = NULL; + int status; + char error[128], buffer[256]; + + self = (pJulCho)userData; + pCon = (SConnection *)callData; + + assert(self != NULL); + + status = UpdateJulChoParameters(self); + if(status != 1 && pCon != NULL){ + JulChoErrorcodeToString(self->errorCode, error,127); + snprintf(buffer,255,"ERROR: %s occurred reading par",error); + SCWrite(pCon,buffer,eError); + } + return status; +} +/*--------------------------------------------------------------------------*/ +static int AppendJulChoROPar(pHdb parent, char *name, int type){ + pHdb res = NULL; + hdbValue v; + + memset(&v,0,sizeof(hdbValue)); + v.dataType = type; + res = MakeSICSROPar(name,v); + if(res == NULL){ + return 0; + } + AddHipadabaChild(parent,res); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int ConfigureSingleJulCho(pHdb parent, pJulCho driv, + char *prefix, char *postfix){ + pHdb child = NULL; + pHdbCallback pCb = NULL; + + /* + * write parameters + */ + + child = MakeHipadabaNode("nomphase",HIPFLOAT,0); + if(child == NULL){ + return 0; + } + pCb = MakeCheckPermissionCallback(usUser); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(child,HCBSET,pCb); + pCb = MakeFloatRangeCallback(5.0, 355.); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(child,HCBSET,pCb); + pCb = MakeJulChoSetCallback(driv,"SPH", prefix,postfix); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(child,HCBSET,pCb); + AddHipadabaChild(parent,child); + + child = MakeHipadabaNode("gatewidth",HIPFLOAT,0); + if(child == NULL){ + return 0; + } + pCb = MakeCheckPermissionCallback(usUser); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(child,HCBSET,pCb); + pCb = MakeJulChoSetCallback(driv,"SGW", prefix,postfix); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(child,HCBSET,pCb); + AddHipadabaChild(parent,child); + + if(!AppendJulChoROPar(parent,"nomspeed",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"actspeed",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"actphase",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"nomcurrent",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"actcurrent",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"voltage",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"inverter_temperature",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"stator_temperature",HIPFLOAT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"microok",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"atspeed",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"atphase",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"magneton",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"dcon",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"driveon",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"currentdc",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"lockopen",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"diskopen",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"diskclosed",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"speedoverflow",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"bearingfailed",HIPINT)){ + return 0; + } + + if(!AppendJulChoROPar(parent,"voltagetohigh",HIPINT)){ + return 0; + } + + /* + * append get callbacks + */ + child = parent->child; + while(child != NULL){ + AppendHipadabaCallback(child,HCBREAD, + MakeHipadabaCallback(JulChoGetCallback,driv,NULL,-1,-1)); + child = child->next; + } + return 1; +} +/*--------------------------------------------------------------------------*/ +static int InitJulChoPar(pJulCho driv){ + pHdb child = NULL, parChild = NULL; + pHdbCallback pCb = NULL; + + + child = MakeHipadabaNode(CH1N,HIPNONE,0); + if(child == NULL){ + return 0; + } + if(!ConfigureSingleJulCho(child,driv,"","::::")){ + return 0; + } + AddHipadabaChild(driv->parNode,child); + + child = MakeHipadabaNode(CH2N,HIPNONE,0); + if(child == NULL){ + return 0; + } + if(!ConfigureSingleJulCho(child,driv,":",":::")){ + return 0; + } + /** + * the master speed can be set, the slaves not, thus remove the read only + * set callback and replace by a speed setting callback + */ + parChild = GetHipadabaNode(child,"nomspeed"); + assert(parChild != NULL); + DeleteCallbackChain(parChild->writeCallbacks); + parChild->writeCallbacks = NULL; + + pCb = MakeCheckPermissionCallback(usUser); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(parChild,HCBSET,pCb); + pCb = MakeJulChoSetCallback(driv,"SMS","",""); + if(pCb == NULL){ + return 0; + } + AppendHipadabaCallback(parChild,HCBSET,pCb); + AddHipadabaChild(driv->parNode,child); + + child = MakeHipadabaNode(CH3N,HIPNONE,0); + if(child == NULL){ + return 0; + } + if(!ConfigureSingleJulCho(child,driv,"::","::")){ + return 0; + } + AddHipadabaChild(driv->parNode,child); + + child = MakeHipadabaNode(CH4N,HIPNONE,0); + if(child == NULL){ + return 0; + } + if(!ConfigureSingleJulCho(child,driv,":::",":")){ + return 0; + } + AddHipadabaChild(driv->parNode,child); + + child = MakeHipadabaNode(CH5N,HIPNONE,0); + if(child == NULL){ + return 0; + } + if(!ConfigureSingleJulCho(child,driv,"::::","")){ + return 0; + } + AddHipadabaChild(driv->parNode,child); + + return 1; +} +/*================= actual interface functions ==============================*/ +static int JulChoInit(pCodri pDriv){ + pJulCho self = NULL; + int status; + + self = (pJulCho)pDriv->pPrivate; + status = initRS232(self->controller); + if(status < 1){ + self->errorCode = status; + } + setRS232SendTerminator(self->controller,"$"); + setRS232ReplyTerminator(self->controller,"$"); + setRS232Timeout(self->controller,2000); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int JulChoClose(pCodri pDriv){ + pJulCho self = NULL; + + self = (pJulCho)pDriv->pPrivate; + closeRS232(self->controller); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int JulChoKill(pCodri pDriv){ + pJulCho self = NULL; + + if(pDriv == NULL){ + return 1; + } + self = (pJulCho)pDriv->pPrivate; + JulChoClose(pDriv); + if(self->controller != NULL){ + KillRS232(self->controller); + self->controller = NULL; + } + free(self); + return 1; +} +/*--------------------------------------------------------------------------- + * The set and get routines introduce phase and speed values which map to + * nomspeed when setting and actspeed when reading. This ugly hack saves me + * to introduce another drive adapter which set one parameter and reads + * another + *---------------------------------------------------------------------------*/ +static int JulChoSetPar2(pCodri pDriv, char *parname, char *pValue){ + pJulCho self = NULL; + pHdb target = NULL; + hdbValue v; + char error[64], *pPtr = NULL; + int status; + + if(strcmp(parname,"master/speed") == 0){ + status = JulChoSetPar2(pDriv,"master/nomspeed", pValue); + if(status == 1) { + SicsWait(10); + } + return status; + } else if(strcmp(parname,"master/phase") == 0){ + return JulChoSetPar2(pDriv,"master/nomphase", pValue); + } else if(strcmp(parname,"snail/phase") == 0){ + return JulChoSetPar2(pDriv,"snail/nomphase", pValue); + } else if(strcmp(parname,"rabbit/phase") == 0){ + return JulChoSetPar2(pDriv,"rabbit/nomphase", pValue); + } else if(strcmp(parname, "four/phase") == 0){ + return JulChoSetPar2(pDriv,"four/nomphase", pValue); + } else if(strcmp(parname,"five/phase") == 0){ + return JulChoSetPar2(pDriv,"five/nomphase", pValue); + } + + self = (pJulCho)pDriv->pPrivate; + target = GetHipadabaNode(self->parNode,parname); + if(target == NULL){ + self->errorCode = NOTPAR; + return 0; + } + v.dataType = target->value.dataType; + if(!readHdbValue(&v,pValue,error,63)){ + self->errorCode = BADTEXT; + return 0; + } + self->errorCode = 0; + status = SetHipadabaPar(target,v,NULL); + if(status == 0 && self->errorCode == 0){ + self->errorCode = status; + return 0; + } + self->lastUpdate = 0; + return status; +} +/*-------------------------------------------------------------------------------*/ +static int JulChoSetPar(pCodri pDriv, char *parname, float fValue){ + pJulCho self = NULL; + pHdb target = NULL; + hdbValue v; + char error[64]; + int status; + + if(strcmp(parname,"master/speed") == 0){ + status = JulChoSetPar(pDriv,"master/nomspeed", fValue); + if(status == 1) { + SicsWait(10); + } + return status; + } else if(strcmp(parname,"master/phase") == 0){ + return JulChoSetPar(pDriv,"master/nomphase", fValue); + } else if(strcmp(parname,"snail/phase") == 0){ + return JulChoSetPar(pDriv,"snail/nomphase", fValue); + } else if(strcmp(parname,"rabbit/phase") == 0){ + return JulChoSetPar(pDriv,"rabbit/nomphase", fValue); + } else if(strcmp(parname, "four/phase") == 0){ + return JulChoSetPar(pDriv,"four/nomphase", fValue); + } else if(strcmp(parname,"five/phase") == 0){ + return JulChoSetPar(pDriv,"five/nomphase", fValue); + } + + self = (pJulCho)pDriv->pPrivate; + target = GetHipadabaNode(self->parNode,parname); + if(target == NULL){ + self->errorCode = NOTPAR; + return 0; + } + v.dataType = target->value.dataType; + if(v.dataType == HIPINT){ + v.v.intValue = (int)fValue; + } else { + v.v.doubleValue = (double)fValue; + } + self->errorCode = 0; + status = SetHipadabaPar(target,v,NULL); + if(status == 0 && self->errorCode == 0){ + self->errorCode = status; + return 0; + } + self->lastUpdate = 0; + return status; +} +/*---------------------------------------------------------------------------*/ +static int JulChoHalt(pCodri pDriv){ + pJulCho self = NULL; + + self = (pJulCho)pDriv->pPrivate; + self->halt = 1; + /* + * empty function on Philips request + */ + return 1; +} +/*--------------------------------------------------------------------------*/ +static int JulChoGetPar(pCodri pDriv, char *parname, + char *pBuffer, int iBufLen){ + pJulCho self = NULL; + pHdb target = NULL; + pDynString val = NULL; + + if(strcmp(parname,"master/speed") == 0){ + return JulChoGetPar(pDriv,"master/actspeed", pBuffer,iBufLen); + } else if(strcmp(parname,"master/phase") == 0){ + return JulChoGetPar(pDriv,"master/actphase", pBuffer,iBufLen); + } else if(strcmp(parname,"snail/phase") == 0){ + return JulChoGetPar(pDriv,"snail/actphase", pBuffer,iBufLen); + } else if(strcmp(parname,"rabbit/phase") == 0){ + return JulChoGetPar(pDriv,"rabbit/actphase", pBuffer,iBufLen); + } else if(strcmp(parname, "four/phase") == 0){ + return JulChoGetPar(pDriv,"four/actphase", pBuffer,iBufLen); + } else if(strcmp(parname,"five/phase") == 0){ + return JulChoGetPar(pDriv,"five/actphase", pBuffer,iBufLen); + } + + + self = (pJulCho)pDriv->pPrivate; + target = GetHipadabaNode(self->parNode,parname); + if(target == NULL){ + self->errorCode = NOTPAR; + return 0; + } + if(!UpdateJulChoParameters(self)){ + return 0; + } + val = formatValue(target->value); + if(val == NULL){ + self->errorCode = NOMEM; + return 0; + } + strncpy(pBuffer,GetCharArray(val), iBufLen); + DeleteDynString(val); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int JulChoCheckPar(pCodri pDriv, char *parname){ + pJulCho self = NULL; + int i; + char path[256]; + pHdb target = NULL; + char *chNames[] = {CH1N, + CH2N, + CH3N, + CH4N, + CH5N, + NULL}; + + self = (pJulCho)pDriv->pPrivate; + + if(self->halt == 1){ + self->errorCode = HALT; + return HWFault; + } + /* + * For the Juelich chopper this means checking the atspeed, atphase + * flags + */ + if(!ReadJulChoFlags(self)){ + return HWFault; + } + for(i = 0; i < 5; i++){ + snprintf(path,255,"%s/atspeed", chNames[i]); + target = GetHipadabaNode(self->parNode,path); + assert(target != NULL); + if(target->value.v.intValue == 0){ + return HWBusy; + } + snprintf(path,255,"%s/atphase",chNames[i]); + target = GetHipadabaNode(self->parNode,path); + assert(target != NULL); + if(target->value.v.intValue == 0){ + return HWBusy; + } + } + return HWIdle; +} +/*---------------------------------------------------------------------------*/ +static int JulChoError(pCodri pDriv, int *iCode, char *pError, int iLen){ + pJulCho self = NULL; + + self = (pJulCho)pDriv->pPrivate; + + *iCode = self->errorCode; + JulChoErrorcodeToString(self->errorCode, pError, iLen); + return 1; +} +/*---------------------------------------------------------------------------*/ +static int JulChoFix(pCodri pDriv, int code){ + pJulCho self = NULL; + + self = (pJulCho)pDriv->pPrivate; + + switch(code){ + case TIMEOUT: + case CKERROR: + case BADERR: + case BADREPLY: + return CHREDO; + break; + case NOTCONNECTED: + if(initRS232(self->controller) == 1){ + return CHREDO; + } else { + return CHFAIL; + } + break; + case INCOMPLETE: + case BADSEND: + case BADREAD: + closeRS232(self->controller); + if(initRS232(self->controller) == 1){ + return CHREDO; + } else { + return CHFAIL; + } + break; + default: + return CHFAIL; + break; + } +} +/*---------------------------------------------------------------------------*/ +static pCodri MakeJulChoDriver(char *pHost, int port, pHdb parNode){ + pCodri pDriv = NULL; + pJulCho self = NULL; + int i; + pDynString names = NULL; + char *chNames[] = {CH1N, + CH2N, + CH3N, + CH4N, + CH5N, + NULL}; + char path[256], *par = NULL; + + pDriv = malloc(sizeof(Codri)); + self = malloc(sizeof(JulCho)); + names = CreateDynString(256,256); + + if(pDriv == NULL && self == NULL && names == NULL){ + return NULL; + } + memset(pDriv,0,sizeof(Codri)); + memset(self,0,sizeof(JulCho)); + + self->parNode = parNode; + self->controller = createRS232(pHost,port); + if(self->controller == NULL){ + free(self); + free(pDriv); + return NULL; + } + self->updateIntervall = 10; + if(!InitJulChoPar(self)){ + free(self); + free(pDriv); + return NULL; + } + + pDriv->pPrivate = self; + + pDriv->Init = JulChoInit; + pDriv->Close = JulChoClose; + pDriv->Delete = JulChoKill; + pDriv->SetPar = JulChoSetPar; + pDriv->SetPar2 = JulChoSetPar2; + pDriv->GetPar = JulChoGetPar; + pDriv->CheckPar = JulChoCheckPar; + pDriv->GetError = JulChoError; + pDriv->TryFixIt = JulChoFix; + pDriv->Halt = JulChoHalt; + + for(i = 0; i < 5; i++){ + snprintf(path,255,"%s/nomspeed,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/actspeed,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/nomphase,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/actphase,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/gatewidth,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/nomcurrent,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/actcurrent,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/voltage,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/inverter_temperature,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/stator_temperature,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/microok,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/atspeed,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/atphase,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/magneton,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/dcon,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/driveon,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/currentdc,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/lockopen,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/diskopen,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/diskclosed,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/speedoverflow,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/bearingfailed,",chNames[i]); + DynStringConcat(names,path); + snprintf(path,255,"%s/voltagetohigh,",chNames[i]); + DynStringConcat(names,path); + } + par = GetCharArray(names); + par[strlen(par) -1] = '\0'; + pDriv->pParList = strdup(par); + DeleteDynString(names); + if(pDriv->pParList == NULL){ + JulChoKill(pDriv); + free(pDriv); + return NULL; + } + + return pDriv; +} +/*--------------------------------------------------------------------------*/ +int JulChoFactory(SConnection *pCon, SicsInterp *pSics, void *pData, + int argc, char *argv[]){ + pChoco pNew = NULL; + pCodri pDriv = NULL; + pObjectDescriptor pDes = NULL; + char pBueffel[132]; + int iRet, iPort, iChannel; + int iSingle = 0; + + if(argc < 4) + { + SCWrite(pCon, + "ERROR: Insufficient number of arguments to MakeJulCho", + eError); + return 0; + } + + + /* first try to get everything done */ + pNew = (pChoco)malloc(sizeof(Choco)); + pDes = CreateDescriptor("Chopper"); + pDes->parNode = MakeHipadabaNode(argv[1], HIPNONE,0); + /* do driver */ + + pDriv = MakeJulChoDriver(argv[2],atoi(argv[3]),pDes->parNode); + if(pDriv == NULL){ + sprintf(pBueffel,"ERROR: Failed to initialize JulCho Driver"); + SCWrite(pCon,pBueffel,eError); + return 0; + } + if( (pNew == NULL) || (pDes == NULL) || (pDriv == NULL) ) + { + SCWrite(pCon,"ERROR: No memory left to create controller",eError); + return 0; + } + pNew->pDes = pDes; + pNew->pDriv = pDriv; + + /* initialize driver */ + iRet = pDriv->Init(pDriv); + if(!iRet){ + SCWrite(pCon,"ERROR: Failed to initialize driver",eError); + KillChoco(pNew); + return 0; + } + + /* install as command */ + iRet = AddCommand(pSics, argv[1],ChocoAction,KillChoco,pNew); + if(!iRet){ + sprintf(pBueffel,"ERROR: duplicate command %s NOT created", + argv[1]); + SCWrite(pCon,pBueffel,eError); + return 0; + } + return 1; +} + + diff --git a/libpsi.a b/libpsi.a index b77e05f..3a15d81 100644 Binary files a/libpsi.a and b/libpsi.a differ diff --git a/make_gen b/make_gen index 259a73e..1e6f345 100644 --- a/make_gen +++ b/make_gen @@ -17,7 +17,7 @@ OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.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 tcpdornier.o sinqhttp.o\ - dgrambroadcast.o sinq.o tabledrive.o tcpdocho.o + dgrambroadcast.o sinq.o tabledrive.o tcpdocho.o julcho.o MZOBJ=fsm.o logger.o sugar.o pardef.o ease.o strobj.o oxinst.o logreader.o \ ipsdriv.o ilmdriv.o itcdriv.o ighdriv.o euro2kdriv.o modbus.o arrobj.o diff --git a/psi.c b/psi.c index 8448419..64e7aa8 100644 --- a/psi.c +++ b/psi.c @@ -60,7 +60,11 @@ */ extern int VelSelTcpFactory(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]); - +/* + * from julcho.c + */ +extern int JulChoFactory(SConnection *pCon, SicsInterp *pSics, void *pData, + int argc, char *argv[]); /*--------------------------------------------------------------------------*/ void SiteInit(void) { @@ -110,6 +114,7 @@ static void AddPsiCommands(SicsInterp *pInter){ AddCommand(pInter,"MakeTableDrive",TableDriveFactory,NULL,NULL); AddCommand(pInter,"MakeAmorSet",AmorSetFactory,NULL,NULL); AddCommand(pInter,"MakeTCPSelector",VelSelTcpFactory,NULL,NULL); + AddCommand(pInter,"MakeJulCho",JulChoFactory,NULL,NULL); /* AddCommand(pInter,"MakeDifrac",MakeDifrac,NULL,NULL); */ @@ -130,6 +135,7 @@ static void RemovePsiCommands(SicsInterp *pSics){ RemoveCommand(pSics,"MakeStoreAmor"); RemoveCommand(pSics,"MakeAmorStatus"); RemoveCommand(pSics,"MakeTCPSelector"); + RemoveCommand(pSics,"MakeJulCho"); /* RemoveCommand(pSics,"MakeDifrac"); */ @@ -285,7 +291,6 @@ 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;