#include "gyro.h"



#define GYRO_CALIBRATION_THRESHOLD	4.0f          /* 陀螺仪校准阈值 */

#define GYRO_LPF_CUTOFF_FREQ  	    200.0f        /* 低通滤波参数 */

/*传感器对齐定义的机体坐标系的安装方向*/
#define GYRO_ALIGN		CW0_DEG
#define GYRO_SCALE	  16.384f


//陀螺仪校准结构体参数
gyroCalibration_t gyroCalibration = 
{
	.cycleCount = CALIBRATING_GYRO_CYCLES,
};	

Axis3i16 gyroADCRaw;	//陀螺仪原始AD数据
Axis3i16 gyroADC;		//校准的AD数据
Axis3f gyrof;			//转换单位为°/s的数据


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


void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
    gyroCalibration.cycleCount = calibrationCyclesRequired;
}


void performGyroCalibration(Axis3i16 gyroADCSample)
{
    for (int axis = 0; axis < 3; axis++) 
    {	
        //复位数据
        if (gyroCalibration.cycleCount == CALIBRATING_GYRO_CYCLES) 
        {
            gyroCalibration.gyroSum.axis[axis] = 0;
            devClear(&gyroCalibration.var[axis]);
            
        }

        //陀螺数据累加
        gyroCalibration.gyroSum.axis[axis] += gyroADCSample.axis[axis];
        devPush(&gyroCalibration.var[axis], gyroADCSample.axis[axis]);

        //复位零偏 
        gyroCalibration.gyroZero.axis[axis] = 0;
        printf("   cycleCount:%d     \r\n ",gyroCalibration.cycleCount);	
        if (gyroCalibration.cycleCount == 1) 
        {
            const float stddev = devStandardDeviation(&gyroCalibration.var[axis]);
            //检测方差值是否大于陀螺仪受到移动的阈值
            //如果大于设定阈值则返回重新校准
				printf("   axis:%d    ",axis);	
				printf("   stddev:%f    ",stddev);	
				printf(" \r\n ");		
				
            if ((stddev > GYRO_CALIBRATION_THRESHOLD) || (stddev == 0)) 
            {
                gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
                gyroCalibration.ok = 1;
                return;
            }
      
            //校准完成
            gyroCalibration.gyroZero.axis[axis] = gyroCalibration.gyroSum.axis[axis] / CALIBRATING_GYRO_CYCLES;
            gyroCalibration.ok = 0;
				gyroCalibration.gyroZeroFlashSign = 111;
				printf("   gyroCalibration.gyroSum.axis[axis]:%d    ",gyroCalibration.gyroSum.axis[axis]);	
				printf("   gyroZerox:%d    ",gyroCalibration.gyroZero.axis[X]);	
				printf("   gyroZeroy:%d    ",gyroCalibration.gyroZero.axis[Y]);	
				printf("   gyroZeroz:%d    ",gyroCalibration.gyroZero.axis[Z]);	
				printf(" \r\n ");	
				
				
						

        }
    }
  
    gyroCalibration.cycleCount--;
		
		if(gyroCalibration.cycleCount==0)
		{
				gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);		
		}
}

bool GYRO_Calibration(void)
{
		if(gyroCalibration.ok == 0)
			return 1;
		return 0;
}

void GYRO_Calibration_write(uint8_t data)
{
		gyroCalibration.ok = data;
}

bool gyroInit(float gyroUpdateRate)//
{
  //初始化陀螺仪零偏校准
  gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
  gyroCalibration.ok = 0;// 1 校准陀螺仪
  //初始化二阶低通滤波
  for (int axis = 0; axis < 3; axis++)
  {
    biquadFilterInitLPF(&gyroFilterLPF[axis],GYRO_LPF_CUTOFF_FREQ, gyroUpdateRate);
    
  }
  return true;
}


void gyroUpdate(Axis3f *gyro)
{
		if(getAGT()>=0)//读取原始数据
		{
				gyroADCRaw.x=  gyrX1();		
				gyroADCRaw.y=  gyrY1();
				gyroADCRaw.z=  gyrZ1();
			
		}

		//传感器数据方向对齐
		applySensorAlignment(gyroADCRaw.axis, gyroADCRaw.axis, CW90_DEG);
		//陀螺仪校准
		if (gyroCalibration.ok==1) 
		{
			performGyroCalibration(gyroADCRaw);
			
			gyrof.x = 0.0f;
			gyrof.y = 0.0f;
			gyrof.z = 0.0f;
			*gyro = gyrof;
			return;
		}

		//计算gyroADC值，减去零偏
		gyroADC.x = gyroADCRaw.x - gyroCalibration.gyroZero.x;
		gyroADC.y = gyroADCRaw.y - gyroCalibration.gyroZero.y;
		gyroADC.z = gyroADCRaw.z - gyroCalibration.gyroZero.z; 
		
		//板对齐
		// applyBoardAlignment(gyroADC.axis);
		
		//转换为单位 °/s 
		gyrof.x = (float)gyroADC.x / GYRO_SCALE;
		gyrof.y = (float)gyroADC.y / GYRO_SCALE;
		gyrof.z = (float)gyroADC.z / GYRO_SCALE;
		
				//printf("   Xs:%f    ",gyrof.x);		
				//printf("   Ys:%f    ",gyrof.y);	
		//软件二阶低通滤波
//		for (int axis = 0; axis < 3; axis++) 
//		{
//			gyrof.axis[axis] = biquadFilterApply(&gyroFilterLPF[axis], gyrof.axis[axis]);
//		}   
		
		*gyro = gyrof;      
                     
}