From b800eb0fedcff79218b1e9f7c4c58a325d805563 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 21 Sep 2016 19:16:01 -0500
Subject: [PATCH] Add probe option to position_is_reachable

---
 Marlin/Marlin_main.cpp | 38 ++++++++++++++++++++++++++------------
 1 file changed, 26 insertions(+), 12 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 444af5bf0..dce5010aa 100644
--- a/Marlin/Marlin_main.cpp
+++ b/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;
+    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
-
-  return good && 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;
 }
 
 /**************************************************
@@ -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;
       }