Good job, Angel!
If you don't need the copter to keep the heading on take-off & landing (i.e. as a part of the video scene) you can do without.
Usually t-o & l is near where the operator stands, so it does make sense to control the yaw manually.
If the copter also behaves like that on a waypoint/loiter then you'd better try to tune the nav. PIDs first.
I've seen similar behaviours for the yaw - first reacts too slow, speeds up, overshoots, yaws back and so on...
This can be tuned.
But if it only chases the heading on t-o & l then your code might be a solution.
Thanks for your constant investigations and tinkering!
Once confirmed your changes might make it into the next release.
Norbert