Fix ND sizes
Add true heading mode for ND and AP