Test mission with take off (waypoint mission) and RTL