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); }