fsagbuya
/
nix-servo
forked from M-Labs/nix-servo
1
0
Fork 0
Code Pull Requests Activity

modified word_align

This commit is contained in:
Florian Agbuya 2024-09-10 12:28:37 +08:00
parent e22237e347
commit 59c95f5450
1 changed files with 40 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
fast-servo/pyfastservo/adc.py
View File

@ -21,7 +21,7 @@ import mmap
import os import os
import spidev import spidev
from pyfastservo.common import ( from common import (
ADC_AFE_CTRL_ADDR, ADC_AFE_CTRL_ADDR,
ADC_BITSLIP_ADDR, ADC_BITSLIP_ADDR,
ADC_CH0_HIGH_ADDR, ADC_CH0_HIGH_ADDR,
@ -54,7 +54,7 @@ def main_adc_config(test_pattern):
spi.open(MAIN_ADC_BUS, MAIN_ADC_DEVICE) spi.open(MAIN_ADC_BUS, MAIN_ADC_DEVICE)
spi.max_speed_hz = 50000 spi.max_speed_hz = 50000
spi.mode = 0b00 # CPOL = 0 CPHA = 0 spi.mode = 0b00 # CPOL = 0 CPHA = 0
spi.cshigh = False spi.cshigh = True
# spi.read0 = False # spi.read0 = False
spi_buffer = [0x00, 0x80] # reset spi_buffer = [0x00, 0x80] # reset
@ -121,7 +121,7 @@ def main_adc_test_mode(enable):
spi.open(MAIN_ADC_BUS, MAIN_ADC_DEVICE) spi.open(MAIN_ADC_BUS, MAIN_ADC_DEVICE)
spi.max_speed_hz = 50000 spi.max_speed_hz = 50000
spi.mode = 0b00 # CPOL = 0 CPHA = 0 spi.mode = 0b00 # CPOL = 0 CPHA = 0
spi.cshigh = False spi.cshigh = True
# spi.read0 = True # spi.read0 = True
reg_contents = ( reg_contents = (
@ -189,66 +189,58 @@ def write_to_memory(address, value):
def word_align(): def word_align():
value = 0 value = 0
edge_detected = False edge_start = None
transition = False edge_end = None
tap_delay = 0 tap_delay = 0
# Perform initial bitslip if necessary
for i in range(4): for i in range(4):
current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0] current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0]
if current_frame != 0x0C: if current_frame != 0x0C:
print( print(f"Performing bitslip (iteration: {i}). Current frame: 0x{current_frame:02x}")
f"Performing bitslip (bitslip iteration: {i}). Reason: current_frame is 0x{current_frame:02x} instead of 0x0C"
)
write_to_memory(ADC_BITSLIP_ADDR, 1) write_to_memory(ADC_BITSLIP_ADDR, 1)
else: else:
print(f"No bitslip required; Currernt frame = 0x{current_frame:02x}") print(f"No bitslip required; Current frame = 0x{current_frame:02x}")
break break
current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0] # Sweep through all possible delay values
prev_frame = current_frame for tap_delay in range(32):
for i in range(32):
write_to_memory(ADC_DELAY_ADDR, tap_delay) write_to_memory(ADC_DELAY_ADDR, tap_delay)
if edge_detected == 1:
break
current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0] current_frame = read_from_memory(ADC_FRAME_ADDR, 1)[0]
print(f"Tap delay: {tap_delay}") print(f"Tap delay: {tap_delay}, Current frame: 0x{current_frame:02x}")
print(f"Current frame: 0x{current_frame:02x}")
if current_frame == prev_frame: if current_frame == 0x0C:
tap_delay += 1 if edge_start is None:
elif not transition: edge_start = tap_delay
tap_delay += 1 elif edge_start is not None and edge_end is None:
transition = True edge_end = tap_delay - 1
elif transition: break
tap_delay = i // 2
edge_detected = True
prev_frame = current_frame # 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
if not edge_detected: # Set the optimal delay
tap_delay = 11 # empirically tested to work best write_to_memory(ADC_DELAY_ADDR, optimal_delay)
write_to_memory(ADC_DELAY_ADDR, tap_delay) print(f"Setting optimal delay to: {optimal_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) # Verify final setup
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]
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_CH0: 0x{adc_ch0:04x}")
print(f"Final ADC_CH1: 0x{adc_ch1:04x}") print(f"Final ADC_CH1: 0x{adc_ch1:04x}")
return optimal_delay
def modify_bit(original_value, position, bit_value): def modify_bit(original_value, position, bit_value):
mask = 1 << position mask = 1 << position
@ -292,7 +284,7 @@ def adc_aux_read(port, type, pin):
spi.open(1, 3) # AUX ADC 1? spi.open(1, 3) # AUX ADC 1?
spi.max_speed_hz = 5000 spi.max_speed_hz = 5000
spi.mode = 0b00 spi.mode = 0b00
spi.cshigh = False spi.cshigh = True
read_buffer = spi.xfer2(write_buffer) read_buffer = spi.xfer2(write_buffer)
mu_voltage = read_buffer[0] << 8 | read_buffer[1] >> 2 mu_voltage = read_buffer[0] << 8 | read_buffer[1] >> 2
@ -307,13 +299,15 @@ def adc_aux_read(port, type, pin):
def main(): def main():
main_adc_config(0x811F) main_adc_config(0x811F)
word_align() optimal_delay = word_align()
main_adc_test_mode(False) main_adc_test_mode(False)
write_to_memory(ADC_AFE_CTRL_ADDR, 0b1100) # {-, -, ch2_X10, ch1_X10} write_to_memory(ADC_AFE_CTRL_ADDR, 0b1100) # {-, -, ch2_X10, ch1_X10}
print(read_from_memory(ADC_AFE_CTRL_ADDR, 1)[0]) print(read_from_memory(ADC_AFE_CTRL_ADDR, 1)[0])
print(f"Optimal delay used: {optimal_delay}")
if __name__ == "__main__": if __name__ == "__main__":
main() main()