Asynchronous M114 and (R)ealtime position option (#17032)

Marlin/src/module/motion.cpp

@@ -206,17 +206,53 @@ xyz_pos_t cartes;
 /**
  * Output the current position to serial
  */
-void report_current_position() {
-  const xyz_pos_t lpos = current_position.asLogical();
-  SERIAL_ECHOPAIR("X:", lpos.x, " Y:", lpos.y, " Z:", lpos.z, " E:", current_position.e);
 
+inline void report_more_positions() {
   stepper.report_positions();
-
   #if IS_SCARA
     scara_report_positions();
   #endif
 }
 
+// Report the logical position for a given machine position
+inline void report_logical_position(const xyze_pos_t &rpos) {
+  const xyze_pos_t lpos = rpos.asLogical();
+  SERIAL_ECHOPAIR_P(X_LBL, lpos.x, SP_Y_LBL, lpos.y, SP_Z_LBL, lpos.z, SP_E_LBL, lpos.e);
+  report_more_positions();
+}
+
+// Report the real current position according to the steppers.
+// Forward kinematics and un-leveling are applied.
+void report_real_position() {
+  get_cartesian_from_steppers();
+  xyze_pos_t npos = cartes;
+  npos.e = planner.get_axis_position_mm(E_AXIS);
+
+  #if HAS_POSITION_MODIFIERS
+    planner.unapply_modifiers(npos
+      #if HAS_LEVELING
+        , true
+      #endif
+    );
+  #endif
+
+  report_logical_position(npos);
+}
+
+// Report the logical current position according to the most recent G-code command
+void report_current_position() { report_logical_position(current_position); }
+
+/**
+ * Report the logical current position according to the most recent G-code command.
+ * The planner.position always corresponds to the last G-code too. This makes M114
+ * suitable for debugging kinematics and leveling while avoiding planner sync that
+ * definitively interrupts the printing flow.
+ */
+void report_current_position_projected() {
+  report_logical_position(current_position);
+  stepper.report_a_position(planner.position);
+}
+
 /**
  * sync_plan_position
  *
@@ -241,11 +277,7 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
  */
 void get_cartesian_from_steppers() {
   #if ENABLED(DELTA)
-    forward_kinematics_DELTA(
-      planner.get_axis_position_mm(A_AXIS),
-      planner.get_axis_position_mm(B_AXIS),
-      planner.get_axis_position_mm(C_AXIS)
-    );
+    forward_kinematics_DELTA(planner.get_axis_positions_mm());
   #else
     #if IS_SCARA
       forward_kinematics_SCARA(
@@ -663,11 +695,11 @@ void restore_feedrate_and_scaling() {
 
 FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
   const millis_t ms = millis();
-  thermalManager.manage_heater();  // This returns immediately if not really needed.
   if (ELAPSED(ms, next_idle_ms)) {
     next_idle_ms = ms + 200UL;
-    idle();
+    return idle();
   }
+  thermalManager.manage_heater();  // Returns immediately on most calls
 }
 
 #if IS_KINEMATIC
@@ -1324,7 +1356,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
     current_position[axis] = distance;
     line_to_current_position(real_fr_mm_s);
   #else
-    abce_pos_t target = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) };
+    abce_pos_t target = planner.get_axis_positions_mm();
     target[axis] = 0;
     planner.set_machine_position_mm(target);
     target[axis] = distance;

Marlin/src/module/motion.h

@@ -162,7 +162,9 @@ typedef struct { xyz_pos_t min, max; } axis_limits_t;
   #define update_software_endstops(...) NOOP
 #endif
 
+void report_real_position();
 void report_current_position();
+void report_current_position_projected();
 
 void get_cartesian_from_steppers();
 void set_current_from_steppers_for_axis(const AxisEnum axis);

Marlin/src/module/planner.h

@@ -289,6 +289,12 @@ class Planner {
       static float extruder_advance_K[EXTRUDERS];
     #endif
 
+    /**
+     * The current position of the tool in absolute steps
+     * Recalculated if any axis_steps_per_mm are changed by gcode
+     */
+    static xyze_long_t position;
+
     #if HAS_POSITION_FLOAT
       static xyze_pos_t position_float;
     #endif
@@ -305,12 +311,6 @@ class Planner {
 
   private:
 
-    /**
-     * The current position of the tool in absolute steps
-     * Recalculated if any axis_steps_per_mm are changed by gcode
-     */
-    static xyze_long_t position;
-
     /**
      * Speed of previous path line segment
      */
@@ -725,6 +725,16 @@ class Planner {
      */
     static float get_axis_position_mm(const AxisEnum axis);
 
+    static inline abce_pos_t get_axis_positions_mm() {
+      const abce_pos_t out = {
+        get_axis_position_mm(A_AXIS),
+        get_axis_position_mm(B_AXIS),
+        get_axis_position_mm(C_AXIS),
+        get_axis_position_mm(E_AXIS)
+      };
+      return out;
+    }
+
     // SCARA AB axes are in degrees, not mm
     #if IS_SCARA
       FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }

Marlin/src/module/stepper.cpp

@@ -2448,6 +2448,19 @@ int32_t Stepper::triggered_position(const AxisEnum axis) {
   return v;
 }
 
+void Stepper::report_a_position(const xyz_long_t &pos) {
+  #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
+    SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
+  #else
+    SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
+  #endif
+  #if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
+    SERIAL_ECHOLNPAIR(" C:", pos.z);
+  #else
+    SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
+  #endif
+}
+
 void Stepper::report_positions() {
 
   #ifdef __AVR__
@@ -2461,16 +2474,7 @@ void Stepper::report_positions() {
     if (was_enabled) wake_up();
   #endif
 
-  #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
-    SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
-  #else
-    SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
-  #endif
-  #if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
-    SERIAL_ECHOLNPAIR(" C:", pos.z);
-  #else
-    SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
-  #endif
+  report_a_position(pos);
 }
 
 #if ENABLED(BABYSTEPPING)

Marlin/src/module/stepper.h

@@ -411,6 +411,7 @@ class Stepper {
     static void set_axis_position(const AxisEnum a, const int32_t &v);
 
     // Report the positions of the steppers, in steps
+    static void report_a_position(const xyz_long_t &pos);
     static void report_positions();
 
     // Quickly stop all steppers