WIP: Manual attitude setpoint for large heading errors

[RomanBapst]

mc pos control: In manual mode calculate attitude setpoint such that it reflects the users intuition of roll and pitch for any given heading error.

TODO:

• extensive testing in SITL
• testing on real multicopter
• review
• add comments to code

Fixes #4556
Fixes #3620

[RomanBapst]

A good way to test this:

1. Comment this line https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1982

2. Assign any value between -pi and pi to _att_sp.yaw_body here https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1979
   e.g. add

_att_sp.yaw_body = -0.9f;

3. Disable absolute yaw control by commenting this line https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp#L708

By doing the above steps the copter will no longer have absolute yaw control but you can still make it turn in yaw since there is a FF on the rates. In this situation you can produce an arbitrary yaw error and verify that roll and pitch commands match the users intuition.

[RomanBapst]

Log where absolute yaw control was disabled -> arbitrary high yaw error: User was still able to control roll and pitch as he expected:
http://logs.uaventure.com/view/svVXALTKZbkZbCwuyD37v8

Log where absolute yaw control was enabled
http://logs.uaventure.com/view/fBnVjFEUm9BJf6Mmhr9zhX

[LorenzMeier]

@Tumbili The logic looks right, but I would like to clarify if the only derivation we can choose is through the proposed mix of euler angles, quaternions, trigonometric functions and rotation matrices. It would be good to have a second pass and see if we can't express this more compact using entries of a rotation matrix. I'm concerned that while we fix the common yaw case we might miss extreme angles, in particular for racers.

[RomanBapst]

@LorenzMeier Tested in SITL and the behaviour is exactly the same. Maybe I can try to add even better comments.

[LorenzMeier]

Looks good, thanks!

[LorenzMeier]

Thanks for the quick fix of this issue, I'm sure a lot of users will really appreciate it. I've back ported this to master.