diff --git a/fast-servo/autolock_pipeline.patch b/fast-servo/autolock_pipeline.patch
new file mode 100644
index 0000000..2e8c60c
--- /dev/null
+++ b/fast-servo/autolock_pipeline.patch
@@ -0,0 +1,66 @@
+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(
diff --git a/fast-servo/iir_pipeline.patch b/fast-servo/iir_pipeline.patch
new file mode 100644
index 0000000..b64b209
--- /dev/null
+++ b/fast-servo/iir_pipeline.patch
@@ -0,0 +1,13 @@
+diff --git a/gateware/logic/iir.py b/gateware/logic/iir.py
+index 2380dd7..60bfeb7 100644
+--- a/gateware/logic/iir.py
++++ b/gateware/logic/iir.py
+@@ -89,7 +89,7 @@ class Iir(Filter):
+                 zr = Signal.like(z)
+                 self.sync += zr.eq(z)
+                 z = Signal.like(zr)
+-                self.comb += z.eq(zr + signal * c[coeff])
++                self.sync += z.eq(zr + signal * c[coeff])
+             self.comb += y_next.eq(z)
+             self.latency.value = Constant(order + 1)
+             self.interval.value = Constant(1)
diff --git a/fast-servo/linien_module_pipeline.patch b/fast-servo/linien_module_pipeline.patch
new file mode 100644
index 0000000..4206247
--- /dev/null
+++ b/fast-servo/linien_module_pipeline.patch
@@ -0,0 +1,57 @@
+diff --git a/gateware/linien_module.py b/gateware/linien_module.py
+index a958896..a64714c 100644
+--- a/gateware/linien_module.py
++++ b/gateware/linien_module.py
+@@ -233,23 +233,46 @@ class LinienModule(Module, AutoCSR):
+             self.fast_a.adc.eq(soc.analog.adc_a),
+             self.fast_b.adc.eq(soc.analog.adc_b),
+         ]
+-
++        
+         # now, we combine the output of the two paths, with a variable factor each.
+         mixed = Signal(
+             (2 + ((signal_width + 1) + self.logic.chain_a_factor.size), True)
+         )
++
++        chain_a_factor_mult_fast_a_out_i = Signal(
++            (2 + ((signal_width + 1) + self.logic.chain_a_factor.size), True)
++        )
++
++        chain_b_factor_mult_fast_b_out_i = Signal(
++            (2 + ((signal_width + 1) + self.logic.chain_a_factor.size), True)
++        )
++        combined_offset_signed_left_shifted = Signal(
++            (2 + ((signal_width + 1) + self.logic.chain_a_factor.size), True)
++        )
++        fast_a_out_i_left_shifted = Signal(
++            (2 + ((signal_width + 1) + self.logic.chain_a_factor.size), True)
++        )
++
++        self.sync += [
++            chain_a_factor_mult_fast_a_out_i.eq(self.logic.chain_a_factor.storage * self.fast_a.out_i),
++            chain_b_factor_mult_fast_b_out_i.eq(self.logic.chain_b_factor.storage * self.fast_b.out_i),
++            combined_offset_signed_left_shifted.eq(self.logic.combined_offset_signed << (chain_factor_bits + s)),
++            fast_a_out_i_left_shifted.eq(self.fast_a.out_i << chain_factor_bits),
++        ] 
++
++
+         self.comb += [
+             If(
+                 self.logic.dual_channel.storage,
+                 mixed.eq(
+-                    (self.logic.chain_a_factor.storage * self.fast_a.out_i)
+-                    + (self.logic.chain_b_factor.storage * self.fast_b.out_i)
+-                    + (self.logic.combined_offset_signed << (chain_factor_bits + s))
++                    chain_a_factor_mult_fast_a_out_i
++                    + chain_b_factor_mult_fast_b_out_i
++                    + combined_offset_signed_left_shifted
+                 ),
+             ).Else(
+                 mixed.eq(
+-                    (self.fast_a.out_i << chain_factor_bits)
+-                    + (self.logic.combined_offset_signed << (chain_factor_bits + s))
++                    fast_a_out_i_left_shifted
++                    + combined_offset_signed_left_shifted
+                 )
+             )
+         ]
diff --git a/fast-servo/pid_pipeline.patch b/fast-servo/pid_pipeline.patch
new file mode 100644
index 0000000..26d2986
--- /dev/null
+++ b/fast-servo/pid_pipeline.patch
@@ -0,0 +1,64 @@
+diff --git a/gateware/logic/pid.py b/gateware/logic/pid.py
+index 4320f94..64e1a74 100644
+--- a/gateware/logic/pid.py
++++ b/gateware/logic/pid.py
+@@ -56,10 +56,13 @@ class PID(Module, AutoCSR):
+         self.comb += [kp_signed.eq(self.kp.storage)]
+ 
+         kp_mult = Signal((self.width + self.coeff_width, True))
+-        self.comb += [kp_mult.eq(self.error * kp_signed)]
++        kp_mult_reg = Signal((self.width + self.coeff_width, True))
++        self.sync += kp_mult.eq(kp_mult_reg >> (self.coeff_width - 2))
++
++        self.comb += [kp_mult_reg.eq(self.error * kp_signed)]
+ 
+         self.output_p = Signal((self.width, True))
+-        self.comb += [self.output_p.eq(kp_mult >> (self.coeff_width - 2))]
++        self.comb += [self.output_p.eq(kp_mult)]
+ 
+         self.kp_mult = kp_mult
+ 
+@@ -71,8 +74,10 @@ class PID(Module, AutoCSR):
+         self.comb += [ki_signed.eq(self.ki.storage)]
+ 
+         self.ki_mult = Signal((1 + self.width + self.coeff_width, True))
++        self.ki_mult_reg = Signal((1 + self.width + self.coeff_width, True))
++        self.sync += self.ki_mult.eq(self.ki_mult_reg)
++        self.comb += self.ki_mult_reg.eq((self.error * ki_signed) >> 4)
+ 
+-        self.comb += [self.ki_mult.eq((self.error * ki_signed) >> 4)]
+ 
+         int_reg_width = self.width + self.coeff_width + 4
+         extra_width = int_reg_width - self.width
+@@ -110,15 +115,17 @@ class PID(Module, AutoCSR):
+         self.kd = CSRStorage(self.coeff_width)
+         kd_signed = Signal((self.coeff_width, True))
+         kd_mult = Signal((mult_width, True))
++        kd_mult_reg = Signal((mult_width, True))
++        self.sync += kd_mult.eq(kd_mult_reg)
+ 
+-        self.comb += [kd_signed.eq(self.kd.storage), kd_mult.eq(self.error * kd_signed)]
++        self.comb += [kd_signed.eq(self.kd.storage), kd_mult_reg.eq(self.error * kd_signed >> (self.coeff_width - self.d_shift))]
+ 
+         kd_reg = Signal((out_width, True))
+         kd_reg_r = Signal((out_width, True))
+ 
+         self.output_d = Signal((out_width, True))
+         self.sync += [
+-            kd_reg.eq(kd_mult >> (self.coeff_width - self.d_shift)),
++            kd_reg.eq(kd_mult),
+             kd_reg_r.eq(kd_reg),
+             self.output_d.eq(kd_reg - kd_reg_r),
+         ]
+@@ -143,4 +150,12 @@ class PID(Module, AutoCSR):
+ 
+         # sync is required here, otherwise we get artifacts when one of the
+         # signals changes sign
+-        self.sync += [self.pid_sum.eq(self.output_p + self.int_out + self.output_d)]
++        self.sync += [
++            If(
++                self.running,
++                self.pid_sum.eq(self.output_p + self.int_out + self.output_d),
++            )
++            .Else(self.pid_sum.eq(0))
++        ]
diff --git a/flake.nix b/flake.nix
index d23d814..f3d89b0 100644
--- a/flake.nix
+++ b/flake.nix
@@ -164,7 +164,11 @@
         '';
         patches = [
           fast-servo/linien-fast-servo-gateware.patch
-          fast-servo/linien-fast-servo-server.patch
+          fast-servo/linien-fast-servo-server.patch 
+          fast-servo/autolock_pipeline.patch
+          fast-servo/iir_pipeline.patch
+          fast-servo/linien_module_pipeline.patch
+          fast-servo/pid_pipeline.patch
         ];
         nativeBuildInputs = [
           (pkgs.python3.withPackages(ps: [