void Cycle_Time_Init()
{
u8 i;
for(i=0;i<GET_TIME_NUM;i++)
{
Get_Cycle_T(i);
}
}
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 Duty_1ms()
{
Get_Cycle_T(1);
ANO_DT_Data_Exchange(); //数传通信定时调用
}
