Add a proper flag for ABL enabled

Marlin/Marlin_main.cpp

 
   /**
    * Reset calibration results to zero.
+   *
+   * TODO: Proper functions to disable / enable
+   *       bed leveling via a flag, correcting the
+   *       current position in each case.
    */
   void reset_bed_level() {
+    planner.abl_enabled = false;
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
     #endif
       planner.bed_level_matrix.set_to_identity();
     #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
       memset(bed_level_grid, 0, sizeof(bed_level_grid));
-      nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
     #endif
   }
 
 
     stepper.synchronize();
 
-    if (!dryrun) {
+    // Disable auto bed leveling during G29
+    bool auto_bed_leveling_was_enabled = planner.abl_enabled,
+         abl_should_reenable = auto_bed_leveling_was_enabled;
 
-      // Reset the bed_level_matrix because leveling
-      // needs to be done without leveling enabled.
-      reset_bed_level();
+    planner.abl_enabled = false;
 
-      //
+    if (!dryrun) {
       // Re-orient the current position without leveling
       // based on where the steppers are positioned.
-      //
       get_cartesian_from_steppers();
       memcpy(current_position, cartes, sizeof(cartes));
 
     setup_for_endstop_or_probe_move();
 
     // Deploy the probe. Probe will raise if needed.
-    if (DEPLOY_PROBE()) return;
+    if (DEPLOY_PROBE()) {
+      planner.abl_enabled = abl_should_reenable;
+      return;
+    }
 
     float xProbe = 0, yProbe = 0, measured_z = 0;
 
 
       #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
-        nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
-        nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
         float zoffset = zprobe_zoffset;
         if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
 
+        if (xGridSpacing != nonlinear_grid_spacing[X_AXIS] || yGridSpacing != nonlinear_grid_spacing[Y_AXIS]) {
+          nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
+          nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
+          // Can't re-enable (on error) until the new grid is written
+          abl_should_reenable = false;
+        }
+
       #elif ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
 
         /**
 
           measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
 
+          if (measured_z == NAN) {
+            planner.abl_enabled = abl_should_reenable;
+            return;
+          }
+
           #if ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
 
             mean += measured_z;
         measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
       }
 
+      if (measured_z == NAN) {
+        planner.abl_enabled = abl_should_reenable;
+        return;
+      }
+
       if (!dryrun) {
         vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
         if (planeNormal.z < 0) {
           planeNormal.z *= -1;
         }
         planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_reenable = false;
       }
 
     #endif // AUTO_BED_LEVELING_3POINT
 
     // Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
-    if (STOW_PROBE()) return;
+    if (STOW_PROBE()) {
+      planner.abl_enabled = abl_should_reenable;
+      return;
+    }
+
+    //
+    // Unless this is a dry run, auto bed leveling will
+    // definitely be enabled after this point
+    //
 
     // Restore state after probing
     clean_up_after_endstop_or_probe_move();
     report_current_position();
 
     KEEPALIVE_STATE(IN_HANDLER);
+
+    // Auto Bed Leveling is complete! Enable if possible.
+    planner.abl_enabled = dryrun ? abl_should_reenable : true;
   }
 
 #endif // AUTO_BED_LEVELING_FEATURE
 
   // Get the Z adjustment for non-linear bed leveling
   float nonlinear_z_offset(float cartesian[XYZ]) {
-    if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return 0; // G29 not done!
+    if (planner.abl_enabled) return;
 
     int half_x = (ABL_GRID_POINTS_X - 1) / 2,
         half_y = (ABL_GRID_POINTS_Y - 1) / 2;

Marlin/planner.cpp

       Planner::max_e_jerk,
       Planner::min_travel_feedrate_mm_s;
 
+#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  bool Planner::abl_enabled = false; // Flag that auto bed leveling is enabled
+#endif
+
 #if ENABLED(AUTO_BED_LEVELING_LINEAR)
   matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
 #endif
 #if PLANNER_LEVELING
 
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
+
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      if (!abl_enabled) return;
+    #endif
+
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
   }
 
   void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
+
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      if (!abl_enabled) return;
+    #endif
+
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
        dz = target[Z_AXIS] - position[Z_AXIS];
 
   /*
-  SERIAL_ECHOPGM("  Planner ");
-  SERIAL_ECHOPAIR("FR:", fr_mm_s);
+  SERIAL_ECHOPAIR("  Planner FR:", fr_mm_s);
   SERIAL_CHAR(' ');
   #if IS_KINEMATIC
     SERIAL_ECHOPAIR("A:", lx);

Marlin/planner.h

     static float min_travel_feedrate_mm_s;
 
     #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      static bool abl_enabled;            // Flag that bed leveling is enabled
       static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
     #endif