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