# Correct Quadcopter Yaw control implementation

I have built a quadcopter from scratch, including my own flight controller.

I have implemented a sensor fusion algorithm (Madgwick algorithm), which returns me current Yaw, Pitch and Roll angles. Then, a PID control algorithm based on these measured angles and desired angles (which are set by the control sticks on a transmitter) adjusts the speed of the motors.

For pitch and roll controls the algorithm is obvious and works just fine (i.e, pitch stick is 100% forward - desired pitch is "30 degrees", and current measured pitch is trying to catch up with that value using PID control). However for Yaw I would like the yaw stick to rotate the quad in the corresponding direction as long as I am holding the "yaw stick" in a non-zero position, with a rotating speed, proportional to the yaw stick angle. Since I have current measured Yaw angle (relative to a "reference yaw" which is the yaw that the quad had when the throttle was zero, not relative to the "absolute magnetic North-origined yaw"), I am basically just adding a "quadRotationSpeed = yawStickValue*delta_t" to the "desired yaw angle" in each iteration of the control loop, and then do the rest as a regular PID control: based on desiredYaw and measuredYaw I am calculating the motor torques.

Now the questions:

1) Is this approach wrong?

2) When I turn the yaw stick, the quad seems to be rotating with the speed, proportional to the stick angle, but when I let go off the stick, it rapidly turns back half way and then keeps turning slowly back to the "desired angle" I set with the yaw stick.

3) After this yaw maneuver the quad seems to be much less stable during the flight, but gradually it stabilizes.