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.
My quad is about 800 grams and is quite large, about 60 cm in diagonal.
