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

Even my algorithm looks almost the same but haven't tested it.
2. Dont need to calculate offset everytime you turn on the electronics.
3. Accelerometer cannot determine yaw its purely done by gyro.
5. Do not use atan function as it takes so many clock cycles thus increasing looptime and lowering cycle frequency. You can instead use some linear conversion techniques for converting pitch,roll just by multiplying with some factor.
Rest of the things looks good.

Thank you Balakrishna Reddy.
If you can just elaborate the point 5 .how to get the angles by using linear conversion.

Sent from my Lenovo P1ma40 using Tapatalk

Why not use the inbuilt DMP instead? You can achieve a loop rate of 190 to 200 Hz which is great for 250+ sized quads. Also, DMP gives better yaw results than any self implemented algorithm. 

There are errors in your final equations. Are you using the standard motor configuration?

I am not using the DMP as the documentation for it is non-existent .And here is the motor configuration

The documentation is available but needs some patience to understand and implement.

If you don't want to do it from scratch, use Jeff Rowberg's I2C and motionapps libraries. It'll take a little more memory but save you a lot of head banging.

The motor outputs are okay.

I use the following formula:
error =  desired position - actual position

That's why the signs appeared wrong to me. My bad.

The PID algorithm needs improvement though. You need to make changes for initialization, derivative kick, reset windup, Ki bumps etc.

Try to use a constant loop time to get better results. My team got much better results with constant time and integer math in the PID.

Thank you Swapnil for the input.

Sent from my Lenovo P1ma40 using Tapatalk