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; }
//计算飞行器姿态 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()); }