diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index 3794eb4c4..2ba5d68c2 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -416,11 +416,19 @@ void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){
 }
 
 void update_position_1_step(const uint8_t axis, const uint8_t dir) {
-	count_position[axis] += dir & _BV(axis) ? -1L : 1L;
+	for (uint8_t i = X_AXIS, mask = X_AXIS_MASK; i <= Z_AXIS; i++, mask <<= 1) {
+		if (axis & mask) {
+			count_position[i] += dir & mask ? -1L : 1L;
+		}
+	}
 }
 
 void __attribute__((noinline)) set_axes_dir(const uint8_t axis, const uint8_t dir) {
-	sm4_set_dir(axis, dir & _BV(axis));
+	for (uint8_t i = X_AXIS, mask = X_AXIS_MASK; i <= Z_AXIS; i++, mask <<= 1) {
+		if (axis & mask) {
+			sm4_set_dir(i, dir & mask);
+		}
+	}
 }
 
 /// Accelerate up to max.speed (defined by @min_delay_us)