/*************************************************************
*
* exceptionHandler - Wait for Messages that indicate some problem
* with the system. Then perform appropriate recovery.
*
* Author Greg Brissey 12-7-94
*/
pHandler(MSG_Q_ID msges)
{
Cntlr_Comm msge;
int *val;
int bytes;
void recovery(Cntlr_Comm *);
void extrecovery(Cntlr_Comm *);
DPRINT(1,"pHandler :Server LOOP Ready & Waiting.\n");
FOREVER
{
markReady(PHANDLER_FLAGBIT);
memset( &msge, 0, sizeof( Cntlr_Comm ) );
bytes = msgQReceive(pMsgesToPHandlr, (char*) &msge,
sizeof( Cntlr_Comm ), WAIT_FOREVER);
markBusy(PHANDLER_FLAGBIT);
DPRINT5(-1,"'%s': pHandler: recv: %d bytes, From Cntlr: '%s', Exception Type: %d, Event: %d \n",
hostName,bytes, msge.cntlrId, msge.cmd, msge.arg1);
if (strcmp(msge.cntlrId,hostName) == 0)
{
recovery( &msge );
}
else
{
extrecovery( &msge );
DPRINT(-1,"From another Controller, No action yet.\n");
}
/* prevent false starts from Sync glitches, controller reboots, or FPGA reloads */
cntrlFifoClearStartMode();
}
}
MBUFFER_ID mBufferCreate(MBUF_DESC *clusters, int numClusterSize, int eventid, int memReserveMB)
/* MBUF_DESC *clusters - tables of clusters sizes to allocate for the memory pool */
/* numClusterSize - number of cluster sets. */
/* eventid WindViwe Event ID */
{
MBUFFER_ID pMBufferId;
int toP;
long sizeNBytes;
int i,j,index;
DPRINT1(1,"mbCLTbl: bufSzie: %d\n",clusters[1].clSize);
pMBufferId = (MBUFFER_ID) malloc(sizeof(MBUFFER_OBJ)); /* create structure */
if (pMBufferId == NULL)
return (NULL);
/* zero out structure */
memset(pMBufferId,0,sizeof(MBUFFER_OBJ));
pMBufferId->mbclusters = clusters;
pMBufferId->numLists = numClusterSize;
pMBufferId->memReserveLevel = memReserveMB * 1048576;
/* malloc space for the different size buffers */
for ( i=0; i < numClusterSize; i++)
{
clusters[i].memSize = clusters[i].clNum * ( clusters[i].clSize + sizeof(long));
/* clusters[i].memArea = (char *) malloc( clusters[i].memSize ); */
/* clusters[i].memArea = (char *) memalign(4, clusters[i].memSize ); /* align to 4 bytes */
clusters[i].memArea = (char *) memalign(_CACHE_ALIGN_SIZE, clusters[i].memSize ); /* align to cache line 32 bytes for 405 */
DPRINT5(1,"cluster[%d]: size: %d, num: %d, memSize: %d, memArea: 0x%lx\n",
i,clusters[i].clSize,clusters[i].clNum,clusters[i].memSize,clusters[i].memArea);
if (clusters[i].memArea == NULL)
{
free(pMBufferId);
for (j=i-1; j >= 0; j--)
free(clusters[i].memArea);
return(NULL);
}
pMBufferId->listSizes[i] = clusters[i].clSize;
pMBufferId->freeList[i] = rngLCreate(clusters[i].clNum,"ClusterFreeList");
}
fillRngwAddrs(pMBufferId);
return(pMBufferId);
}
RTIBool Lock_CmdCallback(const NDDSRecvInfo *issue, NDDSInstance *instance,
void *callBackRtnParam)
{
Lock_Cmd *recvIssue;
void LockCmdParser(Lock_Cmd *issue);
int *param;
if (issue->status == NDDS_FRESH_DATA)
{
recvIssue = (Lock_Cmd *) instance;
DPRINT5(-1,"Lock_Cmd CallBack: cmd: %d, arg1: %d, arg2: %d, arg3: %lf, arg4: %lf crc: 0x%lx\n",
recvIssue->cmd,recvIssue->arg1, recvIssue->arg2, recvIssue->arg3, recvIssue->arg4);
msgQSend(pMsgesToLockParser, (char*) recvIssue, sizeof(Lock_Cmd), NO_WAIT, MSG_PRI_NORMAL);
/* LockCmdParser(recvIssue); */
}
return RTI_TRUE;
}
static int
readBondTableOverlay(char *filename)
{
char buf[4096];
char *token;
int lineNumber = 0;
int err;
int protons;
int group;
int period;
char *parentSymbol;
char *symbol;
char *name;
double mass;
double rvdW;
double evdW;
int nBonds;
double rCovalent;
double charge;
int isVirtual;
double ks;
double r0;
double de;
double beta;
double inflectionR;
double x1;
double y1;
double x2;
double y2;
int quality;
int quadratic;
double ktheta;
double theta0;
double cutoffRadiusStart;
double cutoffRadiusEnd;
double value;
double angleUnits = 1.0; // default to radians
FILE *f = fopen(filename, "r");
if (f == NULL) {
// silent about not finding file
return 0;
}
write_traceline("# reading parameter file: %s\n", filename);
while (fgets(buf, 4096, f)) {
lineNumber++;
token = strtok(buf, " \n");
if (token) {
if (!strncmp(token, "#", 1) ) {
continue;
} else if (!strcmp(token, "units")) {
err = 0;
name = strtok(NULL, " \n");
if (!strcmp(name, "degrees")) {
angleUnits = Pi / 180.0;
} else if (!strcmp(name, "radians")) {
angleUnits = 1.0;
} else {
fprintf(stderr, "unknown unit at file %s line %d\n", filename, lineNumber);
}
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
}
} else if (!strcmp(token, "element")) {
err = 0;
protons = tokenizeInt(&err);
group = tokenizeInt(&err);
period = tokenizeInt(&err);
parentSymbol = strtok(NULL, " \n");
symbol = strtok(NULL, " \n");
name = strtok(NULL, " \n");
mass = tokenizeDouble(&err);
rvdW = tokenizeDouble(&err);
evdW = tokenizeDouble(&err);
nBonds = tokenizeInt(&err);
rCovalent = tokenizeDouble(&err);
charge = tokenizeDouble(&err);
isVirtual = tokenizeInt(&err);
if (err || symbol == NULL || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
setElement(protons, group, period, parentSymbol, symbol, name, mass, rvdW, evdW, nBonds, rCovalent, charge, isVirtual);
DPRINT13(D_READER, "setElement: %d %d %d %s %s %s %f %f %f %d %f %f %d\n", protons, group, period, parentSymbol, symbol, name, mass, rvdW, evdW, nBonds, rCovalent, charge, isVirtual);
}
} else if (!strcmp(token, "stretch")) {
err = 0;
ks = tokenizeDouble(&err);
r0 = tokenizeDouble(&err);
de = tokenizeDouble(&err);
beta = tokenizeDouble(&err);
inflectionR = tokenizeDouble(&err);
quality = tokenizeInt(&err);
quadratic = tokenizeInt(&err);
name = strtok(NULL, " \n");
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
addInitialBondStretch(ks, r0, de, beta, inflectionR, quality, quadratic, name);
DPRINT8(D_READER, "addBondStretch: %f %f %f %f %f %d %d %s\n", ks, r0, de, beta, inflectionR, quality, quadratic, name);
}
} else if (!strcmp(token, "bend")) {
err = 0;
ktheta = tokenizeDouble(&err);
theta0 = tokenizeDouble(&err) * angleUnits;
quality = tokenizeInt(&err);
name = strtok(NULL, " \n");
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
addInitialBendData(ktheta, theta0, quality, name);
DPRINT4(D_READER, "addBendData: %f %f %d %s\n", ktheta, theta0, quality, name);
}
} else if (!strcmp(token, "vdw")) {
err = 0;
rvdW = tokenizeDouble(&err);
evdW = tokenizeDouble(&err);
cutoffRadiusStart = tokenizeDouble(&err);
cutoffRadiusEnd = tokenizeDouble(&err);
name = strtok(NULL, " \n");
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
addVanDerWaalsInteraction(name, rvdW, evdW, cutoffRadiusStart, cutoffRadiusEnd);
DPRINT5(D_READER, "addVanDerWaalsInteraction: %f %f %f %f %s\n", rvdW, evdW, cutoffRadiusStart, cutoffRadiusEnd, name);
}
} else if (!strcmp(token, "pattern")) {
err = 0;
name = strtok(NULL, " \n");
value = tokenizeDouble(&err);
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
addPatternParameter(name, value, angleUnits, NULL);
DPRINT2(D_READER, "addPatternParameter: %s %f\n", name, value);
}
} else if (!strcmp(token, "strut")) {
err = 0;
name = strtok(NULL, " \n");
ks = tokenizeDouble(&err);
r0 = tokenizeDouble(&err);
x1 = tokenizeDouble(&err);
y1 = tokenizeDouble(&err);
x2 = tokenizeDouble(&err);
y2 = tokenizeDouble(&err);
if (err || name == NULL) {
fprintf(stderr, "format error at file %s line %d\n", filename, lineNumber);
} else {
addStrutDefinition(name, ks, r0, x1, y1, x2, y2);
DPRINT7(D_READER, "addStrutDefinition: %s %f %f %f %f %f %f\n", name, ks, r0, x1, y1, x2, y2);
}
//} else {
//fprintf(stderr, "unrecognized line type at file %s line %d: %s\n", filename, lineNumber, token);
}
}
}
fclose(f);
return 1;
}
int setThinShim( int dac, int value )
{
int token1, token2, m1,m2,val1;
int ttoken1, ttoken2;
int priorvalue;
int retval;
char hstr[24];
strcpy(hstr,"bogus");
switch (dac)
{
case 16: case 18: // pair 1 - X/XZ
m1 = 16; m2 = 18;
strcpy(hstr,"x/xz");
break;
case 17: case 21: // pair 2 Y/YZ
m1 = 17; m2 = 21;
strcpy(hstr,"y/yz");
break;
case 20: case 25: // pair 3 c2 zc2
m1 = 20; m2 = 25;
strcpy(hstr,"c2/zc2 [x2-y2,zx2-y2]");
break;
case 19: case 27: // pair 4 s2 zs2
m1 = 19; m2 = 27;
strcpy(hstr,"s2/zs2 [xy,zxy]");
break;
case 22: case 12: // pair 5 c3 zc3
m1 = 22; m2 = 12;
strcpy(hstr,"c3/zc3 [x3,zx3]");
break;
case 23: case 13: // pair 6 s3 zs3
m1 = 23; m2= 13;
strcpy(hstr,"s3/zs3 [y3,zy3]");
break;
default:
m1 = 0;
}
if (m1 == 0)
{
retval = setTimShim(
shimPort,
(int) timShimTable[ dac ].board,
(int) timShimTable[ dac ].dac,
value,
0 );
}
else
{ // since both are updated no decisions..
input_record[dac] = value;
ttoken1 = input_record[m1] + input_record[m2];
ttoken2 = input_record[m1] - input_record[m2];
if ((ttoken1 > 32767) || (ttoken1 < -32767) ||
(ttoken2 > 32767) || (ttoken2 < -32767))
{
input_record[token1] = priorvalue;
errLogRet(LOGIT,debugInfo,"Shim DAC pair at limit\n");
//DPRINT2(-3,"index = %d reset to %d\n",token1,priorvalue);
return(-1); /* don't set these values*/
/*return(HDWAREERROR+SHIMDACLIM); */ /* nothing handles this error */
}
DPRINT5(-2,"\n Shim pair %s: %d <= %d , %d <= %d",hstr,m1,ttoken1,m2,ttoken2);
retval = setTimShim( shimPort,
(int) timShimTable[ m1 ].board,
(int) timShimTable[ m1 ].dac,
ttoken1, 0 );
if ( ! retval )
{
retval = setTimShim( shimPort,
(int) timShimTable[ m2 ].board,
(int) timShimTable[ m2 ].dac,
ttoken2, 0 );
}
}
return( retval );
}
void spi2ShimInterface(int *paramvec, int *index, int count, int fifoFlag)
{
int token1, token2, m1,m2,val1;
int ttoken1, ttoken2;
int priorvalue;
char hstr[24];
strcpy(hstr,"bogus");
if (shimDelay < 2000) shimDelay=2000;
while (*index < count)
{
(*index)++; // skip over the SHIMDAC token
token1 = paramvec[*index]; (*index)++;
token2 = paramvec[*index]; (*index)++;
priorvalue = input_record[token1]; // save for clip test
// DPRINT2(-5,"priorvalue at entry = %d,%d\n",priorvalue,input_record[token1]);
if (XYZgradflag) // gradient amps present
{ if ((token1==16) || (token1==17) || (token1==2))// x, y or z1?
{ if (fifoFlag == 0)
DPRINT2(-3,"send2Grad (1) %d %d\n",token1,token2);
send2Grad(13,token1,token2); // let gradient cntrl do it
return;
}
}
// token1 is shim coil id and token 2 the value
input_record[token1] = token2; // keep the value
switch (token1)
{
case 16: case 18: // pair 1 - X/XZ
m1 = 16; m2 = 18;
strcpy(hstr,"x/xz");
break;
case 17: case 21: // pair 2 Y/YZ
m1 = 17; m2 = 21;
strcpy(hstr,"y/yz");
break;
case 20: case 25: // pair 3 c2 zc2
m1 = 20; m2 = 25;
strcpy(hstr,"c2/zc2 [x2-y2,zx2-y2]");
break;
case 19: case 27: // pair 4 s2 zs2
m1 = 19; m2 = 27;
strcpy(hstr,"s2/zs2 [xy,zxy]");
break;
case 22: case 12: // pair 5 c3 zc3
m1 = 22; m2 = 12;
strcpy(hstr,"c3/zc3 [x3,zx3]");
break;
case 23: case 13: // pair 6 s3 zs3
m1 = 23; m2= 13;
strcpy(hstr,"s3/zs3 [y3,zy3]");
break;
default:
m1 = 0;
}
/* if (m1 == 0)
DPRINT2(-5,"non paired shim = %d, value = %d\n",token1,token2); */
if (m1 == 0)
{
pCurrentStatBlock->AcqShimValues[token1] = token2;
sendSpiShim(token1,token2,fifoFlag);
}
else
{ // since both are updated no decisions..
ttoken1 = input_record[m1] + input_record[m2];
ttoken2 = input_record[m1] - input_record[m2];
if ((ttoken1 > 32767) || (ttoken1 < -32767) || (ttoken2 > 32767) || (ttoken2 < -32767))
{
pCurrentStatBlock->AcqShimValues[token1] = priorvalue;
input_record[token1] = priorvalue;
errLogRet(LOGIT,debugInfo,"Shim DAC pair at limit\n");
//DPRINT2(-3,"index = %d reset to priorvalue = %d\n",token1,priorvalue);
return; /* don't set these values*/
/*return(HDWAREERROR+SHIMDACLIM); /* nothing handles this error */
}
DPRINT5(-2,"\n Shim pair %s: %d <= %d , %d <= %d",hstr,m1,ttoken1,m2,ttoken2);
pCurrentStatBlock->AcqShimValues[token1] = token2;
sendSpiShim(m1,ttoken1,fifoFlag);
sendSpiShim(m2,ttoken2,fifoFlag);
}
}
}
void LockCmdParser(Lock_Cmd *issue)
{
int len;
int cmd;
DPRINT5(-2,"LockCmdParser cmd: %d, arg1: %d, arg2: %d, arg3: %lf, arg4: %lf\n", issue->cmd,
issue->arg1,issue->arg2,issue->arg3,issue->arg4);
#ifdef NEED_SOME_TOJUST_PULSE_THELED
if (cmdledtog)
{
panelLedOn(2); cmdledtog = 0;
}
else
{
panelLedOff(2); cmdledtog = 1;
}
#endif
switch( issue->cmd )
{
case LK_SET_GAIN:
{
DPRINT(-1,"LK_SET_GAIN\n");
setLockGain(issue->arg1);
}
break;
case LK_SET_POWER:
{
DPRINT(-1,"LK_SET_POWER\n");
setLockPower(issue->arg1);
}
break;
case LK_SET_PHASE:
{
DPRINT(-1,"LK_SET_PHASE\n");
setLockPhase(issue->arg1);
}
break;
case LK_ON:
{
DPRINT(-1,"LK_ON\n");
pulser_on();
}
break;
case LK_OFF:
{
DPRINT(-1,"LK_OFF\n");
pulser_off();
}
break;
case LKRATE:
{
DPRINT1(-1,"LKRATE @ %f",issue->arg3);
/* -12 is used as a key, depending on the value of lockpower */
/* If lockpower is 0, use 0, else use 12.0 */
lockRate(issue->arg3,-12.0);
}
break;
case LK_SET_RATE:
{
DPRINT(-1,"LK_SET_MODE\n");
/* lockRate(double hz, double duty) */
lockRate(issue->arg3, issue->arg4);
}
break;
case LK_SET_FREQ:
{
DPRINT1(-1,"LK_SET_FREQ %f\n",issue->arg3);
/* void setLockDDS(double freq) */
setLockDDS(issue->arg3*1e6);
}
break;
/* was going to put autolock on lock controller, but many things are controller by master
and it just makes it simplier at this point to put it on the master
GMB 1/6/05
*/
#ifdef AUTOLOCK_ON_LOCKCNTLR
case LK_AUTOLOCK:
{
DPRINT3(-1,"LK_AUTOLOCK: Mode: %d, Max. Power: %d, Max. Gain: %d\n",issue->arg1, issue->arg2, (int) issue->arg3);
/* do_autolock(mode,maxpower,maxgain); */
/* msgQSend( ); */
}
break;
#endif
default:
DPRINT1(-1,"Unknown command: %d\n",issue->cmd);
break;
}
return;
}
/* 4x callback */
void Lock_CmdCallback(void* listener_data, DDS_DataReader* reader)
{
Lock_Cmd *recvIssue;
DDS_ReturnCode_t retcode;
DDS_Boolean result;
struct DDS_SampleInfoSeq info_seq = DDS_SEQUENCE_INITIALIZER;
struct Lock_CmdSeq data_seq = DDS_SEQUENCE_INITIALIZER;
struct DDS_SampleInfo* info = NULL;
long i,numIssues;
Lock_CmdDataReader *LockCmd_reader = NULL;
DDS_TopicDescription *topicDesc;
LockCmd_reader = Lock_CmdDataReader_narrow(pLockCmdSubs[0]->pDReader);
if ( LockCmd_reader == NULL)
{
errLogRet(LOGIT,debugInfo, "Lock_CmdCallback: DataReader narrow error.\n");
return;
}
topicDesc = DDS_DataReader_get_topicdescription(reader);
DPRINT2(-1,"Lock_CmdCallback; Type: '%s', Name: '%s'\n",
DDS_TopicDescription_get_type_name(topicDesc), DDS_TopicDescription_get_name(topicDesc));
while(1)
{
// Given DDS_HANDLE_NIL as a parameter, take_next_instance returns
// a sequence containing samples from only the next (in a well-determined
// but unspecified order) un-taken instance.
retcode = Lock_CmdDataReader_take_next_instance(
LockCmd_reader,
&data_seq, &info_seq, DDS_LENGTH_UNLIMITED,
&DDS_HANDLE_NIL,
DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE);
// retcode = Lock_CmdDataReader_take(LockCmd_reader,
// &data_seq, &info_seq,
// DDS_LENGTH_UNLIMITED, DDS_ANY_SAMPLE_STATE,
// DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE);
if (retcode == DDS_RETCODE_NO_DATA) {
break ; // return;
} else if (retcode != DDS_RETCODE_OK) {
errLogRet(LOGIT,debugInfo, "Lock_CmdCallback: next instance error %d\n",retcode);
break ; // return;
}
numIssues = Lock_CmdSeq_get_length(&data_seq);
for (i=0; i < numIssues; i++)
{
info = DDS_SampleInfoSeq_get_reference(&info_seq, i);
if ( info->valid_data)
{
recvIssue = Lock_CmdSeq_get_reference(&data_seq,i);
DPRINT5(-1,"Lock_Cmd CallBack: cmd: %d, arg1: %d, arg2: %d, arg3: %lf, arg4: %lf crc: 0x%lx\n",
recvIssue->cmd,recvIssue->arg1, recvIssue->arg2, recvIssue->arg3, recvIssue->arg4);
msgQSend(pMsgesToLockParser, (char*) recvIssue, sizeof(Lock_Cmd), NO_WAIT, MSG_PRI_NORMAL);
}
}
retcode = Lock_CmdDataReader_return_loan( LockCmd_reader,
&data_seq, &info_seq);
DDS_SampleInfoSeq_set_maximum(&info_seq, 0);
} // while
return;
}