Made yaw able to switch between SITL and Vicon