missionlib: waypoint yaw fix

Fixes https://github.com/PX4/Firmware/issues/495.

Not tested yet, but should be ok.