Set invert_probe only once conditionally for G38.x commands

diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index b16a3d1..23c6b75 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -326,7 +326,6 @@ void ZProbe::on_gcode_received(void *argument)
 
     } else if(gcode->has_g && gcode->g == 38 ) { // G38.2 Straight Probe with error, G38.3 straight probe without error
         // linuxcnc/grbl style probe http://www.linuxcnc.org/docs/2.5/html/gcode/gcode.html#sec:G38-probe
-        invert_probe = false;
         if(gcode->subcode < 2 || gcode->subcode > 5) {
             gcode->stream->printf("error:Only G38.2 to G38.5 are supported\n");
             return;
@@ -366,6 +365,8 @@ void ZProbe::on_gcode_received(void *argument)
 
         if(gcode->subcode == 4 || gcode->subcode == 5) {
             invert_probe = true;
+        } else {
+            invert_probe = false;
         }
 
         probe_XYZ(gcode, x, y, z);