Ardupilot code for following aircraft