/******************************************************************************
** Function: CFE_PSP_InitLocalTime()
**
** Purpose: Initializes the vxWorks Aux and TimeStamp timers.
**
** Arguments: none
******************************************************************************/
void CFE_PSP_InitLocalTime(void)
{
/* Set the sys clock rate */
sysClkRateSet(200);
/*
** Disable the Aux timer interrupt
*/
sysAuxClkDisable();
/*
** Set the Aux timer rate
*/
if(sysAuxClkRateGet() != CFE_PSP_TIMER_AUX_TICK_PER_SEC)
{
if(sysAuxClkRateSet(CFE_PSP_TIMER_AUX_TICK_PER_SEC) == ERROR)
{
OS_printf("CFE_PSP: Unable to set Aux Clock Rate!\n");
}
if(CFE_PSP_TIMER_PRINT_DBG == TRUE)
{
OS_printf("CFE_PSP: Aux Clock Rate %d.\n", sysAuxClkRateGet());
}
}
}
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" );
}
}
void CFE_PSP_InitLocalTime(void)
{
/* Set the sys clock rate */
sysClkRateSet(200);
/*
** Disable the Aux timer interrupt, and disable the Timestamp timer
*/
sysAuxClkDisable();
if(sysTimestampDisable() == ERROR)
{
OS_printf("CFE_PSP: Unable to disable the Timestamp timer!\n");
}
/*
** Set the Aux timer
*/
if(sysAuxClkRateGet() != CFE_PSP_TIMER_AUX_TICK_PER_SEC)
{
if(sysAuxClkRateSet(CFE_PSP_TIMER_AUX_TICK_PER_SEC) == ERROR)
{
OS_printf("CFE_PSP: Unable to set Aux Clock Rate!\n");
}
if(CFE_PSP_TIMER_PRINT_DBG == TRUE)
{
OS_printf("CFE_PSP: Aux Clock Rate %d.\n", sysAuxClkRateGet());
OS_printf("CFE_PSP: Timestamp Frequency %u.\n", sysTimestampFreq());
OS_printf("CFE_PSP: Timestamp Period %u.\n", sysTimestampPeriod());
}
}
}/* end CFE_PSP_InitLocalTime */
