From 3a07b4412d24a6afd430f6d7e1b8c7f3c442a3c2 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 2 Mar 2020 21:52:53 -0600 Subject: [PATCH] Asynchronous M114 and (R)ealtime position option (#17032) --- Marlin/Configuration_adv.h | 6 ++-- Marlin/src/core/types.h | 26 +++++++++++++++- Marlin/src/gcode/host/M114.cpp | 30 +++++++++++++++--- Marlin/src/module/motion.cpp | 56 ++++++++++++++++++++++++++-------- Marlin/src/module/motion.h | 2 ++ Marlin/src/module/planner.h | 22 +++++++++---- Marlin/src/module/stepper.cpp | 24 +++++++++------ Marlin/src/module/stepper.h | 1 + 8 files changed, 131 insertions(+), 36 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 03173f67ef..1040307355 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 c56ce41895..acab36ec5c 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 asInt() const { return { int16_t(x), int16_t(y) }; } FI XYval asLong() { return { int32_t(x), int32_t(y) }; } FI XYval asLong() const { return { int32_t(x), int32_t(y) }; } + FI XYval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; } + FI XYval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; } FI XYval asFloat() { return { float(x), float(y) }; } FI XYval asFloat() const { return { float(x), float(y) }; } FI XYval 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 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 asInt() const { return { int16_t(x), int16_t(y), int16_t(z) }; } FI XYZval asLong() { return { int32_t(x), int32_t(y), int32_t(z) }; } FI XYZval asLong() const { return { int32_t(x), int32_t(y), int32_t(z) }; } + FI XYZval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; } + FI XYZval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; } FI XYZval asFloat() { return { float(x), float(y), float(z) }; } FI XYZval asFloat() const { return { float(x), float(y), float(z) }; } FI XYZval reciprocal() const { return { _RECIP(x), _RECIP(y), _RECIP(z) }; } @@ -397,12 +413,20 @@ struct XYZEval { FI void set(const XYval pxy, const T pz, const T pe) { x = pxy.x; y = pxy.y; z = pz; e = pe; } FI void set(const XYval pxy, const XYval pze) { x = pxy.x; y = pxy.y; z = pze.z; e = pze.e; } FI void set(const XYZval 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 copy() const { return *this; } FI XYZEval ABS() const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(e)) }; } FI XYZEval asInt() { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; } FI XYZEval asInt() const { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; } - FI XYZEval asLong() const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; } FI XYZEval asLong() { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; } + FI XYZEval asLong() const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; } + FI XYZEval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; } + FI XYZEval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; } FI XYZEval asFloat() { return { float(x), float(y), float(z), float(e) }; } FI XYZEval asFloat() const { return { float(x), float(y), float(z), float(e) }; } FI XYZEval 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 fc67f762d2..1a8655135b 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 ac0909fcc1..1ce225cae5 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 8e0eee7e33..055c6eeecd 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 6942ec6028..3e5d9d902e 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 d4bbe36f13..dacef0f2cb 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 6671b946df..46c6c1c16a 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