diff --git a/macro/machine/M5000.g b/macro/machine/M5000.g
index 5df3e0b..3103f7e 100644
--- a/macro/machine/M5000.g
+++ b/macro/machine/M5000.g
@@ -5,6 +5,10 @@
 ; position
 ; Return machine information
 
+; Make sure this file is not executed by the secondary motion system
+if { !inputs[state.thisInput].active }
+    M99
+
 if { !exists(param.P) }
     abort { "M5011: No machine information requested with P parameter." }
 
diff --git a/macro/movement/G6512.g b/macro/movement/G6512.g
index d93af4d..f59d710 100644
--- a/macro/movement/G6512.g
+++ b/macro/movement/G6512.g
@@ -140,6 +140,9 @@ if { var.manualProbe }
 else
     G6512.1 I{ param.I } X{ var.tPX } Y{ var.tPY } Z{ var.tPZ } R{ exists(param.R) ? param.R : null } E{ exists(param.E) ? param.E : 1 }
 
+; Save probed position
+var pP = { global.mosMI }
+
 ; Move to safe height
 ; If probing move is called with D parameter,
 ; we stay at the same height.
@@ -173,15 +176,15 @@ var mag = { sqrt(pow(var.tPX - var.sX, 2) + pow(var.tPY - var.sY, 2)) }
 if { var.mag != 0 }
     ; Adjust the final position along the direction of movement in X and Y
     ; by the tool radius, subtracting the deflection on each axis.
-    set global.mosMI[0] = { global.mosMI[0] + (global.mosTT[state.currentTool][0] - global.mosTT[state.currentTool][1][0]) * ((var.tPX - var.sX) / var.mag) }
-    set global.mosMI[1] = { global.mosMI[1] + (global.mosTT[state.currentTool][0] - global.mosTT[state.currentTool][1][1]) * ((var.tPY - var.sY) / var.mag) }
+    set var.pP[0] = { var.pP[0] + (global.mosTT[state.currentTool][0] - global.mosTT[state.currentTool][1][0]) * ((var.tPX - var.sX) / var.mag) }
+    set var.pP[1] = { var.pP[1] + (global.mosTT[state.currentTool][0] - global.mosTT[state.currentTool][1][1]) * ((var.tPY - var.sY) / var.mag) }
 
 ; We do not adjust by the tool radius in Z.
 
 ; Now we can apply any probe offsets, if they exist.
 if { exists(param.I) }
-    set global.mosMI[0] = { global.mosMI[0] + sensors.probes[param.I].offsets[0] }
-    set global.mosMI[1] = { global.mosMI[1] + sensors.probes[param.I].offsets[1] }
+    set var.pP[0] = { var.pP[0] + sensors.probes[param.I].offsets[0] }
+    set var.pP[1] = { var.pP[1] + sensors.probes[param.I].offsets[1] }
 
 ; This does bring up an interesting conundrum though. If you're probing in 2 axes where
 ; one is Z, then you have no way of knowing whether the probe was triggered by the Z
@@ -197,6 +200,6 @@ if { exists(param.I) }
 var sDig = 1000
 
 ; Round the output variables to 3 decimal places
-set global.mosMI[0] = { ceil(global.mosMI[0] * var.sDig) / var.sDig }
-set global.mosMI[1] = { ceil(global.mosMI[1] * var.sDig) / var.sDig }
-set global.mosMI[2] = { ceil(global.mosMI[2] * var.sDig) / var.sDig }
\ No newline at end of file
+set global.mosMI[0] = { ceil(var.pP[0] * var.sDig) / var.sDig }
+set global.mosMI[1] = { ceil(var.pP[1] * var.sDig) / var.sDig }
+set global.mosMI[2] = { ceil(var.pP[2] * var.sDig) / var.sDig }
\ No newline at end of file
diff --git a/macro/movement/G6550.g b/macro/movement/G6550.g
index 67ca353..1ae4ee6 100644
--- a/macro/movement/G6550.g
+++ b/macro/movement/G6550.g
@@ -39,7 +39,6 @@ var manualProbe = { !exists(param.I) || param.I == null }
 ; Get current machine position
 M5000 P0
 
-
 ; Generate target position and defaults
 var tPX = { (exists(param.X)? param.X : global.mosMI[0]) }
 var tPY = { (exists(param.Y)? param.Y : global.mosMI[1]) }