diff --git a/src/modules/tools/atc/ATCHandler.cpp b/src/modules/tools/atc/ATCHandler.cpp
index a242d9e3..e7858e5d 100644
--- a/src/modules/tools/atc/ATCHandler.cpp
+++ b/src/modules/tools/atc/ATCHandler.cpp
@@ -1074,9 +1074,11 @@ void ATCHandler::fill_cali_scripts(bool is_probe, bool clear_z, int repeat_count
 	}
 	
 	for(int i = 1; i <= repeat_count; i++){
+		if(i == 1){
 		// lift z to safe position with fast speed
 		snprintf(buff, sizeof(buff), "G53 G0 Z%.3f", THEROBOT->from_millimeters(clear_z ? this->clearance_z : this->safe_z_mm));
 		this->script_queue.push(buff);
+		}
 		// move x and y to calibrate position
 		// Use one-off offsets if configured, otherwise use standard probe position
 		float probe_x = probe_mx_mm + (this->probe_oneoff_configured ? this->probe_oneoff_x : 0.0);
@@ -1109,13 +1111,17 @@ void ATCHandler::fill_cali_scripts(bool is_probe, bool clear_z, int repeat_count
 			// save new tool offset
 			snprintf(buff, sizeof(buff), "M493.1 R%d", i);
 			this->script_queue.push(buff);
+			// lift z to safe position with fast speed
+			snprintf(buff, sizeof(buff), "G53 G0 Z%.3f", THEROBOT->from_millimeters(this->clearance_z));
+			this->script_queue.push(buff);
 		}else{
 			// save new tool offset
 			this->script_queue.push("M493.1");
+			// lift a bit
+			snprintf(buff, sizeof(buff), "G91 G0 Z%.3f", THEROBOT->from_millimeters(25.0));
+			this->script_queue.push(buff);
 		}
-		// lift z to safe position with fast speed
-		snprintf(buff, sizeof(buff), "G53 G0 Z%.3f", THEROBOT->from_millimeters(this->safe_z_mm));
-		this->script_queue.push(buff);
+		
 	}
 	// check if wireless probe is will be triggered
 	if (is_probe) {