Updates to G29 for probe error handling

Marlin/Marlin_main.cpp

@@ -2222,7 +2222,14 @@ static void clean_up_after_endstop_or_probe_move() {
     return false;
   }
 
-  static bool do_probe_move(float z, float fr_mm_m) {
+  /**
+   * @brief Used by run_z_probe to do a single Z probe move.
+   *
+   * @param  z        Z destination
+   * @param  fr_mm_s  Feedrate in mm/s
+   * @return true to indicate an error
+   */
+  static bool do_probe_move(const float z, const float fr_mm_m) {
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS(">>> do_probe_move", current_position);
     #endif
@@ -2241,7 +2248,7 @@ static void clean_up_after_endstop_or_probe_move() {
 
     // Check to see if the probe was triggered
     const bool probe_triggered = TEST(Endstops::endstop_hit_bits,
-      #ifdef Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN
+      #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
         Z_MIN
       #else
         Z_MIN_PROBE
@@ -2273,9 +2280,14 @@ static void clean_up_after_endstop_or_probe_move() {
     return !probe_triggered;
   }
 
-  // Do a single Z probe and return with current_position[Z_AXIS]
+  /**
-  // at the height where the probe triggered.
+   * @details Used by probe_pt to do a single Z probe.
-  static float run_z_probe(bool printable=true) {
+   *          Leaves current_position[Z_AXIS] at the height where the probe triggered.
+   *
+   * @param  short_move Flag for a shorter probe move towards the bed
+   * @return The raw Z position where the probe was triggered
+   */
+  static float run_z_probe(const bool short_move=true) {
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS(">>> run_z_probe", current_position);
@@ -2313,7 +2325,7 @@ static void clean_up_after_endstop_or_probe_move() {
     #endif
 
     // move down slowly to find bed
-    if (do_probe_move(-10 + (printable ? 0 : -(Z_MAX_LENGTH)), Z_PROBE_SPEED_SLOW)) return NAN;
+    if (do_probe_move(-10 + (short_move ? 0 : -(Z_MAX_LENGTH)), Z_PROBE_SPEED_SLOW)) return NAN;
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("<<< run_z_probe", current_position);
@@ -2410,6 +2422,12 @@ static void clean_up_after_endstop_or_probe_move() {
 
     feedrate_mm_s = old_feedrate_mm_s;
 
+    if (isnan(measured_z)) {
+      LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
+      SERIAL_ERROR_START();
+      SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
+    }
+
     return measured_z;
   }
 
@@ -3775,9 +3793,7 @@ inline void gcode_G4() {
     // If an endstop was not hit, then damage can occur if homing is continued.
     // This can occur if the delta height (DELTA_HEIGHT + home_offset[Z_AXIS]) is
     // not set correctly.
-    if (!(TEST(Endstops::endstop_hit_bits, X_MAX) ||
+    if (!(Endstops::endstop_hit_bits & (_BV(X_MAX) | _BV(Y_MAX) | _BV(Z_MAX)))) {
-          TEST(Endstops::endstop_hit_bits, Y_MAX) ||
-          TEST(Endstops::endstop_hit_bits, Z_MAX))) {
       LCD_MESSAGEPGM(MSG_ERR_HOMING_FAILED);
       SERIAL_ERROR_START();
       SERIAL_ERRORLNPGM(MSG_ERR_HOMING_FAILED);
@@ -4126,20 +4142,6 @@ void home_all_axes() { gcode_G28(true); }
 
 #endif
 
-#if HAS_BED_PROBE
-
-  static bool nan_error(const float v) {
-    const bool is_nan = isnan(v);
-    if (is_nan) {
-      LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
-      SERIAL_ERROR_START();
-      SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
-    }
-    return is_nan;
-  }
-
-#endif // HAS_BED_PROBE
-
 #if ENABLED(MESH_BED_LEVELING)
 
   // Save 130 bytes with non-duplication of PSTR
@@ -4675,18 +4677,16 @@ void home_all_axes() { gcode_G28(true); }
         SYNC_PLAN_POSITION_KINEMATIC();
       }
 
-      if (!faux) setup_for_endstop_or_probe_move();
-
-      //xProbe = yProbe = measured_z = 0;
-
       #if HAS_BED_PROBE
         // Deploy the probe. Probe will raise if needed.
         if (DEPLOY_PROBE()) {
           planner.abl_enabled = abl_should_enable;
-          goto FAIL;
+          return;
         }
       #endif
 
+      if (!faux) setup_for_endstop_or_probe_move();
+
       #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
         #if ENABLED(PROBE_MANUALLY)
@@ -4907,7 +4907,7 @@ void home_all_axes() { gcode_G28(true); }
         bool zig = PR_OUTER_END & 1;  // Always end at RIGHT and BACK_PROBE_BED_POSITION
 
         // Outer loop is Y with PROBE_Y_FIRST disabled
-        for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END; PR_OUTER_VAR++) {
+        for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
 
           int8_t inStart, inStop, inInc;
 
@@ -4944,9 +4944,9 @@ void home_all_axes() { gcode_G28(true); }
 
             measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
 
-            if (nan_error(measured_z)) {
+            if (isnan(measured_z)) {
               planner.abl_enabled = abl_should_enable;
-              goto FAIL;
+              break;
             }
 
             #if ENABLED(AUTO_BED_LEVELING_LINEAR)
@@ -4980,14 +4980,14 @@ void home_all_axes() { gcode_G28(true); }
           xProbe = LOGICAL_X_POSITION(points[i].x);
           yProbe = LOGICAL_Y_POSITION(points[i].y);
           measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
-          if (nan_error(measured_z)) {
+          if (isnan(measured_z)) {
             planner.abl_enabled = abl_should_enable;
-            goto FAIL;
+            break;
           }
           points[i].z = measured_z;
         }
 
-        if (!dryrun) {
+        if (!dryrun && !isnan(measured_z)) {
           vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
           if (planeNormal.z < 0) {
             planeNormal.x *= -1;
@@ -5005,7 +5005,7 @@ void home_all_axes() { gcode_G28(true); }
       // Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
       if (STOW_PROBE()) {
         planner.abl_enabled = abl_should_enable;
-        goto FAIL;
+        measured_z = NAN;
       }
     }
     #endif // !PROBE_MANUALLY
@@ -5032,6 +5032,7 @@ void home_all_axes() { gcode_G28(true); }
     #endif
 
     // Calculate leveling, print reports, correct the position
+    if (!isnan(measured_z)) {
       #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
         if (!dryrun) extrapolate_unprobed_bed_level();
@@ -5226,8 +5227,7 @@ void home_all_axes() { gcode_G28(true); }
 
       // Auto Bed Leveling is complete! Enable if possible.
       planner.abl_enabled = dryrun ? abl_should_enable : true;
-
+    } // !isnan(measured_z)
-    FAIL:
 
     // Restore state after probing
     if (!faux) clean_up_after_endstop_or_probe_move();
@@ -5272,7 +5272,7 @@ void home_all_axes() { gcode_G28(true); }
 
     const float measured_z = probe_pt(xpos, ypos, parser.boolval('S', true), 1);
 
-    if (!nan_error(measured_z)) {
+    if (!isnan(measured_z)) {
       SERIAL_PROTOCOLPAIR("Bed X: ", FIXFLOAT(xpos));
       SERIAL_PROTOCOLPAIR(" Y: ", FIXFLOAT(ypos));
       SERIAL_PROTOCOLLNPAIR(" Z: ", FIXFLOAT(measured_z));