mma7455l_flip.c

mtk 3D动力传感器驱动源码

/*********************************************************************
 
  (c) copyright Freescale Semiconductor Ltd. 2008
  ALL RIGHTS RESERVED
 
 ********************************************************************
 Ver 2.0.1	Released Sep.2008
 ********************************************************************/


/*
Function Name: Flip_Sense();
Arguments: void;
Return: FlipReport;

Notes:
1). This function use global variables Xnew16, Ynew16 and Znew16 as inputs;
2). Xnew16, Ynew16 and Znew16 are 16-bit signed int read from register 0 ~ 5 of 7455;
3). Xnew16, Ynew16 and Znew16 should be sampled as fast as possible.
*/

#include "kal_release.h"
#include "custom_config.h"
#include "gpio_sw.h"                    //Hardware IO interface
#include "kal_trace.h"

#ifdef G_SENSOR_SUPPORT
#include "MMA7455L_Flip.h"


#define MMA_FLIP_DEBUG 0
extern VUINT8 Data_Is_Ready(void);
extern void mma745x_ReadXYZ10(VINT16 *pX, VINT16 *pY, VINT16 *pZ);

VINT16 Xnew16, Ynew16, Znew16;

VUINT8 FlipSenseStep = 0;
VUINT8 FlipDelay = 0;
VINT16 Xfst, Xsnd, Yfst, Ysnd, Zfst, Zsnd;
FlipDir FlipReport = NOFLIP;
VUINT8 Flip_Sensitivity = 10; // 11 //15;
VINT16 Flip_HighTh = Flip_HighThValue;
VINT16 Flip_LowTh = Flip_LowThValue;
VINT16 Flip_AbsTh = Flip_MinAbsValue;

VUINT8 vt_test_step = 0;
void Flip_SetSensitivity(void)
{       //Set the sensitivity for flip detection
    if((Flip_Sensitivity<=6)||(Flip_Sensitivity>=51)) Flip_Sensitivity = 15;
    //For better performance, flip sensitivity should be larger than 10.
    Flip_HighTh = Flip_Sensitivity*10;
    Flip_LowTh = Flip_Sensitivity*(-10);
    Flip_AbsTh = Flip_HighTh*2;
}

VUINT8 Flip_Sense(void)
{
    if(!MMA745x_Read_RawData(&Xnew16, &Ynew16, &Znew16)) return 0;
    
    
#if  MMA_FLIP_DEBUG
    kal_prompt_trace(MOD_SOC, "-----------------------");
    if(Xnew16 > Flip_HighTh || Xnew16 < Flip_LowTh)
        kal_prompt_trace(MOD_SOC, "***Xnew16 = [%d]***", Xnew16);
    //else
    //	kal_prompt_trace(MOD_SOC, "Xnew16 = [%d]", Xnew16);
    
    if(Ynew16 > Flip_HighTh || Ynew16 < Flip_LowTh)
        kal_prompt_trace(MOD_SOC, "###Ynew16 = [%d]###", Ynew16);
    //else
    //	kal_prompt_trace(MOD_SOC, "Ynew16 = [%d]", Ynew16);
    
    if(Znew16 > Flip_HighTh || Znew16 < Flip_LowTh)
        kal_prompt_trace(MOD_SOC, "+++Znew16 = [%d]+++", Znew16);
    //else
    //	kal_prompt_trace(MOD_SOC, "Znew16 = [%d]", Znew16);
    
    kal_prompt_trace(MOD_SOC, " ??????FlipSenseStep = [ %d ]????????", FlipSenseStep);
    
    kal_prompt_trace(MOD_SOC, " ----vt_test_step = [ %d ]----", vt_test_step);
#endif
    
    switch(FlipSenseStep)
    {
    case(1):             //Step 1 to track axes's peak value: find the first peak
        FlipSenseStep = 2;
        //If the first value is positive and it's increasing, refresh the peak value
        if((Xfst>=0)&&(Xnew16>=Flip_HighTh)&&(Xnew16>=Xfst))
        {
            Xfst = Xnew16;
            Xsnd = Xnew16;
            FlipSenseStep = 1;
            vt_test_step = 1;
        }
        //If the first value is negative and it's decreasing, refresh the peak value
        else if((Xfst<=0)&&(Xnew16<=Flip_LowTh)&&(Xnew16<=Xfst))
        {
            Xfst = Xnew16;
            Xsnd = Xnew16;
            FlipSenseStep = 1;
            vt_test_step = 2;
        }
        //Same for other axes
        if((Yfst>=0)&&(Ynew16>=Flip_HighTh)&&(Ynew16>=Yfst))
        {
            Yfst = Ynew16;
            Ysnd = Ynew16;
            FlipSenseStep = 1;
            vt_test_step = 3;
        }
        else if((Yfst<=0)&&(Ynew16<=Flip_LowTh)&&(Ynew16<=Yfst))
        {
            Yfst = Ynew16;
            Ysnd = Ynew16;
            FlipSenseStep = 1;
            vt_test_step = 4;
        }
        if((Zfst>=0)&&(Znew16>=Flip_HighTh)&&(Znew16>=Zfst))
        {
            Zfst = Znew16;
            Zsnd = Znew16;
            FlipSenseStep = 1;
            vt_test_step = 5;
        }
        else if((Zfst<=0)&&(Znew16<=Flip_LowTh)&&(Znew16<=Zfst))
        {
            Zfst = Znew16;
            Zsnd = Znew16;
            FlipSenseStep = 1;
            vt_test_step = 6;
        }
        break;                            // If the first peak values found, enter step 2
    case(2):                          // Step 2: find the second peak
        FlipSenseStep = 3;
        //If the first peak is positive, find the miminum value for second peak
        if((Xfst>=Flip_HighTh)&&(Xnew16<Xsnd))
        {
            Xsnd = Xnew16;
            FlipSenseStep = 2;
            vt_test_step = 8;
        }
        //If the first peak is negative, find the maximum value for second peak
        else if((Xfst<=Flip_LowTh)&&(Xnew16>Xsnd))
        {
            Xsnd = Xnew16;
            FlipSenseStep = 2;
            vt_test_step = 9;
        }
        if((Yfst>=Flip_HighTh)&&(Ynew16<Ysnd))
        {
            Ysnd = Ynew16;
            FlipSenseStep = 2;
            vt_test_step = 10;
        }
        else if((Yfst<=Flip_LowTh)&&(Ynew16>Ysnd))
        {
            Ysnd = Ynew16;
            FlipSenseStep = 2;
            vt_test_step = 11;
        }
        if((Zfst>=Flip_HighTh)&&(Znew16<Zsnd))
        {
            Zsnd = Znew16;
            FlipSenseStep = 2;
            vt_test_step = 12;
        }
        else if((Zfst<=Flip_LowTh)&&(Znew16>Zsnd))
        {
            Zsnd = Znew16;
            FlipSenseStep = 2;
            vt_test_step = 13;
        }
        if(FlipSenseStep==3)      //If the second peak found, enter step 3: judge flip direction and axis
        {
            FlipSenseStep = 4;
            FlipReport = XFPOS;      //Assume flip on axis X and positive direction
            FlipDelay = Flip_DelayTime;
            Xsnd = abs(Xfst - Xsnd); //The axis with maximum absolute value between the first and second peaks is the fliping axis
            Ysnd = abs(Yfst - Ysnd);
            Zsnd = abs(Zfst - Zsnd);
            
#if  MMA_FLIP_DEBUG
            kal_prompt_trace(MOD_SOC, "~~Xsnd = [%d]", Xsnd);
            kal_prompt_trace(MOD_SOC, "~~Ysnd= [%d]", Ysnd);
            kal_prompt_trace(MOD_SOC, "~~Zsnd = [%d]", Zsnd);
#endif
            if(Ysnd>=Xsnd)
            {
                FlipReport = YFPOS;       //Select axis Y
                Xfst = Yfst;
                Xsnd = Ysnd;
            }
            if(Zsnd>=Xsnd)
            {
                FlipReport = ZFPOS;       //Select axis Z
                Xfst = Zfst;
                Xsnd = Zsnd;
            }
            if(Xfst<0) FlipReport++; //If the first peak is negative, flip direction is negative
            
            if(Xsnd<Flip_AbsTh) FlipReport = NOFLIP;
            
#if  MMA_FLIP_DEBUG
            if (FlipReport != NOFLIP)
            {
                kal_prompt_trace(MOD_SOC, "{{{{{{{FlipReport = %d}}}}}}}}", FlipReport);
            }
#endif
        }
        break;
    case(4):                            // Step 4: delay between flips
        if(!(FlipDelay--))
       {
           FlipSenseStep = 0;//Back to step 0 when delay finished
           FlipReport = NOFLIP;
        }
        break;
    default:
        FlipSenseStep = 0;                // Initialize tracking records
        Xfst = 0;
        Xsnd = 0;
        Yfst = 0;
        Ysnd = 0;
        Zfst = 0;
        Zsnd = 0;
        // Check if any axis value beyond threshold
        if((Xnew16>=Flip_HighTh)||(Xnew16<=Flip_LowTh)) Xfst = Xnew16;
        if((Ynew16>=Flip_HighTh)||(Ynew16<=Flip_LowTh)) Yfst = Ynew16;
        if((Znew16>=Flip_HighTh)||(Znew16<=Flip_LowTh)) Zfst = Znew16;
        
        if(Xfst||Yfst||Zfst){             // If any axis g-value beyond threshold, start tracking
            FlipSenseStep = 1;
        }
        break;
  }
  return 1;
}

VUINT8 Flip_Get_Report(void)
{
    return FlipReport;
}

void Flip_Set_Report(FlipDir value)
{
    FlipReport = value;
}

#endif /* G_SENSOR_SUPPORT */