FDP_EXPORTED FDP_SHM* FDP_OpenSHM(const char* pShmName)
{
void* pSharedFDPSHM = OpenShm(pShmName, FDP_SHM_SHARED_SIZE);
if (pSharedFDPSHM == NULL)
{
return NULL;
}
//TODO : !
char aCpuShmName[512];
strcpy_s(aCpuShmName, sizeof(aCpuShmName), "CPU_");
strcat_s(aCpuShmName, sizeof(aCpuShmName), pShmName);
void* pCpuShm = OpenShm(aCpuShmName, sizeof(FDP_CPU_CTX));
if (pCpuShm == NULL)
{
printf("Failed to OpenShm(%s)\n", aCpuShmName);
return NULL;
}
FDP_SHM* pFDPSHM = (FDP_SHM*)malloc(sizeof(FDP_SHM));
if (pFDPSHM == NULL)
{
//TODO : CloseShm
return NULL;
}
pFDPSHM->pSharedFDPSHM = (FDP_SHM_SHARED*)pSharedFDPSHM;
pFDPSHM->pCpuShm = pCpuShm;
return pFDPSHM;
}
int main(void)
{
OpenShm();
if(shm_fd != -1 || shm_addr != MAP_FAILED)
{
printf ("data (@ %#x): '%s'\n", (unsigned int) shm_addr, shm_addr);
while(*shm_addr != 'a')
{}
printf ("data (@ %#x): '%s'\n", (unsigned int) shm_addr, shm_addr);
*shm_addr = 'A';
printf ("data (@ %#x): '%s'\n", (unsigned int) shm_addr, shm_addr);
CloseShm();
}
return (0);
}
int main(int argc, char *argv[]) {
char *line, word[12], cmdChar, *ip;
int n;
pthread_t driverTid;
#if _POSIX_VERSION >= 199506
struct sched_param param;
#else /* _POSIX_VERSION >= 199506 */
pthread_attr_t attr;
#endif /* _POSIX_VERSION >= 199506 */
if(processPresent("excl_Track")) {
printf("Another Track type process is running\n");
exit(1);
}
setpriority(PRIO_PROCESS, (0), (TRACKUSERPRIO));
readline_initialize_everything();
read_history(HIST_FILE);
tsshm = OpenShm(TSSHMNAME, TSSHMSZ);
tsshm->azCmd = OFF_CMD;
tsshm->elCmd = OFF_CMD;
tsshm->az = tsshm->encAz;
tsshm->el = tsshm->encEl;
tsshm->azVel = 0;
tsshm->elVel = 0;
tsshm->msecCmd = tsshm->msec;
/* Start the driver thread */
#if _POSIX_VERSION >= 199506
if(pthread_create(&driverTid, NULL, driver, (void *)0) < 0) {
perror("Failed to create driver");
exit(1);
}
param.sched_priority = TRACKPRIO;
pthread_setschedparam(driverTid, SCHED_DEFAULT, ¶m);
#else /* _POSIX_VERSION >= 199506 */
pthread_attr_create(&attr);
if(pthread_create(&driverTid, attr, driver, (void *)0) < 0) {
perror("Failed to create driver");
exit(1);
}
pthread_setprio(driverTid, TRACKPRIO);
#endif /* _POSIX_VERSION >= 199506 */
/* This main loop communicates with the user */
for(;;) {
line = readline("trk: ");
if(*line) { /* Readline removes the '\n' */
add_history(line);
/* At this point the main part of the program should run. */
cmdChar = *line;
/* Skip the remainder of the first word */
for(ip = 1 + line; ! isspace(*ip); ip++) ;
switch(cmdChar) {
case 'p':
n = 1;
printf("Fault = %d az cmd %d, state %d "
"el cmd %d, state %d\n", tsshm->fault,
tsshm->azCmd, tsshm->azState,
tsshm->elCmd, tsshm->elState);
printf("az %9.4f, azVel %9.4f curAz %9.4f, encAz %9.4f\n",
tsshm->az/MSEC_PER_DEG, tsshm->azVel/MSEC_PER_DEG,
(double)tsshm->cmdAz/MSEC_PER_DEG,
(double)tsshm->encAz/MSEC_PER_DEG);
printf("el %9.4f, elVel %9.4f curEl %9.4f, encEl %9.4f\n",
tsshm->el/MSEC_PER_DEG, tsshm->elVel/MSEC_PER_DEG,
(double)tsshm->cmdEl/MSEC_PER_DEG,
(double)tsshm->encEl/MSEC_PER_DEG);
printf("day %d, msec %d, msecCmd %d "
"msecAccept %d\n", tsshm->day,
tsshm->msec, tsshm->msecCmd, tsshm->msecAccept);
printf("MSEC_PER_DEG = %9.4f\n", (double)n*MSEC_PER_DEG);
printf("m3Cmd = %d = %s m3State = %d = %s\n", tsshm->m3Cmd,
(tsshm->m3Cmd == CLOSE_M3_CMD)? "Closed":
(tsshm->m3Cmd == OPEN_M3_CMD)? "open":"Unknown",
tsshm->m3State,
(tsshm->m3State == CLOSED_ST)? "Closed":
(tsshm->m3State == OPEN_ST)? "open":"Unknown");
printf("lastMsec = %d, servoKnownDead = %d\n", lastMsec,
servoKnownDead);
break;
case 'a':
if(line[1] == 'a') {
n = sscanf(ip, "%lf %lf", &newP, &newV);
switch(n) {
case 1:
newV = 0;
case 2: /* This is supposed to fall through */
if(line[2] == 's') {
newP = tsshm->az/MSEC_PER_DEG + newP/3600;
newV = tsshm->azVel/MSEC_PER_DEG + newV/3600;
} else {
newP = tsshm->az/MSEC_PER_DEG + newP;
newV = tsshm->azVel/MSEC_PER_DEG + newV;
}
newAz = 1; /* Signal driver new az pos, vel. */
break;
default:
printf("??\n");
break;
}
break;
} else if(line[1] == 'h') {
if(sscanf(ip, "%s", word)) {
if(strcmp("on", word) == 0) {
tsshm->sendEncToPalm = AZ_TO_PALM;
} else if(strcmp("off", word) == 0) {
tsshm->sendEncToPalm = NOTHING_TO_PALM;
} else {
printf("??\n");
}
}
break;
}
n = sscanf(ip, "%lf %lf", &newP, &newV);
switch(n) {
case 1:
newV = 0;
case 2: /* This is supposed to fall through */
newAz = 1; /* Signal driver new az pos, vel. */
break;
case 0:
if(sscanf(ip, "%s", word)) {
if(strcmp("on", word) == 0) {
tsshm->msecCmd = tsshm->msec;
usleep(10000);
tsshm->azCmd = ON_CMD;
} else if(strcmp("off", word) == 0) {
tsshm->azCmd = OFF_CMD;
} else {
printf("??\n");
}
}
break;
default:
printf("??\n");
break;
}
break;
case 'e':
if(line[1] == 'a') {
n = sscanf(ip, "%lf %lf", &newP, &newV);
switch(n) {
case 1:
newV = 0;
case 2: /* This is supposed to fall through */
if(line[2] == 's') {
newP = tsshm->el/MSEC_PER_DEG + newP/3600;
newV = tsshm->elVel/MSEC_PER_DEG + newV/3600;
} else {
newP = tsshm->el/MSEC_PER_DEG + newP;
newV = tsshm->elVel/MSEC_PER_DEG + newV;
}
newEl = 1; /* Signal driver new az pos, vel. */
break;
default:
printf("??\n");
break;
}
break;
} else if(line[1] == 'h') {
if(sscanf(ip, "%s", word)) {
if(strcmp("on", word) == 0) {
tsshm->sendEncToPalm = EL_TO_PALM;
} else if(strcmp("off", word) == 0) {
tsshm->sendEncToPalm = NOTHING_TO_PALM;
} else {
printf("??\n");
}
}
break;
}
n = sscanf(ip, "%lf %lf", &newP, &newV);
switch(n) {
case 1:
newV = 0;
case 2: /* This is supposed to fall through */
newEl = 1; /* Signal driver */
break;
case 0:
if(sscanf(ip, "%s", word)) {
if(strcmp("on", word) == 0) {
tsshm->msecCmd = tsshm->msec;
usleep(10000);
tsshm->elCmd = ON_CMD;
} else if(strcmp("off", word) == 0) {
tsshm->msecCmd = tsshm->msec;
tsshm->elCmd = OFF_CMD;
} else {
printf("??\n");
}
}
break;
default:
printf("??\n");
break;
}
break;
case 'm':
if(sscanf(ip, "%s", word)) {
if(strcmp("open", word) == 0)
tsshm->m3Cmd = OPEN_M3_CMD;
else if(strcmp("closed", word) == 0)
tsshm->m3Cmd = CLOSE_M3_CMD;
else {
printf("??\n");
}
}
break;
case 'q':
tsshm->azCmd = OFF_CMD;
tsshm->elCmd = OFF_CMD;
write_history(HIST_FILE);
return(0);
default:
printf("??\n");
break;
}
/* end of the main loop */
}
free(line);
}
fprintf(stderr, "!!! Control should not get here !!!\n");
/* shm_unlink(TSSHMNAME); */
return(0);
}
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);
}
