This was lost when moving the handling the the trajectory points from within the vehicle.

Fix crash caused by clear method not resetting internals correctly

Better flight trajectory point algorithm