diff --git a/Marlin/src/gcode/geometry/G92.cpp b/Marlin/src/gcode/geometry/G92.cpp index 0b10aab5b8..933a6e1cdb 100644 --- a/Marlin/src/gcode/geometry/G92.cpp +++ b/Marlin/src/gcode/geometry/G92.cpp @@ -33,8 +33,6 @@ */ void GcodeSuite::G92() { - stepper.synchronize(); - #if ENABLED(CNC_COORDINATE_SYSTEMS) switch (parser.subcode) { case 1: @@ -94,10 +92,8 @@ void GcodeSuite::G92() { COPY(coordinate_system[active_coordinate_system], position_shift); #endif - if (didXYZ) - SYNC_PLAN_POSITION_KINEMATIC(); - else if (didE) - sync_plan_position_e(); + if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC(); + else if (didE) sync_plan_position_e(); report_current_position(); } diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 8aa08cd54b..4be4b2af59 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1382,15 +1382,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] const float esteps_float = de * e_factor[extruder]; const int32_t esteps = abs(esteps_float) + 0.5; - // Calculate the buffer head after we push this byte - const uint8_t next_buffer_head = next_block_index(block_buffer_head); - - // If the buffer is full: good! That means we are well ahead of the robot. - // Rest here until there is room in the buffer. - while (block_buffer_tail == next_buffer_head) idle(); - - // Prepare to set up new block - block_t* block = &block_buffer[block_buffer_head]; + // Wait for the next available block + uint8_t next_buffer_head; + block_t * const block = get_next_free_block(next_buffer_head); // Clear all flags, including the "busy" bit block->flag = 0x00; @@ -2032,6 +2026,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] } // _buffer_steps() +/** + * Planner::buffer_sync_block + * Add a block to the buffer that just updates the position + */ +void Planner::buffer_sync_block() { + // Wait for the next available block + uint8_t next_buffer_head; + block_t * const block = get_next_free_block(next_buffer_head); + + block->steps[A_AXIS] = position[A_AXIS]; + block->steps[B_AXIS] = position[B_AXIS]; + block->steps[C_AXIS] = position[C_AXIS]; + block->steps[E_AXIS] = position[E_AXIS]; + + block->flag = BLOCK_FLAG_SYNC_POSITION; + + block_buffer_head = next_buffer_head; + stepper.wake_up(); +} // buffer_sync_block() + /** * Planner::buffer_segment * @@ -2160,19 +2174,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c #else #define _EINDEX E_AXIS #endif - const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), - nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), - nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), - ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); + position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), + position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), + position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), + position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); #if HAS_POSITION_FLOAT - position_float[X_AXIS] = a; - position_float[Y_AXIS] = b; - position_float[Z_AXIS] = c; + position_float[A_AXIS] = a; + position_float[B_AXIS] = b; + position_float[C_AXIS] = c; position_float[E_AXIS] = e; #endif - stepper.set_position(na, nb, nc, ne); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. ZERO(previous_speed); + buffer_sync_block(); } void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { @@ -2220,23 +2234,23 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) { #if HAS_POSITION_FLOAT position_float[axis] = v; #endif - stepper.set_position(axis, position[axis]); previous_speed[axis] = 0.0; + buffer_sync_block(); } // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2 void Planner::reset_acceleration_rates() { #if ENABLED(DISTINCT_E_FACTORS) - #define HIGHEST_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder) + #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder) #else - #define HIGHEST_CONDITION true + #define AXIS_CONDITION true #endif uint32_t highest_rate = 1; LOOP_XYZE_N(i) { max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i]; - if (HIGHEST_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]); + if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]); } - cutoff_long = 4294967295UL / highest_rate; + cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL } // Recalculate position, steps_to_mm if axis_steps_per_mm changes! diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 031cee7451..eceb31f3a1 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -57,14 +57,18 @@ enum BlockFlagBit : char { BLOCK_BIT_BUSY, // The block is segment 2+ of a longer move - BLOCK_BIT_CONTINUED + BLOCK_BIT_CONTINUED, + + // Sync the stepper counts from the block + BLOCK_BIT_SYNC_POSITION }; enum BlockFlag : char { BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE), BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH), BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY), - BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED) + BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED), + BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION) }; /** @@ -422,6 +426,20 @@ class Planner { #endif + + /** + * Planner::get_next_free_block + * + * - Get the next head index (passed by reference) + * - Wait for a space to open up in the planner + * - Return the head block + */ + FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) { + next_buffer_head = next_block_index(block_buffer_head); + while (block_buffer_tail == next_buffer_head) idle(); // while (is_full) + return &block_buffer[block_buffer_head]; + } + /** * Planner::_buffer_steps * @@ -439,6 +457,12 @@ class Planner { , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 ); + /** + * Planner::buffer_sync_block + * Add a block to the buffer that just updates the position + */ + static void buffer_sync_block(); + /** * Planner::buffer_segment * @@ -518,7 +542,7 @@ class Planner { static void set_position_mm_kinematic(const float (&cart)[XYZE]); static void set_position_mm(const AxisEnum axis, const float &v); FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } - FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); } + FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); } /** * Sync from the stepper positions. (e.g., after an interrupted move) @@ -528,7 +552,7 @@ class Planner { /** * Does the buffer have any blocks queued? */ - static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } + FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } /** * "Discard" the block and "release" the memory. diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index ec547c5980..4e6747a124 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1217,6 +1217,16 @@ void Stepper::isr() { // Anything in the buffer? if ((current_block = planner.get_current_block())) { + // Sync block? Sync the stepper counts and return + while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) { + _set_position( + current_block->steps[A_AXIS], current_block->steps[B_AXIS], + current_block->steps[C_AXIS], current_block->steps[E_AXIS] + ); + planner.discard_current_block(); + if (!(current_block = planner.get_current_block())) return; + } + // Initialize the trapezoid generator from the current block. static int8_t last_extruder = -1; @@ -1976,12 +1986,7 @@ void Stepper::synchronize() { while (planner.has_blocks_queued() || cleaning_buf * This allows get_axis_position_mm to correctly * derive the current XYZ position later on. */ -void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { - - synchronize(); // Bad to set stepper counts in the middle of a move - - CRITICAL_SECTION_START; - +void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { #if CORE_IS_XY // corexy positioning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html @@ -2004,21 +2009,7 @@ void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, count_position[Y_AXIS] = b; count_position[Z_AXIS] = c; #endif - count_position[E_AXIS] = e; - CRITICAL_SECTION_END; -} - -void Stepper::set_position(const AxisEnum &axis, const int32_t &v) { - CRITICAL_SECTION_START; - count_position[axis] = v; - CRITICAL_SECTION_END; -} - -void Stepper::set_e_position(const int32_t &e) { - CRITICAL_SECTION_START; - count_position[E_AXIS] = e; - CRITICAL_SECTION_END; } /** diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 5fad6a8f7a..19bfeec685 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -191,9 +191,32 @@ class Stepper { // // Set the current position in steps // - static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); - static void set_position(const AxisEnum &a, const int32_t &v); - static void set_e_position(const int32_t &e); + static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); + + FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; } + + FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { + synchronize(); + CRITICAL_SECTION_START; + _set_position(a, b, c, e); + CRITICAL_SECTION_END; + } + + static void set_position(const AxisEnum a, const int32_t &v) { + synchronize(); + CRITICAL_SECTION_START; + count_position[a] = v; + CRITICAL_SECTION_END; + } + + FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; } + + static void set_e_position(const int32_t &e) { + synchronize(); + CRITICAL_SECTION_START; + count_position[E_AXIS] = e; + CRITICAL_SECTION_END; + } // // Set direction bits for all steppers