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); }