Fixes to waypointing