Implementing bank and heading matching, reverted to angle-based pitch control