static void traiteErr(int n, char * L)
{
int v;
ERROR=n;
if (D_Cod==0) {
if (ECHOOFF) printf("\n");
/* if (InDebugFct==0) fprintf(stderr,"\n"); A VOIR ! */
printf("%s : %s !!\a\n",L,TabErr[n]);
if (InDebugFct==0) fprintf(stderr,"%s : %s !!\a\n",L,TabErr[n]);
}
if (inSonProc) exit(1);
if (fctEnCours) {
if (D_Cod==0) ErrPrintf("Compilation aborted !\n");
else
if (ADDRONE == VIDE) ErrPrintf("Inside compilation aborted !\n");
_MODIF_fctEnCours_(0);
rmLastFct();
}
if (ADDRONE != VIDE) return;
if (FD_IN) {
printf("In loading file %s : line %d !\n", getFDname(),getFDlig());
if (InDebugFct==0) fprintf(stderr, "In loading file %s : line %d !\n", getFDname(),getFDlig());
closeFD();
}
if (iTERM) {
printf("In loading stdin : line %d !\n", getFDlig());
if (InDebugFct==0) fprintf(stderr,"In loading stdin : line %d !\n", getFDlig());
close(FD_IN); /* pipe ou autre */
v = dup(iTERM); /* stdin on term */
iTERM = 0;
}
}
void WaitTickGetTime(void) {
static int oldusec = -1;
int stat;
ReadCntr(); /* Wait for the 48 ms heartbeat */
#if USE_SYSTEM_TIME
gettimeofday(&tv, &tz);
tmp = gmtime(&tv.tv_sec);
tsshm->msec = ((tmp->tm_hour * 60 + tmp->tm_min) * 60 + tmp->tm_sec) *
1000 + (tv.tv_usec + 500) / 1000;
tsshm->day = tmp->tm_yday;
#else /* USE_SYSTEM_TIME */
/* get the current time in any case */
if((stat = read(ttfd, &ttime, sizeof(ttime))) < 0) {
ErrPrintf("Error %d reading TrueTime\n", stat);
}
if(ttime.input_reference_error || (ttime.phase_locked ^ 1)) {
irig_error_seen = 1;
}
lastTimeStep = (ttime.usec - oldusec) / 1e6; /* Time step (sec) */
if(lastTimeStep < 0) {
lastTimeStep += 1;
}
oldusec = ttime.usec;
/* Convert the time now */
tsshm->msec = ((ttime.hour * 60 + ttime.min) * 60 + ttime.sec) * 1000 +
(ttime.usec + 500) / 1000;
tsshm->day = ttime.yday;
#endif /* USE_SYSTEM_TIME */
ddprintf("day %d msec %d\n", tsshm->day, tsshm->msec);
}
int main(int argc, char *argv[]) {
int i,dsm_status;
enum DRVSTATE oldAzState, oldElState;
char msg[80];
DAEMONSET
#if USE_SIGTIMEDWAIT
sigemptyset(&sigs);
sigaddset(&sigs, SIGHUP); /* 1 */
sigaddset(&sigs, SIGINT); /* 2 */
sigaddset(&sigs, SIGQUIT); /* 3 */
sigaddset(&sigs, SIGTERM); /* 15 */
#else /* USE_SIGTIMEDWAIT */
#if CATCH_SIGNALS
if(signal(SIGINT, SigIntHndlr) == SIG_ERR) {
ErrPrintf("Error setting INT signal disposition\n");
exit(SYSERR_RTN);
}
if(signal(SIGQUIT, SigQuitHndlr) == SIG_ERR) {
ErrPrintf("Error setting QUIT signal disposition\n");
exit(SYSERR_RTN);
}
if(signal(SIGTERM, SigQuitHndlr) == SIG_ERR) {
ErrPrintf("Error setting TERM signal disposition\n");
exit(SYSERR_RTN);
}
if(signal(SIGHUP, SigQuitHndlr) == SIG_ERR) {
ErrPrintf("Error setting HUP signal disposition\n");
exit(SYSERR_RTN);
}
#endif /* CATCH_SIGNALS */
#endif /* USE_SIGTIMEDWAIT */
az_amax = AZ_AMAX;
el_amax = EL_AMAX;
for (i=1; i<argc; i++) {
if (strstr(argv[i],"-h") != NULL) {
servoUsage(argv[0]);
}
if(strstr(argv[i],"-v") != NULL) {
verbose++;
}
}
/* Set some constants */
trAzVmax = 3*MAS;
trElVmax = 3*MAS;
setpriority(PRIO_PROCESS, (0), (SERVOPRIO));
umask(0111);
tsshm = OpenShm(TSSHMNAME, TSSHMSZ);
if(verbose) {
fprintf(stderr, "tsshm = %d. ", (int)tsshm);
fprintf(stderr, "Starting check of tsshm..");
for(i = 0; i < TSSHMSZ; i++) {
msg[i%20] = ((char *)tsshm)[i];
}
fprintf(stderr, "Done\n");
}
/* Set samp buffer full so by default servo will not collect data */
tsshm->sampIn = tsshm->sampOut = 0;
if(tsshm->azCmd != OFF_CMD || tsshm->elCmd != OFF_CMD) {
ErrPrintf("Warning: Track was not commanding drives off\n");
tsshm->azCmd = tsshm->elCmd = OFF_CMD;
}
tsshm->sendEncToPalm = 0;
#if SIMULATING
scbStatus = 0;
encAz = 0;
encEl = 45*MAS;
elState = azState = 0;
#else /* SIMULATING */
ttfd = open("/dev/vme_sg_simple", O_RDWR, 0);
if(ttfd <= 0) {
ErrPrintf("Error opening TrueTime - /dev/vme_sg_simple\n");
exit(SYSERR_RTN);
}
i = VME_SG_SIMPLE_RATE_1K;
ioctl(ttfd, VME_SG_SIMPLE_SET_PULSE_RATE, &i);
tsshm->fault = NO_FAULT;
OpenCntr(); /* Open the heartbeat counter */
dprintf("Counter, ");
SetupCanBus();
dprintf("Canbus, ");
/*
SafeOpenDsm();
*/
/* Since SafeOpenDsm() did not work, I am adding dsm_open here... NAP 26June12*/
dsm_status = dsm_open();
if(dsm_status != DSM_SUCCESS) {
dsm_error_message(dsm_status, "dsm_open failed.");
exit(-1);
}
dprintf("Dsm, ");
WriteDsmMonitorPoints();
dprintf("Monitor points, ");
/* Set a few things in a safe state. */
azState = SERVOOFF;
elState = SERVOOFF;
oldAzState = oldElState = SERVOOFF;
#endif /* SIMULATING */
/* Set a few things in a safe state. */
azState = SERVOOFF;
elState = SERVOOFF;
oldAzState = oldElState = SERVOOFF;
#if AZ_VERBOSE > 1 || EL_VERBOSE > 1
azPrintNext = 0;
elPrintNext = 0;
#endif /* VERBOSE */
WaitTickGetTime(); /* Wait for the 48 ms heartbeat */
dprintf("Tick and time, ");
GetACUPosAndStatus();
dprintf("ACU pos, status, ");
nxtAz[0] = lastAz / ACU_TURNS_TO_MAS;
nxtAz[1] = 0; /* set zero velocity */
nxtEl[0] = lastEl / ACU_TURNS_TO_MAS;
nxtEl[1] = 0; /* set zero velocity */
/* if the drives are on, set to standby, otherwise to shutdown */
if(ACUAccessMode == REMOTE) {
if(azDriveMode >= STANDBY) {
azModeCmd = STANDBY;
}
if(elDriveMode >= STANDBY) {
elModeCmd = STANDBY;
}
SetACUMode(azDriveMode, elDriveMode);
}
if((tsshm->el < 10*MAS) || (tsshm->el > 90*MAS) ||
(tsshm->az > 270*MAS) || (tsshm->az < -270*MAS)) {
tsshm->az = lastAz;
tsshm->azVel = 0;
tsshm->el = lastEl;
tsshm->elVel = 0;
}
trAz = trAzRaw = tsshm->az;
trEl = trElRaw = tsshm->el;
dprintf("completed\n");
/* Here the infinite loop begins */
dprintf("Entering servo's main loop\n");
while(shutdownSignal == 0) {
tsshm->azState = azState;
tsshm->elState = elState;
/* Get time and read encoder */
WaitTickGetTime();
GetACUPosAndStatus();
tsshm->encAz = lastAz;
tsshm->encEl = lastEl;
#if 0
CheckTrCmds(); /* Is there a new command from Track? */
#else
trAz = trAzRaw = tsshm->az;
trEl = trElRaw = tsshm->el;
trAzVel = trAzVelRaw = tsshm->azVel;
trElVel = trElVelRaw = tsshm->elVel;
trMsecCmd = tsshm->msecCmd;
tsshm->msecAccept = tsshm->msecCmd;
#endif
SaI.msec = tsshm->msec;
dt = (tsshm->msec - trMsecCmd)/1000.;
if(dt < -3600)
dt += 24*3600;
tsshm->azTrError = trAzRaw + trAzVelRaw * dt - lastAz;
tsshm->elTrError = trElRaw + trElVelRaw * dt - lastEl;
#if AZ_VERBOSE || EL_VERBOSE
if(azState != oldAzState || elState != oldElState) {
printf("at %.3f azState = %d, elState = %d\n",
tsshm->msec/1000., azState, elState);
oldAzState = azState;
oldElState = elState;
}
#endif /* AZ_VERBOSE || EL_VERBOSE */
i = 0;
if(tsshm->azCmd != trAzCmd) {
if(tsshm->azCmd == OFF_CMD && azState != SERVOOFF) {
azState = STOPPING1;
i = 1;
} else if(tsshm->azCmd == ON_CMD && azState == SERVOOFF) {
azTry = 0;
azCnt = 0;
azState = STARTING1;
}
trAzCmd = tsshm->azCmd;
}
if(tsshm->elCmd != trElCmd) {
if(tsshm->elCmd == OFF_CMD && elState != SERVOOFF) {
elState = STOPPING1;
i = 1;
}
if(tsshm->elCmd == ON_CMD && elState == SERVOOFF) {
elTry = 0;
elCnt = 0;
elState = STARTING1;
}
trElCmd = tsshm->elCmd;
}
if(i) {
if(i == 1) {
ErrPrintf("servo received OFF_CMD from Track\n");
}
}
if(fabs(dt) > 3.0 && (
(azState != SERVOOFF && azState != STOPPING1 && azState != STOPPING2) ||
(elState != SERVOOFF && elState != STOPPING1 && elState != STOPPING2))) {
ErrPrintf("Track timeout or clock jump, dt %.2f sec.,"
"avg. dt %.2f sec.\n", dt, avgDt);
#if !SIMULATING
vSendOpMessage(OPMSG_SEVERE, 19, 60, "Track timeout");
#endif /* !SIMULATING */
if(azState != SERVOOFF && azState != STOPPING1 && azState != STOPPING2) {
azState = STOPPING1;
}
if(elState != SERVOOFF && elState != STOPPING1 && elState != STOPPING2) {
elState = STOPPING1;
}
}
avgDt += (dt - avgDt)/10;
/* If svdata is not running, keep the sample buffer moving */
if(SFULL) {
INC_OUT;
}
AzCycle();
ElCycle();
/* Save the data from this cycle */
tsshm->sampIn = NEXT_SAMP(tsshm->sampIn);
/* Set up expected positions and velocities for next data cycle */
SaI.curAz = trAz + trAzVel * (dt + HEARTBEAT_PERIOD);
SaI.cmdAzVel = trAzVel;
SaI.curEl = trEl + trElVel * (dt + HEARTBEAT_PERIOD);
SaI.cmdElVel = trElVel;
if(azState != oldAzState || elState != oldElState) {
if(elState >= TRACKING && azState >= TRACKING) {
/* Both drives are up, so report the old FaultWord
* and clear it. */
}
if(azState != oldAzState && azState == SERVOOFF) {
dprintf("Az drive is off\n");
}
if(elState != oldElState && elState == SERVOOFF) {
dprintf("El drive is off\n");
}
oldElState = elState;
oldAzState = azState;
}
/* read the parameters from the ACU and send them to dsm at the
* change of the second */
curSec = tsshm->msec/1000;
if(curSec != prevSec ) {
WriteDsmMonitorPoints();
prevSec = curSec;
}
#if !SIMULATING
/* Make checks of the scb less frequently than once/cycle */
auxCycle = (tsshm->msec / 10) % 100;
/* Compute average pointing errors */
if((auxCycle % 10) == 9) {
static double ssq = 0;
/* Instantaneous az error (commanded - actual) */
azTrErrorArcSec = (float)tsshm->azTrError *
cos(RAD_PER_MAS * (trEl + trElVel * dt)) * 0.001;
ssq += azTrErrorArcSec * azTrErrorArcSec;
elTrErrorArcSec = (float)tsshm->elTrError * 0.001;
ssq += elTrErrorArcSec * elTrErrorArcSec;
/* commenting out the following dsm_write since these are now in dsmsub.h
dsm_write(dsm_host, "DSM_AZ_TRACKING_ERROR_F", &azTrErrorArcSec);
dsm_write(dsm_host, "DSM_EL_TRACKING_ERROR_F", &elTrErrorArcSec);
*/
if(auxCycle == 99) {
static int trErrCnt = 0;
static char m[] = "Tracking error is excessive";
tsshm->avgTrErrorArcSec = sqrt(ssq / 10.);
ssq = 0.;
if(tsshm->azCmd > 0 && tsshm->elCmd > 0 &&
tsshm->avgTrErrorArcSec > 10 &&
(fabs(tsshm->tachAzVel) < 1.0) &&
(fabs(tsshm->tachElVel) < 1.0)) {
if(trErrCnt == 10) {
vSendOpMessage(OPMSG_SEVERE, 18, 0, m);
}
if(trErrCnt < 15)
trErrCnt++;
} else {
if(trErrCnt > 0) {
if(trErrCnt-- == 10) {
vSendOpMessage(OPMSG_SEVERE, 18, 0, "");
trErrCnt = 0;
}
}
}
}
}
#endif /* !SIMULATING */
}
return((gotQuit)? QUIT_RTN: NORMAL_RTN);
}
