Exemplos de LPF_1st em C++ (Cpp)

Exemplo n.º 1

Arquivo: MultiRotor_ahrs.c Projeto: sensiki/HT-Hawk

void AHRS_getValues(void)
{
	static float x,y,z;
	
	MPU6050_Dataanl();
	
	HMC5883lRead(MAG);
	
	// 加速度计IIR滤波
	sensor.acc.averag.x = IIR_I_Filter(sensor.acc.origin.x, InPut_IIR[0], OutPut_IIR[0], b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
	sensor.acc.averag.y = IIR_I_Filter(sensor.acc.origin.y, InPut_IIR[1], OutPut_IIR[1], b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
	sensor.acc.averag.z = IIR_I_Filter(sensor.acc.origin.z, InPut_IIR[2], OutPut_IIR[2], b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
	
	// 陀螺仪一阶低通滤波
 	sensor.gyro.averag.x = LPF_1st(x,sensor.gyro.radian.x * Gyro_G,0.386f);	x = sensor.gyro.averag.x;
 	sensor.gyro.averag.y = LPF_1st(y,sensor.gyro.radian.y * Gyro_G,0.386f);	y = sensor.gyro.averag.y;
 	sensor.gyro.averag.z = LPF_1st(z,sensor.gyro.radian.z * Gyro_G,0.386f);	z = sensor.gyro.averag.z;
}

Exemplo n.º 2

Arquivo: ANO_IMU.cpp Projeto: YuMeiLau/UAV

//计算飞行器姿态
void ANO_IMU::getAttitude()
{
	float deltaT;
	Vector3d accTemp, gyroTemp;
	
	//加速度数据一阶低通滤波
	acc_lpf = LPF_1st(acc_lpf, acc, ano.factor.acc_lpf);


	//计算实际测量的加速度和重力加速度的比值
	accRatio = acc_lpf.length_squared() * 100 / (ACC_1G * ACC_1G);		
	
	deltaT = getDeltaT(GetSysTime_us());
	
}