Fix TVC controls to connect lateral drift PD to goal orientation, removing race condition between PD 2 & 3