🐛 Fix Leveling apply/unapply (#24188)

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

Marlin/src/feature/bedlevel/bedlevel.cpp

     _report_leveling();
     planner.synchronize();
 
-    if (planner.leveling_active) {      // leveling from on to off
-      // change unleveled current_position to physical current_position without moving steppers.
-      planner.apply_leveling(current_position);
-      planner.leveling_active = false;  // disable only AFTER calling apply_leveling
-    }
-    else {                              // leveling from off to on
-      planner.leveling_active = true;   // enable BEFORE calling unapply_leveling, otherwise ignored
-      // change physical current_position to unleveled current_position without moving steppers.
-      planner.unapply_leveling(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();

Marlin/src/gcode/bedlevel/abl/G29.cpp

   #endif
 #endif
 
+static void pre_g29_return(const bool retry, const bool did) {
+  if (!retry) {
+    TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE, false));
+  }
+  if (did) {
+    TERN_(HAS_DWIN_E3V2_BASIC, DWIN_LevelingDone());
+    TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone());
+  }
+}
+
 #define G29_RETURN(retry, did) do{ \
-  if (TERN(G29_RETRY_AND_RECOVER, !retry, true)) { \
-    TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE, false)); \
-  } \
-  if (did) { \
-    TERN_(HAS_DWIN_E3V2_BASIC, DWIN_LevelingDone()); \
-    TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone()); \
-  } \
+  pre_g29_return(TERN0(G29_RETRY_AND_RECOVER, retry), did); \
   return TERN_(G29_RETRY_AND_RECOVER, retry); \
 }while(0)
 
           bedlevel.z_values[i][j] = rz;
           bedlevel.refresh_bed_level();
           TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz));
-          set_bed_leveling_enabled(abl.reenable);
-          if (abl.reenable) report_current_position();
+          if (abl.reenable) {
+            set_bed_leveling_enabled(true);
+            report_current_position();
+          }
         }
         G29_RETURN(false, false);
       } // parser.seen_test('W')
 
           #endif
 
-          abl.reenable = false;
+          abl.reenable = false; // Don't re-enable after modifying the mesh
           idle_no_sleep();
 
         } // inner
         current_position = converted;
 
         if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
-      }
 
-    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        abl.reenable = true;
+      }
 
-      if (!abl.dryrun) {
-        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("G29 uncorrected Z:", current_position.z);
+      // Auto Bed Leveling is complete! Enable if possible.
+      if (abl.reenable) {
+        planner.leveling_active = true;
+        sync_plan_position();
+      }
 
-        // Unapply the offset because it is going to be immediately applied
-        // and cause compensation movement in Z
-        current_position.z -= bedlevel.get_z_correction(current_position)
-          TERN_(ENABLE_LEVELING_FADE_HEIGHT, * planner.fade_scaling_factor_for_z(current_position.z));
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
-        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(" corrected Z:", current_position.z);
-      }
+      // Auto Bed Leveling is complete! Enable if possible.
+      if (!abl.dryrun || abl.reenable) set_bed_leveling_enabled(true);
 
-    #endif // ABL_PLANAR
+    #endif
 
-    // Auto Bed Leveling is complete! Enable if possible.
-    planner.leveling_active = !abl.dryrun || abl.reenable;
   } // !isnan(abl.measured_z)
 
   // Restore state after probing
   if (!faux) restore_feedrate_and_scaling();
 
-  // Sync the planner from the current_position
-  if (planner.leveling_active) sync_plan_position();
-
   TERN_(HAS_BED_PROBE, probe.move_z_after_probing());
 
   #ifdef Z_PROBE_END_SCRIPT

Marlin/src/module/planner.cpp

   }
 
   void Planner::unapply_leveling(xyz_pos_t &raw) {
+    if (!leveling_active) return;
 
-    if (leveling_active) {
-
-      #if ABL_PLANAR
-
-        matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
+    #if ABL_PLANAR
 
-        xy_pos_t d = raw - level_fulcrum;
-        inverse.apply_rotation_xyz(d.x, d.y, raw.z);
-        raw = d + level_fulcrum;
+      matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
-      #elif HAS_MESH
+      xy_pos_t d = raw - level_fulcrum;
+      inverse.apply_rotation_xyz(d.x, d.y, raw.z);
+      raw = d + level_fulcrum;
 
-        TERN_(MESH_BED_LEVELING, raw.z -= bedlevel.get_z_offset());
+    #elif HAS_MESH
 
-        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-          const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
-          if (fade_scaling_factor) raw.z -= fade_scaling_factor * bedlevel.get_z_correction(raw);
-        #else
-          raw.z -= bedlevel.get_z_correction(raw);
-        #endif
+      const float z_correction = bedlevel.get_z_correction(raw),
+                  z_full_fade = DIFF_TERN(MESH_BED_LEVELING, raw.z, bedlevel.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)                              // 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
-    }
+
+    #endif
   }
 
 #endif // HAS_LEVELING