drive: sync_heading function, refactor heading err calc