The algorithm to find the closest waypoint has been implemented and needs  some extensive testing. The navigation software still needs a few things to be fully operational. A 24h record is in process to test the precision of the GPS receiver, possibly tomorrow I will post the results.