Copter: add advance track call to actually move towards waypoint

Also added unrelated check that waypoint is valid before updating
waypoint distance.
Copter: enable inav xy by default