diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 22a086ac56..8de2db5c06 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -236,7 +236,6 @@ void Planner::init() { } #if ENABLED(S_CURVE_ACCELERATION) - #ifdef __AVR__ /** * This routine returns 0x1000000 / d, getting the inverse as fast as possible. @@ -1570,7 +1569,7 @@ void Planner::synchronize() { #endif #endif - void Planner::add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block, float (&delta_mm)[ABCE]) { + void Planner::add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block) { static uint8_t last_direction_bits; uint8_t changed_dir = last_direction_bits ^ dm; // Ignore direction change if no steps are taken in that direction @@ -1598,25 +1597,21 @@ void Planner::synchronize() { if (!changed_dir) return; #endif - const bool positive[XYZ] = { da > 0, db > 0, dc > 0 }; - #ifdef BACKLASH_SMOOTHING_MM - const bool non_zero[XYZ] = { da != 0, db != 0, dc != 0 }; - #endif - bool made_adjustment = false; + LOOP_XYZ(axis) { + if (backlash_distance_mm[axis]) { + const bool reversing = TEST(dm,axis); - LOOP_XYZ(i) { - if (backlash_distance_mm[i]) { // When an axis changes direction, add axis backlash to the residual error - if (TEST(changed_dir, i)) - residual_error[i] += backlash_correction * (positive[i] ? 1.0f : -1.0f) * backlash_distance_mm[i] * planner.settings.axis_steps_per_mm[i]; + if (TEST(changed_dir, axis)) + residual_error[axis] += backlash_correction * (reversing ? -1.0f : 1.0f) * backlash_distance_mm[axis] * planner.settings.axis_steps_per_mm[axis]; // Decide how much of the residual error to correct in this segment - int32_t error_correction = residual_error[i]; + int32_t error_correction = residual_error[axis]; #ifdef BACKLASH_SMOOTHING_MM if (error_correction && backlash_smoothing_mm != 0) { // Take up a portion of the residual_error in this segment, but only when // the current segment travels in the same direction as the correction - if (non_zero[i] && positive[i] == (error_correction > 0)) { + if (reversing == (error_correction < 0)) { if (segment_proportion == 0) segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm); error_correction *= segment_proportion; @@ -1627,17 +1622,11 @@ void Planner::synchronize() { #endif // Making a correction reduces the residual error and modifies delta_mm if (error_correction) { - block->steps[i] += ABS(error_correction); - residual_error[i] -= error_correction; - delta_mm[i] = (positive[i] ? 1.0f : -1.0f) * block->steps[i] * steps_to_mm[i]; - made_adjustment = true; + block->steps[axis] += ABS(error_correction); + residual_error[axis] -= error_correction; } } } - - // If any of the axes were adjusted, recompute block->millimeters - if (made_adjustment) - block->millimeters = SQRT(sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])); } #endif // BACKLASH_COMPENSATION @@ -1889,11 +1878,17 @@ bool Planner::_populate_block(block_t * const block, bool split_move, #endif ); + /** + * At this point at least one of the axes has more steps than + * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as + * zero-length. It's important to not apply corrections + * to blocks that would get dropped! + * + * A correction function is permitted to add steps to an axis, it + * should *never* remove steps! + */ #if ENABLED(BACKLASH_COMPENSATION) - // If we make it here, at least one of the axes has more steps than - // MIN_STEPS_PER_SEGMENT, so the segment won't get dropped by Marlin - // and it is okay to add steps for backlash correction. - add_backlash_correction_steps(da, db, dc, dm, block, delta_mm); + add_backlash_correction_steps(da, db, dc, dm, block); #endif } @@ -2344,7 +2339,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move, float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2 #if ENABLED(JUNCTION_DEVIATION) - /** * Compute maximum allowable entry speed at junction by centripetal acceleration approximation. * Let a circle be tangent to both previous and current path line segments, where the junction diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 2592f45b54..45c18365ab 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -339,7 +339,7 @@ class Planner { #endif #if ENABLED(BACKLASH_COMPENSATION) - static void add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block, float (&delta_mm)[ABCE]); + static void add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block); #endif public: