gcode: Position returned by M114 should be relative to last G92

It looks like OctoPrint is expecting the result from M114 to be
relative to the last G92 command.  Also, introduce GET_POSITION to
report the actual location that the printer is at.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

3 files changed, 42 insertions, 17 deletions

diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index e2c7ea2c..9e9cd8fe 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -63,6 +63,8 @@ The following standard commands are supported:
 - `QUERY_ENDSTOPS`: Probe the axis endstops and report if they are
   "triggered" or in an "open" state. This command is typically used to
   verify that an endstop is working correctly.
+- `GET_POSITION`: Return information on the current location of the
+  toolhead.
 - `RESTART`: This will cause the host software to reload its config
   and perform an internal reset. This command will not clear error
   state from the micro-controller (see FIRMWARE_RESTART) nor will it
diff --git a/klippy/gcode.py b/klippy/gcode.py
index 4c1e4c83..dee03a16 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -367,9 +367,9 @@ class GCodeParser:
         self.run_script(self.extruder.get_activate_gcode(True))
     all_handlers = [
         'G1', 'G4', 'G28', 'M18', 'M400',
-        'G20', 'M82', 'M83', 'G90', 'G91', 'G92', 'M206', 'M220', 'M221',
+        'G20', 'M82', 'M83', 'G90', 'G91', 'G92', 'M114', 'M206', 'M220', 'M221',
         'M105', 'M104', 'M109', 'M140', 'M190', 'M106', 'M107',
-        'M112', 'M114', 'M115', 'IGNORE', 'QUERY_ENDSTOPS', 'PID_TUNE',
+        'M112', 'M115', 'IGNORE', 'QUERY_ENDSTOPS', 'GET_POSITION', 'PID_TUNE',
         'RESTART', 'FIRMWARE_RESTART', 'ECHO', 'STATUS', 'HELP']
     # G-Code movement commands
     cmd_G1_aliases = ['G0']
@@ -463,6 +463,12 @@ class GCodeParser:
             self.base_position[p] = self.last_position[p] - offset
         if not offsets:
             self.base_position = list(self.last_position)
+    cmd_M114_when_not_ready = True
+    def cmd_M114(self, params):
+        # Get Current Position
+        p = [lp - bp for lp, bp in zip(self.last_position, self.base_position)]
+        p[3] /= self.extrude_factor
+        self.respond("X:%.3f Y:%.3f Z:%.3f E:%.3f" % tuple(p))
     def cmd_M206(self, params):
         # Set home offset
         offsets = { self.axis2pos[a]: self.get_float(a, params)
@@ -513,17 +519,6 @@ class GCodeParser:
     def cmd_M112(self, params):
         # Emergency Stop
         self.printer.invoke_shutdown("Shutdown due to M112 command")
-    cmd_M114_when_not_ready = True
-    def cmd_M114(self, params):
-        # Get Current Position
-        if self.toolhead is None:
-            self.cmd_default(params)
-            return
-        raw_pos = homing.query_position(self.toolhead)
-        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],
-            " ".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
@@ -542,6 +537,38 @@ class GCodeParser:
         res = homing.query_endstops(self.toolhead)
         self.respond(" ".join(["%s:%s" % (name, ["open", "TRIGGERED"][not not t])
                                for name, t in res]))
+    cmd_GET_POSITION_when_not_ready = True
+    def cmd_GET_POSITION(self, params):
+        if self.toolhead is None:
+            self.cmd_default(params)
+            return
+        kin = self.toolhead.get_kinematics()
+        steppers = kin.get_steppers()
+        mcu_pos = " ".join(["%s:%d" % (s.name, s.mcu_stepper.get_mcu_position())
+                            for s in steppers])
+        stepper_pos = " ".join(
+            ["%s:%.6f" % (s.name, s.mcu_stepper.get_commanded_position())
+             for s in steppers])
+        kinematic_pos = " ".join(["%s:%.6f"  % (a, v)
+                                  for a, v in zip("XYZE", kin.get_position())])
+        toolhead_pos = " ".join(["%s:%.6f" % (a, v) for a, v in zip(
+            "XYZE", self.toolhead.get_position())])
+        gcode_pos = " ".join(["%s:%.6f"  % (a, v)
+                              for a, v in zip("XYZE", self.last_position)])
+        origin_pos = " ".join(["%s:%.6f"  % (a, v)
+                               for a, v in zip("XYZE", self.base_position)])
+        homing_pos = " ".join(["%s:%.6f"  % (a, v)
+                               for a, v in zip("XYZE", self.homing_add)])
+        self.respond_info(
+            "mcu: %s\n"
+            "stepper: %s\n"
+            "kinematic: %s\n"
+            "toolhead: %s\n"
+            "gcode: %s\n"
+            "gcode origin: %s\n"
+            "gcode homing: %s" % (
+                mcu_pos, stepper_pos, kinematic_pos, toolhead_pos,
+                gcode_pos, origin_pos, homing_pos))
     cmd_PID_TUNE_help = "Run PID Tuning"
     cmd_PID_TUNE_aliases = ["M303"]
     def cmd_PID_TUNE(self, params):
diff --git a/klippy/homing.py b/klippy/homing.py
index 6bd11304..97879730 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -123,10 +123,6 @@ def query_endstops(toolhead):
             out.append((name, mcu_endstop.query_endstop_wait()))
     return out
 
-def query_position(toolhead):
-    steppers = toolhead.get_kinematics().get_steppers()
-    return [(s.name.upper(), s.mcu_stepper.get_mcu_position()) for s in steppers]
-
 class EndstopError(Exception):
     pass