158 lines
4.2 KiB
Rust
158 lines
4.2 KiB
Rust
use std::io::{BufReader, Read};
|
|
use num_traits::clamp;
|
|
|
|
pub struct Dpll {
|
|
ftw_min: i64,
|
|
ftw_max: i64,
|
|
ki: i64,
|
|
kp: i64,
|
|
|
|
ftw: i64,
|
|
integrator: i64,
|
|
|
|
phase: i64,
|
|
phase_unwrapped: i64,
|
|
|
|
was_locked: bool,
|
|
wait_lock: Option<u32>
|
|
}
|
|
|
|
impl Dpll {
|
|
pub const TURN: i64 = 0x100000000; // One turn in DPLL phase units.
|
|
|
|
pub fn new(ftw_min: i64, ftw_max: i64, ki: i64, kp: i64) -> Dpll {
|
|
assert!(ftw_min < Dpll::TURN/2);
|
|
assert!(ftw_max < Dpll::TURN/2);
|
|
assert!(Dpll::TURN & (Dpll::TURN - 1) == 0); // must be a power of 2
|
|
let init_ftw = (ftw_min + ftw_max)/2;
|
|
Dpll {
|
|
ftw_min: ftw_min,
|
|
ftw_max: ftw_max,
|
|
ki: ki,
|
|
kp: kp,
|
|
ftw: init_ftw,
|
|
integrator: init_ftw,
|
|
phase: 0,
|
|
phase_unwrapped: 0,
|
|
was_locked: false,
|
|
wait_lock: Some(0)
|
|
}
|
|
}
|
|
|
|
pub fn frequency_to_ftw(frequency: f64, sample_rate: f64) -> i64 {
|
|
(frequency*(Dpll::TURN as f64)/sample_rate) as i64
|
|
}
|
|
|
|
pub fn tick(&mut self, edge: bool) {
|
|
self.phase = (self.phase + self.ftw) & (Dpll::TURN - 1);
|
|
self.phase_unwrapped = self.phase_unwrapped.wrapping_add(self.ftw);
|
|
if edge {
|
|
let pe = Dpll::TURN/2 - self.phase;
|
|
self.integrator = clamp(self.integrator + (pe*self.ki >> 32),
|
|
self.ftw_min, self.ftw_max);
|
|
self.ftw = clamp(self.integrator + (pe*self.kp >> 32),
|
|
self.ftw_min, self.ftw_max);
|
|
|
|
if pe.abs() <= (self.ftw + self.ftw/3) {
|
|
if let Some(wait_lock) = self.wait_lock {
|
|
if wait_lock < 1000000 {
|
|
self.wait_lock = Some(wait_lock + 1);
|
|
} else {
|
|
self.wait_lock = None;
|
|
}
|
|
}
|
|
} else {
|
|
self.wait_lock = Some(0);
|
|
}
|
|
|
|
if self.locked() & !self.was_locked {
|
|
eprintln!("DPLL locked");
|
|
}
|
|
if !self.locked() & self.was_locked {
|
|
eprintln!("DPLL lost lock");
|
|
}
|
|
self.was_locked = self.locked();
|
|
}
|
|
}
|
|
|
|
pub fn get_phase_unwrapped(&self) -> i64 {
|
|
self.phase_unwrapped
|
|
}
|
|
|
|
pub fn locked(&self) -> bool {
|
|
self.wait_lock.is_none()
|
|
}
|
|
}
|
|
|
|
pub struct PositionTracker {
|
|
last_phase: i64,
|
|
current_position: i64
|
|
}
|
|
|
|
impl PositionTracker {
|
|
pub fn new() -> PositionTracker {
|
|
PositionTracker {
|
|
last_phase: 0,
|
|
current_position: 0
|
|
}
|
|
}
|
|
|
|
pub fn edge(&mut self, phase: i64) -> i64 {
|
|
let phase_diff = phase.wrapping_sub(self.last_phase);
|
|
self.last_phase = phase;
|
|
self.current_position += Dpll::TURN - phase_diff;
|
|
self.current_position
|
|
}
|
|
}
|
|
|
|
pub struct Decimator {
|
|
accumulator: i64,
|
|
current_count: u32,
|
|
max_count: u32
|
|
}
|
|
|
|
impl Decimator {
|
|
pub fn new(max_count: u32) -> Decimator {
|
|
Decimator {
|
|
accumulator: 0,
|
|
current_count: 0,
|
|
max_count: max_count
|
|
}
|
|
}
|
|
|
|
pub fn input(&mut self, data: i64) -> Option<i64> {
|
|
self.accumulator += data;
|
|
self.current_count += 1;
|
|
if self.current_count == self.max_count {
|
|
let average = self.accumulator/(self.current_count as i64);
|
|
self.accumulator = 0;
|
|
self.current_count = 0;
|
|
Some(average)
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn sample(command: &str, mut callback: impl FnMut(u8, u8)) {
|
|
let child = std::process::Command::new("sh")
|
|
.arg("-c")
|
|
.arg(command)
|
|
.stdout(std::process::Stdio::piped())
|
|
.spawn()
|
|
.unwrap();
|
|
let mut reader = BufReader::new(child.stdout.unwrap());
|
|
let mut buffer = [0; 1];
|
|
let mut last_sample = 0;
|
|
loop {
|
|
reader.read_exact(&mut buffer).unwrap();
|
|
for shift in [4u8, 0u8].iter() {
|
|
let sample = (buffer[0] >> shift) & 0x0f;
|
|
let rising = sample & !last_sample;
|
|
let falling = !sample & last_sample;
|
|
callback(rising, falling);
|
|
last_sample = sample;
|
|
}
|
|
}
|
|
}
|