diff --git a/uCNC/src/cnc_hal_config_helper.h b/uCNC/src/cnc_hal_config_helper.h
index 714420bb3..8e6e62753 100644
--- a/uCNC/src/cnc_hal_config_helper.h
+++ b/uCNC/src/cnc_hal_config_helper.h
@@ -195,6 +195,12 @@ extern "C"
 #define PROBE_PULLUP
 #endif
 
+#ifdef ENABLE_RT_PROBE_CHECKING
+#ifdef PROBE_ISR
+#undef PROBE_ISR
+#endif
+#endif
+
 #ifdef ESTOP_PULLUP_ENABLE
 #define ESTOP_PULLUP
 #endif
diff --git a/uCNC/src/core/motion_control.c b/uCNC/src/core/motion_control.c
index 3daf91a13..f599ad510 100644
--- a/uCNC/src/core/motion_control.c
+++ b/uCNC/src/core/motion_control.c
@@ -930,21 +930,17 @@ uint8_t mc_probe(float *target, uint8_t flags, motion_data_t *block_data)
 		}
 	} while (!itp_is_empty() || !planner_buffer_is_empty());
 
+	// wait for a stop
+	while (cnc_dotasks() && cnc_get_exec_state(EXEC_RUN));
 	// disables the probe
 	io_disable_probe();
 	itp_clear();
 	planner_clear();
 	// clears hold
 	cnc_clear_exec_state(EXEC_HOLD);
-	parser_update_probe_pos();
 
 	// sync the position of the motion control
 	mc_sync_position();
-	// HALT could not be cleared. Something is wrong
-	if (cnc_get_exec_state(EXEC_UNHOMED))
-	{
-		return STATUS_CRITICAL_FAIL;
-	}
 
 	cnc_delay_ms(g_settings.debounce_ms); // adds a delay before reading io pin (debounce)
 	probe_ok = io_get_probe();
@@ -957,7 +953,6 @@ uint8_t mc_probe(float *target, uint8_t flags, motion_data_t *block_data)
 		}
 		return STATUS_OK;
 	}
-
 #endif
 
 	return STATUS_PROBE_SUCCESS;
@@ -1133,7 +1128,7 @@ uint8_t mc_build_hmap(float *target, float *offset, float retract_h, motion_data
 
 			// store position
 			int32_t probe_position[STEPPER_COUNT];
-			itp_get_rt_position(probe_position);
+			parser_get_probe(probe_position);
 			kinematics_steps_to_coordinates(probe_position, position);
 			hmap_offsets[i + H_MAPING_GRID_FACTOR * j] = position[AXIS_TOOL];
 			protocol_send_probe_result(1);
diff --git a/uCNC/src/core/parser.c b/uCNC/src/core/parser.c
index 279ad4393..9ae0ce79e 100644
--- a/uCNC/src/core/parser.c
+++ b/uCNC/src/core/parser.c
@@ -291,6 +291,10 @@ void parser_sync_probe(void)
 	itp_get_rt_position(rt_probe_step_pos);
 }
 
+void parser_get_probe(int32_t *position){
+	memcpy(position, rt_probe_step_pos, sizeof(rt_probe_step_pos));
+}
+
 void parser_update_probe_pos(void)
 {
 	kinematics_steps_to_coordinates(rt_probe_step_pos, parser_parameters.last_probe_position);
@@ -1711,8 +1715,12 @@ uint8_t parser_exec_command(parser_state_t *new_state, parser_words_t *words, pa
 			}
 			else
 			{
+				// failed at this position
+				parser_sync_probe();
 				parser_parameters.last_probe_ok = 0;
 			}
+			// sync probe position
+			parser_update_probe_pos();
 
 			if (error == STATUS_OK)
 			{
diff --git a/uCNC/src/core/parser.h b/uCNC/src/core/parser.h
index 28aafa3af..9d55004f8 100644
--- a/uCNC/src/core/parser.h
+++ b/uCNC/src/core/parser.h
@@ -301,6 +301,7 @@ extern "C"
 	void parser_get_coordsys(uint8_t system_num, float *axis);
 	bool parser_get_wco(float *axis);
 	void parser_sync_probe(void);
+	void parser_get_probe(int32_t *position);
 	void parser_update_probe_pos(void);
 	uint8_t parser_get_probe_result(void);
 	void parser_parameters_load(void);