fixed small bugs

new waypoint is placed too far away