Comment/cleanup of motion code

Marlin/src/feature/bedlevel/abl/abl.cpp

@@ -366,6 +366,7 @@ float bilinear_z_offset(const float raw[XYZ]) {
    * splitting the move where it crosses grid borders.
    */
   void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
+    // Get current and destination cells for this line
     int cx1 = CELL_INDEX(X, current_position[X_AXIS]),
         cy1 = CELL_INDEX(Y, current_position[Y_AXIS]),
         cx2 = CELL_INDEX(X, destination[X_AXIS]),
@@ -375,8 +376,8 @@ float bilinear_z_offset(const float raw[XYZ]) {
     cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2);
     cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2);
 
+    // Start and end in the same cell? No split needed.
     if (cx1 == cx2 && cy1 == cy2) {
-      // Start and end on same mesh square
       buffer_line_to_destination(fr_mm_s);
       set_current_from_destination();
       return;
@@ -385,25 +386,30 @@ float bilinear_z_offset(const float raw[XYZ]) {
     #define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
 
     float normalized_dist, end[XYZE];
-
-    // Split at the left/front border of the right/top square
     const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+
+    // Crosses on the X and not already split on this X?
+    // The x_splits flags are insurance against rounding errors.
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
+      // Split on the X grid line
+      CBI(x_splits, gcx);
       COPY(end, destination);
       destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx;
       normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
       destination[Y_AXIS] = LINE_SEGMENT_END(Y);
-      CBI(x_splits, gcx);
     }
+    // Crosses on the Y and not already split on this Y?
     else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+      // Split on the Y grid line
+      CBI(y_splits, gcy);
       COPY(end, destination);
       destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy;
       normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
       destination[X_AXIS] = LINE_SEGMENT_END(X);
-      CBI(y_splits, gcy);
     }
     else {
-      // Already split on a border
+      // Must already have been split on these border(s)
+      // This should be a rare case.
       buffer_line_to_destination(fr_mm_s);
       set_current_from_destination();
       return;

Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp

@@ -59,6 +59,7 @@
      * splitting the move where it crosses mesh borders.
      */
     void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
+      // Get current and destination cells for this line
       int cx1 = mbl.cell_index_x(current_position[X_AXIS]),
           cy1 = mbl.cell_index_y(current_position[Y_AXIS]),
           cx2 = mbl.cell_index_x(destination[X_AXIS]),
@@ -68,8 +69,8 @@
       NOMORE(cx2, GRID_MAX_POINTS_X - 2);
       NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
 
+      // Start and end in the same cell? No split needed.
       if (cx1 == cx2 && cy1 == cy2) {
-        // Start and end on same mesh square
         buffer_line_to_destination(fr_mm_s);
         set_current_from_destination();
         return;
@@ -78,25 +79,30 @@
       #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
 
       float normalized_dist, end[XYZE];
-
-      // Split at the left/front border of the right/top square
       const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+
+      // Crosses on the X and not already split on this X?
+      // The x_splits flags are insurance against rounding errors.
       if (cx2 != cx1 && TEST(x_splits, gcx)) {
+        // Split on the X grid line
+        CBI(x_splits, gcx);
         COPY(end, destination);
         destination[X_AXIS] = mbl.index_to_xpos[gcx];
         normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
         destination[Y_AXIS] = MBL_SEGMENT_END(Y);
-        CBI(x_splits, gcx);
       }
+      // Crosses on the Y and not already split on this Y?
       else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+        // Split on the Y grid line
+        CBI(y_splits, gcy);
         COPY(end, destination);
         destination[Y_AXIS] = mbl.index_to_ypos[gcy];
         normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
         destination[X_AXIS] = MBL_SEGMENT_END(X);
-        CBI(y_splits, gcy);
       }
       else {
-        // Already split on a border
+        // Must already have been split on these border(s)
+        // This should be a rare case.
         buffer_line_to_destination(fr_mm_s);
         set_current_from_destination();
         return;

Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

@@ -475,30 +475,17 @@
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
     // so we call _buffer_line directly here.  Per-segmented leveling and kinematics performed first.
 
-    inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) {
+    inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
 
       #if ENABLED(DELTA)  // apply delta inverse_kinematics
 
-        const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS]
-                                         - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
-                                                   delta_tower[A_AXIS][Y_AXIS] - ry ));
-
-        const float delta_B = rz + SQRT( delta_diagonal_rod_2_tower[B_AXIS]
-                                         - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
-                                                   delta_tower[B_AXIS][Y_AXIS] - ry ));
-
-        const float delta_C = rz + SQRT( delta_diagonal_rod_2_tower[C_AXIS]
-                                         - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
-                                                   delta_tower[C_AXIS][Y_AXIS] - ry ));
-
-        planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
+        DELTA_RAW_IK();
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
 
       #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
 
-        const float lseg[XYZ] = { rx, ry, rz };
-
-        inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
-                                  // should move the feedrate scaling to scara inverse_kinematics
+        inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
+                                 // should move the feedrate scaling to scara inverse_kinematics
 
         const float adiff = FABS(delta[A_AXIS] - scara_oldA),
                     bdiff = FABS(delta[B_AXIS] - scara_oldB);
@@ -506,11 +493,11 @@
         scara_oldB = delta[B_AXIS];
         float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
 
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], e, s_feedrate, active_extruder);
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
 
       #else // CARTESIAN
 
-        planner._buffer_line(rx, ry, rz, e, fr, active_extruder);
+        planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
 
       #endif
 
@@ -528,12 +515,14 @@
       if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS]))  // fail if moving outside reachable boundary
         return true; // did not move, so current_position still accurate
 
-      const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS],
-                  tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS],
-                  tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS],
-                  tot_de = rtarget[E_AXIS] - current_position[E_AXIS];
+      const float total[XYZE] = {
+        rtarget[X_AXIS] - current_position[X_AXIS],
+        rtarget[Y_AXIS] - current_position[Y_AXIS],
+        rtarget[Z_AXIS] - current_position[Z_AXIS],
+        rtarget[E_AXIS] - current_position[E_AXIS]
+      };
 
-      const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy);  // total horizontal xy distance
+      const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]);  // total horizontal xy distance
 
       #if IS_KINEMATIC
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
@@ -553,41 +542,30 @@
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
       #endif
 
-      const float seg_dx = tot_dx * inv_segments,
-                  seg_dy = tot_dy * inv_segments,
-                  seg_dz = tot_dz * inv_segments,
-                  seg_de = tot_de * inv_segments;
+      const float diff[XYZE] = {
+        total[X_AXIS] * inv_segments,
+        total[Y_AXIS] * inv_segments,
+        total[Z_AXIS] * inv_segments,
+        total[E_AXIS] * inv_segments
+      };
 
       // Note that E segment distance could vary slightly as z mesh height
       // changes for each segment, but small enough to ignore.
 
-      float seg_rx = current_position[X_AXIS],
-            seg_ry = current_position[Y_AXIS],
-            seg_rz = current_position[Z_AXIS],
-            seg_le = current_position[E_AXIS];
+      float raw[XYZE] = {
+        current_position[X_AXIS],
+        current_position[Y_AXIS],
+        current_position[Z_AXIS],
+        current_position[E_AXIS]
+      };
 
       // Only compute leveling per segment if ubl active and target below z_fade_height.
-
       if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) {   // no mesh leveling
-
-        do {
-
-          if (--segments) {     // not the last segment
-            seg_rx += seg_dx;
-            seg_ry += seg_dy;
-            seg_rz += seg_dz;
-            seg_le += seg_de;
-          } else {              // last segment, use exact destination
-            seg_rx = rtarget[X_AXIS];
-            seg_ry = rtarget[Y_AXIS];
-            seg_rz = rtarget[Z_AXIS];
-            seg_le = rtarget[E_AXIS];
-          }
-
-          ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate);
-
-        } while (segments);
-
+        while (--segments) {
+          LOOP_XYZE(i) raw[i] += diff[i];
+          ubl_buffer_segment_raw(raw, feedrate);
+        }
+        ubl_buffer_segment_raw(rtarget, feedrate);
         return false; // moved but did not set_current_from_destination();
       }
 
@@ -598,10 +576,7 @@
       #endif
 
       // increment to first segment destination
-      seg_rx += seg_dx;
-      seg_ry += seg_dy;
-      seg_rz += seg_dz;
-      seg_le += seg_de;
+      LOOP_XYZE(i) raw[i] += diff[i];
 
       for(;;) {  // for each mesh cell encountered during the move
 
@@ -612,8 +587,8 @@
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
         // for mesh inset area.
 
-        int8_t cell_xi = (seg_rx - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
-               cell_yi = (seg_ry - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
+        int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
+               cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
 
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
@@ -631,8 +606,8 @@
         if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
         if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
 
-        float cx = seg_rx - x0,   // cell-relative x and y
-              cy = seg_ry - y0;
+        float cx = raw[X_AXIS] - x0,   // cell-relative x and y
+              cy = raw[Y_AXIS] - y0;
 
         const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
                     z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
@@ -650,40 +625,34 @@
         // and the z_cxym slope will change, both as a function of cx within the cell, and
         // each change by a constant for fixed segment lengths.
 
-        const float z_sxy0 = z_xmy0 * seg_dx,                                     // per-segment adjustment to z_cxy0
-                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx;  // per-segment adjustment to z_cxym
+        const float z_sxy0 = z_xmy0 * diff[X_AXIS],                                     // per-segment adjustment to z_cxy0
+                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS];  // per-segment adjustment to z_cxym
 
         for(;;) {  // for all segments within this mesh cell
 
-          float z_cxcy = z_cxy0 + z_cxym * cy;      // interpolated mesh z height along cx at cy
+          if (--segments == 0)                      // if this is last segment, use rtarget for exact
+            COPY(raw, rtarget);
 
+          float z_cxcy = z_cxy0 + z_cxym * cy;      // interpolated mesh z height along cx at cy
           #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
             z_cxcy *= fade_scaling_factor;          // apply fade factor to interpolated mesh height
           #endif
 
-          if (--segments == 0) {                    // if this is last segment, use rtarget for exact
-            seg_rx = rtarget[X_AXIS];
-            seg_ry = rtarget[Y_AXIS];
-            seg_rz = rtarget[Z_AXIS];
-            seg_le = rtarget[E_AXIS];
-          }
-
-          ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
+          const float z = raw[Z_AXIS];
+          raw[Z_AXIS] += z_cxcy;
+          ubl_buffer_segment_raw(raw, feedrate);
+          raw[Z_AXIS] = z;
 
           if (segments == 0)                        // done with last segment
             return false;                           // did not set_current_from_destination()
 
-          seg_rx += seg_dx;
-          seg_ry += seg_dy;
-          seg_rz += seg_dz;
-          seg_le += seg_de;
+          LOOP_XYZE(i) raw[i] += diff[i];
 
-          cx += seg_dx;
-          cy += seg_dy;
+          cx += diff[X_AXIS];
+          cy += diff[Y_AXIS];
 
-          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
+          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST))    // done within this cell, break to next
             break;
-          }
 
           // Next segment still within same mesh cell, adjust the per-segment
           // slope and intercept to compute next z height.

Marlin/src/module/motion.cpp

@@ -587,12 +587,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
     float raw[XYZE];
     COPY(raw, current_position);
 
-    // Drop one segment so the last move is to the exact target.
-    // If there's only 1 segment, loops will be skipped entirely.
-    --segments;
 
     // Calculate and execute the segments
-    for (uint16_t s = segments + 1; --s;) {
+    while (--segments) {
 
       static millis_t next_idle_ms = millis() + 200UL;
       thermalManager.manage_heater();  // This returns immediately if not really needed.
@@ -691,16 +688,12 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
       // SERIAL_ECHOPAIR("mm=", cartesian_mm);
       // SERIAL_ECHOLNPAIR(" segments=", segments);
 
-      // Drop one segment so the last move is to the exact target.
-      // If there's only 1 segment, loops will be skipped entirely.
-      --segments;
-
       // Get the raw current position as starting point
       float raw[XYZE];
       COPY(raw, current_position);
 
       // Calculate and execute the segments
-      for (uint16_t s = segments + 1; --s;) {
+      while (--segments) {
         static millis_t next_idle_ms = millis() + 200UL;
         thermalManager.manage_heater();  // This returns immediately if not really needed.
         if (ELAPSED(millis(), next_idle_ms)) {

Marlin/src/module/planner.h

@@ -505,8 +505,8 @@ class Planner {
     /**
      * Get the index of the next / previous block in the ring buffer
      */
-    static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
-    static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
+    static int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
+    static int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
 
     /**
      * Calculate the distance (not time) it takes to accelerate

Marlin/src/module/stepper.cpp

@@ -409,8 +409,7 @@ void Stepper::isr() {
   // If there is no current block, attempt to pop one from the buffer
   if (!current_block) {
     // Anything in the buffer?
-    current_block = planner.get_current_block();
-    if (current_block) {
+    if ((current_block = planner.get_current_block())) {
       trapezoid_generator_reset();
 
       // Initialize Bresenham counters to 1/2 the ceiling