Add OPTIMIZED_MESH_STORAGE option (for UBL) (#20371)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/src/feature/bedlevel/ubl/ubl.cpp

@@ -42,9 +42,7 @@
 
   #include "math.h"
 
-  void unified_bed_leveling::echo_name() {
-    SERIAL_ECHOPGM("Unified Bed Leveling");
-  }
+  void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); }
 
   void unified_bed_leveling::report_current_mesh() {
     if (!leveling_is_valid()) return;
@@ -86,9 +84,7 @@
 
   volatile int16_t unified_bed_leveling::encoder_diff;
 
-  unified_bed_leveling::unified_bed_leveling() {
-    reset();
-  }
+  unified_bed_leveling::unified_bed_leveling() { reset(); }
 
   void unified_bed_leveling::reset() {
     const bool was_enabled = planner.leveling_active;
@@ -113,6 +109,31 @@
     }
   }
 
+  #if ENABLED(OPTIMIZED_MESH_STORAGE)
+
+    constexpr float mesh_store_scaling = 1000;
+    constexpr int16_t Z_STEPS_NAN = INT16_MAX;
+
+    void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
+      auto z_to_store = [](const float &z) {
+        if (isnan(z)) return Z_STEPS_NAN;
+        const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
+        if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
+          return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
+        return int16_t(z_scaled);
+      };
+      GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
+    }
+
+    void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
+      auto store_to_z = [](const int16_t z_scaled) {
+        return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
+      };
+      GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
+    }
+
+  #endif // OPTIMIZED_MESH_STORAGE
+
   static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
     SERIAL_ECHO_SP(sp);
     SERIAL_CHAR('(');
@@ -127,7 +148,7 @@
 
   static void serial_echo_column_labels(const uint8_t sp) {
     SERIAL_ECHO_SP(7);
-    for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+    LOOP_L_N(i, GRID_MAX_POINTS_X) {
       if (i < 10) SERIAL_CHAR(' ');
       SERIAL_ECHO(i);
       SERIAL_ECHO_SP(sp);

Marlin/src/feature/bedlevel/ubl/ubl.h

@@ -41,6 +41,10 @@ struct mesh_index_pair;
 #define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
 #define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
 
+#if ENABLED(OPTIMIZED_MESH_STORAGE)
+  typedef int16_t mesh_store_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+#endif
+
 class unified_bed_leveling {
   private:
 
@@ -106,6 +110,10 @@ class unified_bed_leveling {
     static int8_t storage_slot;
 
     static bed_mesh_t z_values;
+    #if ENABLED(OPTIMIZED_MESH_STORAGE)
+      static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values);
+      static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values);
+    #endif
     static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
                        _mesh_index_to_ypos[GRID_MAX_POINTS_Y];
 
@@ -182,6 +190,12 @@ class unified_bed_leveling {
       return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
     }
 
+    #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+      #define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH
+    #else
+      #define _UBL_OUTER_Z_RAISE NAN
+    #endif
+
     /**
      * z_correction_for_x_on_horizontal_mesh_line is an optimization for
      * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
@@ -195,13 +209,7 @@ class unified_bed_leveling {
         }
 
         // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
-        return (
-          #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
-            UBL_Z_RAISE_WHEN_OFF_MESH
-          #else
-            NAN
-          #endif
-        );
+        return _UBL_OUTER_Z_RAISE;
       }
 
       const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
@@ -224,13 +232,7 @@ class unified_bed_leveling {
         }
 
         // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
-        return (
-          #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
-            UBL_Z_RAISE_WHEN_OFF_MESH
-          #else
-            NAN
-          #endif
-        );
+        return _UBL_OUTER_Z_RAISE;
       }
 
       const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),

Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp

@@ -543,7 +543,7 @@
           }
           else {
             const float cvf = parser.value_float();
-            switch ((int)truncf(cvf * 10.0f) - 30) {   // 3.1 -> 1
+            switch ((int)TRUNC(cvf * 10.0f) - 30) {   // 3.1 -> 1
               #if ENABLED(UBL_G29_P31)
                 case 1: {