Add manual override option and fix yaw control