Enqueueing but not processing// marlin gcode issue

Hello, RepRap users.
I want to configure the automatic z offset setting with customized g code.
The algorithm in customized g code(M 422 to 426) is simple,(i split the g code because of processing issue and also for function testing )
1. homing
2. move probe at the centre
3. lift z-axis until the end of trigger point of the probe

Here is the code I added in Marlin_main.cpp.

case 423: // M423: Z-offset calibration/ move probe at the centre point
gcode_G28(false);
enqueue_and_echo_commands_P(PSTR("G1 Z1.8"));
stepper.synchronize();
enqueue_and_echo_commands_P(PSTR("G1 X25 Y50"));
stepper.synchronize();

break;

case 424: // M424: Z-offset calibration/ read probe status, testing
if (current_position[X_AXIS] == 25) {
gcode_M119();
READ(Z_MIN_PROBE_PIN) ^ Z_MIN_PROBE_ENDSTOP_INVERTING;
probe = (READ(Z_MIN_PROBE_PIN) ^ Z_MIN_PROBE_ENDSTOP_INVERTING);
if (probe = true) {
SERIAL_PROTOCOLLNPGM("probe = true.");
}
}

break;

case 425: // M424: Z-offset calibration/ increase the Z height by 0.1

if (probe) {
double Z = current_position[Z_AXIS] + 0.1;
char zaxis[20];//buffer for the real number printing/scaning
SERIAL_PROTOCOLLNPGM("probe = true!.");//for testing
dtostrf(Z, 3, 1, zaxis);
sprintf_P(cmd, PSTR("G1 Z%s F300"), zaxis);
enqueue_and_echo_command(cmd);//send the cmd in the queue
stepper.synchronize();

Z = Z + 0.1;
}

break;

case 426: // M424: Z-offset calibration/ lift Z height(M425) until probe trigger point(probe status = false)
while (probe == true ) {
safe_delay(2000);
enqueue_and_echo_commands_P(PSTR("M425"));
stepper.synchronize();
safe_delay(500);
}

break;

M423 - M425 is working properly, but with (while) M426, terminal says it is enqueueing but not processing.
So it returns the false value.
It seems like the command is not added successfully by full buffer or some reasons.
Here is what the terminal says.
echo:Filament settings: Disabled
echo: M200 D1.75
echo: M200 D0
echoteps per unit:
echo: M92 X228.57 Y228.57 Z1828.57 E756.20
echo:Maximum feedrates (units/s):
echo: M203 X70.00 Y70.00 Z20.00 E10.00
echo:Maximum Acceleration (units/s2):
echo: M201 X500 Y500 Z100 E10
echo:Acceleration (units/s2): P R T
echo: M204 P500.00 R500.00 T500.00
echo:Advanced: S T B X Z E
echo: M205 S0.00 T0.00 B20000 X4.00 Y4.00 Z0.20 E0.10
echo:Home offset:
echo: M206 X0.00 Y0.00 Z0.00
echo:Auto Bed Leveling:
echo: M420 S0
echoID settings:
echo: M301 P18.88 I0.80 D110.96
echo: M304 P88.61 I15.91 D123.40
echo:Z-Probe Offset (mm):
echo: M851 Z-2.60
echo:busy: processing
echo:busy: processing
echo:busy: processing
X:60.00 Y:62.00 Z:0.00 E:0.00 Count X:13714 Y:14171 Z:0
echo:enqueueing "G1 Z1.8"
echo:enqueueing "G1 X25 Y50"
ok
echoD init fail
Reporting endstop status
x_min: open
y_min: open
z_min: TRIGGERED
z_probe: open
probe = true.
ok
probe = true!.
echo:enqueueing "G1 Z1.9 F300"
ok
echo:enqueueing "M425"
echo:enqueueing "M425"
echo:enqueueing "M425"
echo:enqueueing "M425"
echo:enqueueing "M425"
echo:enqueueing "M425"FAILED.
echo:enqueueing "M425"FAILED.
echo:enqueueing "M425"FAILED.
echo:enqueueing "M425"FAILED.
echo:enqueueing "M425"FAILED.


Does anybody have some solutions with this issue?
Thanks in advance!