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

Marlin/Configuration_adv.h

   #define AUTOTEMP_OLDWEIGHT 0.98
 #endif
 
-// Show extra position information with 'M114 D'
-//#define M114_DETAIL
+// Extra options for the M114 "Current Position" report
+//#define M114_DETAIL         // Use 'M114` for details to check planner calculations
+//#define M114_REALTIME       // Real current position based on forward kinematics
+//#define M114_LEGACY         // M114 used to synchronize on every call. Enable if needed.
 
 // Show Temperature ADC value
 // Enable for M105 to include ADC values read from temperature sensors.

Marlin/src/core/types.h

   };
   FI void set(const T px)                               { x = px; }
   FI void set(const T px, const T py)                   { x = px; y = py; }
+  FI void set(const T (&arr)[XY])                       { x = arr[0]; y = arr[1]; }
+  FI void set(const T (&arr)[XYZ])                      { x = arr[0]; y = arr[1]; }
+  FI void set(const T (&arr)[XYZE])                     { x = arr[0]; y = arr[1]; }
+  #if XYZE_N > XYZE
+    FI void set(const T (&arr)[XYZE_N])                 { x = arr[0]; y = arr[1]; }
+  #endif
   FI void reset()                                       { x = y = 0; }
   FI T magnitude()                                const { return (T)sqrtf(x*x + y*y); }
   FI operator T* ()                                     { return pos; }
   FI XYval<int16_t>    asInt()                    const { return { int16_t(x), int16_t(y) }; }
   FI XYval<int32_t>   asLong()                          { return { int32_t(x), int32_t(y) }; }
   FI XYval<int32_t>   asLong()                    const { return { int32_t(x), int32_t(y) }; }
+  FI XYval<int32_t>   ROUNDL()                          { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
+  FI XYval<int32_t>   ROUNDL()                    const { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
   FI XYval<float>    asFloat()                          { return {   float(x),   float(y) }; }
   FI XYval<float>    asFloat()                    const { return {   float(x),   float(y) }; }
   FI XYval<float> reciprocal()                    const { return {  _RECIP(x),  _RECIP(y) }; }
   FI void set(const T px, const T py)                  { x = px; y = py; }
   FI void set(const T px, const T py, const T pz)      { x = px; y = py; z = pz; }
   FI void set(const XYval<T> pxy, const T pz)          { x = pxy.x; y = pxy.y; z = pz; }
+  FI void set(const T (&arr)[XY])                      { x = arr[0]; y = arr[1]; }
+  FI void set(const T (&arr)[XYZ])                     { x = arr[0]; y = arr[1]; z = arr[2]; }
+  FI void set(const T (&arr)[XYZE])                    { x = arr[0]; y = arr[1]; z = arr[2]; }
+  #if XYZE_N > XYZE
+    FI void set(const T (&arr)[XYZE_N])                { x = arr[0]; y = arr[1]; z = arr[2]; }
+  #endif
   FI void reset()                                      { x = y = z = 0; }
   FI T magnitude()                               const { return (T)sqrtf(x*x + y*y + z*z); }
   FI operator T* ()                                    { return pos; }
   FI XYZval<int16_t>   asInt()                   const { return { int16_t(x), int16_t(y), int16_t(z) }; }
   FI XYZval<int32_t>  asLong()                         { return { int32_t(x), int32_t(y), int32_t(z) }; }
   FI XYZval<int32_t>  asLong()                   const { return { int32_t(x), int32_t(y), int32_t(z) }; }
+  FI XYZval<int32_t>  ROUNDL()                         { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
+  FI XYZval<int32_t>  ROUNDL()                   const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
   FI XYZval<float>   asFloat()                         { return {   float(x),   float(y),   float(z) }; }
   FI XYZval<float>   asFloat()                   const { return {   float(x),   float(y),   float(z) }; }
   FI XYZval<float> reciprocal()                  const { return {  _RECIP(x),  _RECIP(y),  _RECIP(z) }; }
   FI void set(const XYval<T> pxy, const T pz, const T pe)     { x = pxy.x;  y = pxy.y;  z = pz;     e = pe;    }
   FI void set(const XYval<T> pxy, const XYval<T> pze)         { x = pxy.x;  y = pxy.y;  z = pze.z;  e = pze.e; }
   FI void set(const XYZval<T> pxyz, const T pe)               { x = pxyz.x; y = pxyz.y; z = pxyz.z; e = pe;    }
+  FI void set(const T (&arr)[XY])                             { x = arr[0]; y = arr[1]; }
+  FI void set(const T (&arr)[XYZ])                            { x = arr[0]; y = arr[1]; z = arr[2]; }
+  FI void set(const T (&arr)[XYZE])                           { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
+  #if XYZE_N > XYZE
+    FI void set(const T (&arr)[XYZE_N])                       { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
+  #endif
   FI XYZEval<T>          copy()                         const { return *this; }
   FI XYZEval<T>           ABS()                         const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(e)) }; }
   FI XYZEval<int16_t>   asInt()                               { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
   FI XYZEval<int16_t>   asInt()                         const { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
-  FI XYZEval<int32_t>  asLong()                         const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
   FI XYZEval<int32_t>  asLong()                               { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
+  FI XYZEval<int32_t>  asLong()                         const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
+  FI XYZEval<int32_t>  ROUNDL()                               { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
+  FI XYZEval<int32_t>  ROUNDL()                         const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
   FI XYZEval<float>   asFloat()                               { return {   float(x),   float(y),   float(z),   float(e) }; }
   FI XYZEval<float>   asFloat()                         const { return {   float(x),   float(y),   float(z),   float(e) }; }
   FI XYZEval<float> reciprocal()                        const { return {  _RECIP(x),  _RECIP(y),  _RECIP(z),  _RECIP(e) }; }

Marlin/src/gcode/host/M114.cpp

     #include "../../core/debug_out.h"
   #endif
 
-  void report_xyze(const xyze_pos_t &pos, const uint8_t n=4, const uint8_t precision=3) {
+  void report_xyze(const xyze_pos_t &pos, const uint8_t n=XYZE, const uint8_t precision=3) {
     char str[12];
     for (uint8_t a = 0; a < n; a++) {
       SERIAL_CHAR(' ', axis_codes[a], ':');
     }
     SERIAL_EOL();
   }
+  inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }
 
   void report_xyz(const xyz_pos_t &pos, const uint8_t precision=3) {
     char str[12];
     }
     SERIAL_EOL();
   }
-  inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }
 
   void report_current_position_detail() {
 
+    // Position as sent by G-code
     SERIAL_ECHOPGM("\nLogical:");
     report_xyz(current_position.asLogical());
 
+    // Cartesian position in native machine space
     SERIAL_ECHOPGM("Raw:    ");
     report_xyz(current_position);
 
     xyze_pos_t leveled = current_position;
 
     #if HAS_LEVELING
+      // Current position with leveling applied
       SERIAL_ECHOPGM("Leveled:");
       planner.apply_leveling(leveled);
       report_xyz(leveled);
 
+      // Test planner un-leveling. This should match the Raw result.
       SERIAL_ECHOPGM("UnLevel:");
       xyze_pos_t unleveled = leveled;
       planner.unapply_leveling(unleveled);
     #endif
 
     #if IS_KINEMATIC
+      // Kinematics applied to the leveled position
       #if IS_SCARA
         SERIAL_ECHOPGM("ScaraK: ");
       #else
 #endif // M114_DETAIL
 
 /**
- * M114: Report current position to host
+ * M114: Report the current position to host.
+ *       Since steppers are moving, the count positions are
+ *       projected by using planner calculations.
+ *   D - Report more detail. This syncs the planner. (Requires M114_DETAIL)
+ *   E - Report E stepper position (Requires M114_DETAIL)
+ *   R - Report the realtime position instead of projected.
  */
 void GcodeSuite::M114() {
 
   #if ENABLED(M114_DETAIL)
     if (parser.seen('D')) {
+      #if DISABLED(M114_LEGACY)
+        planner.synchronize();
+      #endif
+      report_current_position();
       report_current_position_detail();
       return;
     }
     }
   #endif
 
-  planner.synchronize();
-  report_current_position();
+  #if ENABLED(M114_REALTIME)
+    if (parser.seen('R')) { report_real_position(); return; }
+  #endif
+
+  #if ENABLED(M114_LEGACY)
+    planner.synchronize();
+  #endif
+  report_current_position_projected();
 }

Marlin/src/module/motion.cpp

 /**
  * 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
  *
  */
 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(
 
 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
     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

   #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

       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
   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
      */
     static xyze_float_t previous_speed;
      */
     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

   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__
     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

     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