Some improvement, heading code simplification, and vehicle info message simplification