mma7455l_turn.c

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

/*********************************************************************

  (c) copyright Freescale Semiconductor Ltd. 2008
  ALL RIGHTS RESERVED
  
********************************************************************
	Ver 0	Initial release
********************************************************************/

#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_Turn.h"
#include "FSL_data.h"

#define MMA_TURN_DEBUG  0
VUINT8 RawDataPointer;
VINT8 Xraw[RawDataLength], Yraw[RawDataLength], Zraw[RawDataLength];
VINT8 Xavg8, Yavg8, Zavg8;
VINT8 Xsmth, Ysmth, Zsmth;

VINT8 turn_report_value = 0;

extern INT8 mma745x_IICRead(UINT8 RegAdd);
extern VUINT8 Data_Is_Ready(void);


VINT8 TurnDetection_Step;
VUINT16  TurnDetection_Delay;
VINT8 Zrcrd;

#define FilterLength 8

VUINT8 FilterPointer;
VINT8 Xflt[FilterLength], Yflt[FilterLength], Zflt[FilterLength];

VUINT8 XYZ_Turn_Filter(void)
{
    VUINT8 i;
    VINT16 temp16;
    
    if(!Data_Is_Ready()) return 0;
    
    if((++RawDataPointer)>=RawDataLength) RawDataPointer = 0;
    Xraw[RawDataPointer] = mma745x_IICRead(XOUT8);
    Yraw[RawDataPointer] = mma745x_IICRead(YOUT8);
    Zraw[RawDataPointer] = mma745x_IICRead(ZOUT8);
    
#if  MMA_TURN_DEBUG
    kal_prompt_trace(MOD_SOC, "x = %d", Xraw[RawDataPointer]);
    kal_prompt_trace(MOD_SOC, "y = %d", Yraw[RawDataPointer]);
    kal_prompt_trace(MOD_SOC, "z = %d", Zraw[RawDataPointer]);
#endif
    
    for(i=0, temp16=0;i<RawDataLength;i++)
    {
        temp16 += Xraw[i];
    }
    Xavg8 = (signed char)(temp16/RawDataLength);
    for(i=0, temp16=0;i<RawDataLength;i++)
    {
        temp16 += Yraw[i];
    }
    Yavg8 = (signed char)(temp16/RawDataLength);
    for(i=0, temp16=0;i<RawDataLength;i++)
    {
        temp16 += Zraw[i];
    }
    Zavg8 = (signed char)(temp16/RawDataLength);
    
    if((++FilterPointer)>=FilterLength) FilterPointer = 0;
    Xflt[FilterPointer] = Xavg8;
    Yflt[FilterPointer] = Yavg8;
    Zflt[FilterPointer] = Zavg8;
    
    for(i=0, temp16=0;i<FilterLength;i++)
    {
        temp16 += Xflt[i];
    }
    Xsmth = (VINT8)(temp16/FilterLength);
    for(i=0, temp16=0;i<FilterLength;i++)
    {
        temp16 += Yflt[i];
    }
    Ysmth = (VINT8)(temp16/FilterLength);
    for(i=0, temp16=0;i<FilterLength;i++)
    {
        temp16 += Zflt[i];
    }
    Zsmth = (VINT8)(temp16/FilterLength);
    
    return 1;
}


VUINT8 Turn_Sense(void)
{
    if(!XYZ_Turn_Filter()) 
    {
        turn_report_value = 0;
        return 0;
    }
    switch(TurnDetection_Step)
    {
    case(1):
        if(abs(Zsmth-Zrcrd)>5) TurnDetection_Step = 2;
        break;
    case(2):
        if(((++TurnDetection_Delay)<TurnDelayValue))
        {
            if(Zrcrd>30)
            {
                if((Zrcrd-Zsmth)>=80)
                {
                    TurnDetection_Step = 0;
#if  MMA_TURN_DEBUG
                    kal_prompt_trace(MOD_SOC, "return 1");
#endif
                    
                    turn_report_value = 1; /* viewtran */
                    return 1;
                }
            }
            else if(Zrcrd<-30)
            {
                if((Zsmth-Zrcrd)>=80)
                {
                    TurnDetection_Step = 0;
#if  MMA_TURN_DEBUG
                    kal_prompt_trace(MOD_SOC, "return 2");
#endif
                    turn_report_value = 2; /* viewtran */
                    return 2;
                }
            }
        }
        else
        {
            TurnDetection_Step = 0;
        }
        break;
    default:
        TurnDetection_Step = 0;
        Zrcrd = 0;
        if(Zsmth==Zavg8)
        {
            Zrcrd = Zsmth;
            TurnDetection_Step = 1;
            TurnDetection_Delay = 0;
        }
        break;
    }
#if  MMA_TURN_DEBUG
    kal_prompt_trace(MOD_SOC, "return 0");
#endif
    turn_report_value = 0;
    return 0;
}

VUINT8 Turn_Get_report(void)
{
#if  MMA_TURN_DEBUG
    kal_prompt_trace(MOD_SOC, "HYQ turn report %d", turn_report_value);
#endif
    return turn_report_value;
}

#endif /* G_SENSOR_SUPPORT */