#include "accelerometer.h"


typedef struct accCalibration_s
{
  Axis3i32 accSum;
  Axis3i16 accZero;
  stdev_t var[3];
  uint16_t cycleCount;
  int ok;
  uint16_t accZeroFlashSign;
} accCalibration_t;


//软件二阶低通滤波参数（单位Hz）
#define ACCEL_LPF_CUTOFF_FREQ 	200.0f

//传感器对齐定义的机体坐标系的安装方向
#define ACCEL_ALIGN		CW0_DEG

//mpu6000初始化加速度量程为FSR_8G，ACC_1G_ADC = 65536 / (2 * 16) = 4096  
#define ACC_1G_ADC	4096.0f 

Axis3i16 accADCRaw;		//加速度原始AD数据
Axis3i16 accADC;		//校准后的AD数据			
Axis3f accf;			//转换单位为G的数据

static accCalibration_t accCalibration;	//加速度校准结构体参数

biquadFilter_t accFilterLPF[XYZ_AXIS_COUNT];//二阶低通滤波器



void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
    accCalibration.cycleCount = calibrationCyclesRequired;
}


//执行校准
static void performAcclerationCalibration(Axis3i16 accADCSample)
{
    for (int axis = 0; axis < 3; axis++) 
    {
        //复位数据
        if (accCalibration.cycleCount == CALIBRATING_ACC_CYCLES) 
        {
            accCalibration.accSum.axis[axis] = 0;
            devClear(&accCalibration.var[axis]);
            
        }

        //陀螺数据累加
        accCalibration.accSum.axis[axis] += accADCSample.axis[axis];
        
        //复位零偏 
        accCalibration.accZero.axis[axis] = 0;

        if (accCalibration.cycleCount == 1) 
        {    
            //校准完成
            accCalibration.accZero.axis[axis] = (accCalibration.accSum.axis[axis] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
            if(axis==2)
            accCalibration.accZero.axis[axis] -= (int)ACC_1G_ADC;
            accCalibration.ok = 0;   
            //EEPROM_write(3);
        }
    }
    accCalibration.cycleCount--;
    
}


bool ACC_Calibration(void)
{
		if(accCalibration.ok == 0)
			return 1;
		return 0;
}



bool accInit(float accUpdateRate)
{

  //初始化软件二阶低通滤波
  for (int axis = 0; axis < 3; axis++)
  {
    biquadFilterInitLPF(&accFilterLPF[axis], ACCEL_LPF_CUTOFF_FREQ , accUpdateRate);
  }
  accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
  accCalibration.ok = 0;
  
  return true;
}




void accUpdate(Axis3f *acc)
{
		
		//读取原始数据
		accADCRaw.x= accX1();
		accADCRaw.y= accY1();
		accADCRaw.z= accZ1();  
		
		//传感器数据方向对齐
		applySensorAlignment(accADCRaw.axis, accADCRaw.axis, CW90_DEG);
		
		//加速度计校准
		if (accCalibration.ok==1) 
		{
			performAcclerationCalibration(accADCRaw);
			return;
		}
		
		//计算accADC值，减去零偏
		accADC.x = accADCRaw.x - accCalibration.accZero.x;
		accADC.y = accADCRaw.y - accCalibration.accZero.y;
		accADC.z = accADCRaw.z - accCalibration.accZero.z;
		
		////板对齐
		//applyBoardAlignment(accADC.axis);
		
		//转换为单位 g (9.8m/s^2)
		accf.x = (float)accADC.x / ACC_1G_ADC;
		accf.y = (float)accADC.y / ACC_1G_ADC;
		accf.z = (float)accADC.z / ACC_1G_ADC;
		
//		//软件低通滤波
//		for (int axis = 0; axis < 3; axis++) 
//		{
//			accf.axis[axis] = biquadFilterApply(&accFilterLPF[axis], accf.axis[axis]);
//		}
		
		*acc = accf; 
		 
}