diff --git a/gateware/logic/autolock.py b/gateware/logic/autolock.py
index a6dc764..1a8407f 100644
--- a/gateware/logic/autolock.py
+++ b/gateware/logic/autolock.py
@@ -148,14 +148,17 @@ class RobustAutolock(Module, AutoCSR):
         final_waited_for = Signal(bits_for(N_points))
 
         # this is the signal that's used for detecting peaks
-        sum_diff = Signal((len(self.sum_diff_calculator.output), True))
-        abs_sum_diff = Signal.like(sum_diff)
+        self.sum_diff = Signal((len(self.sum_diff_calculator.output), True))
+        abs_sum_diff = Signal.like(self.sum_diff)
         self.comb += [
             self.sum_diff_calculator.writing_data_now.eq(self.writing_data_now),
             self.sum_diff_calculator.restart.eq(self.at_start),
             self.sum_diff_calculator.input.eq(self.input),
             self.sum_diff_calculator.delay_value.eq(self.time_scale.storage),
-            sum_diff.eq(self.sum_diff_calculator.output),
+        ]
+
+        self.sync += [
+            self.sum_diff.eq(self.sum_diff_calculator.output),
         ]
 
         # has this signal at the moment the same sign as the peak we are looking for?
@@ -168,16 +171,17 @@ class RobustAutolock(Module, AutoCSR):
         all_instructions_triggered = Signal()
 
         self.comb += [
-            sign_equal.eq((sum_diff > 0) == (current_peak_height > 0)),
-            If(sum_diff >= 0, abs_sum_diff.eq(sum_diff)).Else(
-                abs_sum_diff.eq(-1 * sum_diff)
+            sign_equal.eq((self.sum_diff > 0) == (current_peak_height > 0)),
+            If(self.sum_diff >= 0, abs_sum_diff.eq(self.sum_diff)).Else(
+                abs_sum_diff.eq(-1 * self.sum_diff)
             ),
             If(
                 current_peak_height >= 0,
                 abs_current_peak_height.eq(current_peak_height),
             ).Else(abs_current_peak_height.eq(-1 * current_peak_height)),
             over_threshold.eq(abs_sum_diff >= abs_current_peak_height),
-            waited_long_enough.eq(waited_for > current_wait_for),
+            # HACK: To compensate the lock position output for the pipeline delay
+            waited_long_enough.eq((waited_for >= current_wait_for - 1) & (waited_for != 2 ** bits_for(N_points) - 1) & (current_wait_for - 1 != 2 ** bits_for(N_points) - 1)),
             all_instructions_triggered.eq(
                 self.current_instruction_idx >= self.N_instructions.storage
             ),
@@ -190,7 +194,7 @@ class RobustAutolock(Module, AutoCSR):
         self.sync += [
             If(
                 self.at_start,
-                waited_for.eq(0),
+                waited_for.eq(-1),
                 # fpga robust autolock algorithm registeres trigger events delayed.
                 # Therefore, we give it a head start for `final_waited_for`
                 final_waited_for.eq(ROBUST_AUTOLOCK_FPGA_DELAY),
@@ -213,7 +217,8 @@ class RobustAutolock(Module, AutoCSR):
                         self.current_instruction_idx.eq(
                             self.current_instruction_idx + 1
                         ),
-                        waited_for.eq(0),
+                        # HACK: To compensate the lock position output for the pipeline delay
+                        waited_for.eq(-1),
                     ).Else(waited_for.eq(waited_for + 1)),
                 ),
                 If(