Smart-Fill and Mesh-Tilting (both 3-point and grid) working!

Also...   The memory corruption issue may be fixed.   The GCC compiler
was inlining static functions and this caused the G29() stack frame to
become much larger than the AVR could handle.

Marlin/least_squares_fit.cpp

@@ -26,7 +26,9 @@
  * This algorithm is high speed and has a very small code footprint.
  * Its results are identical to both the Iterative Least-Squares published
  * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
- * it saves roughly 10K of program memory.
+ * it saves roughly 10K of program memory.   It also does not require all of 
+ * coordinates to be present during the calculations.  Each point can be 
+ * probed and then discarded.
  *
  */
 
@@ -34,84 +36,60 @@
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)  // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
 
-#include "ubl.h"
-#include "Marlin.h"
 #include "macros.h"
 #include <math.h>
 
-double linear_fit_average(double m[], const int);
-//double linear_fit_average_squared(double m[], const int);
-//double linear_fit_average_mixed_terms(double m1[], double m2[], const int);
-double linear_fit_average_product(double matrix1[], double matrix2[], const int n);
-void   linear_fit_subtract_mean(double matrix[], double bar, const int n);
-double linear_fit_max_abs(double m[], const int);
-
-linear_fit linear_fit_results;
-
-linear_fit* lsf_linear_fit(double x[], double y[], double z[], const int n) {
-  double xbar, ybar, zbar,
-         x2bar, y2bar,
-         xybar, xzbar, yzbar,
-         D;
-
-  linear_fit_results.A = 0.0;
-  linear_fit_results.B = 0.0;
-  linear_fit_results.D = 0.0;
-
-  xbar = linear_fit_average(x, n);
-  ybar = linear_fit_average(y, n);
-  zbar = linear_fit_average(z, n);
-
-  linear_fit_subtract_mean(x, xbar, n);
-  linear_fit_subtract_mean(y, ybar, n);
-  linear_fit_subtract_mean(z, zbar, n);
-
-  x2bar = linear_fit_average_product(x, x, n);
-  y2bar = linear_fit_average_product(y, y, n);
-  xybar = linear_fit_average_product(x, y, n);
-  xzbar = linear_fit_average_product(x, z, n);
-  yzbar = linear_fit_average_product(y, z, n);
-
-  D = x2bar * y2bar - xybar * xybar;
-  for (int i = 0; i < n; i++) {
-    if (fabs(D) <= 1e-15 * (linear_fit_max_abs(x, n) + linear_fit_max_abs(y, n))) {
-      printf("error: x,y points are collinear at index:%d\n", i);
-      return NULL;
+#include "least_squares_fit.h"
+
+void incremental_LSF_reset(struct linear_fit_data *lsf) {
+	lsf->n = 0;
+	lsf->A = 0.0;					// probably a memset() can be done to zero 
+	lsf->B = 0.0;                                   // this whole structure
+	lsf->D = 0.0;
+	lsf->xbar = lsf->ybar = lsf->zbar = 0.0;
+	lsf->x2bar = lsf->y2bar = lsf->z2bar = 0.0;
+	lsf->xybar = lsf->xzbar = lsf->yzbar = 0.0;
+	lsf->max_absx = lsf->max_absy = 0.0;
     }
-  }
-
-  linear_fit_results.A = -(xzbar * y2bar - yzbar * xybar) / D;
-  linear_fit_results.B = -(yzbar * x2bar - xzbar * xybar) / D;
-  // linear_fit_results.D = -(zbar - linear_fit_results->A * xbar - linear_fit_results->B * ybar);
-  linear_fit_results.D = -(zbar + linear_fit_results.A * xbar + linear_fit_results.B * ybar);
 
-  return &linear_fit_results;
-}
-
-double linear_fit_average(double *matrix, const int n) {
-  double sum = 0.0;
-  for (int i = 0; i < n; i++)
-    sum += matrix[i];
-  return sum / (double)n;
-}
-
-double linear_fit_average_product(double *matrix1, double *matrix2, const int n) {
-  double sum = 0.0;
-  for (int i = 0; i < n; i++)
-    sum += matrix1[i] * matrix2[i];
-  return sum / (double)n;
-}
-
-void linear_fit_subtract_mean(double *matrix, double bar, const int n) {
-  for (int i = 0; i < n; i++)
-    matrix[i] -= bar;
-}
+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 += x*x;
+	lsf->y2bar += y*y;
+	lsf->z2bar += z*z;
+	lsf->xybar += x*y;
+	lsf->xzbar += x*z;
+	lsf->yzbar += y*z;
+	lsf->max_absx = (fabs(x) > lsf->max_absx) ? fabs(x) : lsf->max_absx;
+	lsf->max_absy = (fabs(y) > lsf->max_absy) ? fabs(y) : lsf->max_absy;
+	lsf->n++;
+	return;
+  }
 
-double linear_fit_max_abs(double *matrix, const int n) {
-  double max_abs = 0.0;
-  for (int i = 0; i < n; i++)
-    NOLESS(max_abs, fabs(matrix[i]));
-  return max_abs;
+int finish_incremental_LSF(struct linear_fit_data *lsf) {
+	float DD, N;
+
+	N = (float) lsf->n;
+	lsf->xbar /= N;
+	lsf->ybar /= N;
+	lsf->zbar /= N;
+	lsf->x2bar = lsf->x2bar/N - lsf->xbar*lsf->xbar;
+	lsf->y2bar = lsf->y2bar/N - lsf->ybar*lsf->ybar;
+	lsf->z2bar = lsf->z2bar/N - lsf->zbar*lsf->zbar;
+	lsf->xybar = lsf->xybar/N - lsf->xbar*lsf->ybar;
+	lsf->yzbar = lsf->yzbar/N - lsf->ybar*lsf->zbar;
+	lsf->xzbar = lsf->xzbar/N - lsf->xbar*lsf->zbar;
+
+	DD = lsf->x2bar*lsf->y2bar - lsf->xybar*lsf->xybar;
+	if (fabs(DD) <= 1e-10*(lsf->max_absx+lsf->max_absy)) 
+	  return -1;
+	
+	lsf->A = (lsf->yzbar*lsf->xybar - lsf->xzbar*lsf->y2bar) / DD;
+	lsf->B = (lsf->xzbar*lsf->xybar - lsf->yzbar*lsf->x2bar) / DD;
+	lsf->D = -(lsf->zbar + lsf->A*lsf->xbar + lsf->B*lsf->ybar);
+	return 0;
 }
 #endif
 

Marlin/least_squares_fit.h

@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Incremental Least Squares Best Fit  By Roxy and Ed Williams
+ *
+ * This algorithm is high speed and has a very small code footprint.
+ * Its results are identical to both the Iterative Least-Squares published
+ * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
+ * it saves roughly 10K of program memory.   And even better...  the data
+ * fed into the algorithm does not need to all be present at the same time.  
+ * A point can be probed and its values fed into the algorithm and then discarded.
+ *
+ */
+
+#include "MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)  // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
+
+#include "Marlin.h"
+#include "macros.h"
+#include <math.h>
+
+struct linear_fit_data {
+  int n;
+  float xbar, ybar, zbar;
+  float x2bar, y2bar, z2bar;
+  float xybar, xzbar, yzbar;
+  float max_absx, max_absy;
+  float A, B, D;
+};
+
+void incremental_LSF_reset(struct linear_fit_data *); 
+void incremental_LSF(struct linear_fit_data *, float x, float y, float z);
+int finish_incremental_LSF(struct linear_fit_data *);
+
+#endif
+

Marlin/ubl.cpp

@@ -27,6 +27,7 @@
 
   #include "ubl.h"
   #include "hex_print_routines.h"
+  #include "temperature.h"
 
   /**
    * These support functions allow the use of large bit arrays of flags that take very
@@ -38,6 +39,8 @@
   void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
   bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
 
+  int ubl_cnt=0;
+
   static void serial_echo_xy(const uint16_t x, const uint16_t y) {
     SERIAL_CHAR('(');
     SERIAL_ECHO(x);
@@ -50,9 +53,6 @@
   static void serial_echo_12x_spaces() {
     for (uint8_t i = GRID_MAX_POINTS_X - 1; --i;) {
       SERIAL_ECHOPGM("            ");
-      #if TX_BUFFER_SIZE > 0
-        MYSERIAL.flushTX();
-      #endif
       safe_delay(10);
     }
   }
@@ -70,11 +70,13 @@
   bool unified_bed_leveling::g26_debug_flag = false,
        unified_bed_leveling::has_control_of_lcd_panel = false;
 
-  int8_t unified_bed_leveling::eeprom_start = -1;
+  int16_t unified_bed_leveling::eeprom_start = -1;  // Please stop changing this to 8 bits in size
+                                                    // It needs to hold values bigger than this.
 
   volatile int unified_bed_leveling::encoder_diff;
 
   unified_bed_leveling::unified_bed_leveling() {
+    ubl_cnt++;  // Debug counter to insure we only have one UBL object present in memory.
     reset();
   }
 

Marlin/ubl.h

@@ -26,11 +26,10 @@
 #include "MarlinConfig.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
-
   #include "Marlin.h"
+  #include "planner.h"
   #include "math.h"
   #include "vector_3.h"
-  #include "planner.h"
 
   #define UBL_VERSION "1.00"
   #define UBL_OK false
@@ -49,10 +48,8 @@
   void debug_current_and_destination(const char * const title);
   void ubl_line_to_destination(const float&, uint8_t);
   void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
-  vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&);
   float measure_business_card_thickness(const float&);
   mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
-  void find_mean_mesh_height();
   void shift_mesh_height();
   bool g29_parameter_parsing();
   void g29_what_command();
@@ -67,10 +64,8 @@
   void gcode_G26();
   void gcode_G28();
   void gcode_G29();
-  extern char conv[9];
 
-  void save_ubl_active_state_and_disable();
-  void restore_ubl_active_state_and_leave();
+  extern int ubl_cnt;
 
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -79,7 +74,6 @@
     void lcd_quick_feedback();
   #endif
 
-  enum MBLStatus { MBL_STATUS_NONE = 0, MBL_STATUS_HAS_MESH_BIT = 0, MBL_STATUS_ACTIVE_BIT = 1 };
 
   #define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
   #define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
@@ -97,6 +91,43 @@
 
     public:
 
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
+      void find_mean_mesh_height();
+      void shift_mesh_height();
+      void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
+      void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
+      void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
+      void manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &card_thickness, const bool do_ubl_mesh_map);
+      void save_ubl_active_state_and_disable();
+      void restore_ubl_active_state_and_leave();
+      void g29_what_command(); 
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
+      void g29_eeprom_dump() ;
+      void g29_compare_current_mesh_to_stored_mesh();
+      void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
+      void smart_fill_mesh();
+      void display_map(const int);
+      void reset();
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
+      void invalidate();
+      void store_state();
+      void load_state();
+      void store_mesh(const int16_t);
+      void load_mesh(const int16_t);
+      bool sanity_check();
+
       static ubl_state state;
 
       static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
@@ -125,32 +156,27 @@
 
       static bool g26_debug_flag, has_control_of_lcd_panel;
 
-      static int8_t eeprom_start;
+      static int16_t eeprom_start;    // Please do no change this to 8 bits in size
+                                      // It needs to hold values bigger than this.
 
       static volatile int encoder_diff; // Volatile because it's changed at interrupt time.
 
       unified_bed_leveling();
 
-      static void display_map(const int);
-
-      static void reset();
-      static void invalidate();
-
-      static void store_mesh(const int16_t);
-      static void load_mesh(const int16_t);
-
-      static bool sanity_check();
-
-      static FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
-
-      static int8_t get_cell_index_x(const float &x) {
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
+      FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+        int8_t get_cell_index_x(const float &x) {
         const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
         return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1);   // -1 is appropriate if we want all movement to the X_MAX
       }                                                     // position. But with this defined this way, it is possible
                                                             // to extrapolate off of this point even further out. Probably
                                                             // that is OK because something else should be keeping that from
                                                             // happening and should not be worried about at this level.
-      static int8_t get_cell_index_y(const float &y) {
+      int8_t get_cell_index_y(const float &y) {
         const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
         return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
       }                                                     // position. But with this defined this way, it is possible
@@ -158,12 +184,17 @@
                                                             // that is OK because something else should be keeping that from
                                                             // happening and should not be worried about at this level.
 
-      static int8_t find_closest_x_index(const float &x) {
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
+      int8_t find_closest_x_index(const float &x) {
         const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
         return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
       }
 
-      static int8_t find_closest_y_index(const float &y) {
+      int8_t find_closest_y_index(const float &y) {
         const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
         return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
       }
@@ -183,15 +214,20 @@
        *  It is fairly expensive with its 4 floating point additions and 2 floating point
        *  multiplications.
        */
-      static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+      FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
         return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
       }
+//
+// Please do not put STATIC qualifiers in front of ANYTHING in this file.   You WILL cause problems by doing that.
+// The GCC optimizer inlines static functions and this DRAMATICALLY increases the size of the stack frame of 
+// functions that call STATIC functions.
+//
 
       /**
        * z_correction_for_x_on_horizontal_mesh_line is an optimization for
        * the rare occasion when a point lies exactly on a Mesh line (denoted by index yi).
        */
-      static inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
+      inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
         if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
           SERIAL_ECHOPAIR("? in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
           SERIAL_ECHOPAIR(",x1_i=", x1_i);
@@ -210,7 +246,7 @@
       //
       // See comments above for z_correction_for_x_on_horizontal_mesh_line
       //
-      static inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
+      inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
         if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
           SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_x(ly0=", ly0);
           SERIAL_ECHOPAIR(", x1_i=", xi);
@@ -232,7 +268,7 @@
        * Z-Height at both ends. Then it does a linear interpolation of these heights based
        * on the Y position within the cell.
        */
-      static float get_z_correction(const float &lx0, const float &ly0) {
+      float get_z_correction(const float &lx0, const float &ly0) {
         const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
                      cy = get_cell_index_y(RAW_Y_POSITION(ly0));
 
@@ -308,7 +344,7 @@
        */
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
-        static FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
+        FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
           if (planner.z_fade_height == 0.0) return 1.0;
 
           static float fade_scaling_factor = 1.0;

Marlin/utility.cpp

@@ -31,6 +31,7 @@ void safe_delay(millis_t ms) {
     thermalManager.manage_heater();
   }
   delay(ms);
+  thermalManager.manage_heater();	// This keeps us safe if too many small safe_delay() calls are made
 }
 
 #if ENABLED(ULTRA_LCD)