From af3812e2581fed7239f34b1e69b9690c3c3dcee5 Mon Sep 17 00:00:00 2001
From: Florian Agbuya <fa@m-labs.ph>
Date: Tue, 10 Sep 2024 12:28:37 +0800
Subject: [PATCH] modified word_align

---
 fast-servo/pyfastservo/adc.py | 78 ++++++++++++++++-------------------
 1 file changed, 36 insertions(+), 42 deletions(-)

diff --git a/fast-servo/pyfastservo/adc.py b/fast-servo/pyfastservo/adc.py
index e04a47e..ebb5e93 100644
--- a/fast-servo/pyfastservo/adc.py
+++ b/fast-servo/pyfastservo/adc.py
@@ -189,66 +189,58 @@ def write_to_memory(address, value):
 
 
 def word_align():
-
     value = 0
-    edge_detected = False
-    transition = False
+    edge_start = None
+    edge_end = None
     tap_delay = 0
 
+    # Perform initial bitslip if necessary
     for i in range(4):
         current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0]
         if current_frame != 0x0C:
-            print(
-                f"Performing bitslip (bitslip iteration: {i}). Reason: current_frame is 0x{current_frame:02x} instead of 0x0C"
-            )
+            print(f"Performing bitslip (iteration: {i}). Current frame: 0x{current_frame:02x}")
             write_to_memory(ADC_BITSLIP_ADDR, 1)
         else:
-            print(f"No bitslip required; Currernt frame = 0x{current_frame:02x}")
+            print(f"No bitslip required; Current frame = 0x{current_frame:02x}")
             break
 
-    current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0]
-    prev_frame = current_frame
-
-    for i in range(32):
+    # Sweep through all possible delay values
+    for tap_delay in range(32):
         write_to_memory(ADC_DELAY_ADDR, tap_delay)
-        if edge_detected == 1:
-            break
         current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0]
+        
+        print(f"Tap delay: {tap_delay}, Current frame: 0x{current_frame:02x}")
 
-        print(f"Tap delay: {tap_delay}")
-        print(f"Current frame: 0x{current_frame:02x}")
+        if current_frame == 0x0C:
+            if edge_start is None:
+                edge_start = tap_delay
+        elif edge_start is not None and edge_end is None:
+            edge_end = tap_delay - 1
+            break
 
-        if current_frame == prev_frame:
-            tap_delay += 1
-        elif not transition:
-            tap_delay += 1
-            transition = True
-        elif transition:
-            tap_delay = i // 2
-            edge_detected = True
+    # Analyze the sweep results
+    if edge_start is None:
+        print("No stable region found. Using default delay.")
+        optimal_delay = 11  # Default value
+    elif edge_end is None:
+        print("Stable region extends to the end. Using middle of detected stable region.")
+        optimal_delay = (edge_start + 31) // 2
+    else:
+        print(f"Stable region detected from {edge_start} to {edge_end}")
+        optimal_delay = (edge_start + edge_end) // 2
 
-        prev_frame = current_frame
+    # Set the optimal delay
+    write_to_memory(ADC_DELAY_ADDR, optimal_delay)
+    print(f"Setting optimal delay to: {optimal_delay}")
 
-    if not edge_detected:
-        tap_delay = 11  # empirically tested to work best
-        write_to_memory(ADC_DELAY_ADDR, tap_delay)
-        print(f"No edge detected; setting iDelay to: {tap_delay}")
-    if edge_detected:
-        write_to_memory(ADC_DELAY_ADDR, tap_delay + 2)
-        print(f"Edge detected; setting iDelay to (tap_delay + 2): {tap_delay} + 2")
-
-    adc_ch0 = read_from_memory(ADC_CH0_HIGH_ADDR, 4)
-    print(f"ADC_CH0: 0x{adc_ch0}")
-
-    adc_ch0 = (read_from_memory(ADC_CH0_HIGH_ADDR, 1)[0] << 8) | read_from_memory(
-        ADC_CH0_LOW_ADDR, 1
-    )[0]
-    adc_ch1 = (read_from_memory(ADC_CH1_HIGH_ADDR, 1)[0] << 8) | read_from_memory(
-        ADC_CH1_LOW_ADDR, 1
-    )[0]
+    # Verify final setup
+    adc_ch0 = (read_from_memory(ADC_CH0_HIGH_ADDR, 1)[0] << 8) | read_from_memory(ADC_CH0_LOW_ADDR, 1)[0]
+    adc_ch1 = (read_from_memory(ADC_CH1_HIGH_ADDR, 1)[0] << 8) | read_from_memory(ADC_CH1_LOW_ADDR, 1)[0]
     print(f"Final ADC_CH0: 0x{adc_ch0:04x}")
     print(f"Final ADC_CH1: 0x{adc_ch1:04x}")
 
+    return optimal_delay
+
 
 def modify_bit(original_value, position, bit_value):
     mask = 1 << position
@@ -307,13 +299,15 @@ def adc_aux_read(port, type, pin):
 
 def main():
     main_adc_config(0x811F)
-    word_align()
+    optimal_delay = word_align()
 
     main_adc_test_mode(False)
 
     write_to_memory(ADC_AFE_CTRL_ADDR, 0b1100) # {-, -, ch2_X10, ch1_X10}
     print(read_from_memory(ADC_AFE_CTRL_ADDR, 1)[0])
 
+    print(f"Optimal delay used: {optimal_delay}")
+
 
 if __name__ == "__main__":
     main()
\ No newline at end of file