diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 03173f67ef6e9851027b90792a016d78a81bf5e2..10403073550b8b0f14bf38f4a904a1f3276926ba 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -276,8 +276,10 @@
#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.
diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h
index c56ce41895e9b06cf33432e6b452a43fb4b48844..acab36ec5cad0788cfe70f584bb201a9d4867779 100644
--- a/Marlin/src/core/types.h
+++ b/Marlin/src/core/types.h
@@ -187,6 +187,12 @@ struct XYval {
};
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; }
@@ -197,6 +203,8 @@ struct XYval {
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) }; }
@@ -290,6 +298,12 @@ struct XYZval {
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; }
@@ -300,6 +314,8 @@ struct XYZval {
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) }; }
@@ -397,12 +413,20 @@ struct XYZEval {
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) }; }
diff --git a/Marlin/src/gcode/host/M114.cpp b/Marlin/src/gcode/host/M114.cpp
index fc67f762d24ec8606323493f662f96f1cf294c0c..1a8655135bc35d6d211824a6ab909a9bbcf137ab 100644
--- a/Marlin/src/gcode/host/M114.cpp
+++ b/Marlin/src/gcode/host/M114.cpp
@@ -34,7 +34,7 @@
#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], ':');
@@ -42,6 +42,7 @@
}
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];
@@ -51,23 +52,26 @@
}
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);
@@ -75,6 +79,7 @@
#endif
#if IS_KINEMATIC
+ // Kinematics applied to the leveled position
#if IS_SCARA
SERIAL_ECHOPGM("ScaraK: ");
#else
@@ -180,12 +185,21 @@
#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;
}
@@ -195,6 +209,12 @@ void GcodeSuite::M114() {
}
#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();
}
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index ac0909fcc1255c313ddff01b74fe19087b41092c..1ce225cae5829e2d172687c70a192a838a14788f 100644
--- a/Marlin/src/module/motion.cpp
+++ b/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;
diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
index 8e0eee7e33c3d0f96c87e5fa60127df07227f9e3..055c6eeecde2f2908c62c99fd3d79cab3c025b38 100644
--- a/Marlin/src/module/motion.h
+++ b/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);
diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h
index 6942ec6028764b03e9f93efdc2649ae275485796..3e5d9d902eeaa86f715d7688646b284aa47450b2 100644
--- a/Marlin/src/module/planner.h
+++ b/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); }
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index d4bbe36f1340e6a14afcd87c15783fd309753bf5..dacef0f2cb6ea8dbc83e20e595545f48baf07140 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/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)
diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
index 6671b946dfcfd53a13d349ab8b1f5099cf547704..46c6c1c16a78c8ef3a23aa91a2bc9627f6373beb 100644
--- a/Marlin/src/module/stepper.h
+++ b/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