Heading Hold Mode should be good to test once inner loops are tuned.