Ubl delta fixes and improvements (#6941)

* Change all direct changes of ubl.state.active to
  set_bed_leveling_enabled() which handles apply/unapply
  leveling to maintain current_position consistency.

Fix invalidation of UBL mesh to invalid unreachable
  mesh points as well (delta corners).

Fix UBL_DELTA unapply_leveling logic and when
it gets applied, including fade_height changes.

Add optional M114 D for detailed position information,
disabled from compilation by default (M114_DETAIL).

* UBL_DELTA raw and inline kinematics

* UBL planner fall through fix

* consistent variable names

* Cleanup orphaned code and whitespace changes.
Use _O2.

* compile warnings cleanup

* Remove redundant #ifdef condition

Marlin/G26_Mesh_Validation_Tool.cpp

     /**
      * Wait until all parameters are verified before altering the state!
      */
-    state.active = !parser.seen('D');
+    set_bed_leveling_enabled(!parser.seen('D'));
 
     return UBL_OK;
   }
    * wait for them to get up to temperature.
    */
   bool unified_bed_leveling::turn_on_heaters() {
-    millis_t next;
+    millis_t next = millis() + 5000UL;
     #if HAS_TEMP_BED
       #if ENABLED(ULTRA_LCD)
         if (g26_bed_temp > 25) {
       #endif
           has_control_of_lcd_panel = true;
           thermalManager.setTargetBed(g26_bed_temp);
-          next = millis() + 5000UL;
           while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
             if (ubl_lcd_clicked()) return exit_from_g26();
             if (PENDING(millis(), next)) {

Marlin/Marlin_main.cpp

 #endif
 
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
-static void report_current_position();
+void report_current_position();
+void report_current_position_detail();
 
 #if ENABLED(DEBUG_LEVELING_FEATURE)
   void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
     #endif
 
-    if ( current_position[X_AXIS] == destination[X_AXIS]
-      && current_position[Y_AXIS] == destination[Y_AXIS]
-      && current_position[Z_AXIS] == destination[Z_AXIS]
-      && current_position[E_AXIS] == destination[E_AXIS]
-    ) return;
-
     refresh_cmd_timeout();
-    planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
+
+    #if UBL_DELTA
+      // 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
+      if ( current_position[X_AXIS] == destination[X_AXIS]
+        && current_position[Y_AXIS] == destination[Y_AXIS]
+        && current_position[Z_AXIS] == destination[Z_AXIS]
+        && current_position[E_AXIS] == destination[E_AXIS]
+      ) return;
+
+      planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
+    #endif
+    
     set_current_to_destination();
   }
 #endif // IS_KINEMATIC
         if (enabling) planner.unapply_leveling(current_position);
 
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
-
         #if PLANNER_LEVELING
-
-          if (!enable)   // leveling from on to off
+          if (ubl.state.active) {                       // leveling from on to off
+            // change unleveled current_position to physical current_position without moving steppers.
             planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
-          else
-            planner.unapply_leveling(current_position);
-
+            ubl.state.active = false;                   // disable only AFTER calling apply_leveling
+          }
+          else {                                        // leveling from off to on
+            ubl.state.active = true;                    // enable BEFORE calling unapply_leveling, otherwise ignored
+            // change physical current_position to unleveled current_position without moving steppers.
+            planner.unapply_leveling(current_position); 
+          }
+        #else
+          ubl.state.active = enable;                    // just flip the bit, current_position will be wrong until next move.
         #endif
 
-        ubl.state.active = enable;
-
       #else // ABL
 
         #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
   #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
     void set_z_fade_height(const float zfh) {
-      planner.z_fade_height = zfh;
-      planner.inverse_z_fade_height = RECIPROCAL(zfh);
 
-      if (leveling_is_active())
-        set_current_from_steppers_for_axis(
-          #if ABL_PLANAR
-            ALL_AXES
-          #else
-            Z_AXIS
-          #endif
-        );
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+        const bool level_active = leveling_is_active();
+        if (level_active) {
+          set_bed_leveling_enabled(false);  // turn off before changing fade height for proper apply/unapply leveling to maintain current_position
+        }
+        planner.z_fade_height = zfh;
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
+        if (level_active) {
+          set_bed_leveling_enabled(true);  // turn back on after changing fade height
+        }
+
+      #else
+
+        planner.z_fade_height = zfh;
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
+
+        if (leveling_is_active()) {
+          set_current_from_steppers_for_axis(
+            #if ABL_PLANAR
+              ALL_AXES
+            #else
+              Z_AXIS
+            #endif
+          );
+        }
+      #endif
     }
 
   #endif // LEVELING_FADE_HEIGHT
   #if ENABLED(DELTA)
 
     home_delta();
+    UNUSED(always_home_all);
 
   #else // NOT DELTA
 
 /**
  * Output the current position to serial
  */
-static void report_current_position() {
+void report_current_position() {
   SERIAL_PROTOCOLPGM("X:");
   SERIAL_PROTOCOL(current_position[X_AXIS]);
   SERIAL_PROTOCOLPGM(" Y:");
   #endif
 }
 
+#ifdef M114_DETAIL
+
+  static const char axis_char[XYZE] = {'X','Y','Z','E'};
+
+  void report_xyze(const float pos[XYZE], uint8_t n = 4, uint8_t precision = 3) {
+    char str[12];
+    for(uint8_t i=0; i<n; i++) {
+      SERIAL_CHAR(' ');
+      SERIAL_CHAR(axis_char[i]);
+      SERIAL_CHAR(':');
+      SERIAL_PROTOCOL(dtostrf(pos[i],8,precision,str));
+    }
+    SERIAL_EOL;
+  }
+
+  inline void report_xyz(const float pos[XYZ]) {
+    report_xyze(pos,3);
+  }
+
+  void report_current_position_detail() {
+
+    stepper.synchronize();
+
+    SERIAL_EOL;
+    SERIAL_PROTOCOLPGM("Logical:");
+    report_xyze(current_position);
+
+    SERIAL_PROTOCOLPGM("Raw:    ");
+    const float raw[XYZ] = {
+      RAW_X_POSITION(current_position[X_AXIS]),
+      RAW_Y_POSITION(current_position[Y_AXIS]),
+      RAW_Z_POSITION(current_position[Z_AXIS])
+    };
+    report_xyz(raw);
+
+    SERIAL_PROTOCOLPGM("Leveled:");
+    float leveled[XYZ] = {
+      current_position[X_AXIS],
+      current_position[Y_AXIS],
+      current_position[Z_AXIS]
+    };
+    planner.apply_leveling(leveled);
+    report_xyz(leveled);
+
+    SERIAL_PROTOCOLPGM("UnLevel:");
+    float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
+    planner.unapply_leveling(unleveled);
+    report_xyz(unleveled);
+
+    #if IS_KINEMATIC
+      #if IS_SCARA
+        SERIAL_PROTOCOLPGM("ScaraK: ");
+      #else
+        SERIAL_PROTOCOLPGM("DeltaK: ");
+      #endif
+      inverse_kinematics(leveled);  // writes delta[]
+      report_xyz(delta);
+    #endif
+
+    SERIAL_PROTOCOLPGM("Stepper:");
+    const float step_count[XYZE] = {
+      (float)stepper.position(X_AXIS),
+      (float)stepper.position(Y_AXIS),
+      (float)stepper.position(Z_AXIS),
+      (float)stepper.position(E_AXIS)
+    };
+    report_xyze(step_count,4,0);
+
+    #if IS_SCARA
+      const float deg[XYZ] = {
+        stepper.get_axis_position_degrees(A_AXIS),
+        stepper.get_axis_position_degrees(B_AXIS)
+      };
+      SERIAL_PROTOCOLPGM("Degrees:");
+      report_xyze(deg,2);
+    #endif
+
+    SERIAL_PROTOCOLPGM("FromStp:");
+    get_cartesian_from_steppers();  // writes cartes[XYZ] (with forward kinematics)
+    const float from_steppers[XYZE] = {
+      cartes[X_AXIS],
+      cartes[Y_AXIS],
+      cartes[Z_AXIS],
+      stepper.get_axis_position_mm(E_AXIS)
+    };
+    report_xyze(from_steppers);
+
+    const float diff[XYZE] = {
+      from_steppers[X_AXIS] - leveled[X_AXIS],
+      from_steppers[Y_AXIS] - leveled[Y_AXIS],
+      from_steppers[Z_AXIS] - leveled[Z_AXIS],
+      from_steppers[E_AXIS] - current_position[E_AXIS]
+    };
+    SERIAL_PROTOCOLPGM("Differ: ");
+    report_xyze(diff);
+  }
+#endif // M114_DETAIL
+
 /**
  * M114: Output current position to serial port
  */
-inline void gcode_M114() { stepper.synchronize(); report_current_position(); }
+inline void gcode_M114() {
+
+  #ifdef M114_DETAIL
+    if ( parser.seen('D') ) {
+      report_current_position_detail();
+      return;
+    }
+  #endif
+
+  stepper.synchronize();
+  report_current_position();
+  }
 
 /**
  * M115: Capabilities string
   /**
    * Constrain the given coordinates to the software endstops.
    */
+
+  // NOTE: This makes no sense for delta beds other than Z-axis.
+  //       For delta the X/Y would need to be clamped at
+  //       DELTA_PRINTABLE_RADIUS from center of bed, but delta
+  //       now enforces is_position_reachable for X/Y regardless
+  //       of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
+  //       redundant here.  Probably should #ifdef out the X/Y
+  //       axis clamps here for delta and just leave the Z clamp.
+
   void clamp_to_software_endstops(float target[XYZ]) {
     if (!soft_endstops_enabled) return;
     #if ENABLED(MIN_SOFTWARE_ENDSTOPS)
   if (
     #if IS_KINEMATIC
       #if UBL_DELTA
-        ubl.prepare_linear_move_to(destination, feedrate_mm_s)
+        ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
       #else
         prepare_kinematic_move_to(destination)
       #endif
     #elif ENABLED(DUAL_X_CARRIAGE)
       prepare_move_to_destination_dualx()
     #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
-      ubl.prepare_linear_move_to(destination, feedrate_mm_s)
+      ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
     #else
       prepare_move_to_destination_cartesian()
     #endif

Marlin/planner.cpp

    */
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
 
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA  // probably should also be enabled for UBL without UBL_DELTA
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
       if (!ubl.state.active) return;
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
         // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
       if (!abl_enabled) return;
     #endif
 
-    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_UBL)
       static float z_fade_factor = 1.0, last_raw_lz = -999.0;
       if (z_fade_height) {
         const float raw_lz = RAW_Z_POSITION(lz);
 
   void Planner::unapply_leveling(float logical[XYZ]) {
 
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
 
       if (ubl.state.active) {
 
-        const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
-                    z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
-              float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
+        const float z_physical = RAW_Z_POSITION(logical[Z_AXIS]);
+        const float z_ublmesh  = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
+        const float z_virtual  = z_physical - ubl.state.z_offset - z_ublmesh;
+              float z_logical  = LOGICAL_Z_POSITION(z_virtual);
 
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
-          // for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height,
-          // Given L==U+O+M(1-U/H) (faded mesh correction formula for U<H)
-          //  then U==L-O-M(1-U/H)
-          //    so U==L-O-M+MU/H
-          //    so U-MU/H==L-O-M
-          //    so U(1-M/H)==L-O-M
-          //    so U==(L-O-M)/(1-M/H) for U<H
+          // for P=physical_z, L=logical_z, M=mesh_z, O=z_offset, H=fade_height,
+          // Given P=L+O+M(1-L/H) (faded mesh correction formula for L<H)
+          //  then L=P-O-M(1-L/H)
+          //    so L=P-O-M+ML/H
+          //    so L-ML/H=P-O-M
+          //    so L(1-M/H)=P-O-M
+          //    so L=(P-O-M)/(1-M/H) for L<H
 
           if (planner.z_fade_height) {
-            const float z_unfaded = z_unlevel / (1.0 - z_ublmesh * planner.inverse_z_fade_height);
-            if (z_unfaded < planner.z_fade_height)  // don't know until after compute
-              z_unlevel = z_unfaded;
+            if (z_logical < planner.z_fade_height )
+              z_logical = z_logical / (1.0 - (z_ublmesh * planner.inverse_z_fade_height));
+            if (z_logical >= planner.z_fade_height)
+              z_logical = LOGICAL_Z_POSITION(z_physical - ubl.state.z_offset);
           }
 
         #endif // ENABLE_LEVELING_FADE_HEIGHT
 
-        logical[Z_AXIS] = z_unlevel;
+        logical[Z_AXIS] = z_logical;
       }
 
-      return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic
+      return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
 
     #endif
 

Marlin/ubl.cpp

   }
 
   void unified_bed_leveling::reset() {
-    state.active = false;
+    set_bed_leveling_enabled(false);
     state.z_offset = 0;
     state.storage_slot = -1;
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
   }
 
   void unified_bed_leveling::invalidate() {
-    state.active = false;
+    set_bed_leveling_enabled(false);
     state.z_offset = 0;
-    for (int x = 0; x < GRID_MAX_POINTS_X; x++)
-      for (int y = 0; y < GRID_MAX_POINTS_Y; y++)
-        z_values[x][y] = NAN;
+    set_all_mesh_points_to_value(NAN);
+  }
+
+  void unified_bed_leveling::set_all_mesh_points_to_value(float value) {
+    for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
+      for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
+        z_values[x][y] = value;
+      }
+    }
   }
 
   void unified_bed_leveling::display_map(const int map_type) {

Marlin/ubl.h

       static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
       static void reset();
       static void invalidate();
+      static void set_all_mesh_points_to_value(float);
       static bool sanity_check();
 
       static void G29() _O0;                          // O0 for no optimization
       FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
       FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
 
-      static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate);
+      static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate);
       static void line_to_destination_cartesian(const float &fr, uint8_t e);
 
   }; // class unified_bed_leveling

Marlin/ubl_G29.cpp

   #include "configuration_store.h"
   #include "ultralcd.h"
   #include "stepper.h"
+  #include "planner.h"
   #include "gcode.h"
 
   #include <math.h>
   extern long babysteps_done;
   extern float probe_pt(const float &x, const float &y, bool, int);
   extern bool set_probe_deployed(bool);
+  extern void set_bed_leveling_enabled(bool);
 
   #define SIZE_OF_LITTLE_RAISE 1
   #define BIG_RAISE_NOT_NEEDED 0
     if (parser.seen('I')) {
       uint8_t cnt = 0;
       g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
-      while (g29_repetition_cnt--) {
-        if (cnt > 20) { cnt = 0; idle(); }
-        const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
-        if (location.x_index < 0) {
-          SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
-          break;            // No more invalid Mesh Points to populate
+      if (g29_repetition_cnt >= GRID_MAX_POINTS) {
+        set_all_mesh_points_to_value(NAN);
+      } else {
+        while (g29_repetition_cnt--) {
+          if (cnt > 20) { cnt = 0; idle(); }
+          const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
+          if (location.x_index < 0) {
+            // No more REACHABLE mesh points to invalidate, so we ASSUME the user
+            // meant to invalidate the ENTIRE mesh, which cannot be done with
+            // find_closest_mesh_point loop which only returns REACHABLE points.
+            set_all_mesh_points_to_value(NAN);
+            SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
+            break;            // No more invalid Mesh Points to populate
+          }
+          z_values[location.x_index][location.y_index] = NAN;
+          cnt++;
         }
-        z_values[location.x_index][location.y_index] = NAN;
-        cnt++;
       }
       SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
     }
            *   - Specify a constant with the 'C' parameter.
            *   - Allow 'G29 P3' to choose a 'reasonable' constant.
            */
+
           if (g29_c_flag) {
             if (g29_repetition_cnt >= GRID_MAX_POINTS) {
-              for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
-                for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
-                  z_values[x][y] = g29_constant;
-                }
-              }
+              set_all_mesh_points_to_value(g29_constant);
             }
             else {
               while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
                 const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
-                if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
+                if (location.x_index < 0) {
+                  // No more REACHABLE INVALID mesh points to populate, so we ASSUME
+                  // user meant to populate ALL INVALID mesh points to value
+                  for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
+                    for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
+                      if ( isnan(z_values[x][y])) {
+                        z_values[x][y] = g29_constant;
+                      }
+                    }
+                  }
+                  break; // No more invalid Mesh Points to populate
+                }
                 z_values[location.x_index][location.y_index] = g29_constant;
               }
             }
 
       serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
 
+      const float z_step = 0.01;                                        // existing behavior: 0.01mm per click, occasionally step
+      //const float z_step = 1.0 / planner.axis_steps_per_mm[Z_AXIS];   // approx one step each click
+
       while (ubl_lcd_clicked()) delay(50);             // wait for user to release encoder wheel
       delay(50);                                       // debounce
       while (!ubl_lcd_clicked()) {                     // we need the loop to move the nozzle based on the encoder wheel here!
         idle();
         if (encoder_diff) {
-          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0);
+          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * z_step);
           encoder_diff = 0;
         }
       }
         SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
         return UBL_ERR;
       }
-      state.active = true;
+      set_bed_leveling_enabled(true);
       report_state();
     }
     else if (parser.seen('D')) {
-      state.active = false;
+      set_bed_leveling_enabled(false);
       report_state();
     }
 
       return;
     }
     ubl_state_at_invocation = state.active;
-    state.active = 0;
+    set_bed_leveling_enabled(false);
   }
 
   void unified_bed_leveling::restore_ubl_active_state_and_leave() {
       lcd_quick_feedback();
       return;
     }
-    state.active = ubl_state_at_invocation;
+    set_bed_leveling_enabled(ubl_state_at_invocation);
   }
 
   /**
         SERIAL_EOL;
       }
     #endif
+
+    if (do_ubl_mesh_map) display_map(g29_map_type);
   }
 
   #if ENABLED(UBL_G29_P31)

Marlin/ubl_motion.cpp

 
   extern float destination[XYZE];
   extern void set_current_to_destination();
-  extern float delta_segments_per_second;
+
+#if ENABLED(DELTA)
+
+  extern float delta[ABC],
+               endstop_adj[ABC];
+
+  extern float delta_radius,
+               delta_tower_angle_trim[2],
+               delta_tower[ABC][2],
+               delta_diagonal_rod,
+               delta_calibration_radius,
+               delta_diagonal_rod_2_tower[ABC],
+               delta_segments_per_second,
+               delta_clip_start_height;
+
+  extern float delta_safe_distance_from_top();
+
+#endif
+
 
   static void debug_echo_axis(const AxisEnum axis) {
     if (current_position[axis] == destination[axis])
     #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 performed first.
+    // so we call _buffer_line directly here.  Per-segmented leveling and kinematics performed first.
 
-    static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
+    inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) {
 
-      #if IS_KINEMATIC
+      #if ENABLED(DELTA)  // apply delta inverse_kinematics
 
-        inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget
-        float feedrate = fr_mm_s;
+        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 ));
 
-        #if IS_SCARA // scale the feed rate from mm/s to degrees/s
-          float adiff = abs(delta[A_AXIS] - scara_oldA),
-                bdiff = abs(delta[B_AXIS] - scara_oldB);
-          scara_oldA = delta[A_AXIS];
-          scara_oldB = delta[B_AXIS];
-          feedrate = max(adiff, bdiff) * scara_feed_factor;
-        #endif
+        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, le, fr, active_extruder);
+
+      #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
+
+        const float lseg[XYZ] = { LOGICAL_X_POSITION(rx),
+                                  LOGICAL_Y_POSITION(ry),
+                                  LOGICAL_Z_POSITION(rz)
+                                };
+
+        inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
+                                  // should move the feedrate scaling to scara inverse_kinematics
 
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
+        float adiff = abs(delta[A_AXIS] - scara_oldA),
+              bdiff = abs(delta[B_AXIS] - scara_oldB);
+        scara_oldA = delta[A_AXIS];
+        scara_oldB = delta[B_AXIS];
+        float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
 
-      #else // cartesian
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder);
 
-        planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
+      #else // CARTESIAN
+
+        // Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates
+
+        const float lx = LOGICAL_X_POSITION(rx),
+                    ly = LOGICAL_Y_POSITION(ry),
+                    lz = LOGICAL_Z_POSITION(rz);
+
+        planner._buffer_line(lx, ly, lz, le, fr, active_extruder);
 
       #endif
+
     }
 
+
     /**
-     * Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
+     * 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.
-     * Returns true if the caller did NOT update current_position, otherwise false.
+     * Returns true if did NOT move, false if moved (requires current_position update).
      */
 
-    static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
+    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) {
 
       if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS]))  // fail if moving outside reachable boundary
         return true; // did not move, so current_position still accurate
 
-      const float difference[XYZE] = {    // cartesian distances moved in XYZE
-                    ltarget[X_AXIS] - current_position[X_AXIS],
-                    ltarget[Y_AXIS] - current_position[Y_AXIS],
-                    ltarget[Z_AXIS] - current_position[Z_AXIS],
-                    ltarget[E_AXIS] - current_position[E_AXIS]
-                  };
+      const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS],
+                  tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS],
+                  tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS],
+                  tot_de = ltarget[E_AXIS] - current_position[E_AXIS];
 
-      const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]);         // total horizontal xy distance
+      const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy);  // total horizontal xy distance
 
       #if IS_KINEMATIC
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
       #endif
 
-      const float segment_distance[XYZE] = {            // length for each segment
-                    difference[X_AXIS] * inv_segments,
-                    difference[Y_AXIS] * inv_segments,
-                    difference[Z_AXIS] * inv_segments,
-                    difference[E_AXIS] * inv_segments
-                  };
+      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;
 
       // Note that E segment distance could vary slightly as z mesh height
       // changes for each segment, but small enough to ignore.
 
+      float seg_rx = RAW_X_POSITION(current_position[X_AXIS]),
+            seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]),
+            seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]),
+            seg_le = current_position[E_AXIS];
+
       const bool above_fade_height = (
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
           planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
 
         const float z_offset = state.active ? state.z_offset : 0.0;
 
-        float seg_dest[XYZE];                   // per-segment destination,
-        COPY_XYZE(seg_dest, current_position);  // starting from current position
+        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 = RAW_X_POSITION(ltarget[X_AXIS]);
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
+            seg_le = ltarget[E_AXIS];
+          }
 
-        while (--segments) {
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
-          float ztemp = seg_dest[Z_AXIS];
-          seg_dest[Z_AXIS] += z_offset;
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
-          seg_dest[Z_AXIS] = ztemp;
-        }
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_offset, seg_le, feedrate );
+
+        } while (segments);
 
-        // Since repeated adding segment_distance accumulates small errors, final move to exact destination.
-        COPY_XYZE(seg_dest, ltarget);
-        seg_dest[Z_AXIS] += z_offset;
-        ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
         return false; // moved but did not set_current_to_destination();
       }
 
         const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
       #endif
 
-      float seg_dest[XYZE];  // per-segment destination, initialize to first segment
-      LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
-
-      const float &dx_seg = segment_distance[X_AXIS];  // alias for clarity
-      const float &dy_seg = segment_distance[Y_AXIS];
-
-      float rx = RAW_X_POSITION(seg_dest[X_AXIS]),  // assume raw vs logical coordinates shifted but not scaled.
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
+      // increment to first segment destination
+      seg_rx += seg_dx;
+      seg_ry += seg_dy;
+      seg_rz += seg_dz;
+      seg_le += seg_de;
 
       for(;;) {  // for each mesh cell encountered during the move
 
         // 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 = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
-               cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
+        int8_t cell_xi = (seg_rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
+               cell_yi = (seg_ry - (UBL_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);
 
-        const float x0 = mesh_index_to_xpos(cell_xi),     // 64 byte table lookup avoids mul+add
-                    y0 = mesh_index_to_ypos(cell_yi),     // 64 byte table lookup avoids mul+add
-                    x1 = mesh_index_to_xpos(cell_xi + 1), // 64 byte table lookup avoids mul+add
-                    y1 = mesh_index_to_ypos(cell_yi + 1); // 64 byte table lookup avoids mul+add
+        const float x0 = mesh_index_to_xpos(cell_xi),   // 64 byte table lookup avoids mul+add
+                    y0 = mesh_index_to_ypos(cell_yi);
 
-        float cx = rx - x0,   // cell-relative x
-              cy = ry - y0,   // cell-relative y
-              z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
+        float z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
               z_x1y0 = z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
               z_x0y1 = z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
               z_x1y1 = z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
         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;
+
         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)
 
-              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx
+              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx (changes for each cx in cell)
 
         const float z_cxy1 = z_x0y1 + z_xmy1 * cx,            // z height along y1 at cx
                     z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cx from y0 to y1
 
-              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1
+              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1 (changes for each cx in cell)
 
         //    float z_cxcy = z_cxy0 + z_cxym * cy;            // interpolated mesh z height along cx at cy (do inside the segment loop)
 
         // 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 * dx_seg,                                     // per-segment adjustment to z_cxy0
-                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg;  // per-segment adjustment to z_cxym
+        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
 
         for(;;) {  // for all segments within this mesh cell
 
             z_cxcy *= fade_scaling_factor;          // apply fade factor to interpolated mesh height
           #endif
 
-          z_cxcy += state.z_offset;             // add fixed mesh offset from G29 Z
+          z_cxcy += state.z_offset;                 // add fixed mesh offset from G29 Z
 
           if (--segments == 0) {                    // if this is last segment, use ltarget for exact
-            COPY_XYZE(seg_dest, ltarget);
-            seg_dest[Z_AXIS] += z_cxcy;
-            ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
-            return false;   // did not set_current_to_destination()
+            seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
+            seg_le = ltarget[E_AXIS];
           }
 
-          const float z_orig = seg_dest[Z_AXIS];  // remember the pre-leveled segment z value
-          seg_dest[Z_AXIS] = z_orig + z_cxcy;     // adjust segment z height per mesh leveling
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
-          seg_dest[Z_AXIS] = z_orig;              // restore pre-leveled z before incrementing
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate );
+
+          if (segments == 0 )                       // done with last segment
+            return false;                           // did not set_current_to_destination()
 
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];  // adjust seg_dest for next segment
+          seg_rx += seg_dx;
+          seg_ry += seg_dy;
+          seg_rz += seg_dz;
+          seg_le += seg_de;
 
-          cx += dx_seg;
-          cy += dy_seg;
+          cx += seg_dx;
+          cy += seg_dy;
 
           if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
-            rx = RAW_X_POSITION(seg_dest[X_AXIS]);
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
             break;
           }