Ah ha, actually make this thing interpolate properly between waypoints! Thought it was buggy :) should know better than to write code late at night.