Fixes for debug messages and vehicle alignment