controller of yaw added, direction to test!