yeah and so does the ardupilot documentation including examples. A basic guidance system, which is like single waypoint navigation, is a solved problem. It's in basically every prosumer drone too.