revert safetydist
fix stupid bug in waypoints