navigator: takeoff fix

Always reach takeoff altitude, even if first waypoint is lower.