Apply formatting, coding standards to UBL

Marlin/G26_Mesh_Validation_Tool.cpp

@@ -258,8 +258,8 @@
         : find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now.
 
       if (location.x_index >= 0 && location.y_index >= 0) {
-        const float circle_x = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])),
-                    circle_y = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index]));
+        const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
+                    circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
 
         // Let's do a couple of quick sanity checks.  We can pull this code out later if we never see it catch a problem
         #ifdef DELTA
@@ -401,8 +401,8 @@
     for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
       for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
         if (!is_bit_set(circle_flags, i, j)) {
-          const float mx = pgm_read_float(&(ubl.mesh_index_to_xpos[i])),  // We found a circle that needs to be printed
-                      my = pgm_read_float(&(ubl.mesh_index_to_ypos[j]));
+          const float mx = pgm_read_float(&ubl.mesh_index_to_xpos[i]),  // We found a circle that needs to be printed
+                      my = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
 
           // Get the distance to this intersection
           float f = HYPOT(X - mx, Y - my);
@@ -446,11 +446,11 @@
               // We found two circles that need a horizontal line to connect them
               // Print it!
               //
-              sx = pgm_read_float(&(ubl.mesh_index_to_xpos[  i  ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
-              ex = pgm_read_float(&(ubl.mesh_index_to_xpos[i + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
+              sx = pgm_read_float(&ubl.mesh_index_to_xpos[  i  ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
+              ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
 
               sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);
-              sy = ey = constrain(pgm_read_float(&(ubl.mesh_index_to_ypos[j])), Y_MIN_POS + 1, Y_MAX_POS - 1);
+              sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1);
               ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
 
               if (ubl.g26_debug_flag) {
@@ -477,10 +477,10 @@
                 // We found two circles that need a vertical line to connect them
                 // Print it!
                 //
-                sy = pgm_read_float(&(ubl.mesh_index_to_ypos[  j  ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
-                ey = pgm_read_float(&(ubl.mesh_index_to_ypos[j + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
+                sy = pgm_read_float(&ubl.mesh_index_to_ypos[  j  ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
+                ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
 
-                sx = ex = constrain(pgm_read_float(&(ubl.mesh_index_to_xpos[i])), X_MIN_POS + 1, X_MAX_POS - 1);
+                sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1);
                 sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
                 ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 

Marlin/ubl_G29.cpp

@@ -352,7 +352,6 @@
     }
 
     if (code_seen('Q')) {
-
       const int test_pattern = code_has_value() ? code_value_int() : -1;
       if (!WITHIN(test_pattern, 0, 2)) {
         SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
@@ -433,13 +432,14 @@
           //
           SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n");
           do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
-          if (!x_flag && !y_flag) {      // use a good default location for the path
-            // The flipped > and < operators on these two comparisons is
-            // intentional. It should cause the probed points to follow a
-            // nice path on Cartesian printers. It may make sense to
-            // have Delta printers default to the center of the bed.
-            // For now, until that is decided, it can be forced with the X
-            // and Y parameters.
+          if (!x_flag && !y_flag) {
+            /**
+             * Use a good default location for the path.
+             * The flipped > and < operators in these comparisons is intentional.
+             * It should cause the probed points to follow a nice path on Cartesian printers.
+             * It may make sense to have Delta printers default to the center of the bed.
+             * Until that is decided, this can be forced with the X and Y parameters.
+             */
             x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? UBL_MESH_MAX_X : UBL_MESH_MIN_X;
             y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? UBL_MESH_MAX_Y : UBL_MESH_MIN_Y;
           }
@@ -461,27 +461,28 @@
           }
           manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M'));
           SERIAL_PROTOCOLLNPGM("G29 P2 finished");
-          }
-          break;
+
+        } break;
 
         case 3: {
-          //
-          // Populate invalid Mesh areas.  Two choices are available to the user.  The user can
-          // specify the constant to be used with a C # paramter.   Or the user can allow the G29 P3 command to
-          // apply a 'reasonable' constant to the invalid mesh point.  Some caution and scrutiny should be used
-          // on either of these paths!
-          //
+          /**
+           * Populate invalid mesh areas. Proceed with caution.
+           * Two choices are available:
+           *   - Specify a constant with the 'C' parameter.
+           *   - Allow 'G29 P3' to choose a 'reasonable' constant.
+           */
           if (c_flag) {
-          while (repetition_cnt--) {
+            while (repetition_cnt--) {
               const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
-            if (location.x_index < 0) break; // No more invalid Mesh Points to populate
-              ubl.z_values[location.x_index][location.y_index] = ubl_constant;
+              if (location.x_index < 0) break; // No more invalid Mesh Points to populate
+                ubl.z_values[location.x_index][location.y_index] = ubl_constant;
             }
             break;
-          } else                    // The user wants to do a 'Smart' fill where we use the surrounding known
-              smart_fill_mesh();    // values to provide a good guess of what the unprobed mesh point should be
-          break;
           }
+          else
+            smart_fill_mesh(); // Do a 'Smart' fill using nearby known values
+
+        } break;
 
         case 4:
           //
@@ -535,9 +536,9 @@
 
     if (code_seen('T')) {
 
-      float z1 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
-            z2 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
-            z3 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
+      float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
+            z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
+            z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
 
       //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
       //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
@@ -606,8 +607,8 @@
               SERIAL_ECHOPAIR(" J ", y);
               SERIAL_ECHOPGM(" Z ");
               SERIAL_ECHO_F(ubl.z_values[x][y], 6);
-              SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[x]))));
-              SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[y]))));
+              SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[x])));
+              SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[y])));
               SERIAL_EOL;
             }
         return;
@@ -653,9 +654,9 @@
         } while (!ubl_lcd_clicked());
 
         ubl.has_control_of_lcd_panel = true;   // There is a race condition for the Encoder Wheel getting clicked.
-                                          // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
-                                          // or here. So, until we are done looking for a long Encoder Wheel Press,
-                                          // we need to take control of the panel
+                                               // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
+                                               // or here. So, until we are done looking for a long Encoder Wheel Press,
+                                               // we need to take control of the panel
 
         KEEPALIVE_STATE(IN_HANDLER);
 
@@ -692,44 +693,39 @@
   }
 
   void unified_bed_leveling::find_mean_mesh_height() {
-    uint8_t x, y;
-    int n;
-    float sum, sum_of_diff_squared, sigma, difference, mean;
-
-    sum = sum_of_diff_squared = 0.0;
-    n = 0;
-    for (x = 0; x < GRID_MAX_POINTS_X; x++)
-      for (y = 0; y < GRID_MAX_POINTS_Y; y++)
+    float sum = 0.0;
+    int n = 0;
+    for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
+      for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
         if (!isnan(ubl.z_values[x][y])) {
           sum += ubl.z_values[x][y];
           n++;
         }
 
-    mean = sum / n;
+    const float mean = sum / n;
 
     //
     // Now do the sumation of the squares of difference from mean
     //
-    for (x = 0; x < GRID_MAX_POINTS_X; x++)
-      for (y = 0; y < GRID_MAX_POINTS_Y; y++)
-        if (!isnan(ubl.z_values[x][y])) {
-          difference = (ubl.z_values[x][y] - mean);
-          sum_of_diff_squared += difference * difference;
-        }
+    float sum_of_diff_squared = 0.0;
+    for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
+      for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
+        if (!isnan(ubl.z_values[x][y]))
+          sum_of_diff_squared += sq(ubl.z_values[x][y] - mean);
 
     SERIAL_ECHOLNPAIR("# of samples: ", n);
     SERIAL_ECHOPGM("Mean Mesh Height: ");
     SERIAL_ECHO_F(mean, 6);
     SERIAL_EOL;
 
-    sigma = sqrt(sum_of_diff_squared / (n + 1));
+    const float sigma = sqrt(sum_of_diff_squared / (n + 1));
     SERIAL_ECHOPGM("Standard Deviation: ");
     SERIAL_ECHO_F(sigma, 6);
     SERIAL_EOL;
 
     if (c_flag)
-      for (x = 0; x < GRID_MAX_POINTS_X; x++)
-        for (y = 0; y < GRID_MAX_POINTS_Y; y++)
+      for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
+        for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
           if (!isnan(ubl.z_values[x][y]))
             ubl.z_values[x][y] -= mean + ubl_constant;
   }
@@ -767,8 +763,8 @@
       location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, do_furthest);
       if (location.x_index >= 0 && location.y_index >= 0) {
 
-        const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])),
-                    rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index]));
+        const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
+                    rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
 
         // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
         if (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) {
@@ -797,7 +793,6 @@
   }
 
   void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
-    float d, t, inv_z;
     int i, j;
 
     matrix_3x3 rotation;
@@ -818,96 +813,94 @@
      * However, we don't know its direction. We need it to point up. So if
      * Z is negative, we need to invert the sign of all components of the vector
      */
-    if ( normal.z < 0.0 ) {
+    if (normal.z < 0.0) {
       normal.x = -normal.x;
       normal.y = -normal.y;
       normal.z = -normal.z;
     }
 
-    rotation = matrix_3x3::create_look_at( vector_3( normal.x,  normal.y, 1));
+    rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1));
 
-    if (g29_verbose_level>2) {
+    if (g29_verbose_level > 2) {
       SERIAL_ECHOPGM("bed plane normal = [");
-      SERIAL_PROTOCOL_F( normal.x, 7);
-      SERIAL_ECHOPGM(",");
-      SERIAL_PROTOCOL_F( normal.y, 7);
-      SERIAL_ECHOPGM(",");
-      SERIAL_PROTOCOL_F( normal.z, 7);
-      SERIAL_ECHOPGM("]\n");
-      rotation.debug("rotation matrix:");
+      SERIAL_PROTOCOL_F(normal.x, 7);
+      SERIAL_PROTOCOLCHAR(',');
+      SERIAL_PROTOCOL_F(normal.y, 7);
+      SERIAL_PROTOCOLCHAR(',');
+      SERIAL_PROTOCOL_F(normal.z, 7);
+      SERIAL_ECHOLNPGM("]");
+      rotation.debug(PSTR("rotation matrix:"));
     }
 
     //
     // All of 3 of these points should give us the same d constant
     //
 
-    t = normal.x * UBL_PROBE_PT_1_X + normal.y * UBL_PROBE_PT_1_Y;
-    d = t + normal.z * z1;
+    float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
+          d = t + normal.z * z1;
 
     if (g29_verbose_level>2) {
       SERIAL_ECHOPGM("D constant: ");
-      SERIAL_PROTOCOL_F( d, 7);
-      SERIAL_ECHOPGM(" \n");
+      SERIAL_PROTOCOL_F(d, 7);
+      SERIAL_ECHOLNPGM(" ");
     }
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) {
-    SERIAL_ECHOPGM("d from 1st point: ");
-    SERIAL_ECHO_F(d, 6);
-    SERIAL_EOL;
-        t = normal.x * UBL_PROBE_PT_2_X + normal.y * UBL_PROBE_PT_2_Y;
-    d = t + normal.z * z2;
-    SERIAL_ECHOPGM("d from 2nd point: ");
-    SERIAL_ECHO_F(d, 6);
-    SERIAL_EOL;
-        t = normal.x * UBL_PROBE_PT_3_X + normal.y * UBL_PROBE_PT_3_Y;
-    d = t + normal.z * z3;
-    SERIAL_ECHOPGM("d from 3rd point: ");
-    SERIAL_ECHO_F(d, 6);
-    SERIAL_EOL;
+        SERIAL_ECHOPGM("d from 1st point: ");
+        SERIAL_ECHO_F(d, 6);
+        SERIAL_EOL;
+        t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
+        d = t + normal.z * z2;
+        SERIAL_ECHOPGM("d from 2nd point: ");
+        SERIAL_ECHO_F(d, 6);
+        SERIAL_EOL;
+        t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
+        d = t + normal.z * z3;
+        SERIAL_ECHOPGM("d from 3rd point: ");
+        SERIAL_ECHO_F(d, 6);
+        SERIAL_EOL;
       }
     #endif
 
-    for (i = 0; i < GRID_MAX_POINTS_X; i++) {
-      for (j = 0; j < GRID_MAX_POINTS_Y; j++) {
-        float x_tmp, y_tmp, z_tmp;
-          x_tmp = pgm_read_float(ubl.mesh_index_to_xpos[i]);
-          y_tmp = pgm_read_float(ubl.mesh_index_to_ypos[j]);
-          z_tmp = ubl.z_values[i][j];
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOPGM("before rotation = [");
-              SERIAL_PROTOCOL_F( x_tmp, 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( y_tmp, 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( z_tmp, 7);
-              SERIAL_ECHOPGM("]   ---> ");
-              safe_delay(20);
+    for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+      for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
+        float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]),
+              y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]),
+              z_tmp = ubl.z_values[i][j];
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPGM("before rotation = [");
+            SERIAL_PROTOCOL_F(x_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(y_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(z_tmp, 7);
+            SERIAL_ECHOPGM("]   ---> ");
+            safe_delay(20);
+          }
+        #endif
+        apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPGM("after rotation = [");
+            SERIAL_PROTOCOL_F(x_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(y_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(z_tmp, 7);
+            SERIAL_ECHOLNPGM("]");
+            safe_delay(55);
+          }
+        #endif
+        ubl.z_values[i][j] += z_tmp - d;
       }
-          #endif
-          apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOPGM("after rotation = [");
-              SERIAL_PROTOCOL_F( x_tmp, 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( y_tmp, 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( z_tmp, 7);
-              SERIAL_ECHOPGM("]\n");
-              safe_delay(55);
-    }
-          #endif
-          ubl.z_values[i][j] += z_tmp - d;
-  }
     }
-    return;
   }
 
   float use_encoder_wheel_to_measure_point() {
 
-    while (ubl_lcd_clicked()) delay(50);;  // wait for user to release encoder wheel
+    while (ubl_lcd_clicked()) delay(50);  // wait for user to release encoder wheel
     delay(50);  // debounce
 
     KEEPALIVE_STATE(PAUSED_FOR_USER);
@@ -922,24 +915,29 @@
     return current_position[Z_AXIS];
   }
 
-  float measure_business_card_thickness(const float &in_height) {
+  static void say_and_take_a_measurement() {
+    SERIAL_PROTOCOLLNPGM(" and take a measurement.");
+  }
 
+  float measure_business_card_thickness(const float &in_height) {
     ubl.has_control_of_lcd_panel = true;
-    ubl.save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
+    ubl.save_ubl_active_state_and_disable();   // Disable bed level correction for probing
 
     do_blocking_move_to_z(in_height);
-    do_blocking_move_to_xy((float(UBL_MESH_MAX_X) - float(UBL_MESH_MIN_X)) / 2.0, (float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / 2.0);
-      //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0);
+    do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
+      //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]) / 2.0);
 
     stepper.synchronize();
-    SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement.");
-
+    SERIAL_PROTOCOLPGM("Place shim under nozzle");
+    say_and_take_a_measurement();
 
     const float z1 = use_encoder_wheel_to_measure_point();
     do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
-
     stepper.synchronize();
-    SERIAL_PROTOCOLLNPGM("Remove Shim and Measure Bed Height.");
+
+    SERIAL_PROTOCOLPGM("Remove shim");
+    say_and_take_a_measurement();
+
     const float z2 = use_encoder_wheel_to_measure_point();
     do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
 
@@ -968,8 +966,8 @@
       // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
       if (location.x_index < 0 && location.y_index < 0) continue;
 
-      const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])),
-                  rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index]));
+      const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
+                  rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
 
       // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
       if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) {
@@ -999,11 +997,9 @@
 
       if (do_ubl_mesh_map) ubl.display_map(map_type);  // show user where we're probing
 
-
-      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!
+      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 (ubl.encoder_diff) {
           do_blocking_move_to_z(current_position[Z_AXIS] + float(ubl.encoder_diff) / 100.0);
@@ -1011,7 +1007,6 @@
         }
       }
 
-
       const millis_t nxt = millis() + 1500L;
       while (ubl_lcd_clicked()) {     // debounce and watch for abort
         idle();
@@ -1044,33 +1039,43 @@
     do_blocking_move_to_xy(lx, ly);
   }
 
+  static void say_ubl_name() {
+    SERIAL_PROTOCOLPGM("Unified Bed Leveling ");
+  }
+
+  static void report_ubl_state() {
+    say_ubl_name();
+    SERIAL_PROTOCOLPGM("System ");
+    if (!ubl.state.active) SERIAL_PROTOCOLPGM("de");
+    SERIAL_PROTOCOLLNPGM("activated.\n");
+  }
+ 
   bool g29_parameter_parsing() {
     bool err_flag = false;
 
-      LCD_MESSAGEPGM("Doing G29 UBL!");
+    LCD_MESSAGEPGM("Doing G29 UBL!");
+    lcd_quick_feedback();
+
     ubl_constant = 0.0;
     repetition_cnt = 0;
-      lcd_quick_feedback();
 
     x_flag = code_seen('X') && code_has_value();
     x_pos = x_flag ? code_value_float() : current_position[X_AXIS];
-
     y_flag = code_seen('Y') && code_has_value();
     y_pos = y_flag ? code_value_float() : current_position[Y_AXIS];
 
-
     repeat_flag = code_seen('R');
     if (repeat_flag) {
       repetition_cnt = code_has_value() ? code_value_int() : (GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y);
       if (repetition_cnt < 1) {
-        SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n");
+        SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n");
         return UBL_ERR;
       }
     }
 
     g29_verbose_level = code_seen('V') ? code_value_int() : 0;
     if (!WITHIN(g29_verbose_level, 0, 4)) {
-      SERIAL_PROTOCOLLNPGM("Invalid Verbose Level specified. (0-4)\n");
+      SERIAL_PROTOCOLLNPGM("?(V)erbose Level is implausible (0-4)\n");
       err_flag = true;
     }
 
@@ -1099,32 +1104,35 @@
 
     if (err_flag) return UBL_ERR;
 
-    if (code_seen('A')) {     // Activate the Unified Bed Leveling System
+    // Activate or deactivate UBL
+    if (code_seen('A')) {
+      if (code_seen('D')) {
+        SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
+        return UBL_ERR;
+      }
       ubl.state.active = 1;
-      SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System activated.\n");
+      report_ubl_state();
     }
-
-    c_flag = code_seen('C');
-    if (c_flag)
-      ubl_constant = code_value_float();
-
-    if (code_seen('D')) {     // Disable the Unified Bed Leveling System
+    else if (code_seen('D')) {
       ubl.state.active = 0;
-      SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System de-activated.\n");
+      report_ubl_state();
     }
 
+    // Set global 'C' flag and its value
+    if ((c_flag = code_seen('C')))
+      ubl_constant = code_value_float();
+
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
       if (code_seen('F') && code_has_value()) {
         const float fh = code_value_float();
         if (!WITHIN(fh, 0.0, 100.0)) {
-          SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n");
+          SERIAL_PROTOCOLLNPGM("?(F)ade height for Bed Level Correction not plausible.\n");
           return UBL_ERR;
         }
         set_z_fade_height(fh);
       }
     #endif
 
-
     map_type = code_seen('O') && code_has_value() ? code_value_int() : 0;
     if (!WITHIN(map_type, 0, 1)) {
       SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
@@ -1146,7 +1154,7 @@
    * This function goes away after G29 debug is complete. But for right now, it is a handy
    * routine to dump binary data structures.
    */
-/*
+  /*
   void dump(char * const str, const float &f) {
     char *ptr;
 
@@ -1164,7 +1172,7 @@
 
     SERIAL_EOL;
   }
-*/
+  //*/
 
   static int ubl_state_at_invocation = 0,
              ubl_state_recursion_chk = 0;
@@ -1191,7 +1199,6 @@
     ubl.state.active = ubl_state_at_invocation;
   }
 
-
   /**
    * Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
    * good to have the extra information. Soon... we prune this to just a few items
@@ -1199,7 +1206,8 @@
   void g29_what_command() {
     const uint16_t k = E2END - ubl.eeprom_start;
 
-    SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version " UBL_VERSION " ");
+    say_ubl_name();
+    SERIAL_PROTOCOLPGM("System Version " UBL_VERSION " ");
     if (ubl.state.active)
       SERIAL_PROTOCOLCHAR('A');
     else
@@ -1230,11 +1238,11 @@
     SERIAL_EOL;
     safe_delay(25);
 
-    SERIAL_PROTOCOLLNPAIR("ubl.eeprom_start=0x", hex_word(ubl.eeprom_start));
+    SERIAL_PROTOCOLLNPAIR("ubl.eeprom_start=", hex_address((void*)ubl.eeprom_start));
 
     SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: ");
     for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
-      SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[i]))), 1);
+      SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[i])), 1);
       SERIAL_PROTOCOLPGM("  ");
       safe_delay(50);
     }
@@ -1242,7 +1250,7 @@
 
     SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
     for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) {
-      SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[i]))), 1);
+      SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[i])), 1);
       SERIAL_PROTOCOLPGM("  ");
       safe_delay(50);
     }
@@ -1296,8 +1304,10 @@
     SERIAL_EOL;
     safe_delay(50);
 
-    if (!ubl.sanity_check())
-      SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed.");
+    if (!ubl.sanity_check()) {
+      say_ubl_name();
+      SERIAL_PROTOCOLLNPGM("sanity checks passed.");
+    }
   }
 
   /**
@@ -1357,18 +1367,18 @@
         ubl.z_values[x][y] -= tmp_z_values[x][y];
   }
 
-  mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], bool far_flag) {
-    float distance, closest = far_flag ? -99999.99 : 99999.99;
-    mesh_index_pair return_val;
-
-    return_val.x_index = return_val.y_index = -1;
+  mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], const bool far_flag) {
+    mesh_index_pair out_mesh;
+    out_mesh.x_index = out_mesh.y_index = -1;
 
     const float current_x = current_position[X_AXIS],
                 current_y = current_position[Y_AXIS];
 
     // Get our reference position. Either the nozzle or probe location.
-    const float px = lx - (probe_as_reference==USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
-                py = ly - (probe_as_reference==USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
+    const float px = lx - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
+                py = ly - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
+
+    float closest = far_flag ? -99999.99 : 99999.99;
 
     for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
       for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
@@ -1380,13 +1390,13 @@
 
           // We only get here if we found a Mesh Point of the specified type
 
-          const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[i])), // Check if we can probe this mesh location
-                      rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[j]));
+          const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // Check if we can probe this mesh location
+                      rawy = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
 
           // If using the probe as the reference there are some unreachable locations.
           // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
 
-          if (probe_as_reference==USE_PROBE_AS_REFERENCE &&
+          if (probe_as_reference == USE_PROBE_AS_REFERENCE &&
             (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y))
           ) continue;
 
@@ -1396,30 +1406,38 @@
           const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location
                       my = LOGICAL_Y_POSITION(rawy);
 
-          distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.1;
-
-          if (far_flag) {                                           // If doing the far_flag action, we want to be as far as possible
-            for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) {   // from the starting point and from any other probed points.  We
-              for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) { // want the next point spread out and filling in any blank spaces
-                if (!isnan(ubl.z_values[k][l])) {                       // in the mesh. So we add in some of the distance to every probed
-                  distance += sq(i - k) * (MESH_X_DIST) * .05       // point we can find.
+          float distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.1;
+
+          /**
+           * If doing the far_flag action, we want to be as far as possible
+           * from the starting point and from any other probed points. We
+           * want the next point spread out and filling in any blank spaces
+           * in the mesh. So we add in some of the distance to every probed
+           * point we can find.
+           */
+          if (far_flag) {
+            for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
+              for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
+                if (!isnan(ubl.z_values[k][l])) {
+                  distance += sq(i - k) * (MESH_X_DIST) * .05
                             + sq(j - l) * (MESH_Y_DIST) * .05;
                 }
               }
             }
           }
 
-          if (far_flag == (distance > closest) && distance != closest) {  // if far_flag, look for farthest point
+          // if far_flag, look for farthest point
+          if (far_flag == (distance > closest) && distance != closest) {
             closest = distance;       // We found a closer/farther location with
-            return_val.x_index = i;   // the specified type of mesh value.
-            return_val.y_index = j;
-            return_val.distance = closest;
+            out_mesh.x_index = i;     // the specified type of mesh value.
+            out_mesh.y_index = j;
+            out_mesh.distance = closest;
           }
         }
       } // for j
     } // for i
 
-    return return_val;
+    return out_mesh;
   }
 
   void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
@@ -1439,15 +1457,15 @@
     do_blocking_move_to_xy(lx, ly);
     do {
       location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false);
-                                                                                              // It doesn't matter if the probe can not reach this
-                                                                                              // location. This is a manual edit of the Mesh Point.
+                                                                  // It doesn't matter if the probe can't reach this
+                                                                  // location. This is a manual edit of the Mesh Point.
       if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points.
 
       bit_clear(not_done, location.x_index, location.y_index);  // Mark this location as 'adjusted' so we will find a
                                                                 // different location the next time through the loop
 
-      const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])),
-                  rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index]));
+      const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
+                  rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
 
       // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
       if (!WITHIN(rawx, X_MIN_POS, X_MAX_POS) || !WITHIN(rawy, Y_MIN_POS, Y_MAX_POS)) { // In theory, we don't need this check.
@@ -1464,45 +1482,31 @@
         do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);    // Move the nozzle to where we are going to edit
         do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy));
 
-
-
-
-
-
         round_off = (int32_t)(new_z * 1000.0);    // we chop off the last digits just to be clean. We are rounding to the
         new_z = float(round_off) / 1000.0;
 
-
-
         KEEPALIVE_STATE(PAUSED_FOR_USER);
         ubl.has_control_of_lcd_panel = true;
 
-
         if (do_ubl_mesh_map) ubl.display_map(map_type);  // show the user which point is being adjusted
 
-
-
         lcd_implementation_clear();
 
         lcd_mesh_edit_setup(new_z);
 
-
-
         do {
           new_z = lcd_mesh_edit();
           idle();
         } while (!ubl_lcd_clicked());
 
-
         lcd_return_to_status();
 
-
-
-
-        ubl.has_control_of_lcd_panel = true; // There is a race condition for the Encoder Wheel getting clicked.
-                                             // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
-                                             // or here.
+        // There is a race condition for the Encoder Wheel getting clicked.
+        // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
+        // or here.
+        ubl.has_control_of_lcd_panel = true;
       }
+
       const millis_t nxt = millis() + 1500UL;
       while (ubl_lcd_clicked()) { // debounce and watch for abort
         idle();
@@ -1621,120 +1625,115 @@
 
 
   void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
-    int8_t i, j ,k, xCount, yCount, xi, yi;  // counter variables
-    int8_t ix, iy, zig_zag=0, status;
+    constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
+                      x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X),
+                      y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y),
+                      y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y);
+
+    const float dx = float(x_max - x_min) / (grid_size - 1.0),
+                dy = float(y_max - y_min) / (grid_size - 1.0);
 
-    float dx, dy, x, y, measured_z, inv_z;
     struct linear_fit_data lsf_results;
-    matrix_3x3 rotation;
-    vector_3 normal;
+    incremental_LSF_reset(&lsf_results);
 
-    int16_t x_min = max((MIN_PROBE_X),(UBL_MESH_MIN_X)),
-            x_max = min((MAX_PROBE_X),(UBL_MESH_MAX_X)),
-            y_min = max((MIN_PROBE_Y),(UBL_MESH_MIN_Y)),
-            y_max = min((MAX_PROBE_Y),(UBL_MESH_MAX_Y));
+    bool zig_zag = false;
+    for (uint8_t ix = 0; ix < grid_size; ix++) {
+      const float x = float(x_min) + ix * dx;
+      for (int8_t iy = 0; iy < grid_size; iy++) {
+        const float y = float(y_min) + dy * (zig_zag ? grid_size - 1 - iy : iy);
+        float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_CHAR('(');
+            SERIAL_PROTOCOL_F(x, 7);
+            SERIAL_CHAR(',');
+            SERIAL_PROTOCOL_F(y, 7);
+            SERIAL_ECHOPGM(")   logical: ");
+            SERIAL_CHAR('(');
+            SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
+            SERIAL_CHAR(',');
+            SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
+            SERIAL_ECHOPGM(")   measured: ");
+            SERIAL_PROTOCOL_F(measured_z, 7);
+            SERIAL_ECHOPGM("   correction: ");
+            SERIAL_PROTOCOL_F(ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
+          }
+        #endif
 
-    dx = ((float)(x_max-x_min)) / (grid_size-1.0);
-    dy = ((float)(y_max-y_min)) / (grid_size-1.0);
+        measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
 
-    incremental_LSF_reset(&lsf_results);
-    for(ix=0; ix<grid_size; ix++) {
-      x = ((float)x_min) + ix*dx;
-      for(iy=0; iy<grid_size; iy++) {
-        if (zig_zag)
-          y = ((float)y_min) + (grid_size-iy-1)*dy;
-        else
-          y = ((float)y_min) + iy*dy;
-          measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level);
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOPGM("(");
-              SERIAL_PROTOCOL_F( x, 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( y, 7);
-              SERIAL_ECHOPGM(")   logical: ");
-              SERIAL_ECHOPGM("(");
-              SERIAL_PROTOCOL_F( LOGICAL_X_POSITION(x), 7);
-              SERIAL_ECHOPGM(",");
-              SERIAL_PROTOCOL_F( LOGICAL_X_POSITION(y), 7);
-              SERIAL_ECHOPGM(")   measured: ");
-              SERIAL_PROTOCOL_F( measured_z, 7);
-              SERIAL_ECHOPGM("   correction: ");
-              SERIAL_PROTOCOL_F( ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPGM("   final >>>---> ");
+            SERIAL_PROTOCOL_F(measured_z, 7);
+            SERIAL_EOL;
           }
-          #endif
-          measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
-
-          #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOPGM("   final >>>---> ");
-              SERIAL_PROTOCOL_F( measured_z, 7);
-              SERIAL_ECHOPGM("\n");
-            }
-          #endif
-          incremental_LSF(&lsf_results, x, y, measured_z);
-        }
+        #endif
 
-        zig_zag = !zig_zag;
+        incremental_LSF(&lsf_results, x, y, measured_z);
       }
 
-    status = finish_incremental_LSF(&lsf_results);
-    if (g29_verbose_level>3) {
+      zig_zag ^= true;
+    }
+
+    const int status = finish_incremental_LSF(&lsf_results);
+
+    if (g29_verbose_level > 3) {
       SERIAL_ECHOPGM("LSF Results A=");
-      SERIAL_PROTOCOL_F( lsf_results.A, 7);
+      SERIAL_PROTOCOL_F(lsf_results.A, 7);
       SERIAL_ECHOPGM("  B=");
-      SERIAL_PROTOCOL_F( lsf_results.B, 7);
+      SERIAL_PROTOCOL_F(lsf_results.B, 7);
       SERIAL_ECHOPGM("  D=");
-      SERIAL_PROTOCOL_F( lsf_results.D, 7);
-      SERIAL_CHAR('\n');
+      SERIAL_PROTOCOL_F(lsf_results.D, 7);
+      SERIAL_EOL;
     }
 
-    normal = vector_3( lsf_results.A, lsf_results.B, 1.0000);
-    normal = normal.get_normal();
+    vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
 
-    if (g29_verbose_level>2) {
+    if (g29_verbose_level > 2) {
       SERIAL_ECHOPGM("bed plane normal = [");
-      SERIAL_PROTOCOL_F( normal.x, 7);
-      SERIAL_ECHOPGM(",");
-      SERIAL_PROTOCOL_F( normal.y, 7);
-      SERIAL_ECHOPGM(",");
-      SERIAL_PROTOCOL_F( normal.z, 7);
-      SERIAL_ECHOPGM("]\n");
+      SERIAL_PROTOCOL_F(normal.x, 7);
+      SERIAL_PROTOCOLCHAR(',');
+      SERIAL_PROTOCOL_F(normal.y, 7);
+      SERIAL_PROTOCOLCHAR(',');
+      SERIAL_PROTOCOL_F(normal.z, 7);
+      SERIAL_ECHOLNPGM("]");
     }
 
-    rotation = matrix_3x3::create_look_at( vector_3( lsf_results.A,  lsf_results.B, 1));
+    matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
+
+    for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+      for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
+        float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]),
+              y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]),
+              z_tmp = ubl.z_values[i][j];
 
-    for (i = 0; i < GRID_MAX_POINTS_X; i++) {
-      for (j = 0; j < GRID_MAX_POINTS_Y; j++) {
-        float x_tmp, y_tmp, z_tmp;
-        x_tmp = pgm_read_float(&(ubl.mesh_index_to_xpos[i]));
-        y_tmp = pgm_read_float(&(ubl.mesh_index_to_ypos[j]));
-        z_tmp = ubl.z_values[i][j];
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
             SERIAL_ECHOPGM("before rotation = [");
-            SERIAL_PROTOCOL_F( x_tmp, 7);
-            SERIAL_ECHOPGM(",");
-            SERIAL_PROTOCOL_F( y_tmp, 7);
-            SERIAL_ECHOPGM(",");
-            SERIAL_PROTOCOL_F( z_tmp, 7);
+            SERIAL_PROTOCOL_F(x_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(y_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(z_tmp, 7);
             SERIAL_ECHOPGM("]   ---> ");
             safe_delay(20);
           }
         #endif
+
         apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
+
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
             SERIAL_ECHOPGM("after rotation = [");
-            SERIAL_PROTOCOL_F( x_tmp, 7);
-            SERIAL_ECHOPGM(",");
-            SERIAL_PROTOCOL_F( y_tmp, 7);
-            SERIAL_ECHOPGM(",");
-            SERIAL_PROTOCOL_F( z_tmp, 7);
-            SERIAL_ECHOPGM("]\n");
+            SERIAL_PROTOCOL_F(x_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(y_tmp, 7);
+            SERIAL_PROTOCOLCHAR(',');
+            SERIAL_PROTOCOL_F(z_tmp, 7);
+            SERIAL_ECHOLNPGM("]");
             safe_delay(55);
           }
-
         #endif
 
         ubl.z_values[i][j] += z_tmp - lsf_results.D;
@@ -1743,27 +1742,26 @@
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) {
-        rotation.debug("rotation matrix:");
+        rotation.debug(PSTR("rotation matrix:"));
         SERIAL_ECHOPGM("LSF Results A=");
-        SERIAL_PROTOCOL_F( lsf_results.A, 7);
+        SERIAL_PROTOCOL_F(lsf_results.A, 7);
         SERIAL_ECHOPGM("  B=");
-        SERIAL_PROTOCOL_F( lsf_results.B, 7);
+        SERIAL_PROTOCOL_F(lsf_results.B, 7);
         SERIAL_ECHOPGM("  D=");
-        SERIAL_PROTOCOL_F( lsf_results.D, 7);
-        SERIAL_CHAR('\n');
+        SERIAL_PROTOCOL_F(lsf_results.D, 7);
+        SERIAL_EOL;
         safe_delay(55);
 
         SERIAL_ECHOPGM("bed plane normal = [");
-        SERIAL_PROTOCOL_F( normal.x, 7);
-        SERIAL_ECHOPGM(",");
-        SERIAL_PROTOCOL_F( normal.y, 7);
-        SERIAL_ECHOPGM(",");
-        SERIAL_PROTOCOL_F( normal.z, 7);
+        SERIAL_PROTOCOL_F(normal.x, 7);
+        SERIAL_PROTOCOLCHAR(',');
+        SERIAL_PROTOCOL_F(normal.y, 7);
+        SERIAL_PROTOCOLCHAR(',');
+        SERIAL_PROTOCOL_F(normal.z, 7);
         SERIAL_ECHOPGM("]\n");
-        SERIAL_CHAR('\n');
+        SERIAL_EOL;
       }
     #endif
-    return;
   }
 
 #endif // AUTO_BED_LEVELING_UBL

Marlin/ubl_motion.cpp

@@ -154,7 +154,7 @@
        * 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 = (RAW_X_POSITION(end[X_AXIS]) - pgm_read_float(&(ubl.mesh_index_to_xpos[cell_dest_xi]))) * (1.0 / (MESH_X_DIST)),
+      const float xratio = (RAW_X_POSITION(end[X_AXIS]) - pgm_read_float(&ubl.mesh_index_to_xpos[cell_dest_xi])) * (1.0 / (MESH_X_DIST)),
                   z1 = ubl.z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
                       (ubl.z_values[cell_dest_xi + 1][cell_dest_yi    ] - ubl.z_values[cell_dest_xi][cell_dest_yi    ]),
                   z2 = ubl.z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
@@ -163,7 +163,7 @@
       // 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 = (RAW_Y_POSITION(end[Y_AXIS]) - pgm_read_float(&(ubl.mesh_index_to_ypos[cell_dest_yi]))) * (1.0 / (MESH_Y_DIST));
+      const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - pgm_read_float(&ubl.mesh_index_to_ypos[cell_dest_yi])) * (1.0 / (MESH_Y_DIST));
 
       float z0 = z1 + (z2 - z1) * yratio;
 
@@ -198,8 +198,8 @@
     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 : 0.0,
-              down_flag = dy < 0.0 ? 1.0 : 0.0;
+    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;
@@ -230,8 +230,8 @@
     const float m = dy / dx,
                 c = start[Y_AXIS] - m * start[X_AXIS];
 
-    const bool inf_normalized_flag=isinf(e_normalized_dist),
-               inf_m_flag=isinf(m);
+    const bool inf_normalized_flag = isinf(e_normalized_dist),
+               inf_m_flag = isinf(m);
     /**
      * 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
@@ -241,7 +241,7 @@
       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 = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi])));
+        const float next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi]));
 
         /**
          * if the slope of the line is infinite, we won't do the calculations
@@ -263,7 +263,7 @@
          */
         if (isnan(z0)) z0 = 0.0;
 
-        const float y = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi])));
+        const float y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi]));
 
         /**
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
@@ -321,7 +321,7 @@
                                 // 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 = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi]))),
+        const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi])),
                     y = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
 
         float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
@@ -337,7 +337,7 @@
          */
         if (isnan(z0)) z0 = 0.0;
 
-        const float x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi])));
+        const float x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi]));
 
         /**
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
@@ -393,8 +393,8 @@
 
     while (xi_cnt > 0 || yi_cnt > 0) {
 
-      const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi + dxi]))),
-                  next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi + dyi]))),
+      const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi + dxi])),
+                  next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi + dyi])),
                   y = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
                   x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
                                                   // (No need to worry about m being zero.