1. Fixed bug whereby the first XY move after a Z move would often be at
a speed much higher than requested.
2. Added support for direct-mode ultrasonic sensor using command M558
P3. Command M558 P4 selects differential-mode ultrasonic sensor.
3. Changed temperature parameter in G31 command from T to S.
