Fix primary axis calculation
Generate debug message when entering stopped state.

Fix handling of multiple queue packets. Make endstop checking more
efficient when not enabled.