It's way past my bed time... But in the morning I'll take a closer look at it. I'll figure out where to add a few extra lines of debugging code so we can zero in on where the problem is...
Update:
OK, I took your Marlin_main.cpp and dropped it into my build environment. I recompiled and flashed my firmware. Here is what I got when I sent it a G28 and then an M114 and a M48 command:
Connecting...
Printer is now online.
>>>G28
SENDING:G28
>>>M114
SENDING:M114
X:76.00 Y:73.00 Z:4.20 E:0.00 Count X: 76.00 Y:73.00 Z:4.20
>>>M48 V 4
SENDING:M48 V 4
M48 Z-Probe Repeatability test. Version 1.85
Positioning probe for the test.
1 of 10 z: 4.225065 mean: 4.225065 sigma: 0.000000
2 of 10 z: 4.214812 mean: 4.219938 sigma: 0.005126
3 of 10 z: 4.221096 mean: 4.220324 sigma: 0.004221
4 of 10 z: 4.217458 mean: 4.219607 sigma: 0.003860
5 of 10 z: 4.221426 mean: 4.219971 sigma: 0.003529
6 of 10 z: 4.213820 mean: 4.218946 sigma: 0.003954
7 of 10 z: 4.209851 mean: 4.217647 sigma: 0.004851
8 of 10 z: 4.206213 mean: 4.216217 sigma: 0.005906
9 of 10 z: 4.198276 mean: 4.214224 sigma: 0.007925
10 of 10 z: 4.203898 mean: 4.213191 sigma: 0.008131
Mean: 4.213191
Standard Deviation: 0.008131
echo:endstops hit: Z:4.20
My Z Probe offset is defined as:
#define Z_PROBE_OFFSET_FROM_EXTRUDER -4.20
so the code is returning a 'correct' value. Let's see a cut & paste of you doing the same 3 commands.