cleanup

MarlinFirmware · May 19, 2022 · 3e4f30c · 3e4f30c
1 parent 547494a
commit 3e4f30c
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 61 deletions.
diff --git a/Marlin/src/HAL/AVR/inc/SanityCheck.h b/Marlin/src/HAL/AVR/inc/SanityCheck.h
@@ -37,16 +37,16 @@
   || X_ENA_PIN  == N || Y_ENA_PIN  == N || Z_ENA_PIN  == N \
 )
 #if CONF_SERIAL_IS(0) // D0-D1. No known conflicts.
-#endif 
+#endif
 #if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19))
   #error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board."
-#endif 
+#endif
 #if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17))
   #error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board."
 #endif
 #if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15))
   #error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board."
-#endif 
+#endif
 #undef CHECK_SERIAL_PIN
 
 /**

diff --git a/Marlin/src/HAL/DUE/inc/SanityCheck.h b/Marlin/src/HAL/DUE/inc/SanityCheck.h
@@ -37,16 +37,16 @@
   || X_ENA_PIN  == N || Y_ENA_PIN  == N || Z_ENA_PIN  == N \
 )
 #if CONF_SERIAL_IS(0) // D0-D1. No known conflicts.
-#endif 
+#endif
 #if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19))
   #error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board."
-#endif 
+#endif
 #if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17))
   #error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board."
 #endif
 #if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15))
   #error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board."
-#endif 
+#endif
 #undef CHECK_SERIAL_PIN
 
 /**

diff --git a/Marlin/src/feature/bedlevel/bedlevel.cpp b/Marlin/src/feature/bedlevel/bedlevel.cpp
@@ -53,8 +53,7 @@ bool leveling_is_valid() {
 }
 
 /**
- * Turn bed leveling on or off, fixing the current
- * position as-needed.
+ * Turn bed leveling on or off, correcting the current position.
  *
  * Disable: Current position = physical position
  *  Enable: Current position = "unleveled" physical position
@@ -65,26 +64,25 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) {
 
   if (can_change && enable != planner.leveling_active) {
 
+    auto _report_leveling = []{
+      if (DEBUGGING(LEVELING)) {
+        if (planner.leveling_active)
+          DEBUG_POS("Leveling ON", current_position);
+        else
+          DEBUG_POS("Leveling OFF", current_position);
+      }
+    };
+
+    _report_leveling();
     planner.synchronize();
 
-    if (planner.leveling_active) {      // leveling from on to off
-      if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling ON", current_position);
-      // change unleveled current_position to physical current_position without moving steppers.
-      planner.apply_modifiers(current_position);
-      planner.leveling_active = false;  // disable only BETWEEN calling apply_modifiers() and unapply_modifiers()
-      planner.unapply_modifiers(current_position);
-      if (DEBUGGING(LEVELING)) DEBUG_POS("...Now OFF", current_position);
-    }
-    else {                              // leveling from off to on
-      if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling OFF", current_position);
-      // change physical current_position to unleveled current_position without moving steppers.
-      planner.apply_modifiers(current_position);
-      planner.leveling_active = true;   // enable BETWEEN calling apply_modifiers() and unapply_modifiers()
-      planner.unapply_modifiers(current_position);
-      if (DEBUGGING(LEVELING)) DEBUG_POS("...Now ON", current_position);
-    }
+    // Get the corrected leveled / unleveled position
+    planner.apply_modifiers(current_position);    // Physical position with all modifiers
+    planner.leveling_active ^= true;              // Toggle leveling between apply and unapply
+    planner.unapply_modifiers(current_position);  // Logical position with modifiers removed
 
     sync_plan_position();
+    _report_leveling();
   }
 }
 

diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
@@ -102,11 +102,9 @@ class mesh_bed_leveling {
     return z1 + delta_a * delta_z;
   }
 
-  static float get_z_correction_fixed() {
-    return z_offset;
-  }
+  static float get_z_offset() { return z_offset; }
 
-  static float get_z_correction_fadable(const xy_pos_t &pos) {
+  static float get_z_correction(const xy_pos_t &pos) {
     const xy_int8_t ind = cell_indexes(pos);
     const float x1 = index_to_xpos[ind.x], x2 = index_to_xpos[ind.x+1],
                 y1 = index_to_xpos[ind.y], y2 = index_to_xpos[ind.y+1],

diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
@@ -1570,6 +1570,14 @@ void Planner::check_axes_activity() {
     TERN(Z_SAFE_HOMING, Z_SAFE_HOMING_Y_POINT, Y_HOME_POS)
   };
 
+  #if ENABLED(MESH_BED_LEVELING)
+    #define BLS mbl
+  #elif ENABLED(AUTO_BED_LEVELING_UBL)
+    #define BLS ubl
+  #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+    #define BLS bbl
+  #endif
+
   /**
    * rx, ry, rz - Cartesian positions in mm
    *              Leveled XYZ on completion
@@ -1591,16 +1599,10 @@ void Planner::check_axes_activity() {
         constexpr float fade_scaling_factor = 1.0;
       #endif
 
-      raw.z += (
-        #if ENABLED(MESH_BED_LEVELING)
-          mbl.get_z_correction_fixed() + 
-          fade_scaling_factor ? fade_scaling_factor * mbl.get_z_correction_fadable(raw) : 0.0
-        #elif ENABLED(AUTO_BED_LEVELING_UBL)
-          fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
-        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-          fade_scaling_factor ? fade_scaling_factor * bbl.get_z_correction(raw) : 0.0
-        #endif
-      );
+      if (fade_scaling_factor)
+        raw.z += fade_scaling_factor * BLS.get_z_correction(raw);
+
+      TERN_(MESH_BED_LEVELING, raw.z += BLS.get_z_offset());
 
     #endif
   }
@@ -1618,31 +1620,17 @@ void Planner::check_axes_activity() {
 
     #elif HAS_MESH
 
-      #if ENABLED(MESH_BED_LEVELING)
-        const float z_correction_fixed = mbl.get_z_correction_fixed();
-      #else
-        constexpr float z_correction_fixed = 0.0f;
-      #endif
-
-      float z_correction_fadable = 
-        #if ENABLED(MESH_BED_LEVELING)
-          mbl.get_z_correction_fadable(raw);
-        #elif ENABLED(AUTO_BED_LEVELING_UBL)
-          ubl.get_z_correction(raw);
-        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-          bbl.get_z_correction(raw);
-        #endif
-
-      const float z_full_fade = raw.z - z_correction_fixed;
-      const float z_no_fade = z_full_fade - z_correction_fadable;
+      const float z_correction = BLS.get_z_correction(raw),
+                  z_full_fade = DIFF_TERN(MESH_BED_LEVELING, raw.z, BLS.get_z_offset()),
+                  z_no_fade = z_full_fade - z_correction;
 
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        if (!z_fade_height || z_no_fade <= 0.0f)
-          raw.z = z_no_fade;
-        else if (z_full_fade >= z_fade_height)
-          raw.z = z_full_fade;
-        else
-          raw.z = z_no_fade / (1.0f - z_correction_fadable * inverse_z_fade_height);
+        if (!z_fade_height || z_no_fade <= 0.0f)                              // Not fading or at bed level?
+          raw.z = z_no_fade;                                                  //  Unapply full mesh Z.
+        else if (z_full_fade >= z_fade_height)                                // Above the fade height?
+          raw.z = z_full_fade;                                                //  Nothing more to unapply.
+        else                                                                  // Within the fade zone?
+          raw.z = z_no_fade / (1.0f - z_correction * inverse_z_fade_height);  // Unapply the faded Z offset
       #else
         raw.z = z_no_fade;
       #endif