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

#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;


//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׵<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λHz<48><7A>
#define ACCEL_LPF_CUTOFF_FREQ 	200.0f

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨<EFBFBD><EBB6A8><EFBFBD>Ļ<EFBFBD><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>İ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
#define ACCEL_ALIGN		CW0_DEG

//mpu6000<30><30>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>ΪFSR_8G<38><47>ACC_1G_ADC = 65536 / (2 * 16) = 4096  
#define ACC_1G_ADC	4096.0f 

Axis3i16 accADCRaw;		//<2F><><EFBFBD>ٶ<EFBFBD>ԭʼAD<41><44><EFBFBD><EFBFBD>
Axis3i16 accADC;		//У׼<D0A3><D7BC><EFBFBD><EFBFBD>AD<41><44><EFBFBD><EFBFBD>			
Axis3f accf;			//ת<><D7AA><EFBFBD><EFBFBD>λΪG<CEAA><47><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

static accCalibration_t accCalibration;	//<2F><><EFBFBD>ٶ<EFBFBD>У׼<D0A3>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

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



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


//ִ<><D6B4>У׼
static void performAcclerationCalibration(Axis3i16 accADCSample)
{
    for (int axis = 0; axis < 3; axis++) 
    {
        //<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
        if (accCalibration.cycleCount == CALIBRATING_ACC_CYCLES) 
        {
            accCalibration.accSum.axis[axis] = 0;
            devClear(&accCalibration.var[axis]);
            
        }

        //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>
        accCalibration.accSum.axis[axis] += accADCSample.axis[axis];
        
        //<2F><>λ<EFBFBD><CEBB>ƫ 
        accCalibration.accZero.axis[axis] = 0;

        if (accCalibration.cycleCount == 1) 
        {    
            //У׼<D0A3><D7BC><EFBFBD><EFBFBD>
            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)
{

  //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׵<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>
  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)
{
		
		//<2F><>ȡԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
		accADCRaw.x= accX1();
		accADCRaw.y= accY1();
		accADCRaw.z= accZ1();  
		
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		applySensorAlignment(accADCRaw.axis, accADCRaw.axis, CW90_DEG);
		
		//<2F><><EFBFBD>ٶȼ<D9B6>У׼
		if (accCalibration.ok==1) 
		{
			performAcclerationCalibration(accADCRaw);
			return;
		}
		
		//<2F><><EFBFBD><EFBFBD>accADCֵ<43><D6B5><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>ƫ
		accADC.x = accADCRaw.x - accCalibration.accZero.x;
		accADC.y = accADCRaw.y - accCalibration.accZero.y;
		accADC.z = accADCRaw.z - accCalibration.accZero.z;
		
		////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		//applyBoardAlignment(accADC.axis);
		
		//ת<><D7AA>Ϊ<EFBFBD><CEAA>λ 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;
		
//		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>˲<EFBFBD>
//		for (int axis = 0; axis < 3; axis++) 
//		{
//			accf.axis[axis] = biquadFilterApply(&accFilterLPF[axis], accf.axis[axis]);
//		}
		
		*acc = accf; 
		 
}