Set invert_probe only once conditionally for G38.x commands

[clinton/Smoothieware.git] / src / modules / tools / zprobe / ZProbe.cpp

diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index e74a8de..23c6b75 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -255,7 +255,8 @@ void ZProbe::on_gcode_received(void *argument)
     Gcode *gcode = static_cast<Gcode *>(argument);
 
     if( gcode->has_g && gcode->g >= 29 && gcode->g <= 32) {
-
+        
+        invert_probe = false;
         // make sure the probe is defined and not already triggered before moving motors
         if(!this->pin.connected()) {
             gcode->stream->printf("ZProbe pin not configured.\n");
@@ -336,7 +337,7 @@ void ZProbe::on_gcode_received(void *argument)
             return;
         }
 
-        if (((gcode->subcode == 4 || gcode->subcode == 5) && (!this->pin.get())) || ((gcode->subcode == 2 || gcode->subcode == 3) && (this->pin.get()))){
+        if(this->pin.get() ^ (gcode->subcode >= 4)) {
             gcode->stream->printf("error:ZProbe triggered before move, aborting command.\n");
             return;
         }
@@ -365,13 +366,12 @@ void ZProbe::on_gcode_received(void *argument)
         if(gcode->subcode == 4 || gcode->subcode == 5) {
             invert_probe = true;
         } else {
-            invert_probe = false;   // not sure this is needed since it is always reset to 0 below
+            invert_probe = false;
         }
+
         probe_XYZ(gcode, x, y, z);
 
-        if(gcode->subcode == 4 || gcode->subcode == 5) {
-            invert_probe = false;
-        }
+        invert_probe = false;
 
         return;