diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index ede84af9..1786c63f 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -62,8 +62,8 @@ class CartKinematics:
             # Set final homed position
             coord[axis] = s.position_endstop + s.get_homed_offset()
             homing_state.set_homed_position(coord)
-    def query_endstops(self, print_time):
-        return homing.query_endstops(print_time, self.steppers)
+    def query_endstops(self, print_time, query_flags):
+        return homing.query_endstops(print_time, query_flags, self.steppers)
     def motor_off(self, print_time):
         self.limits = [(1.0, -1.0)] * 3
         for stepper in self.steppers:
diff --git a/klippy/corexy.py b/klippy/corexy.py
index bde541b2..a730e933 100644
--- a/klippy/corexy.py
+++ b/klippy/corexy.py
@@ -67,8 +67,8 @@ class CoreXYKinematics:
                 # Support endstop phase detection on Z axis
                 coord[axis] = s.position_endstop + s.get_homed_offset()
                 homing_state.set_homed_position(coord)
-    def query_endstops(self, print_time):
-        return homing.query_endstops(print_time, self.steppers)
+    def query_endstops(self, print_time, query_flags):
+        return homing.query_endstops(print_time, query_flags, self.steppers)
     def motor_off(self, print_time):
         self.limits = [(1.0, -1.0)] * 3
         for stepper in self.steppers:
diff --git a/klippy/delta.py b/klippy/delta.py
index 8e7d4b20..e3ec0123 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -123,8 +123,8 @@ class DeltaKinematics:
                 + self.steppers[i].get_homed_offset()
                 for i in StepList]
         homing_state.set_homed_position(self._actuator_to_cartesian(spos))
-    def query_endstops(self, print_time):
-        return homing.query_endstops(print_time, self.steppers)
+    def query_endstops(self, print_time, query_flags):
+        return homing.query_endstops(print_time, query_flags, self.steppers)
     def motor_off(self, print_time):
         self.limit_xy2 = -1.
         for stepper in self.steppers:
diff --git a/klippy/gcode.py b/klippy/gcode.py
index 9d58e7af..a267c85d 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -403,11 +403,11 @@ class GCodeParser:
         if self.toolhead is None:
             self.cmd_default(params)
             return
-        kinpos = self.toolhead.get_position()
-        self.respond("X:%.3f Y:%.3f Z:%.3f E:%.3f Count X:%.3f Y:%.3f Z:%.3f" % (
+        raw_pos = self.toolhead.query_endstops("get_mcu_position")
+        self.respond("X:%.3f Y:%.3f Z:%.3f E:%.3f Count %s" % (
             self.last_position[0], self.last_position[1],
             self.last_position[2], self.last_position[3],
-            kinpos[0], kinpos[1], kinpos[2]))
+            " ".join(["%s:%d" % (n.upper(), p) for n, p in raw_pos])))
     cmd_M115_when_not_ready = True
     def cmd_M115(self, params):
         # Get Firmware Version and Capabilities
diff --git a/klippy/homing.py b/klippy/homing.py
index 9fd816fd..21d3e225 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -57,7 +57,11 @@ class Homing:
     def set_homed_position(self, pos):
         self.toolhead.set_position(self._fill_coord(pos))
 
-def query_endstops(print_time, steppers):
+def query_endstops(print_time, query_flags, steppers):
+    if query_flags == "get_mcu_position":
+        # Only the commanded position is requested
+        return [(s.name.upper(), s.mcu_stepper.get_mcu_position())
+                for s in steppers]
     for s in steppers:
         s.mcu_endstop.query_endstop(print_time)
     out = []
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 676364dd..6ed6099e 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -349,9 +349,9 @@ class ToolHead:
         while (not self.sync_print_time
                or self.print_time >= self.mcu.estimated_print_time(eventtime)):
             eventtime = self.reactor.pause(eventtime + 0.100)
-    def query_endstops(self):
+    def query_endstops(self, query_flags=""):
         last_move_time = self.get_last_move_time()
-        return self.kin.query_endstops(last_move_time)
+        return self.kin.query_endstops(last_move_time, query_flags)
     def set_extruder(self, extruder):
         last_move_time = self.get_last_move_time()
         self.extruder.set_active(last_move_time, False)