1.Initialize MPU-6050

2.Read Gyro Values 2000 times and calculate the offsets for Yaw/Pitch/Roll

3.Read Accelerometer Values for Yaw, Pitch & Roll

4.Read gyro Values for Yaw, Pitch, Roll. Subtract Yaw, Pitch & Roll offsets from Each Reading.

5.Calculate The angles of gyro & accelerometer for Yaw, Pitch & Roll using Atan

6.A) Apply Complimentary Filter Using Angle=0.xx*(Angle+gyro*dt) + (1-0.xx)*Accelerometer for each Yaw ,Pitch & Roll.

b) Read Joystick Yaw, Pitch & Roll

7.Range the Yaw, Pitch & Roll using map/Custom Map function

8.A) Find The error for each Yaw Pitch Roll Error=Present Yaw/Pitch /Roll-Transmitter Yaw/Pitch/Roll

B) Proportional=Kp*Error

C)Integral=Previous Integral + Error*Ki

D)Derivative= (Error-Previous Error) *Kd

9.a)Motor_1=Throttle - PID Output Pitch + PID Output Roll - PID Output Yaw

b)Motor_2=Throttle + PID Output Pitch + PID Output Roll + PID Output Yaw

c)Motor_3=Throttle + PID Output Pitch - PID Output Roll - PID Output Yaw

d)Motor_4=Throttle - PID Output Pitch - PID Output Roll + PID Output Yaw