linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e36e62d8815bafb07f266e40526600a2e718e4ea
sicspsi/julcho.c
koennecke 0eeaaaff9d - Fixed sinqhhtprot.c
2008-06-18 14:08:55 +00:00

1321 lines
38 KiB
C
Raw Blame History

/**
* 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
*
* Parameter updating is to slow, therefore it happens in two groups now:
* actspeed and actphase are requested more frequently, so they go separate
*
* Mark Koennecke, October 2006
*/
#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <sics.h>
#include <unistd.h>
#define CHOCOINTERNAL
#include <choco.h>
#include <SCinter.h>
#include <rs232controller.h>
#include <sicshipadaba.h>
#include <stptok.h>
#include <dynstring.h>
/*----------------------------- 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
/*--------------------- update flags -------------------------------------*/
#define ALL 1
#define SPEED 2
/*--------------------- wait after set -----------------------------------*/
#define STWAIT 5
/*------------------------- 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 speedUpdate;
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 hdbCallbackReturn JulChoSetCallback(pHdb node, void *userData,
pHdbMessage message){
pJulCBData cbData = NULL;
char command[256], reply[256];
pHdbDataMessage mm = NULL;
int status;
cbData = (pJulCBData)userData;
if(cbData == NULL){
return hdbContinue;
}
/*
* Is this for us?
*/
mm = GetHdbSetMessage(message);
if(mm == NULL){
return hdbContinue;
}
if(mm->v->dataType == HIPINT){
snprintf(command,255,"%s %s%d%s",cbData->comCode, cbData->prefix,
(int)mm->v->v.intValue,cbData->postfix);
} else if(mm->v->dataType == HIPFLOAT){
snprintf(command,255,"%s %s%f%s",cbData->comCode, cbData->prefix,
(float)mm->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 hdbAbort;
}
return hdbContinue;
}
/*------------------------------------------------------------------------------*/
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->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);
/* fprintf(stdout,"Chopper flags = %s\n", reply);*/
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 what){
int status, intData[5];
double doubleData[5];
char reply[255];
assert(what == ALL || what == SPEED);
if(what == SPEED){
if(time(NULL) < self->speedUpdate + self->updateIntervall){
return 1;
}
} else{
if(time(NULL) < self->lastUpdate + self->updateIntervall){
return 1;
}
}
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,"RAP",reply,255);
if(status < 0){
self->errorCode = status;
return 0;
}
if(!splitJulChoDouble(reply,doubleData)){
self->errorCode = BADREPLY;
}
setJulChoDoublePar(self->parNode,"actphase",doubleData);
if(what != ALL){
self->speedUpdate = time(NULL);
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,"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,"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);
self->speedUpdate = 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 testParGroup(char *name){
if(strstr(name,"actspeed") != NULL ||
strstr(name,"actphase") != NULL){
return SPEED;
} else {
return ALL;
}
}
/*-------------------------------------------------------------------------*/
static hdbCallbackReturn JulChoGetCallback(pHdb currentNode,
void *userData, pHdbMessage message){
pJulCho self = NULL;
SConnection *pCon = NULL;
int status;
char error[128], buffer[256];
pHdbDataMessage mm = NULL;
self = (pJulCho)userData;
assert(self != NULL);
mm = GetHdbGetMessage(message);
if(mm == NULL){
return hdbContinue;
}
pCon = (SConnection *)mm->callData;
status = UpdateJulChoParameters(self,testParGroup(currentNode->name));
if(status != 1 && pCon != NULL){
JulChoErrorcodeToString(self->errorCode, error,127);
snprintf(buffer,255,"ERROR: %s occurred reading par",error);
SCWrite(pCon,buffer,eError);
}
return hdbContinue;
}
/*--------------------------------------------------------------------------*/
static int AppendJulChoROPar(pHdb parent, char *name, int type){
pHdb child = NULL;
child = AddSICSHdbROPar(parent,name,makeHdbValue(type,1));
if(child != NULL){
return 1;
} else {
return 0;
}
}
/*---------------------------------------------------------------------------*/
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,pCb);
pCb = MakeFloatRangeCallback(5.0, 355.);
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(child,pCb);
pCb = MakeJulChoSetCallback(driv,"SPH", prefix,postfix);
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(child,pCb);
AddHipadabaChild(parent,child,NULL);
child = MakeHipadabaNode("gatewidth",HIPFLOAT,0);
if(child == NULL){
return 0;
}
pCb = MakeCheckPermissionCallback(usUser);
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(child,pCb);
pCb = MakeJulChoSetCallback(driv,"SGW", prefix,postfix);
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(child,pCb);
AddHipadabaChild(parent,child,NULL);
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,
MakeHipadabaCallback(JulChoGetCallback,driv,NULL));
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,NULL);
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);
/*
* delete the callback cahin in order to remove the
* read only callback
*/
DeleteCallbackChain(parChild);
parChild->callBackChain = NULL;
pCb = MakeCheckPermissionCallback(usUser);
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(parChild,pCb);
pCb = MakeJulChoSetCallback(driv,"SMS","","");
if(pCb == NULL){
return 0;
}
AppendHipadabaCallback(parChild,pCb);
AppendHipadabaCallback(parChild,
MakeHipadabaCallback(JulChoGetCallback,driv,NULL));
AddHipadabaChild(driv->parNode,child,NULL);
child = MakeHipadabaNode(CH3N,HIPNONE,0);
if(child == NULL){
return 0;
}
if(!ConfigureSingleJulCho(child,driv,"::","::")){
return 0;
}
AddHipadabaChild(driv->parNode,child,NULL);
child = MakeHipadabaNode(CH4N,HIPNONE,0);
if(child == NULL){
return 0;
}
if(!ConfigureSingleJulCho(child,driv,":::",":")){
return 0;
}
AddHipadabaChild(driv->parNode,child,NULL);
child = MakeHipadabaNode(CH5N,HIPNONE,0);
if(child == NULL){
return 0;
}
if(!ConfigureSingleJulCho(child,driv,"::::","")){
return 0;
}
AddHipadabaChild(driv->parNode,child,NULL);
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;
}
/*
* The SicsWait is here to allow the chopper to update his status flags.
* There were occurrences where the chopper was still driving but the
* status flag did not reflect this.
*/
SicsWait(STWAIT);
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;
}
/*
* The SicsWait is here to allow the chopper to update his status flags.
* There were occurrences where the chopper was still driving but the
* status flag did not reflect this.
*/
SicsWait(STWAIT);
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,testParGroup(parname))){
return 0;
}
val = formatValue(target->value, target);
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, status;
char path[256], reply[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;
}
}
status = JulChoTransact(self,"RSC",reply,255);
if(status < 0){
self->errorCode = status;
return HWFault;
}
/* fprintf(stdout,"Chopper Flags at finish: %s\n", reply);*/
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;
}
Reference in New Issue View Git Blame Copy Permalink