Fixed waypoint parsing, route expanding, and navaid stuff.

Ready for refactor