Add probe option to position_is_reachable

Marlin/Marlin_main.cpp

@@ -2486,28 +2486,36 @@ void unknown_command_error() {
 
 #endif //HOST_KEEPALIVE_FEATURE
 
-bool position_is_reachable(float target[XYZ]) {
+bool position_is_reachable(float target[XYZ]
+  #if HAS_BED_PROBE
+    , bool by_probe=false
+  #endif
+) {
   float dx = RAW_X_POSITION(target[X_AXIS]),
         dy = RAW_Y_POSITION(target[Y_AXIS]),
         dz = RAW_Z_POSITION(target[Z_AXIS]);
 
-  bool good;
+  #if HAS_BED_PROBE
+    if (by_probe) {
+      dx -= X_PROBE_OFFSET_FROM_EXTRUDER;
+      dy -= Y_PROBE_OFFSET_FROM_EXTRUDER;
+    }
+  #endif
+
   #if IS_SCARA
     #if MIDDLE_DEAD_ZONE_R > 0
       const float R2 = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y);
-      good = (R2 >= sq(float(MIDDLE_DEAD_ZONE_R))) && (R2 <= sq(L1 + L2));
+      return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2);
     #else
-      good = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y) <= sq(L1 + L2);
+      return HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y) <= sq(L1 + L2);
     #endif
   #elif ENABLED(DELTA)
-    good = HYPOT2(dx, dy) <= sq(DELTA_PRINTABLE_RADIUS);
+    return HYPOT2(dx, dy) <= sq(DELTA_PRINTABLE_RADIUS);
   #else
-    good = true;
-  #endif
-
-  return good && dx >= X_MIN_POS - 0.0001 && dx <= X_MAX_POS + 0.0001
+    return dx >= X_MIN_POS - 0.0001 && dx <= X_MAX_POS + 0.0001
         && dy >= Y_MIN_POS - 0.0001 && dy <= Y_MAX_POS + 0.0001
         && dz >= Z_MIN_POS - 0.0001 && dz <= Z_MAX_POS + 0.0001;
+  #endif
 }
 
 /**************************************************
@@ -2896,7 +2904,13 @@ inline void gcode_G4() {
       if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
     #endif
 
-    if (position_is_reachable(destination)) {
+    if (position_is_reachable(
+          destination
+          #if HAS_BED_PROBE
+            , true
+          #endif
+        )
+    ) {
       do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
       HOMEAXIS(Z);
     }
@@ -4245,7 +4259,7 @@ inline void gcode_M42() {
       }
     #else
       float pos[XYZ] = { X_probe_location, Y_probe_location, 0 };
-      if (!position_is_reachable(pos)) {
+      if (!position_is_reachable(pos, true)) {
         SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius.");
         return;
       }