Guidance horizontal quaternion fixes

If I'm not mistaken and understand the issues correctly, this should fix #160 and #161

Don't pretend that the commands from guidance_h are actually real euler angles.
Compose a roll/pitch quaternion from simultaneous rotation of roll/pitch,
then rotate around resulting body z-axis to align the heading.

This should "fix" the setpoint passed to the attitude stabilization if in HOVER or NAV
at large angles and in cases of where heading_sp != current heading.

Tested with a normal quad in nps, can't really test the heading saturation case (as you get with e.g. quadshot) in nps right now as we don't have a model for it.

@dewagter do you have any comments?
Would be great if one of you guys in Delft could test this.