diff --git a/Firmware/sm4.c b/Firmware/sm4.c
index 34cf8a3c9..ff8a1cd53 100644
--- a/Firmware/sm4.c
+++ b/Firmware/sm4.c
@@ -191,5 +191,45 @@ uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
 	return nd;
 }
 
+uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz){
+	uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5);
+	uint16_t nd = dd;
+	uint16_t cx = dd;
+	uint16_t cy = dd;
+	uint16_t cz = dd;
+	uint16_t x = 0;
+	uint16_t y = 0;
+	uint16_t z = 0;
+	while (nd){
+		if (sm4_stop_cb && (*sm4_stop_cb)()) break;
+		uint8_t sm = 0; //step mask
+		if (cx <= dx){
+			sm |= 1;
+			cx += dd;
+			x++;
+		}
+		if (cy <= dy){
+			sm |= 2;
+			cy += dd;
+			y++;
+		}
+		if (cz <= dz){
+			sm |= 4;
+			cz += dd;
+			z++;
+		}
+		cx -= dx;
+		cy -= dy;
+		cz -= dz;
+		sm4_do_step(sm);
+		uint16_t delay = SM4_DEFDELAY;
+		if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
+		if (delay) delayMicroseconds(delay);
+		nd--;
+	}
+	if (sm4_update_pos_cb)
+		(*sm4_update_pos_cb)(x, y, z, 0);
+	return nd;
+}
 
 #endif //NEW_XYZCAL
diff --git a/Firmware/sm4.h b/Firmware/sm4.h
index fc64f7a66..ce110caa8 100644
--- a/Firmware/sm4.h
+++ b/Firmware/sm4.h
@@ -1,56 +1,57 @@
-//sm4.h - simple 4-axis stepper control
-#ifndef _SM4_H
-#define _SM4_H
-
-#include <inttypes.h>
-#include "config.h"
-
-
-#if defined(__cplusplus)
-extern "C" {
-#endif //defined(__cplusplus)
-
-
-// callback prototype for stop condition (return 0 - continue, return 1 - stop)
-typedef uint8_t (*sm4_stop_cb_t)();
-
-// callback prototype for updating position counters
-typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
-
-// callback prototype for calculating delay
-typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd);
-
-
-// callback pointer - stop
-extern sm4_stop_cb_t sm4_stop_cb;
-
-// callback pointer - update_pos
-extern sm4_update_pos_cb_t sm4_update_pos_cb;
-
-// callback pointer - calc_delay
-extern sm4_calc_delay_cb_t sm4_calc_delay_cb;
-
-
-// returns direction for single axis (0 - positive, 1 - negative)
-extern uint8_t sm4_get_dir(uint8_t axis);
-
-// set direction for single axis (0 - positive, 1 - negative)
-extern void sm4_set_dir(uint8_t axis, uint8_t dir);
-
-// returns direction of all axes as bitmask (0 - positive, 1 - negative)
-extern uint8_t sm4_get_dir_bits(void);
-
-// set direction for all axes as bitmask (0 - positive, 1 - negative)
-extern void sm4_set_dir_bits(uint8_t dir_bits);
-
-// step axes by bitmask
-extern void sm4_do_step(uint8_t axes_mask);
-
-// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped)
-extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
-
-
-#if defined(__cplusplus)
-}
-#endif //defined(__cplusplus)
-#endif //_SM4_H
+//sm4.h - simple 4-axis stepper control
+#ifndef _SM4_H
+#define _SM4_H
+
+#include <inttypes.h>
+#include "config.h"
+
+
+#if defined(__cplusplus)
+extern "C" {
+#endif //defined(__cplusplus)
+
+
+// callback prototype for stop condition (return 0 - continue, return 1 - stop)
+typedef uint8_t (*sm4_stop_cb_t)();
+
+// callback prototype for updating position counters
+typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
+
+// callback prototype for calculating delay
+typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd);
+
+
+// callback pointer - stop
+extern sm4_stop_cb_t sm4_stop_cb;
+
+// callback pointer - update_pos
+extern sm4_update_pos_cb_t sm4_update_pos_cb;
+
+// callback pointer - calc_delay
+extern sm4_calc_delay_cb_t sm4_calc_delay_cb;
+
+
+// returns direction for single axis (0 - positive, 1 - negative)
+extern uint8_t sm4_get_dir(uint8_t axis);
+
+// set direction for single axis (0 - positive, 1 - negative)
+extern void sm4_set_dir(uint8_t axis, uint8_t dir);
+
+// returns direction of all axes as bitmask (0 - positive, 1 - negative)
+extern uint8_t sm4_get_dir_bits(void);
+
+// set direction for all axes as bitmask (0 - positive, 1 - negative)
+extern void sm4_set_dir_bits(uint8_t dir_bits);
+
+// step axes by bitmask
+extern void sm4_do_step(uint8_t axes_mask);
+
+// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped)
+extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
+extern uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz);
+
+
+#if defined(__cplusplus)
+}
+#endif //defined(__cplusplus)
+#endif //_SM4_H
diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index 973efb366..fed7f5c14 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -252,7 +252,7 @@ bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_
 	sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
 	xyzcal_sm4_delay = delay_us;
 	//	uint32_t u = _micros();
-	bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0) ? true : false;
+	bool ret = sm4_line_xyz_ui(abs(x), abs(y), abs(z)) ? true : false;
 	//	u = _micros() - u;
 	return ret;
 }
@@ -951,7 +951,6 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){
 	st_synchronize();
 	pos_i16_t x = _X;
 	pos_i16_t y = _Y;
-	pos_i16_t z = _Z;
 
 	xyzcal_meassure_enter();
 	if (xyzcal_searchZ()){