mma7455l_calibration.c

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

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


/*
Note: 
      1. When calibrating with this program, the sensor should be kept still and flat
      and Z axis up, so that X = 0, Y = 0 and Z = 63.
      2. If Z axis is reversed, then chang "63" to "-63" on the program.
*/

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

#define CalibrationTimes 3

/*
typedef struct {
VINT16  X;
VINT16  Y;
VINT16  Z;  
} SPARAMETERS;
*/
void Calibration_XYZ(void);
void Sample_n_Times(VUINT8 n, VINT16 *pX, VINT16 *pY, VINT16 *pZ);
char Data_Is_Ready(void);

void Calibration_XYZ(void){
    SPARAMETERS gValue;
    VUINT8 temp, temp1,temp2, i;
    VINT16 temp3;
    
    temp = mma745x_IICRead(MCTL);
    
    mma745x_IICWrite(MCTL, 0x45);
    
    for(i=0;i<CalibrationTimes;i++)
    {
        Sample_n_Times(8, &gValue.X, &gValue.Y, &gValue.Z);
        
        gValue.X *= -2;
        temp1 = mma745x_IICRead(XOFFH);
        temp2 = mma745x_IICRead(XOFFL);
        temp3 = (VINT16)(((VUINT16)(temp1<<8))+(VUINT16)(temp2));
        gValue.X += temp3;
        mma745x_IICWrite(XOFFL, (VUINT8)(gValue.X));
        mma745x_IICWrite(XOFFH, (VUINT8)(gValue.X>>8));
        
        gValue.Y *= -2;
        temp1 = mma745x_IICRead(YOFFH);
        temp2 = mma745x_IICRead(YOFFL);
        temp3 = (VINT16)(((VUINT16)(temp1<<8))+(VUINT16)(temp2));
        gValue.Y += temp3;
        mma745x_IICWrite(YOFFL, (VUINT8)(gValue.Y));
        mma745x_IICWrite(YOFFH, (VUINT8)(gValue.Y>>8));
        
        if(gValue.Z>=0) gValue.Z -= 63; 
        else gValue.Z += 63;
        gValue.Z *= -2;
        temp1 = mma745x_IICRead(ZOFFH);
        temp2 = mma745x_IICRead(ZOFFL);
        temp3 = (VINT16)(((VUINT16)(temp1<<8))+(VUINT16)(temp2));
        gValue.Z += temp3;
        mma745x_IICWrite(ZOFFL, (VUINT8)(gValue.Z));
        mma745x_IICWrite(ZOFFH, (VUINT8)(gValue.Z>>8));
    }
    mma745x_IICWrite(MCTL, temp);
}

void Sample_n_Times(VUINT8 n, VINT16 *pX, VINT16 *pY, VINT16 *pZ) {
    VUINT8 i;
    VINT16 TmpX, TmpY, TmpZ;
    *pX=0; *pY=0; *pZ=0;
    for (i=0;i<n;i++)
    {
        while(!Data_Is_Ready());
        mma745x_ReadXYZ10(&TmpX, &TmpY, &TmpZ);
        *pX += TmpX;   
        *pY += TmpY;   
        *pZ += TmpZ;   
    }
    *pX /= n;
    *pY /= n;
    *pZ /= n;
}

#endif /* G_SENSOR_SUPPORT */