Implement true hdg functionality in AP and ND - no switch in cockpit yet