multirotor_pos_control: yaw setpoint bug fixed

Should fix problems with yaw. Should be tested in flight before merging.
https://github.com/PX4/Firmware/issues/386