void
motor_servo_isr(void)
{
fppSave(&pfc);
++motor_servo_calls;
servo_time += 1./(double)motor_servo_rate;
semGive(motor_servo_sem);
fppRestore(&pfc);
}
static void
fpuInit()
{
FP_CONTEXT fpContext;
fppSave(&fpContext);
/* disable floating-point traps */
#if defined(__i386__)
/* set the fp control word precision to 53-bit for both single and double precision */
fpContext.fpcr = 0xffff027f;
fppRestore(&fpContext);
#elif defined(__sparc__)
fpContext.fsr &= ~0x0f800000;
fppRestore(&fpContext);
fppSave(&fpContext);
#endif
#if 1
{
int options = -1;
int status = taskOptionsGet(0, &options);
assert(status == OK);
assert(options & VX_FP_TASK);
}
#endif
}
/*
** Name: CFE_PSP_ExceptionHook
**
** Purpose: Make the proper call to CFE_ES_ProcessCoreException
**
** Notes: pEsf - pointer to exception stack frame.
** fppSave - When it makes this call, it captures the last floating
** point context - which may not be valid. If a floating
** point exception occurs you can be almost 100% sure
** that this will reflect the proper context. But if another
** type of exception occurred then this has the possibility
** of not being valid. Specifically if a task that is not
** enabled for floating point causes a non-floating point
** exception, then the meaning of the floating point context
** will not be valid. If the task is enabled for floating point,
** then it will be valid.
**
*/
void CFE_PSP_ExceptionHook (int task_id, int vector, uint8* pEsf )
{
char *TaskName;
/*
** Get the vxWorks task name
*/
TaskName = taskName(task_id);
if ( TaskName == NULL )
{
sprintf(CFE_PSP_ExceptionReasonString, "Exception: Vector=0x%06X, vxWorks Task Name=NULL, Task ID=0x%08X",
vector,task_id);
}
else
{
sprintf(CFE_PSP_ExceptionReasonString, "Exception: Vector=0x%06X, vxWorks Task Name=%s, Task ID=0x%08X",
vector, TaskName, task_id);
}
/*
** Save floating point registers
*/
fppSave(&CFE_PSP_ExceptionContext.fp);
/*
** Call the Generic cFE routine to finish processing the exception and
** restart the cFE
*/
CFE_ES_ProcessCoreException((uint32)task_id, (uint8 *)CFE_PSP_ExceptionReasonString,
(uint32 *)&CFE_PSP_ExceptionContext, sizeof(CFE_PSP_ExceptionContext_t));
/*
** No return to here
*/
} /* end function */
