I've been testing the path manager in fillet-mode and it seems that it doesn't know how to handle colinear waypoints (i.e. [100, 500, -100] , [100, 600, -100] and [100, 700, -100]). It seems that the state-machine should just stay in straight line paths instead of banking.
Additionally, @mmmfarrell said he saw this when using dubin's paths.
I've been testing the path manager in fillet-mode and it seems that it doesn't know how to handle colinear waypoints (i.e. [100, 500, -100] , [100, 600, -100] and [100, 700, -100]). It seems that the state-machine should just stay in straight line paths instead of banking.
Additionally, @mmmfarrell said he saw this when using dubin's paths.