UBL G29 -P3.1 smart fill (#6823)

* UBL G29 -P3.1 mesh fill with distance-weighted least squares fit.

* Back to original -O0 on G29 for now.

Marlin/ubl_G29.cpp

@@ -23,8 +23,6 @@
 #include "MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
-  //#include "vector_3.h"
-  //#include "qr_solve.h"
 
   #include "ubl.h"
   #include "Marlin.h"
@@ -36,6 +34,8 @@
   #include <math.h>
   #include "least_squares_fit.h"
 
+  #define UBL_G29_P31
+
   extern float destination[XYZE];
   extern float current_position[XYZE];
 
@@ -55,6 +55,7 @@
   extern float probe_pt(float x, float y, bool, int);
   extern bool set_probe_deployed(bool);
   void smart_fill_mesh();
+  void smart_fill_wlsf(float);
   float measure_business_card_thickness(float &in_height);
   void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
 
@@ -312,7 +313,7 @@
   extern void lcd_setstatus(const char* message, const bool persist);
   extern void lcd_setstatuspgm(const char* message, const uint8_t level);
 
-  void __attribute__((optimize("O0"))) gcode_G29() {
+  void _O0 gcode_G29() {
 
     if (!settings.calc_num_meshes()) {
       SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it");
@@ -529,7 +530,28 @@
               }
             }
           } else {
-            smart_fill_mesh(); // Do a 'Smart' fill using nearby known values
+            const float cvf = code_value_float();
+            switch( (int)truncf( cvf * 10.0 ) - 30 ) {   // 3.1 -> 1
+              #if ENABLED(UBL_G29_P31)
+                case 1: {
+
+                  // P3.1  use least squares fit to fill missing mesh values
+                  // P3.10 zero weighting for distance, all grid points equal, best fit tilted plane
+                  // P3.11 10X weighting for nearest grid points versus farthest grid points
+                  // P3.12 100X distance weighting
+                  // P3.13 1000X distance weighting, approaches simple average of nearest points
+
+                  const float weight_power  = (cvf - 3.10) * 100.0;  // 3.12345 -> 2.345
+                  const float weight_factor = weight_power ? pow( 10.0, weight_power ) : 0;
+                  smart_fill_wlsf( weight_factor );
+                }
+                break;
+              #endif
+              case 0:   // P3 or P3.0
+              default:  // and anything P3.x that's not P3.1
+                smart_fill_mesh();  // Do a 'Smart' fill using nearby known values
+                break;
+            }
           }
           break;
         }
@@ -1694,4 +1716,66 @@
     #endif
   }
 
+  #if ENABLED(UBL_G29_P31)
+
+    // Note: using optimize("O2") for this routine results in smaller
+    // codegen than default optimize("Os") on A2560.
+
+    void _O2 smart_fill_wlsf( float weight_factor ) {
+
+      // For each undefined mesh point, compute a distance-weighted least squares fit
+      // from all the originally populated mesh points, weighted toward the point
+      // being extrapolated so that nearby points will have greater influence on
+      // the point being extrapolated.  Then extrapolate the mesh point from WLSF.
+
+      static_assert( GRID_MAX_POINTS_Y <= 16, "GRID_MAX_POINTS_Y too big" );
+      uint16_t bitmap[GRID_MAX_POINTS_X] = {0};
+      struct linear_fit_data lsf_results;
+
+      SERIAL_ECHOPGM("Extrapolating mesh...");
+
+      const float weight_scaled = weight_factor * max(MESH_X_DIST, MESH_Y_DIST);
+
+      for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++) {
+        for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++) {
+          if ( !isnan( ubl.z_values[jx][jy] )) {
+            bitmap[jx] |= (uint16_t)1 << jy;
+          }
+        }
+      }
+
+      for (uint8_t ix = 0; ix < GRID_MAX_POINTS_X; ix++) {
+        const float px = pgm_read_float(&(ubl.mesh_index_to_xpos[ix]));
+        for (uint8_t iy = 0; iy < GRID_MAX_POINTS_Y; iy++) {
+          const float py = pgm_read_float(&(ubl.mesh_index_to_ypos[iy]));
+          if ( isnan( ubl.z_values[ix][iy] )) {
+            // undefined mesh point at (px,py), compute weighted LSF from original valid mesh points.
+            incremental_LSF_reset(&lsf_results);
+            for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++) {
+              const float rx = pgm_read_float(&(ubl.mesh_index_to_xpos[jx]));
+              for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++) {
+                if ( bitmap[jx] & (uint16_t)1 << jy ) {
+                  const float ry = pgm_read_float(&(ubl.mesh_index_to_ypos[jy]));
+                  const float rz = ubl.z_values[jx][jy];
+                  const float w  = 1.0 + weight_scaled / HYPOT((rx - px),(ry - py));
+                  incremental_WLSF(&lsf_results, rx, ry, rz, w);
+                }
+              }
+            }
+            if (finish_incremental_LSF(&lsf_results)) {
+              SERIAL_ECHOLNPGM("Insufficient data");
+              return;
+            }
+            const float ez = -lsf_results.D - lsf_results.A * px - lsf_results.B * py;
+            ubl.z_values[ix][iy] = ez;
+            idle();   // housekeeping
+          }
+        }
+      }
+
+      SERIAL_ECHOLNPGM("done");
+    }
+  #endif // UBL_G29_P31
+
+
 #endif // AUTO_BED_LEVELING_UBL