//*****************************************************************************
//
// Main function to handler motion events that are triggered by the MPU9150
// data ready interrupt.
//
//*****************************************************************************
void
MotionMain(void)
{
switch(g_ui8MotionState)
{
//
// This is our initial data set from the MPU9150, start the DCM.
//
case MOTION_STATE_INIT:
{
//
// Check the read data buffer of the MPU9150 to see if the
// Magnetometer data is ready and present. This may not be the case
// for the first few data captures.
//
if(g_sMPU9150Inst.pui8Data[14] & AK8975_ST1_DRDY)
{
//
// Get local copy of Accel and Mag data to feed to the DCM
// start.
//
MPU9150DataAccelGetFloat(&g_sMPU9150Inst, g_pfAccel,
g_pfAccel + 1, g_pfAccel + 2);
MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, g_pfMag,
g_pfMag + 1, g_pfMag + 2);
MPU9150DataGyroGetFloat(&g_sMPU9150Inst, g_pfGyro,
g_pfGyro + 1, g_pfGyro + 2);
//
// Feed the initial measurements to the DCM and start it.
// Due to the structure of our MotionMagCallback function,
// the floating point magneto data is already in the local
// data buffer.
//
CompDCMMagnetoUpdate(&g_sCompDCMInst, g_pfMag[0], g_pfMag[1],
g_pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, g_pfAccel[0], g_pfAccel[1],
g_pfAccel[2]);
CompDCMStart(&g_sCompDCMInst);
//
// Proceed to the run state.
//
g_ui8MotionState = MOTION_STATE_RUN;
}
//
// Turn off the LED to show we are done processing motion data.
//
g_pui32RGBColors[RED] = 0;
RGBColorSet(g_pui32RGBColors);
//
// Finished
//
break;
}
//
// DCM has been started and we are ready for normal operations.
//
case MOTION_STATE_RUN:
{
//
// Get the latest Euler data from the DCM. DCMUpdate is done
// inside the interrupt routine to insure it is not skipped and
// that the timing is consistent.
//
CompDCMComputeEulers(&g_sCompDCMInst, g_pfEulers,
g_pfEulers + 1, g_pfEulers + 2);
//
// Pass the latest sensor data back to the Gesture system for
// classification. What state do i think i am in?
//
GestureEmitClassify(&g_sGestureInst, g_pfEulers, g_pfAccel, g_pfGyro);
//
// Update best guess state based on past history and current
// estimate.
//
GestureUpdate(&g_sGestureInst, g_sGestureInst.ui16Emit);
//
// Turn off the LED to show we are done processing motion data.
//
g_pui32RGBColors[RED] = 0;
RGBColorSet(g_pui32RGBColors);
//
// Finished
//
break;
}
//
// An I2C error has occurred at some point. Usually these are due to
// asynchronous resets of the main MCU and the I2C peripherals. This
// can cause the slave to hold the bus and the MCU to think it cannot
// send. In practice there are ways to clear this condition. They are
// not implemented here. To clear power cycle the board.
//
case MOTION_STATE_ERROR:
{
//
// Our tick counter and blink mechanism may not be safe across
// rollovers of the g_ui32SysTickCount variable. This rollover
// only occurs after 1.3+ years of continuous operation.
//
if(g_ui32SysTickCount > (g_ui32RGBMotionBlinkCounter + 20))
{
//
// 20 ticks have expired since we last toggled so turn off the
// LED and reset the counter.
//
g_ui32RGBMotionBlinkCounter = g_ui32SysTickCount;
g_pui32RGBColors[RED] = 0;
RGBColorSet(g_pui32RGBColors);
}
else if(g_ui32SysTickCount == (g_ui32RGBMotionBlinkCounter + 10))
{
//
// 10 ticks have expired since the last counter reset. turn
// on the RED LED.
//
g_pui32RGBColors[RED] = 0xFFFF;
RGBColorSet(g_pui32RGBColors);
}
break;
}
}
}
//*****************************************************************************
//
// Main function to handler motion events that are triggered by the MPU9150
// data ready interrupt.
//
//*****************************************************************************
void
MotionMain(void)
{
switch(g_ui8MotionState) {
//
// This is our initial data set from the MPU9150, start the DCM.
//
case MOTION_STATE_INIT: {
//
// Check the read data buffer of the MPU9150 to see if the
// Magnetometer data is ready and present. This may not be the case
// for the first few data captures.
//
if(g_sMPU9150Inst.pui8Data[14] & AK8975_ST1_DRDY) {
//
// Get local copy of Accel and Mag data to feed to the DCM
// start.
//
MPU9150DataAccelGetFloat(&g_sMPU9150Inst, g_pfAccel,
g_pfAccel + 1, g_pfAccel + 2);
MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, g_pfMag,
g_pfMag + 1, g_pfMag + 2);
MPU9150DataGyroGetFloat(&g_sMPU9150Inst, g_pfGyro,
g_pfGyro + 1, g_pfGyro + 2);
//
// Feed the initial measurements to the DCM and start it.
// Due to the structure of our MotionMagCallback function,
// the floating point magneto data is already in the local
// data buffer.
//
CompDCMMagnetoUpdate(&g_sCompDCMInst, g_pfMag[0], g_pfMag[1],
g_pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, g_pfAccel[0], g_pfAccel[1],
g_pfAccel[2]);
CompDCMStart(&g_sCompDCMInst);
//
// Proceed to the run state.
//
g_ui8MotionState = MOTION_STATE_RUN;
}
//
// Finished
//
break;
}
//
// DCM has been started and we are ready for normal operations.
//
case MOTION_STATE_RUN: {
//
// Get the latest Euler data from the DCM. DCMUpdate is done
// inside the interrupt routine to insure it is not skipped and
// that the timing is consistent.
//
CompDCMComputeEulers(&g_sCompDCMInst, g_pfEulers,
g_pfEulers + 1, g_pfEulers + 2);
//
// Pass the latest sensor data back to the Gesture system for
// classification. What state do i think i am in?
//
GestureEmitClassify(&g_sGestureInst, g_pfEulers, g_pfAccel,
g_pfGyro);
//
// Update best guess state based on past history and current
// estimate.
//
GestureUpdate(&g_sGestureInst, g_sGestureInst.ui16Emit);
//
// This tick counter and blink mechanism may not be safe across
// rollovers of the g_ui32SysTickCount variable. This rollover
// only occurs after 1.3+ years of continuous operation.
//
if(g_ui32SysTickCount > (g_ui32MotionBlinkCounter + 10)) {
//
// 10 ticks have expired since we last toggled so toggle the
// blue LED and reset the counter.
//
g_ui32MotionBlinkCounter = g_ui32SysTickCount;
MAP_GPIOPinWrite(GPIO_PORTQ_BASE, GPIO_PIN_4,
((GPIOPinRead(GPIO_PORTQ_BASE, GPIO_PIN_4)) ^
GPIO_PIN_4));
}
//
// Finished
//
break;
}
//
// An I2C error has occurred at some point. Usually these are due to
// asynchronous resets of the main MCU and the I2C peripherals. This
// can cause the slave to hold the bus and the MCU to think it cannot
// send. In practice there are ways to clear this condition. They are
// not implemented here. To clear power cycle the board.
//
case MOTION_STATE_ERROR: {
//
// Display this error and how to clear.
//
DpyRectFill(g_sContext.psDisplay, &g_sUserInfoRect, ClrBlack);
GrContextForegroundSet(&g_sContext, ClrGray);
GrContextFontSet(&g_sContext, g_psFontCmss16b);
GrStringDraw(&g_sContext, "I2C Error has occurred. Power cycle",
-1, 10, 140, 1);
GrStringDraw(&g_sContext, "board to clear this error.", -1, 10,
160, 1);
GrContextForegroundSet(&g_sContext, ClrWhite);
GrContextFontSet(&g_sContext, g_psFontCm18b);
break;
}
}
}