forked from M-Labs/nix-servo
67 lines
3.3 KiB
Diff
67 lines
3.3 KiB
Diff
|
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(
|