Fixed calculated yaw value immediately being overwritten without being used. Credit to anion155