Check for probe sanity after XY movement
More descriptive height debugging output - added Z height