From 5f1eece67deeb06dc6cdbb969af575edda054066 Mon Sep 17 00:00:00 2001
From: Elijah Insua <tmpvar@gmail.com>
Date: Mon, 22 Sep 2014 14:35:12 -0700
Subject: [PATCH] hop over probe pull-off sequence after probe miss

and while "no errors" is enabled (G38.3, G38.5)
---
 motion_control.c | 48 ++++++++++++++++++++++++++++--------------------
 1 file changed, 28 insertions(+), 20 deletions(-)

diff --git a/motion_control.c b/motion_control.c
index 66cb9ac..f4b66cb 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -287,7 +287,8 @@ void mc_homing_cycle()
   // Finish all queued commands and empty planner buffer before starting probe cycle.
   protocol_buffer_synchronize();
   uint8_t auto_start_state = sys.auto_start; // Store run state
-  
+  uint8_t perform_pull_off = 1;
+
   // After syncing, check if probe is already triggered. If so, halt and issue alarm.
   if (probe_get_state(mode) && probe_errors_enabled(mode)) {
     bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
@@ -313,8 +314,13 @@ void mc_homing_cycle()
   } while ((sys.state != STATE_IDLE) && (sys.state != STATE_QUEUED));
 
   // Probing motion complete. If the probe has not been triggered, error out.
-  if (sys.probe_state & PROBE_ACTIVE && probe_errors_enabled(mode)) {
-    bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
+  if (sys.probe_state & PROBE_ACTIVE) {
+
+    if (probe_errors_enabled(mode)) {
+      bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
+    } else {
+      perform_pull_off = 0;
+    }
   }
   protocol_execute_runtime();   // Check and execute run-time commands
   if (sys.abort) { return; } // Check for system abort
@@ -323,24 +329,26 @@ void mc_homing_cycle()
   st_reset(); // Reest step segment buffer.
   plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
   plan_sync_position(); // Sync planner position to current machine position.
-  
-  // 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.
+  if (perform_pull_off) {
+    // 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