Testing surveyor sim
Gained integrator for better tracking
Testing Pichi waypoint navigation