Initial commit of yaw/range sweeping algorithm for altHoldThrust