diff --git a/fast-servo/autolock_pipeline.patch b/fast-servo/linien-gateware-autolock-pipeline.patch
similarity index 64%
rename from fast-servo/autolock_pipeline.patch
rename to fast-servo/linien-gateware-autolock-pipeline.patch
index 2e8c60c..723f841 100644
--- a/fast-servo/autolock_pipeline.patch
+++ b/fast-servo/linien-gateware-autolock-pipeline.patch
@@ -1,5 +1,5 @@
 diff --git a/gateware/logic/autolock.py b/gateware/logic/autolock.py
-index a6dc764..1a8407f 100644
+index a6dc764..975b23f 100644
 --- a/gateware/logic/autolock.py
 +++ b/gateware/logic/autolock.py
 @@ -148,14 +148,17 @@ class RobustAutolock(Module, AutoCSR):
@@ -23,44 +23,66 @@ index a6dc764..1a8407f 100644
          ]
  
          # has this signal at the moment the same sign as the peak we are looking for?
-@@ -168,16 +171,17 @@ class RobustAutolock(Module, AutoCSR):
+@@ -167,36 +170,41 @@ class RobustAutolock(Module, AutoCSR):
+         # have we detected all peaks (and can turn on the lock)?
          all_instructions_triggered = Signal()
  
-         self.comb += [
+-        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,
+-            ),
+-            If(
+-                current_peak_height >= 0,
++        self.sync += [
++            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),
+-            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.comb += [
++            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)
++            ),
++            over_threshold.eq(abs_sum_diff >= abs_current_peak_height),
++            waited_long_enough.eq(waited_for > current_wait_for),
+             self.turn_on_lock.eq(
+                 all_instructions_triggered
+                 & (final_waited_for >= self.final_wait_time.storage)
+             ),
+         ]
+ 
++        watching_reg = Signal()
          self.sync += [
++            watching.eq(watching_reg),
              If(
                  self.at_start,
 -                waited_for.eq(0),
-+                waited_for.eq(-1),
++                # Compensate pipeline delay
++                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(0),
+-                If(self.request_lock, watching.eq(1)).Else(watching.eq(0)),
++                If(self.request_lock, watching_reg.eq(1)).Else(watching.eq(0), watching_reg.eq(0)),
+             ).Else(
++                # Compensate pipeline delay
+                 # not at start
+                 If(
+                     ~self.request_lock,
+@@ -213,7 +221,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),
++                        # Compensate pipeline delay
++                        waited_for.eq(1),
                      ).Else(waited_for.eq(waited_for + 1)),
                  ),
                  If(
diff --git a/flake.nix b/flake.nix
index 9709a73..4574054 100644
--- a/flake.nix
+++ b/flake.nix
@@ -34,7 +34,7 @@
             ./fast-servo/linien-client-ssh-port-change.patch
             ./fast-servo/linien-server-fast-servo.patch
             ./fast-servo/linien-gateware-fast-servo.patch 
-            ./fast-servo/autolock_pipeline.patch
+            ./fast-servo/linien-gateware-autolock-pipeline.patch
             ./fast-servo/linien-module-iir-coeff-width-set-to-18bit.patch
             ./fast-servo/linien_module_pipeline.patch
             ./fast-servo/pid_pipeline.patch