GeekIMU/2.Firmware/ICM42688P_HAL/Core/Src/gyro.c

#include "gyro.h"



#define GYRO_CALIBRATION_THRESHOLD	4.0f          /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC>ֵ */

#define GYRO_LPF_CUTOFF_FREQ  	    200.0f        /* <20><>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD> */

/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨<EFBFBD><EBB6A8><EFBFBD>Ļ<EFBFBD><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>İ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>*/
#define GYRO_ALIGN		CW0_DEG
#define GYRO_SCALE	  16.384f


//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyroCalibration_t gyroCalibration = 
{
	.cycleCount = CALIBRATING_GYRO_CYCLES,
};	

Axis3i16 gyroADCRaw;	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼAD<41><44><EFBFBD><EFBFBD>
Axis3i16 gyroADC;		//У׼<D0A3><D7BC>AD<41><44><EFBFBD><EFBFBD>
Axis3f gyrof;			//ת<><D7AA><EFBFBD><EFBFBD>λΪ<CEBB><CEAA>/s<><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>


biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];//<2F><><EFBFBD>׵<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>


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


void performGyroCalibration(Axis3i16 gyroADCSample)
{
    for (int axis = 0; axis < 3; axis++) 
    {	
        //<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
        if (gyroCalibration.cycleCount == CALIBRATING_GYRO_CYCLES) 
        {
            gyroCalibration.gyroSum.axis[axis] = 0;
            devClear(&gyroCalibration.var[axis]);
            
        }

        //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>
        gyroCalibration.gyroSum.axis[axis] += gyroADCSample.axis[axis];
        devPush(&gyroCalibration.var[axis], gyroADCSample.axis[axis]);

        //<2F><>λ<EFBFBD><CEBB>ƫ 
        gyroCalibration.gyroZero.axis[axis] = 0;
        printf("   cycleCount:%d     \r\n ",gyroCalibration.cycleCount);	
        if (gyroCalibration.cycleCount == 1) 
        {
            const float stddev = devStandardDeviation(&gyroCalibration.var[axis]);
            //<2F><><EFBFBD>ⷽ<EFBFBD><E2B7BD>ֵ<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>ֵ
            //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD><E8B6A8>ֵ<EFBFBD>򷵻<EFBFBD><F2B7B5BB><EFBFBD><EFBFBD><EFBFBD>У׼
				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;
            }
      
            //У׼<D0A3><D7BC><EFBFBD><EFBFBD>
            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)//
{
  //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫУ׼
  gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
  gyroCalibration.ok = 0;// 1 У׼<D0A3><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>׵<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>
  for (int axis = 0; axis < 3; axis++)
  {
    biquadFilterInitLPF(&gyroFilterLPF[axis],GYRO_LPF_CUTOFF_FREQ, gyroUpdateRate);
    
  }
  return true;
}


void gyroUpdate(Axis3f *gyro)
{
		if(getAGT()>=0)//<2F><>ȡԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
		{
				gyroADCRaw.x=  gyrX1();		
				gyroADCRaw.y=  gyrY1();
				gyroADCRaw.z=  gyrZ1();
			
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		applySensorAlignment(gyroADCRaw.axis, gyroADCRaw.axis, CW90_DEG);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼
		if (gyroCalibration.ok==1) 
		{
			performGyroCalibration(gyroADCRaw);
			
			gyrof.x = 0.0f;
			gyrof.y = 0.0f;
			gyrof.z = 0.0f;
			*gyro = gyrof;
			return;
		}

		//<2F><><EFBFBD><EFBFBD>gyroADCֵ<43><D6B5><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>ƫ
		gyroADC.x = gyroADCRaw.x - gyroCalibration.gyroZero.x;
		gyroADC.y = gyroADCRaw.y - gyroCalibration.gyroZero.y;
		gyroADC.z = gyroADCRaw.z - gyroCalibration.gyroZero.z; 
		
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		// applyBoardAlignment(gyroADC.axis);
		
		//ת<><D7AA>Ϊ<EFBFBD><CEAA>λ <20><>/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);	
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׵<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>
//		for (int axis = 0; axis < 3; axis++) 
//		{
//			gyrof.axis[axis] = biquadFilterApply(&gyroFilterLPF[axis], gyrof.axis[axis]);
//		}   
		
		*gyro = gyrof;      
                     
}