void Duty_5ms()
{
float outer_loop_time;
outer_loop_time = Get_Cycle_T(2); //获取外环准确的执行周期
test[2] = GetSysTime_us()/1000000.0f;
/*IMU更新姿态。输入：半个执行周期，三轴陀螺仪数据（转换到度每秒），三轴加速度计数据（4096--1G）；输出：ROLPITYAW姿态角*/
IMUupdate(0.5f *outer_loop_time,mpu6050.Gyro_deg.x, mpu6050.Gyro_deg.y, mpu6050.Gyro_deg.z, mpu6050.Acc.x, mpu6050.Acc.y, mpu6050.Acc.z,&Roll,&Pitch,&Yaw);
CTRL_2( outer_loop_time ); // 外环角度控制。输入：执行周期，期望角度（摇杆量），姿态角度；输出：期望角速度。<函数未封装>
test[3] = GetSysTime_us()/1000000.0f;
}
void Duty_2ms()
{
float inner_loop_time;
inner_loop_time = Get_Cycle_T(0); //获取内环准确的执行周期
test[0] = GetSysTime_us()/1000000.0f;
MPU6050_Read(); //读取mpu6轴传感器
MPU6050_Data_Prepare( inner_loop_time ); //mpu6轴传感器数据处理
CTRL_1( inner_loop_time ); //内环角速度控制。输入：执行周期，期望角速度，测量角速度，角度前馈；输出：电机PWM占空比。<函数未封装>
RC_Duty( inner_loop_time , Rc_Pwm_In ); // 遥控器通道数据处理 ，输入：执行周期，接收机pwm捕获的数据。
test[1] = GetSysTime_us()/1000000.0f;
}
//计算飞行器姿态
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());
}
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
}
void DelayUs(uint32_t us)
{
uint32_t now = GetSysTime_us();
while (GetSysTime_us() - now < us);
}
