Merge pull request #8729 from thinkyhead/bf1_sort_out_leveling

[1.1.x] UBL - Skew and Dual X Carriage

.travis.yml

   - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING NANODLP_Z_SYNC
   - build_marlin
   #
-  # And with a Sled Z Probe
+  # Add a Sled Z Probe, use UBL Cartesian moves
   #
-  - opt_enable Z_PROBE_SLED
+  - opt_enable Z_PROBE_SLED SKEW_CORRECTION SKEW_CORRECTION_FOR_Z SKEW_CORRECTION_GCODE
+  - opt_disable SEGMENT_LEVELED_MOVES
   - opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING
   - build_marlin
   #
   - opt_enable ULTIMAKERCONTROLLER SDSUPPORT
   - opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG
   - opt_enable_adv BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS
-  - opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU
+  - opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU M114_DETAIL
   - opt_set_adv PWM_MOTOR_CURRENT {1300,1300,1250}
   - opt_set_adv I2C_SLAVE_ADDRESS 63
   - build_marlin

Marlin/Conditionals_post.h

   /**
    * Set granular options based on the specific type of leveling
    */
-  #define UBL_DELTA  (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES)))
-  #define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
-  #define ABL_GRID   (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
-  #define OLDSCHOOL_ABL         (ABL_PLANAR || ABL_GRID)
-  #define HAS_ABL               (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL))
-  #define HAS_LEVELING          (HAS_ABL || ENABLED(MESH_BED_LEVELING))
-  #define HAS_AUTOLEVEL         (HAS_ABL && DISABLED(PROBE_MANUALLY))
-  #define HAS_MESH              (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
-  #define PLANNER_LEVELING      (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA || ENABLED(SKEW_CORRECTION))
+  #define UBL_SEGMENTED  (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES)))
+  #define ABL_PLANAR     (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
+  #define ABL_GRID       (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
+  #define OLDSCHOOL_ABL  (ABL_PLANAR || ABL_GRID)
+  #define HAS_ABL        (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL))
+  #define HAS_LEVELING   (HAS_ABL || ENABLED(MESH_BED_LEVELING))
+  #define HAS_AUTOLEVEL  (HAS_ABL && DISABLED(PROBE_MANUALLY))
+  #define HAS_MESH       (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
+  #define PLANNER_LEVELING      (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION))
   #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
   #if HAS_PROBING_PROCEDURE
     #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))

Marlin/G26_Mesh_Validation_Tool.cpp

   #if ENABLED(ULTRA_LCD)
     extern char lcd_status_message[];
   #endif
-  extern float destination[XYZE];
   void set_destination_from_current();
   void prepare_move_to_destination();
   inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
   void G26_line_to_destination(const float &feed_rate) {
     const float save_feedrate = feedrate_mm_s;
     feedrate_mm_s = feed_rate;      // use specified feed rate
-    prepare_move_to_destination();  // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_DELTA
+    prepare_move_to_destination();  // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_SEGMENTED
     feedrate_mm_s = save_feedrate;  // restore global feed rate
   }
 

Marlin/Marlin.h

   extern volatile bool wait_for_user;
 #endif
 
-extern float current_position[NUM_AXIS];
+extern float current_position[XYZE], destination[XYZE];
 
 // Workspace offsets
 #if HAS_WORKSPACE_OFFSET

Marlin/Marlin_main.cpp

 
 #define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
   static const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \
-  static inline type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
+  static inline type array(const AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
   typedef void __void_##CONFIG##__
 
 XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,   MIN_POS);
 void set_current_from_steppers_for_axis(const AxisEnum axis);
 
 #if ENABLED(ARC_SUPPORT)
-  void plan_arc(float target[XYZE], float* offset, uint8_t clockwise);
+  void plan_arc(const float (&cart)[XYZE], const float (&offset)[2], const bool clockwise);
 #endif
 
 #if ENABLED(BEZIER_CURVE_SUPPORT)
-  void plan_cubic_move(const float offset[4]);
+  void plan_cubic_move(const float (&offset)[4]);
 #endif
 
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
 
     refresh_cmd_timeout();
 
-    #if UBL_DELTA
+    #if UBL_SEGMENTED
       // ubl segmented line will do z-only moves in single segment
       ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
     #else
 
 #elif ENABLED(Z_PROBE_ALLEN_KEY)
 
-  FORCE_INLINE void do_blocking_move_to(const float raw[XYZ], const float &fr_mm_s) {
+  FORCE_INLINE void do_blocking_move_to(const float (&raw)[XYZ], const float &fr_mm_s) {
     do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
   }
 
 
 #ifdef M114_DETAIL
 
-  void report_xyze(const float pos[XYZE], const uint8_t n = 4, const uint8_t precision = 3) {
+  void report_xyze(const float pos[], const uint8_t n = 4, const uint8_t precision = 3) {
     char str[12];
     for (uint8_t i = 0; i < n; i++) {
       SERIAL_CHAR(' ');
     SERIAL_EOL();
   }
 
-  inline void report_xyz(const float pos[XYZ]) { report_xyze(pos, 3); }
+  inline void report_xyz(const float pos[]) { report_xyze(pos, 3); }
 
   void report_current_position_detail() {
 
 #endif // AUTO_BED_LEVELING_BILINEAR
 #endif // IS_CARTESIAN
 
-#if !UBL_DELTA
+#if !UBL_SEGMENTED
 #if IS_KINEMATIC
 
   /**
    * For Unified Bed Leveling (Delta or Segmented Cartesian)
    * the ubl.prepare_segmented_line_to method replaces this.
    */
-  inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
+  inline bool prepare_kinematic_move_to(const float (&rtarget)[XYZE]) {
 
     // Get the top feedrate of the move in the XY plane
     const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
   }
 
 #endif // !IS_KINEMATIC
-#endif // !UBL_DELTA
+#endif // !UBL_SEGMENTED
 
 #if ENABLED(DUAL_X_CARRIAGE)
 
           break;
       }
     }
-    return prepare_move_to_destination_cartesian();
+    return (
+      #if UBL_SEGMENTED
+        ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
+      #else
+        prepare_move_to_destination_cartesian()
+      #endif
+    );
   }
 
 #endif // DUAL_X_CARRIAGE
   #endif
 
   if (
-    #if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
+    #if ENABLED(DUAL_X_CARRIAGE)
+      prepare_move_to_destination_dualx()
+    #elif UBL_SEGMENTED
       ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
     #elif IS_KINEMATIC
       prepare_kinematic_move_to(destination)
-    #elif ENABLED(DUAL_X_CARRIAGE)
-      prepare_move_to_destination_dualx()
     #else
       prepare_move_to_destination_cartesian()
     #endif
    * options for G2/G3 arc generation. In future these options may be GCode tunable.
    */
   void plan_arc(
-    float raw[XYZE],     // Destination position
-    float *offset,       // Center of rotation relative to current_position
-    uint8_t clockwise    // Clockwise?
+    const float (&cart)[XYZE], // Destination position
+    const float (&offset)[2], // Center of rotation relative to current_position
+    const bool clockwise      // Clockwise?
   ) {
     #if ENABLED(CNC_WORKSPACE_PLANES)
       AxisEnum p_axis, q_axis, l_axis;
     const float radius = HYPOT(r_P, r_Q),
                 center_P = current_position[p_axis] - r_P,
                 center_Q = current_position[q_axis] - r_Q,
-                rt_X = raw[p_axis] - center_P,
-                rt_Y = raw[q_axis] - center_Q,
-                linear_travel = raw[l_axis] - current_position[l_axis],
-                extruder_travel = raw[E_AXIS] - current_position[E_AXIS];
+                rt_X = cart[p_axis] - center_P,
+                rt_Y = cart[q_axis] - center_Q,
+                linear_travel = cart[l_axis] - current_position[l_axis],
+                extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
 
     // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
     float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
     if (clockwise) angular_travel -= RADIANS(360);
 
     // Make a circle if the angular rotation is 0 and the target is current position
-    if (angular_travel == 0 && current_position[p_axis] == raw[p_axis] && current_position[q_axis] == raw[q_axis])
+    if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis])
       angular_travel = RADIANS(360);
 
     const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
     }
 
     // Ensure last segment arrives at target location.
-    planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
+    planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
 
     // As far as the parser is concerned, the position is now == target. In reality the
     // motion control system might still be processing the action and the real tool position
 
 #if ENABLED(BEZIER_CURVE_SUPPORT)
 
-  void plan_cubic_move(const float offset[4]) {
+  void plan_cubic_move(const float (&offset)[4]) {
     cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
 
     // As far as the parser is concerned, the position is now == destination. In reality the

Marlin/SanityCheck.h

     #error "Delta probably shouldn't use Z_MIN_PROBE_ENDSTOP. Comment out this line to continue."
   #elif DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
     #error "You probably want to use Max Endstops for DELTA!"
-  #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA
+  #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_SEGMENTED
     #error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL."
   #elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL))
     #error "DELTA_AUTO_CALIBRATION requires either a probe or an LCD Controller."
 #endif
 
 #if ENABLED(SKEW_CORRECTION)
-  #if ENABLED(AUTO_BED_LEVELING_UBL) && !ENABLED(SEGMENT_LEVELED_MOVES)
-    #error "SKEW_CORRECTION with AUTO_BED_LEVELING_UBL requires SEGMENT_LEVELED_MOVES."
-  #endif
   #if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD))
     #error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD."
   #endif

Marlin/planner.cpp

   void Planner::apply_leveling(float &rx, float &ry, float &rz) {
 
     #if ENABLED(SKEW_CORRECTION)
-      if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) {
-        const float tempry = ry - (rz * planner.yz_skew_factor),
-                    temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor)));
-        if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
-          rx = temprx;
-          ry = tempry;
-        }
-      }
+      skew(rx, ry, rz);
     #endif
 
     if (!leveling_active) return;
       #endif
 
       rz += (
-        #if ENABLED(AUTO_BED_LEVELING_UBL) // UBL_DELTA
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
           ubl.get_z_correction(rx, ry) * fade_scaling_factor
         #elif ENABLED(MESH_BED_LEVELING)
           mbl.get_z(rx, ry
     }
 
     #if ENABLED(SKEW_CORRECTION)
-      if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) {
-        const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor,
-                    tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor;
-        if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
-          raw[X_AXIS] = temprx;
-          raw[Y_AXIS] = tempry;
-        }
-      }
+      unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
     #endif
   }
 
 } // _buffer_steps()
 
 /**
- * Planner::_buffer_line
+ * Planner::buffer_segment
  *
  * Add a new linear movement to the buffer in axis units.
  *
  *  fr_mm_s   - (target) speed of the move
  *  extruder  - target extruder
  */
-void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
+void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
   // When changing extruders recalculate steps corresponding to the E position
   #if ENABLED(DISTINCT_E_FACTORS)
     if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
   };
 
   /* <-- add a slash to enable
-    SERIAL_ECHOPAIR("  _buffer_line FR:", fr_mm_s);
+    SERIAL_ECHOPAIR("  buffer_segment FR:", fr_mm_s);
     #if IS_KINEMATIC
       SERIAL_ECHOPAIR(" A:", a);
       SERIAL_ECHOPAIR(" (", position[A_AXIS]);
 
   stepper.wake_up();
 
-} // _buffer_line()
+} // buffer_segment()
 
 /**
  * Directly set the planner XYZ position (and stepper positions)
   ZERO(previous_speed);
 }
 
-void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
+void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
   #if PLANNER_LEVELING
-    float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
-    apply_leveling(lpos);
+    float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
+    apply_leveling(raw);
   #else
-    const float * const lpos = position;
+    const float (&raw)[XYZE] = cart;
   #endif
   #if IS_KINEMATIC
-    inverse_kinematics(lpos);
-    _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]);
+    inverse_kinematics(raw);
+    _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
   #else
-    _set_position_mm(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], position[E_AXIS]);
+    _set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
   #endif
 }
 

Marlin/planner.h

      *            head!=tail : blocks are in the buffer
      *   head==(tail-1)%size : the buffer is full
      *
-     *  Writer of head is Planner::_buffer_line().
+     *  Writer of head is Planner::buffer_segment().
      *  Reader of tail is Stepper::isr(). Always consider tail busy / read-only
      */
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
 
     #endif
 
+    #if ENABLED(SKEW_CORRECTION)
+
+      FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
+        if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
+          const float sx = cx - (cy * xy_skew_factor) - (cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor))),
+                      sy = cy - (cz * yz_skew_factor);
+          if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
+            cx = sx; cy = sy;
+          }
+        }
+      }
+
+      FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
+        if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
+          const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,
+                      sy = cy + cz * yz_skew_factor;
+          if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
+            cx = sx; cy = sy;
+          }
+        }
+      }
+
+    #endif // SKEW_CORRECTION
+
     #if PLANNER_LEVELING
 
       #define ARG_X float rx
        * as it will be given to the planner and steppers.
        */
       static void apply_leveling(float &rx, float &ry, float &rz);
-      static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
+      static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
       static void unapply_leveling(float raw[XYZ]);
 
     #else
     static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder);
 
     /**
-     * Planner::_buffer_line
+     * Planner::buffer_segment
      *
      * Add a new linear movement to the buffer in axis units.
      *
      *  fr_mm_s   - (target) speed of the move
      *  extruder  - target extruder
      */
-    static void _buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
+    static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
 
     static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
 
       #if PLANNER_LEVELING && IS_CARTESIAN
         apply_leveling(rx, ry, rz);
       #endif
-      _buffer_line(rx, ry, rz, e, fr_mm_s, extruder);
+      buffer_segment(rx, ry, rz, e, fr_mm_s, extruder);
     }
 
     /**
      *  fr_mm_s  - (target) speed of the move (mm/s)
      *  extruder - target extruder
      */
-    FORCE_INLINE static void buffer_line_kinematic(const float cart[XYZE], const float &fr_mm_s, const uint8_t extruder) {
+    FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder) {
       #if PLANNER_LEVELING
         float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
         apply_leveling(raw);
       #else
-        const float * const raw = cart;
+        const float (&raw)[XYZE] = cart;
       #endif
       #if IS_KINEMATIC
         inverse_kinematics(raw);
-        _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
+        buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
       #else
-        _buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
+        buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
       #endif
     }
 
       #endif
       _set_position_mm(rx, ry, rz, e);
     }
-    static void set_position_mm_kinematic(const float position[NUM_AXIS]);
+    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); }

Marlin/stepper.cpp

 /**
  * Get a stepper's position in steps.
  */
-long Stepper::position(AxisEnum axis) {
+long Stepper::position(const AxisEnum axis) {
   CRITICAL_SECTION_START;
   const long count_pos = count_position[axis];
   CRITICAL_SECTION_END;
  * Get an axis position according to stepper position(s)
  * For CORE machines apply translation from ABC to XYZ.
  */
-float Stepper::get_axis_position_mm(AxisEnum axis) {
+float Stepper::get_axis_position_mm(const AxisEnum axis) {
   float axis_steps;
   #if IS_CORE
     // Requesting one of the "core" axes?

Marlin/stepper.h

     //
     // Get the position of a stepper, in steps
     //
-    static long position(AxisEnum axis);
+    static long position(const AxisEnum axis);
 
     //
     // Report the positions of the steppers, in steps
     //
     // Get the position (mm) of an axis based on stepper position(s)
     //
-    static float get_axis_position_mm(AxisEnum axis);
+    static float get_axis_position_mm(const AxisEnum axis);
 
     //
     // SCARA AB axes are in degrees, not mm
     //
     #if IS_SCARA
-      FORCE_INLINE static float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); }
+      FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
     #endif
 
     //
     //
     // The direction of a single motor
     //
-    FORCE_INLINE static bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); }
+    FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
 
     #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
       static void digitalPotWrite(const int16_t address, const int16_t value);
     //
     // Handle a triggered endstop
     //
-    static void endstop_triggered(AxisEnum axis);
+    static void endstop_triggered(const AxisEnum axis);
 
     //
     // Triggered position of an axis in mm (not core-savvy)
     //
-    FORCE_INLINE static float triggered_position_mm(AxisEnum axis) {
+    FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
       return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
     }
 

Marlin/stepper_dac.cpp

   static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); }
   static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); }
 
-  uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
+  uint8_t dac_current_get_percent(const AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
   void dac_current_set_percents(const uint8_t pct[XYZE]) {
     LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
     mcp4728_setDrvPct(dac_channel_pct);

Marlin/stepper_dac.h

 void dac_current_raw(uint8_t channel, uint16_t val);
 void dac_print_values();
 void dac_commit_eeprom();
-uint8_t dac_current_get_percent(AxisEnum axis);
+uint8_t dac_current_get_percent(const AxisEnum axis);
 void dac_current_set_percents(const uint8_t pct[XYZE]);
 
 #endif // STEPPER_DAC_H

Marlin/ubl.cpp

     safe_delay(10);
   }
 
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
+
+    static void debug_echo_axis(const AxisEnum axis) {
+      if (current_position[axis] == destination[axis])
+        SERIAL_ECHOPGM("-------------");
+      else
+        SERIAL_ECHO_F(destination[X_AXIS], 6);
+    }
+
+    void debug_current_and_destination(const char *title) {
+
+      // if the title message starts with a '!' it is so important, we are going to
+      // ignore the status of the g26_debug_flag
+      if (*title != '!' && !g26_debug_flag) return;
+
+      const float de = destination[E_AXIS] - current_position[E_AXIS];
+
+      if (de == 0.0) return; // Printing moves only
+
+      const float dx = destination[X_AXIS] - current_position[X_AXIS],
+                  dy = destination[Y_AXIS] - current_position[Y_AXIS],
+                  xy_dist = HYPOT(dx, dy);
+
+      if (xy_dist == 0.0) return;
+
+      SERIAL_ECHOPGM("   fpmm=");
+      const float fpmm = de / xy_dist;
+      SERIAL_ECHO_F(fpmm, 6);
+
+      SERIAL_ECHOPGM("    current=( ");
+      SERIAL_ECHO_F(current_position[X_AXIS], 6);
+      SERIAL_ECHOPGM(", ");
+      SERIAL_ECHO_F(current_position[Y_AXIS], 6);
+      SERIAL_ECHOPGM(", ");
+      SERIAL_ECHO_F(current_position[Z_AXIS], 6);
+      SERIAL_ECHOPGM(", ");
+      SERIAL_ECHO_F(current_position[E_AXIS], 6);
+      SERIAL_ECHOPGM(" )   destination=( ");
+      debug_echo_axis(X_AXIS);
+      SERIAL_ECHOPGM(", ");
+      debug_echo_axis(Y_AXIS);
+      SERIAL_ECHOPGM(", ");
+      debug_echo_axis(Z_AXIS);
+      SERIAL_ECHOPGM(", ");
+      debug_echo_axis(E_AXIS);
+      SERIAL_ECHOPGM(" )   ");
+      SERIAL_ECHO(title);
+      SERIAL_EOL();
+
+    }
+
+  #endif // UBL_DEVEL_DEBUGGING
+
   int8_t unified_bed_leveling::storage_slot;
 
   float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
     uint8_t error_flag = 0;
 
     if (settings.calc_num_meshes() < 1) {
-      SERIAL_PROTOCOLLNPGM("?Insufficient EEPROM storage for a mesh of this size.");
+      SERIAL_PROTOCOLLNPGM("?Mesh too big for EEPROM.");
       error_flag++;
     }
 

Marlin/ubl.h

 #include "MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+  //#define UBL_DEVEL_DEBUGGING
+
   #include "Marlin.h"
   #include "planner.h"
   #include "math.h"
 
   // ubl_motion.cpp
 
-  void debug_current_and_destination(const char * const title);
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
+    void debug_current_and_destination(const char * const title);
+  #else
+    FORCE_INLINE void debug_current_and_destination(const char * const title) { UNUSED(title); }
+  #endif
 
   // ubl_G29.cpp
 
         return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
       }
 
-      static bool prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate);
-      static void line_to_destination_cartesian(const float &fr, uint8_t e);
-
-    #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
-    #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
-    #define ZZER(a) (z_values[a][0] == 0)
-
-    FORCE_INLINE bool mesh_is_valid() {
-      return !(
-        (    CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
-          && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
-        )
-        || isnan(z_values[0][0])
-      );
-    }
+      #if UBL_SEGMENTED
+        static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
+      #else
+        static void line_to_destination_cartesian(const float &fr, const uint8_t e);
+      #endif
+
+      #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
+      #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
+      #define ZZER(a) (z_values[a][0] == 0)
+
+      FORCE_INLINE bool mesh_is_valid() {
+        return !(
+          (    CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
+            && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
+          )
+          || isnan(z_values[0][0])
+        );
+      }
   }; // class unified_bed_leveling
 
   extern unified_bed_leveling ubl;

Marlin/ubl_G29.cpp

 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
 
-  //#define UBL_DEVEL_DEBUGGING
-
   #include "ubl.h"
   #include "Marlin.h"
   #include "hex_print_routines.h"
 
   static uint8_t ubl_state_at_invocation = 0;
 
-  #ifdef UBL_DEVEL_DEBUGGING
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
     static uint8_t ubl_state_recursion_chk = 0;
   #endif
 
   void unified_bed_leveling::save_ubl_active_state_and_disable() {
-    #ifdef UBL_DEVEL_DEBUGGING
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
       ubl_state_recursion_chk++;
       if (ubl_state_recursion_chk != 1) {
         SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
   }
 
   void unified_bed_leveling::restore_ubl_active_state_and_leave() {
-    #ifdef UBL_DEVEL_DEBUGGING
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
       if (--ubl_state_recursion_chk) {
         SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
         #if ENABLED(NEWPANEL)
     SERIAL_EOL();
     safe_delay(50);
 
-    #ifdef UBL_DEVEL_DEBUGGING
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
       SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
       SERIAL_EOL();
       SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);

Marlin/ubl_motion.cpp

   #include <avr/io.h>
   #include <math.h>
 
-  extern float destination[XYZE];
-
   #if AVR_AT90USB1286_FAMILY  // Teensyduino & Printrboard IDE extensions have compile errors without this
     inline void set_current_from_destination() { COPY(current_position, destination); }
   #else
     extern void set_current_from_destination();
   #endif
 
-  static void debug_echo_axis(const AxisEnum axis) {
-    if (current_position[axis] == destination[axis])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[X_AXIS], 6);
-  }
-
-  void debug_current_and_destination(const char *title) {
-
-    // if the title message starts with a '!' it is so important, we are going to
-    // ignore the status of the g26_debug_flag
-    if (*title != '!' && !g26_debug_flag) return;
-
-    const float de = destination[E_AXIS] - current_position[E_AXIS];
-
-    if (de == 0.0) return; // Printing moves only
-
-    const float dx = destination[X_AXIS] - current_position[X_AXIS],
-                dy = destination[Y_AXIS] - current_position[Y_AXIS],
-                xy_dist = HYPOT(dx, dy);
-
-    if (xy_dist == 0.0) return;
-
-    SERIAL_ECHOPGM("   fpmm=");
-    const float fpmm = de / xy_dist;
-    SERIAL_ECHO_F(fpmm, 6);
-
-    SERIAL_ECHOPGM("    current=( ");
-    SERIAL_ECHO_F(current_position[X_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[Y_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[Z_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[E_AXIS], 6);
-    SERIAL_ECHOPGM(" )   destination=( ");
-    debug_echo_axis(X_AXIS);
-    SERIAL_ECHOPGM(", ");
-    debug_echo_axis(Y_AXIS);
-    SERIAL_ECHOPGM(", ");
-    debug_echo_axis(Z_AXIS);
-    SERIAL_ECHOPGM(", ");
-    debug_echo_axis(E_AXIS);
-    SERIAL_ECHOPGM(" )   ");
-    SERIAL_ECHO(title);
-    SERIAL_EOL();
-
-  }
-
-  void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
-    /**
-     * Much of the nozzle movement will be within the same cell. So we will do as little computation
-     * as possible to determine if this is the case. If this move is within the same cell, we will
-     * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
-     */
-    const float start[XYZE] = {
-                  current_position[X_AXIS],
-                  current_position[Y_AXIS],
-                  current_position[Z_AXIS],
-                  current_position[E_AXIS]
-                },
-                end[XYZE] = {
-                  destination[X_AXIS],
-                  destination[Y_AXIS],
-                  destination[Z_AXIS],
-                  destination[E_AXIS]
-                };
-
-    const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
-              cell_start_yi = get_cell_index_y(start[Y_AXIS]),
-              cell_dest_xi  = get_cell_index_x(end[X_AXIS]),
-              cell_dest_yi  = get_cell_index_y(end[Y_AXIS]);
-
-    if (g26_debug_flag) {
-      SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
-      SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
-      SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
-      SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
-      SERIAL_CHAR(')');
-      SERIAL_EOL();
-      debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
-    }
+  #if !UBL_SEGMENTED
 
-    if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+    void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
       /**
-       * we don't need to break up the move
-       *
-       * If we are moving off the print bed, we are going to allow the move at this level.
-       * But we detect it and isolate it. For now, we just pass along the request.
+       * Much of the nozzle movement will be within the same cell. So we will do as little computation
+       * as possible to determine if this is the case. If this move is within the same cell, we will
+       * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
        */
+      #if ENABLED(SKEW_CORRECTION)
+        // For skew correction just adjust the destination point and we're done
+        float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
+              end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
+        planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
+        planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
+      #else
+        const float (&start)[XYZE] = current_position,
+                      (&end)[XYZE] = destination;
+      #endif
 
-      if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
+      const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
+                cell_start_yi = get_cell_index_y(start[Y_AXIS]),
+                cell_dest_xi  = get_cell_index_x(end[X_AXIS]),
+                cell_dest_yi  = get_cell_index_y(end[Y_AXIS]);
+
+      if (g26_debug_flag) {
+        SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]);
+        SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
+        SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
+        SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
+        SERIAL_CHAR(')');
+        SERIAL_EOL();
+        debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
+      }
 
-        // Note: There is no Z Correction in this case. We are off the grid and don't know what
-        // a reasonable correction would be.
+      if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+        /**
+         * we don't need to break up the move
+         *
+         * If we are moving off the print bed, we are going to allow the move at this level.
+         * But we detect it and isolate it. For now, we just pass along the request.
+         */
 
-        planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
-        set_current_from_destination();
+        if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
+
+          // Note: There is no Z Correction in this case. We are off the grid and don't know what
+          // a reasonable correction would be.
+
+          planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
+          set_current_from_destination();
+
+          if (g26_debug_flag)
+            debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
+
+          return;
+        }
+
+        FINAL_MOVE:
+
+        /**
+         * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
+         * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
+         * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
+         * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
+         * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
+         * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
+         */
+
+        const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+
+        float z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
+                  (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]),
+              z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
+                  (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+
+        if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+
+        // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
+        // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+
+        const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
+        float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
+
+        /**
+         * If part of the Mesh is undefined, it will show up as NAN
+         * in z_values[][] and propagate through the
+         * calculations. If our correction is NAN, we throw it out
+         * because part of the Mesh is undefined and we don't have the
+         * information we need to complete the height correction.
+         */
+        if (isnan(z0)) z0 = 0.0;
+
+        planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
 
         if (g26_debug_flag)
-          debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
+          debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
 
+        set_current_from_destination();
         return;
       }
 
-      FINAL_MOVE:
+      /**
+       * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
+       * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
+       * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
+       * computation and in fact most lines are of this nature. We will check for that in the following
+       * blocks of code:
+       */
+
+      const float dx = end[X_AXIS] - start[X_AXIS],
+                  dy = end[Y_AXIS] - start[Y_AXIS];
+
+      const int left_flag = dx < 0.0 ? 1 : 0,
+                down_flag = dy < 0.0 ? 1 : 0;
+
+      const float adx = left_flag ? -dx : dx,
+                  ady = down_flag ? -dy : dy;
+
+      const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
+                dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
 
       /**
-       * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
-       * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
-       * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
-       * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
-       * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
-       * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
+       * Compute the scaling factor for the extruder for each partial move.
+       * We need to watch out for zero length moves because it will cause us to
+       * have an infinate scaling factor. We are stuck doing a floating point
+       * divide to get our scaling factor, but after that, we just multiply by this
+       * number. We also pick our scaling factor based on whether the X or Y
+       * component is larger. We use the biggest of the two to preserve precision.
        */
 
-      const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+      const bool use_x_dist = adx > ady;
 
-      float z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
-                (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]),
-            z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
-                (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+      float on_axis_distance = use_x_dist ? dx : dy,
+            e_position = end[E_AXIS] - start[E_AXIS],
+            z_position = end[Z_AXIS] - start[Z_AXIS];
 
-      if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+      const float e_normalized_dist = e_position / on_axis_distance,
+                  z_normalized_dist = z_position / on_axis_distance;
 
-      // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
-      // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+      int current_xi = cell_start_xi,
+          current_yi = cell_start_yi;
 
-      const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
-      float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
+      const float m = dy / dx,
+                  c = start[Y_AXIS] - m * start[X_AXIS];
 
+      const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
+                 inf_m_flag = (isinf(m) != 0);
       /**
-       * If part of the Mesh is undefined, it will show up as NAN
-       * in z_values[][] and propagate through the
-       * calculations. If our correction is NAN, we throw it out
-       * because part of the Mesh is undefined and we don't have the
-       * information we need to complete the height correction.
+       * This block handles vertical lines. These are lines that stay within the same
+       * X Cell column. They do not need to be perfectly vertical. They just can
+       * not cross into another X Cell column.
        */
-      if (isnan(z0)) z0 = 0.0;
-
-      planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
+      if (dxi == 0) {       // Check for a vertical line
+        current_yi += down_flag;  // Line is heading down, we just want to go to the bottom
+        while (current_yi != cell_dest_yi + down_flag) {
+          current_yi += dyi;
+          const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+
+          /**
+           * if the slope of the line is infinite, we won't do the calculations
+           * else, we know the next X is the same so we can recover and continue!
+           * Calculate X at the next Y mesh line
+           */
+          const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
+
+          float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
+
+          const float ry = mesh_index_to_ypos(current_yi);
+
+          /**
+           * Without this check, it is possible for the algorithm to generate a zero length move in the case
+           * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
+           * happens, it might be best to remove the check and always 'schedule' the move because
+           * the planner.buffer_segment() routine will filter it if that happens.
+           */
+          if (ry != start[Y_AXIS]) {
+            if (!inf_normalized_flag) {
+              on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+              e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+              z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+            }
+            else {
+              e_position = end[E_AXIS];
+              z_position = end[Z_AXIS];
+            }
 
-      if (g26_debug_flag)
-        debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
+            planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+          } //else printf("FIRST MOVE PRUNED  ");
+        }
 
-      set_current_from_destination();
-      return;
-    }
+        if (g26_debug_flag)
+          debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
 
-    /**
-     * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
-     * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
-     * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
-     * computation and in fact most lines are of this nature. We will check for that in the following
-     * blocks of code:
-     */
+        //
+        // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
+        //
+        if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+          goto FINAL_MOVE;
 
-    const float dx = end[X_AXIS] - start[X_AXIS],
-                dy = end[Y_AXIS] - start[Y_AXIS];
+        set_current_from_destination();
+        return;
+      }
 
-    const int left_flag = dx < 0.0 ? 1 : 0,
-              down_flag = dy < 0.0 ? 1 : 0;
+      /**
+       *
+       * This block handles horizontal lines. These are lines that stay within the same
+       * Y Cell row. They do not need to be perfectly horizontal. They just can
+       * not cross into another Y Cell row.
+       *
+       */
 
-    const float adx = left_flag ? -dx : dx,
-                ady = down_flag ? -dy : dy;
+      if (dyi == 0) {             // Check for a horizontal line
+        current_xi += left_flag;  // Line is heading left, we just want to go to the left
+                                  // edge of this cell for the first move.
+        while (current_xi != cell_dest_xi + left_flag) {
+          current_xi += dxi;
+          const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
+                      ry = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
+
+          float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
+
+          const float rx = mesh_index_to_xpos(current_xi);
+
+          /**
+           * Without this check, it is possible for the algorithm to generate a zero length move in the case
+           * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
+           * that happens, it might be best to remove the check and always 'schedule' the move because
+           * the planner.buffer_segment() routine will filter it if that happens.
+           */
+          if (rx != start[X_AXIS]) {
+            if (!inf_normalized_flag) {
+              on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+              e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+              z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+            }
+            else {
+              e_position = end[E_AXIS];
+              z_position = end[Z_AXIS];
+            }
 
-    const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
-              dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
+            planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+          } //else printf("FIRST MOVE PRUNED  ");
+        }
 
-    /**
-     * Compute the scaling factor for the extruder for each partial move.
-     * We need to watch out for zero length moves because it will cause us to
-     * have an infinate scaling factor. We are stuck doing a floating point
-     * divide to get our scaling factor, but after that, we just multiply by this
-     * number. We also pick our scaling factor based on whether the X or Y
-     * component is larger. We use the biggest of the two to preserve precision.
-     */
+        if (g26_debug_flag)
+          debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()"));
 
-    const bool use_x_dist = adx > ady;
+        if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+          goto FINAL_MOVE;
 
-    float on_axis_distance = use_x_dist ? dx : dy,
-          e_position = end[E_AXIS] - start[E_AXIS],
-          z_position = end[Z_AXIS] - start[Z_AXIS];
+        set_current_from_destination();
+        return;
+      }
 
-    const float e_normalized_dist = e_position / on_axis_distance,
-                z_normalized_dist = z_position / on_axis_distance;
+      /**
+       *
+       * This block handles the generic case of a line crossing both X and Y Mesh lines.
+       *
+       */
 
-    int current_xi = cell_start_xi,
-        current_yi = cell_start_yi;
+      int xi_cnt = cell_start_xi - cell_dest_xi,
+          yi_cnt = cell_start_yi - cell_dest_yi;
 
-    const float m = dy / dx,
-                c = start[Y_AXIS] - m * start[X_AXIS];
+      if (xi_cnt < 0) xi_cnt = -xi_cnt;
+      if (yi_cnt < 0) yi_cnt = -yi_cnt;
 
-    const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
-               inf_m_flag = (isinf(m) != 0);
-    /**
-     * This block handles vertical lines. These are lines that stay within the same
-     * X Cell column. They do not need to be perfectly vertical. They just can
-     * not cross into another X Cell column.
-     */
-    if (dxi == 0) {       // Check for a vertical line
-      current_yi += down_flag;  // Line is heading down, we just want to go to the bottom
-      while (current_yi != cell_dest_yi + down_flag) {
-        current_yi += dyi;
-        const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+      current_xi += left_flag;
+      current_yi += down_flag;
 
-        /**
-         * if the slope of the line is infinite, we won't do the calculations
-         * else, we know the next X is the same so we can recover and continue!
-         * Calculate X at the next Y mesh line
-         */
-        const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
+      while (xi_cnt > 0 || yi_cnt > 0) {
 
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+        const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
+                    next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
+                    ry = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
+                    rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
+                                                     // (No need to worry about m being zero.
+                                                     //  If that was the case, it was already detected
+                                                     //  as a vertical line move above.)
 
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
+        if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
+          // Yes!  Crossing a Y Mesh Line next
+          float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
 
-        const float ry = mesh_index_to_ypos(current_yi);
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
 
-        /**
-         * Without this check, it is possible for the algorithm to generate a zero length move in the case
-         * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
-         * happens, it might be best to remove the check and always 'schedule' the move because
-         * the planner._buffer_line() routine will filter it if that happens.
-         */
-        if (ry != start[Y_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
             e_position = end[E_AXIS];
             z_position = end[Z_AXIS];
           }
+          planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
+          current_yi += dyi;
+          yi_cnt--;
+        }
+        else {
+          // Yes!  Crossing a X Mesh Line next
+          float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
 
-          planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
-        } //else printf("FIRST MOVE PRUNED  ");
-      }
-
-      if (g26_debug_flag)
-        debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
-
-      //
-      // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
-      //
-      if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
-        goto FINAL_MOVE;
-
-      set_current_from_destination();
-      return;
-    }
-
-    /**
-     *
-     * This block handles horizontal lines. These are lines that stay within the same
-     * Y Cell row. They do not need to be perfectly horizontal. They just can
-     * not cross into another Y Cell row.
-     *
-     */
-
-    if (dyi == 0) {             // Check for a horizontal line
-      current_xi += left_flag;  // Line is heading left, we just want to go to the left
-                                // edge of this cell for the first move.
-      while (current_xi != cell_dest_xi + left_flag) {
-        current_xi += dxi;
-        const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
-                    ry = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
-
-        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        const float rx = mesh_index_to_xpos(current_xi);
-
-        /**
-         * Without this check, it is possible for the algorithm to generate a zero length move in the case
-         * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
-         * that happens, it might be best to remove the check and always 'schedule' the move because
-         * the planner._buffer_line() routine will filter it if that happens.
-         */
-        if (rx != start[X_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
-            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+            on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
+            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
             z_position = end[Z_AXIS];
           }
 
-          planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
-        } //else printf("FIRST MOVE PRUNED  ");
+          planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
+          current_xi += dxi;
+          xi_cnt--;
+        }
+
+        if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
       }
 
       if (g26_debug_flag)
-        debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
+        debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()"));
 
       if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
         goto FINAL_MOVE;
 
       set_current_from_destination();
-      return;
-    }
-
-    /**
-     *
-     * This block handles the generic case of a line crossing both X and Y Mesh lines.
-     *
-     */
-
-    int xi_cnt = cell_start_xi - cell_dest_xi,
-        yi_cnt = cell_start_yi - cell_dest_yi;
-
-    if (xi_cnt < 0) xi_cnt = -xi_cnt;
-    if (yi_cnt < 0) yi_cnt = -yi_cnt;
-
-    current_xi += left_flag;
-    current_yi += down_flag;
-
-    while (xi_cnt > 0 || yi_cnt > 0) {
-
-      const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
-                  next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
-                  ry = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
-                  rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
-                                                   // (No need to worry about m being zero.
-                                                   //  If that was the case, it was already detected
-                                                   //  as a vertical line move above.)
-
-      if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
-        // Yes!  Crossing a Y Mesh Line next
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
-          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
-          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
-        }
-        else {
-          e_position = end[E_AXIS];
-          z_position = end[Z_AXIS];
-        }
-        planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
-        current_yi += dyi;
-        yi_cnt--;
-      }
-      else {
-        // Yes!  Crossing a X Mesh Line next
-        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
-          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
-          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
-        }
-        else {
-          e_position = end[E_AXIS];
-          z_position = end[Z_AXIS];
-        }
-
-        planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
-        current_xi += dxi;
-        xi_cnt--;
-      }
-
-      if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
     }
 
-    if (g26_debug_flag)
-      debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
-
-    if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
-      goto FINAL_MOVE;
-
-    set_current_from_destination();
-  }
-
-  #if UBL_DELTA
-
-    // macro to inline copy exactly 4 floats, don't rely on sizeof operator
-    #define COPY_XYZE( target, source ) { \
-                target[X_AXIS] = source[X_AXIS]; \
-                target[Y_AXIS] = source[Y_AXIS]; \
-                target[Z_AXIS] = source[Z_AXIS]; \
-                target[E_AXIS] = source[E_AXIS]; \
-            }
+  #else // UBL_SEGMENTED
 
     #if IS_SCARA // scale the feed rate from mm/s to degrees/s
       static float scara_feed_factor, scara_oldA, scara_oldB;
     #endif
 
     // 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.
+    // so we call buffer_segment directly here.  Per-segmented leveling and kinematics performed first.
 
-    inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
+    inline void _O2 ubl_buffer_segment_raw(const float (&raw)[XYZE], const float &fr) {
 
       #if ENABLED(DELTA)  // apply delta inverse_kinematics
 
         DELTA_RAW_IK();
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
+        planner.buffer_segment(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)
 
         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], raw[E_AXIS], s_feedrate, active_extruder);
+        planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
 
       #else // CARTESIAN
 
-        planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
+        planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
 
       #endif
     }
 
     /**
      * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
-     * This calls planner._buffer_line multiple times for small incremental moves.
+     * This calls planner.buffer_segment multiple times for small incremental moves.
      * Returns true if did NOT move, false if moved (requires current_position update).
      */
 
-    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) {
+    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&in_target)[XYZE], const float &feedrate) {
 
-      if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS]))  // fail if moving outside reachable boundary
+      if (!position_is_reachable(in_target[X_AXIS], in_target[Y_AXIS]))  // fail if moving outside reachable boundary
         return true; // did not move, so current_position still accurate
 
+      #if ENABLED(SKEW_CORRECTION)
+        // For skew correction just adjust the destination point and we're done
+        float rtarget[XYZE] = { in_target[X_AXIS], in_target[Y_AXIS], in_target[Z_AXIS], in_target[E_AXIS] };
+        planner.skew(rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS]);
+      #else
+        const float (&rtarget)[XYZE] = in_target;
+      #endif
+
       const float total[XYZE] = {
         rtarget[X_AXIS] - current_position[X_AXIS],
         rtarget[Y_AXIS] - current_position[Y_AXIS],
         current_position[E_AXIS]
       };
 
+      #if ENABLED(SKEW_CORRECTION)
+        planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
+      #endif
+
       // 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
         while (--segments) {
       } // cell loop
     }
 
-  #endif // UBL_DELTA
+  #endif // UBL_SEGMENTED
 
 #endif // AUTO_BED_LEVELING_UBL

Marlin/ultralcd.cpp

 
   #if IS_KINEMATIC
     extern float feedrate_mm_s;
-    extern float destination[XYZE];
     void set_destination_from_current();
     void prepare_move_to_destination();
   #endif