From 78410b210bdf860c8e94d4cb4b505cceb3345b4e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Tue, 8 May 2018 11:10:33 -0500
Subject: [PATCH] Add UBL support for G2/G3 and G5 (#10649)

---
 Marlin/Conditionals_post.h |  1 +
 Marlin/Marlin_main.cpp     |  8 ++++++++
 Marlin/planner.cpp         |  6 +++++-
 Marlin/planner.h           | 19 ++++++++++++-------
 Marlin/planner_bezier.cpp  |  8 +++++++-
 5 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 2906c0abd..73d5cb775 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -966,6 +966,7 @@
   #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))
+  #define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT)))
 
   #if ENABLED(AUTO_BED_LEVELING_UBL)
     #undef LCD_BED_LEVELING
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index a3f48f084..b15458d8c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -13598,6 +13598,10 @@ void prepare_move_to_destination() {
         ADJUST_DELTA(raw);
         planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
         oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
+      #elif HAS_UBL_AND_CURVES
+        float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
+        planner.apply_leveling(pos);
+        planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder);
       #else
         planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
       #endif
@@ -13610,6 +13614,10 @@ void prepare_move_to_destination() {
       const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
       if (diff2)
         planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
+    #elif HAS_UBL_AND_CURVES
+      float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
+      planner.apply_leveling(pos);
+      planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], cart[E_AXIS], fr_mm_s, active_extruder);
     #else
       planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
     #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 3620b8ff5..d32290f7d 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1166,7 +1166,7 @@ void Planner::check_axes_activity() {
   }
 #endif
 
-#if PLANNER_LEVELING
+#if PLANNER_LEVELING || HAS_UBL_AND_CURVES
   /**
    * rx, ry, rz - Cartesian positions in mm
    *              Leveled XYZ on completion
@@ -1218,6 +1218,10 @@ void Planner::check_axes_activity() {
     #endif
   }
 
+#endif
+
+#if PLANNER_LEVELING
+
   void Planner::unapply_leveling(float raw[XYZ]) {
 
     if (leveling_active) {
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 79e2c2ee3..136ff9765 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -400,19 +400,24 @@ class Planner {
 
     #endif // SKEW_CORRECTION
 
-    #if PLANNER_LEVELING
-
-      #define ARG_X float rx
-      #define ARG_Y float ry
-      #define ARG_Z float rz
+    #if PLANNER_LEVELING || HAS_UBL_AND_CURVES
 
       /**
        * Apply leveling to transform a cartesian position
        * 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 unapply_leveling(float raw[XYZ]);
+      FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
+
+      #if PLANNER_LEVELING
+
+        #define ARG_X float rx
+        #define ARG_Y float ry
+        #define ARG_Z float rz
+
+        static void unapply_leveling(float raw[XYZ]);
+
+      #endif
 
     #else
 
diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp
index 4686c571e..d6dd35f1c 100644
--- a/Marlin/planner_bezier.cpp
+++ b/Marlin/planner_bezier.cpp
@@ -188,7 +188,13 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
     bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
     clamp_to_software_endstops(bez_target);
-    planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
+    #if HAS_UBL_AND_CURVES
+      float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
+      planner.apply_leveling(pos);
+      planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder);
+    #else
+      planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
+    #endif
   }
 }