Minor changes to navigation: it usually gives the optimal path now.
and at 16Hz with the other stuff.