Updated code for Yaw control.

Now yaw control gets to simulator but the helicopter oscillates back and forth.