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(); //数传通信定时调用 }