integrated new nav with heading processor, added debug statements, ready
for ground test