diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0e737f2520852d0185314946ef26fb38310f6598..3791aae687da645335b505df40a756d2ed8a36a4 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -6258,8 +6258,6 @@ void home_all_axes() { gcode_G28(true); }
*/
inline void gcode_G92() {
- stepper.synchronize();
-
#if ENABLED(CNC_COORDINATE_SYSTEMS)
switch (parser.subcode) {
case 1:
@@ -6319,10 +6317,9 @@ inline void gcode_G92() {
COPY(coordinate_system[active_coordinate_system], position_shift);
#endif
- if (didXYZ)
- SYNC_PLAN_POSITION_KINEMATIC();
- else if (didE)
- sync_plan_position_e();
+ // Update planner/steppers only if the native coordinates changed
+ if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
+ else if (didE) sync_plan_position_e();
report_current_position();
}
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 6e2ecb04e5231417683b1a2facf4a770a91ee96e..8e5733a6d37ff4f49710e491096fe0264e59ca93 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -819,15 +819,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;
@@ -1467,6 +1461,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
*
@@ -1595,19 +1609,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]) {
@@ -1655,23 +1669,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/planner.h b/Marlin/planner.h
index af845600a541596f465f43e54d61e15da0dbb9e7..e051c4a0dcef101f3b129bdf79507d3f614c6bcf 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -53,14 +53,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)
};
/**
@@ -409,6 +413,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
*
@@ -426,6 +444,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
*
@@ -505,7 +529,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)
@@ -515,7 +539,7 @@ class Planner {
/**
* Does the buffer have any blocks queued?
*/
- static inline 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/stepper.cpp b/Marlin/stepper.cpp
index 46707b5737f84812c89c13494b305e2076819b52..06dc29dc181f6eac49110b08045409d3defe4786 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -449,18 +449,30 @@ void Stepper::isr() {
// If there is no current block, attempt to pop one from the buffer
if (!current_block) {
+
// 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;
+ }
+
trapezoid_generator_reset();
// Initialize Bresenham counters to 1/2 the ceiling
counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
-
#if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(i)
counter_m[i] = -(current_block->mix_event_count[i] >> 1);
#endif
+ // No step events completed so far
step_events_completed = 0;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
@@ -469,6 +481,7 @@ void Stepper::isr() {
#endif
#if ENABLED(Z_LATE_ENABLE)
+ // If delayed Z enable, postpone move for 1mS
if (current_block->steps[Z_AXIS] > 0) {
enable_Z();
_NEXT_ISR(2000); // Run at slow speed - 1 KHz
@@ -595,7 +608,6 @@ void Stepper::isr() {
#endif
#if ENABLED(LIN_ADVANCE)
-
counter_E += current_block->steps[E_AXIS];
if (counter_E > 0) {
#if DISABLED(MIXING_EXTRUDER)
@@ -708,7 +720,6 @@ void Stepper::isr() {
acceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
-
if (current_block->use_advance_lead) {
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
@@ -719,7 +730,6 @@ void Stepper::isr() {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
-
#endif // LIN_ADVANCE
}
else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
@@ -742,7 +752,6 @@ void Stepper::isr() {
deceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
-
if (current_block->use_advance_lead) {
if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
@@ -753,16 +762,13 @@ void Stepper::isr() {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
-
#endif // LIN_ADVANCE
}
else {
#if ENABLED(LIN_ADVANCE)
-
// If we have esteps to execute, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
-
#endif
SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
@@ -902,6 +908,7 @@ void Stepper::isr() {
}
void Stepper::advance_isr_scheduler() {
+
// Run main stepping ISR if flagged
if (!nextMainISR) isr();
@@ -1120,12 +1127,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
@@ -1148,21 +1150,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/stepper.h b/Marlin/stepper.h
index cdd8a3b161497bb5b90ad19069feefd2b08c4579..597600e805fbf0e8b24f163f351bf05ba3be81ac 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -202,9 +202,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