Example #1
0
//*****************************************************************************
//
// 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;
        }
    }
}
Example #2
0
//*****************************************************************************
//
// 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;
        }
    }
}