void GetEulerAngle(void)
{
float deltaT;
Struct_3f gy,ac;
GetMPU6050Data();
imuAccIIRLPFilter(&mpu_info.acc, &accelLPF, &accelStoredFilterValues,imuAccLpfAttFactor);
gy.x = mpu_info.gyro.x * Gyro2Radian;
gy.y = mpu_info.gyro.y * Gyro2Radian;
gy.z = mpu_info.gyro.z * Gyro2Radian;
ac.x = accelLPF.x;
ac.y = accelLPF.y;
ac.z = accelLPF.z;
deltaT = GetDeltaT(GetSysTime_us());
DCM_CF(gy,ac,deltaT);
}
//计算飞行器姿态
void IMU_GetAttitude()
{
float deltaT;
#ifdef ANO_IMU_USE_LPF_1st
//加速度数据一阶低通滤波
Acc_lpf = LowPassFilter_1st(Acc_lpf, Acc, ano.factor.acc_lpf);
#endif
#ifdef ANO_IMU_USE_LPF_2nd
//加速度数据二阶低通滤波
imu.Acc_lpf = LowPassFilter_2nd(&imu.Acc_lpf_2nd, imu.Acc);
#endif
deltaT = getDeltaT(GetSysTime_us());
#ifdef ANO_IMU_USE_DCM_CF
DCM_CF(imu.Gyro,imu.Acc_lpf,deltaT);
#endif
#ifdef ANO_IMU_USE_Quaternions_CF
Quaternion_CF(Gyro,Acc_lpf_1st,deltaT);
#endif
}
