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/least_squares_fit.cpp

 
 #include "least_squares_fit.h"
 
-void incremental_LSF_reset(struct linear_fit_data *lsf) {
-  memset(lsf, 0, sizeof(linear_fit_data));
-}
+int finish_incremental_LSF(struct linear_fit_data *lsf) {
 
-void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
-  lsf->xbar += x;
-  lsf->ybar += y;
-  lsf->zbar += z;
-  lsf->x2bar += sq(x);
-  lsf->y2bar += sq(y);
-  lsf->z2bar += sq(z);
-  lsf->xybar += x * y;
-  lsf->xzbar += x * z;
-  lsf->yzbar += y * z;
-  lsf->max_absx = max(fabs(x), lsf->max_absx);
-  lsf->max_absy = max(fabs(y), lsf->max_absy);
-  lsf->n++;
-}
+  const float N = lsf->N;
 
-int finish_incremental_LSF(struct linear_fit_data *lsf) {
-  const float N = (float)lsf->n;
+  if (N == 0.0)
+    return 1;
 
   lsf->xbar /= N;
   lsf->ybar /= N;

Marlin/least_squares_fit.h

 #include <math.h>
 
 struct linear_fit_data {
-  int n;
   float xbar, ybar, zbar,
         x2bar, y2bar, z2bar,
         xybar, xzbar, yzbar,
         max_absx, max_absy,
-        A, B, D;
+        A, B, D, N;
 };
 
-void incremental_LSF_reset(struct linear_fit_data *);
-void incremental_LSF(struct linear_fit_data *, float x, float y, float z);
+void inline incremental_LSF_reset(struct linear_fit_data *lsf) {
+  memset(lsf, 0, sizeof(linear_fit_data));
+}
+
+void inline incremental_WLSF(struct linear_fit_data *lsf, float x, float y, float z, float w) {
+  // weight each accumulator by factor w, including the "number" of samples
+  // (analagous to calling inc_LSF twice with same values to weight it by 2X)
+  lsf->xbar  += w * x;
+  lsf->ybar  += w * y;
+  lsf->zbar  += w * z;
+  lsf->x2bar += w * x * x;  // don't use sq(x) -- let compiler re-use w*x four times
+  lsf->y2bar += w * y * y;
+  lsf->z2bar += w * z * z;
+  lsf->xybar += w * x * y;
+  lsf->xzbar += w * x * z;
+  lsf->yzbar += w * y * z;
+  lsf->N     += w;
+  lsf->max_absx = max(fabs( w * x ), lsf->max_absx);
+  lsf->max_absy = max(fabs( w * y ), lsf->max_absy);
+}
+
+void inline incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
+  lsf->xbar += x;
+  lsf->ybar += y;
+  lsf->zbar += z;
+  lsf->x2bar += sq(x);
+  lsf->y2bar += sq(y);
+  lsf->z2bar += sq(z);
+  lsf->xybar += x * y;
+  lsf->xzbar += x * z;
+  lsf->yzbar += y * z;
+  lsf->max_absx = max(fabs(x), lsf->max_absx);
+  lsf->max_absy = max(fabs(y), lsf->max_absy);
+  lsf->N += 1.0;
+}
+
 int finish_incremental_LSF(struct linear_fit_data *);
 
 #endif

Marlin/macros.h

 
 #define FORCE_INLINE __attribute__((always_inline)) inline
 
+#define _O0 __attribute__((optimize("O0")))
+#define _Os __attribute__((optimize("Os")))
+#define _O1 __attribute__((optimize("O1")))
+#define _O2 __attribute__((optimize("O2")))
+#define _O3 __attribute__((optimize("O3")))
+
 // Bracket code that shouldn't be interrupted
 #ifndef CRITICAL_SECTION_START
   #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();

Marlin/ubl_G29.cpp

 #include "MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
-  //#include "vector_3.h"
-  //#include "qr_solve.h"
 
   #include "ubl.h"
   #include "Marlin.h"
   #include <math.h>
   #include "least_squares_fit.h"
 
+  #define UBL_G29_P31
+
   extern float destination[XYZE];
   extern float current_position[XYZE];
 
   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);
 
   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");
               }
             }
           } 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;
         }
     #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