From d55f6b938cd54a95e14d94e2ad7730d1aeb772a4 Mon Sep 17 00:00:00 2001
From: Sonny Jeon <chamnit@gmail.com>
Date: Mon, 4 Aug 2014 05:52:41 -0600
Subject: [PATCH] Fixed probe position sync error.

- Restored probe position syncing. Had removed a pull-off motion after
a probe cycle completes, but ended up de-synchronizing the g-code
parser and probing cycle positions. Putting the pull-off motion back
fixed the problem.
---
 motion_control.c | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/motion_control.c b/motion_control.c
index bac1f14..102f1d7 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -317,7 +317,25 @@ void mc_homing_cycle()
   plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
   plan_sync_position(); // Sync planner position to current machine position.
   
-  sys.auto_start = auto_start_state; // Restore run state.
+  // Pull-off triggered probe to the trigger location since we had to decelerate a little beyond
+  // it to stop the machine in a controlled manner. 
+  uint8_t idx;
+  for(idx=0; idx<N_AXIS; idx++){
+    // NOTE: The target[] variable updated here will be sent back and synced with the g-code parser.
+    target[idx] = (float)sys.probe_position[idx]/settings.steps_per_mm[idx];
+  }
+  #ifdef USE_LINE_NUMBERS
+    mc_line(target, feed_rate, invert_feed_rate, line_number);
+  #else
+    mc_line(target, feed_rate, invert_feed_rate);
+  #endif
+
+  // Execute pull-off motion and wait until it completes.
+  bit_true_atomic(sys.execute, EXEC_CYCLE_START);
+  protocol_buffer_synchronize(); 
+  if (sys.abort) { return; } // Return if system reset has been issued.
+
+  sys.auto_start = auto_start_state; // Restore run state before returning
 
   #ifdef MESSAGE_PROBE_COORDINATES
     // All done! Output the probe position as message.