/****************************************************************************** ** Function: CFE_PSP_Init1HzTimer() ** ** Purpose: Initializes the 1Hz Timer and connects it to the cFE TIME 1Hz routine ** ** ** NOTE: This function has to be called after CFE_ES_Main() in CFE_PSP_Start() ** because the 1Hz ISR has a semaphore that is created in CFE_ES_Main(). ** ** Arguments: none ******************************************************************************/ void CFE_PSP_Init1HzTimer(void) { /* ** Attach a handler to the timer interrupt */ /* Either the Aux clock */ if(sysAuxClkConnect((FUNCPTR)CFE_PSP_AuxClkHandler, CFE_PSP_TIMER_AUX_TICK_PER_SEC) == ERROR) { printf("CFE_PSP: Unable to connect handler to Aux Clock!\n"); } /* ** Enable the Aux timer interrupt ** Enable the Timestamp timer, which also sets it to zero */ sysAuxClkEnable(); if(sysTimestampEnable() == ERROR) { OS_printf("CFE_PSP: Unable to enable the Timestamp timer!\n"); } g_bTimerInitialized = TRUE; }/* end CFE_PSP_Init1HzTimer */
void init_peer() { //printf("Initializing peer layer.\n"); peerGuardian = peerData + sizeof peerData; peerPointer = peerData; create_peer(); g_kernel.peer->eventProc = peer_event; #if defined(HSP_DEBUG) tBase = ke_get_time(); #endif memset((void*)peerCounters, 0, sizeof(peerCounters)); /* note in VxWorks the auxClock is a shared resource and no arbitration mechanism is provided, so only diagnostic code can be safely put into _peer_ping. e.g., windView will use auxClock for profiling purposes. */ #if defined(__VXWORKS__) && 0 sysAuxClkConnect(_peer_ping, 0); sysAuxClkRateSet(20); /* ping peer every 50ms */ sysAuxClkEnable(); #endif }
void enable_motor_servo( int param ) { int status; int temp; if (!motor_servo_initialized) { printf("ERROR: Motor servo is not initialized\n"); return; } if ( param < 1 ) param = servo_base_rate; if ( servo_enabled == 0 ) { servo_enabled = 1; motor_servo_calls = 0; servo_time = 0; motor_servo_time = 0; motor_servo_rate = param; motor_servo_errors = 0; count_no_receive = 0; count_no_receive_total = 0; count_no_broadcast = 0; changeCollectFreq(param); zero_integrator(); setDefaultPosture(); sysAuxClkDisable(); printf( "sysAuxClkRateGet() returns: %d\n", sysAuxClkRateGet() ); status = sysAuxClkConnect( (FUNCPTR) motor_servo_isr, 0 ); printf( "sysAuxClkConnect() returns: %d (%d)\n", status, OK ); if ( status != OK ) { fprintf( stderr, "sysAuxClkConnect() NOT OK: %d (%d)\n",status, OK ); return; } sysAuxClkRateSet( param ); printf( "sysAuxClkRateGet() returns: %d\n", sysAuxClkRateGet() ); motor_servo_sem = semBCreate(SEM_Q_FIFO,SEM_EMPTY); motor_servo_tid = taskSpawn("motor_servo",0,VX_FP_TASK,10000, (FUNCPTR) motor_servo,0,0,0,0,0,0,0,0,0,0); sysAuxClkEnable(); } else { fprintf( stderr, "motor servo is already on!\n" ); } }
STATUS sysAuxClkRateSet ( int ticksPerSecond /* number of clock interrupts per second */ ) { /* return ERROR if rate is not supported */ if (ticksPerSecond < AUX_CLK_RATE_MIN || ticksPerSecond > AUX_CLK_RATE_MAX) return (ERROR); sysAuxClkTicksPerSecond = ticksPerSecond; if (sysAuxClkRunning) { sysAuxClkDisable (); sysAuxClkEnable (); } return (OK); }
/****************************************************************************** ** Function: CFE_PSP_Init1HzTimer() ** ** Purpose: Initializes the 1Hz Timer and connects it to the cFE TIME 1Hz routine ** The S950 was setup to use the PPS signal as it was on Morpheus, ** where the PPS signal was driven by the GPS PPS. If this signal ** is not driven, then the 1Hz ISR will not run. So you can use the ** AuxClk instead. Comment out/uncomment either the Aux clk ** connect or the PPS connect to choose the one you want to use. ** ** NOTE: This function has to be called after CFE_ES_Main() in CFE_PSP_Start() ** because the 1Hz ISR has a semaphore that is created in CFE_ES_Main(). ** ** Arguments: none ******************************************************************************/ void CFE_PSP_Init1HzTimer(void) { /* ** Attach a handler to the timer interrupt */ /* Either the Aux clock */ if(sysAuxClkConnect((FUNCPTR)CFE_PSP_AuxClkHandler, CFE_PSP_TIMER_AUX_TICK_PER_SEC) == ERROR) { printf("CFE_PSP: Unable to connect handler to Aux Clock!\n"); } sysAuxClkEnable(); /* Or the PPS interrupt available on the S950 board * ppsIntConnect((VOIDFUNCPTR)CFE_PSP_PPSHandler); ppsIntEnable(); */ }/* end CFE_PSP_Init1HzTimer */
STATUS sysAuxClkRateSet ( int ticksPerSecond /* number of clock interrupts per second */ ) { if (ticksPerSecond < AUX_CLK_RATE_MIN || ticksPerSecond > AUX_CLK_RATE_MAX){ logMsg("tick must be 1 ~ 60\n", 0, 0, 0, 0, 0, 0); return (ERROR); } /* if (((RTC_LONG_RATE / ticksPerSecond) < 4) || ((RTC_LONG_RATE / ticksPerSecond) > 0x00ffffff)) return (ERROR); */ sysAuxClkTicksPerSecond = ticksPerSecond; if (sysAuxClkRunning) { sysAuxClkDisable (); sysAuxClkEnable (); } return (OK); }