Some probe_pt error-handling

Marlin/Marlin_main.cpp

           // Retain the last probe position
           xProbe = LOGICAL_X_POSITION(points[i].x);
           yProbe = LOGICAL_Y_POSITION(points[i].y);
-          measured_z = points[i].z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
-        }
-
-        if (isnan(measured_z)) {
-          planner.abl_enabled = abl_should_enable;
-          return;
+          measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
+          if (isnan(measured_z)) {
+            planner.abl_enabled = abl_should_enable;
+            return;
+          }
+          points[i].z = measured_z;
         }
 
         if (!dryrun) {
 
     const float measured_z = probe_pt(xpos, ypos, !code_seen('S') || code_value_bool(), 1);
 
-    SERIAL_PROTOCOLPAIR("Bed X: ", FIXFLOAT(xpos));
-    SERIAL_PROTOCOLPAIR(" Y: ", FIXFLOAT(ypos));
-    SERIAL_PROTOCOLLNPAIR(" Z: ", FIXFLOAT(measured_z));
+    if (!isnan(measured_z)) {
+      SERIAL_PROTOCOLPAIR("Bed X: ", FIXFLOAT(xpos));
+      SERIAL_PROTOCOLPAIR(" Y: ", FIXFLOAT(ypos));
+      SERIAL_PROTOCOLLNPAIR(" Z: ", FIXFLOAT(measured_z));
+    }
 
     clean_up_after_endstop_or_probe_move();
 
 
         if (!do_all_positions && !do_circle_x3) { // probe the center
           setup_for_endstop_or_probe_move();
-          z_at_pt[0] += probe_pt(0.0, 0.0 , true, 1);
+          z_at_pt[0] += probe_pt(0.0, 0.0 , true, 1);   // TODO: Needs error handling
           clean_up_after_endstop_or_probe_move();
         }
         if (probe_center_plus_3) { // probe extra center points
           for (int8_t axis = probe_center_plus_6 ? 11 : 9; axis > 0; axis -= probe_center_plus_6 ? 2 : 4) {
             setup_for_endstop_or_probe_move();
-            z_at_pt[0] += probe_pt(
+            z_at_pt[0] += probe_pt(                     // TODO: Needs error handling
               cos(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius),
               sin(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius), true, 1);
             clean_up_after_endstop_or_probe_move();
                                     do_circle_x2 ? (zig_zag ? 0.5 : 0.0) : 0);
             for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
               setup_for_endstop_or_probe_move();
-              z_at_pt[axis] += probe_pt(
+              z_at_pt[axis] += probe_pt(                // TODO: Needs error handling
                 cos(RADIANS(180 + 30 * axis)) * delta_calibration_radius *
                 (1 + circles * 0.1 * (zig_zag ? 1 : -1)),
                 sin(RADIANS(180 + 30 * axis)) * delta_calibration_radius *
     setup_for_endstop_or_probe_move();
 
     // Move to the first point, deploy, and probe
-    probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level);
+    const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level);
+    if (isnan(t)) return;
 
     randomSeed(millis());
 

Marlin/ubl_G29.cpp

         ubl.save_ubl_active_state_and_disable();
         ubl.tilt_mesh_based_on_probed_grid(code_seen('T'));
         ubl.restore_ubl_active_state_and_leave();
-      } else { // grid_size==0 which means a 3-Point leveling has been requested
-        float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
-              z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
-              z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
+      }
+      else { // grid_size == 0 : A 3-Point leveling has been requested
+        float z3, z2, z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level);
+        if (!isnan(z1)) {
+          z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level);
+          if (!isnan(z2))
+            z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
+        }
 
-        if ( isnan(z1) || isnan(z2) || isnan(z3)) {   // probe_pt will return NAN if unreachable
+        if (isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
           SERIAL_ERROR_START;
           SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
           goto LEAVE;
         }
 
-      //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
-      //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
+        // Adjust z1, z2, z3 by the Mesh Height at these points. Just because they're non-zero
+        // doesn't mean the Mesh is tilted! (Compensate each probe point by what the Mesh says
+        // its height is.)
 
         ubl.save_ubl_active_state_and_disable();
         z1 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
     const float mean = sum / n;
 
     //
-    // Now do the sumation of the squares of difference from mean
+    // Sum the squares of difference from mean
     //
     float sum_of_diff_squared = 0.0;
     for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
         const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
                     rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
 
-        const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level);
+        const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level); // TODO: Needs error handling
         ubl.z_values[location.x_index][location.y_index] = measured_z;
       }
 
       if (do_ubl_mesh_map) ubl.display_map(map_type);
-    } while ((location.x_index >= 0) && (--max_iterations));
+
+    } while (location.x_index >= 0 && --max_iterations);
 
     STOW_PROBE();
     ubl.restore_ubl_active_state_and_leave();
       const float x = float(x_min) + ix * dx;
       for (int8_t iy = 0; iy < grid_size; iy++) {
         const float y = float(y_min) + dy * (zig_zag ? grid_size - 1 - iy : iy);
-        float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level);
+        float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level); // TODO: Needs error handling
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
             SERIAL_CHAR('(');