Add OPTIMIZED_MESH_STORAGE option (for UBL) (MarlinFirmware#20371)

Co-authored-by: Scott Lahteine <[email protected]>
thinkyhead · Apr 29, 2021 · 36777ce · 36777ce
1 parent 5f3ecf1
commit 36777ce
Show file tree

Hide file tree

Showing 6 changed files with 80 additions and 27 deletions.
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
@@ -1739,6 +1739,10 @@
   //#define MESH_MAX_Y Y_BED_SIZE - (MESH_INSET)
 #endif
 
+#if BOTH(AUTO_BED_LEVELING_UBL, EEPROM_SETTINGS)
+  //#define OPTIMIZED_MESH_STORAGE  // Store mesh with less precision to save EEPROM space
+#endif
+
 /**
  * Repeatedly attempt G29 leveling until it succeeds.
  * Stop after G29_MAX_RETRIES attempts.

diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h
@@ -286,6 +286,7 @@
 #define RSQRT(x)    (1.0f / sqrtf(x))
 #define CEIL(x)     ceilf(x)
 #define FLOOR(x)    floorf(x)
+#define TRUNC(x)    truncf(x)
 #define LROUND(x)   lroundf(x)
 #define FMOD(x, y)  fmodf(x, y)
 #define HYPOT(x,y)  SQRT(HYPOT2(x,y))

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.cpp b/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);

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/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),

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/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: {
 

diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp
@@ -2385,12 +2385,14 @@ void MarlinSettings::postprocess() {
                                                           // or down a little bit without disrupting the mesh data
     }
 
+    #define MESH_STORE_SIZE sizeof(TERN(OPTIMIZED_MESH_STORAGE, mesh_store_t, ubl.z_values))
+
     uint16_t MarlinSettings::calc_num_meshes() {
-      return (meshes_end - meshes_start_index()) / sizeof(ubl.z_values);
+      return (meshes_end - meshes_start_index()) / MESH_STORE_SIZE;
     }
 
     int MarlinSettings::mesh_slot_offset(const int8_t slot) {
-      return meshes_end - (slot + 1) * sizeof(ubl.z_values);
+      return meshes_end - (slot + 1) * MESH_STORE_SIZE;
     }
 
     void MarlinSettings::store_mesh(const int8_t slot) {
@@ -2407,9 +2409,17 @@ void MarlinSettings::postprocess() {
         int pos = mesh_slot_offset(slot);
         uint16_t crc = 0;
 
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          int16_t z_mesh_store[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+          ubl.set_store_from_mesh(ubl.z_values, z_mesh_store);
+          uint8_t * const src = (uint8_t*)&z_mesh_store;
+        #else
+          uint8_t * const src = (uint8_t*)&ubl.z_values;
+        #endif
+
         // Write crc to MAT along with other data, or just tack on to the beginning or end
         persistentStore.access_start();
-        const bool status = persistentStore.write_data(pos, (uint8_t *)&ubl.z_values, sizeof(ubl.z_values), &crc);
+        const bool status = persistentStore.write_data(pos, src, MESH_STORE_SIZE, &crc);
         persistentStore.access_finish();
 
         if (status) SERIAL_ECHOLNPGM("?Unable to save mesh data.");
@@ -2435,12 +2445,27 @@ void MarlinSettings::postprocess() {
 
         int pos = mesh_slot_offset(slot);
         uint16_t crc = 0;
-        uint8_t * const dest = into ? (uint8_t*)into : (uint8_t*)&ubl.z_values;
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          int16_t z_mesh_store[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+          uint8_t * const dest = (uint8_t*)&z_mesh_store;
+        #else
+          uint8_t * const dest = into ? (uint8_t*)into : (uint8_t*)&ubl.z_values;
+        #endif
 
         persistentStore.access_start();
-        const uint16_t status = persistentStore.read_data(pos, dest, sizeof(ubl.z_values), &crc);
+        const uint16_t status = persistentStore.read_data(pos, dest, MESH_STORE_SIZE, &crc);
         persistentStore.access_finish();
 
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          if (into) {
+            float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+            ubl.set_mesh_from_store(z_mesh_store, z_values);
+            memcpy(into, z_values, sizeof(z_values));
+          }
+          else
+            ubl.set_mesh_from_store(z_mesh_store, ubl.z_values);
+        #endif
+
         if (status) SERIAL_ECHOLNPGM("?Unable to load mesh data.");
         else        DEBUG_ECHOLNPAIR("Mesh loaded from slot ", slot);