/******************************************************************************
** 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 uninit_peer()
{
#if defined(__VXWORKS__) && 0
sysAuxClkDisable ();
sysAuxClkConnect(0, 0);
#endif
g_kernel.peer->eventProc = 0;
destroy_peer();
}
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" );
}
}
/******************************************************************************
** 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 */
