rework with adapted stolen code :(
This commit is contained in:
parent
9599800c21
commit
0a91445a37
1 changed files with 31 additions and 81 deletions
|
@ -28,92 +28,42 @@ pub fn take_input_pcm<T>(
|
|||
|
||||
let mut fft_planner = FftPlanner::<f64>::new();
|
||||
|
||||
let mut window_offset = 0;
|
||||
let mut prev_phase = f64::MAX;
|
||||
let mut phase_in = 3.0_f64;
|
||||
let freq_in = 1200.0_f64; // what does this mean?
|
||||
let alpha = 0.5;//0.05_f64;
|
||||
|
||||
// window must not extend beyond pcm_buffer
|
||||
while !carrier.acquired && window_offset + fft_size < pcm_buffer.len() {
|
||||
trace!("carrier acq: trying, window offset {window_offset}");
|
||||
let pcm_buffer_contiguous = pcm_buffer.make_contiguous();
|
||||
// Zero-pad for a better freq resolution. 8192 fft should be fast enough
|
||||
const MAX_ZERO_PAD: usize = 8192;
|
||||
let mut fft_frame: Vec<Complex<f64>> = if fft_size < MAX_ZERO_PAD {
|
||||
let mut frm =
|
||||
[Complex { re: 0.0f64, im: 0.0f64 }]
|
||||
.repeat(MAX_ZERO_PAD - fft_size);
|
||||
frm.extend(
|
||||
&pcm_buffer_contiguous[window_offset..(fft_size+window_offset)]
|
||||
);
|
||||
frm
|
||||
} else {
|
||||
pcm_buffer_contiguous[window_offset..(fft_size+window_offset)]
|
||||
.to_vec()
|
||||
};
|
||||
let fft_machine = fft_planner.plan_fft_forward(fft_frame.len());
|
||||
fft_machine.process(&mut fft_frame);
|
||||
let mut phase_out = 0.0_f64;
|
||||
let mut freq_out = 0.0_f64;
|
||||
let beta = 0.5 * alpha * alpha;
|
||||
|
||||
let clock_freq_index =
|
||||
(fft_size as f64 * carrier.clock_hz / sample_rate.0 as f64)
|
||||
.round() as usize;
|
||||
let mut ok_in_a_row = 0;
|
||||
|
||||
if let Some(bin) = fft_frame.get(clock_freq_index) {
|
||||
// if too small to use. threshold is arbitrary and needs tuning
|
||||
let threshold = 0.000000001;
|
||||
if bin.re + bin.im < threshold {
|
||||
trace!(
|
||||
"carrier acq: nothing ({}) below cutoff threshold {}. stop",
|
||||
bin.re + bin.im,
|
||||
threshold,
|
||||
);
|
||||
break;
|
||||
}
|
||||
while pcm_buffer.len() > 1 {
|
||||
// what on earth does this do??
|
||||
//let signal_in = (Complex::i() * phase_in).exp();
|
||||
// Where does the complex component come from????
|
||||
let Some(signal_in) = pcm_buffer.pop_front() else { unreachable!() };
|
||||
let signal_out = (Complex::i() * phase_out).exp();
|
||||
let phase_error = (signal_in * signal_out.conj()).arg();
|
||||
|
||||
let phase = f64::atan2(bin.im, bin.re);
|
||||
let ampl = bin.norm();
|
||||
trace!(
|
||||
"carrier acq: rsi:{}\tisi:{}\trso:{}\tiso:{}\terr:{}",
|
||||
signal_in.re,
|
||||
signal_in.im,
|
||||
signal_out.re,
|
||||
signal_out.im,
|
||||
phase_error,
|
||||
);
|
||||
|
||||
// if the phase just passed 0, increasing:
|
||||
if phase > 0.0 && prev_phase < 0.0 {
|
||||
carrier.acquired = true;
|
||||
debug!("carrier acq: success!");
|
||||
debug!(
|
||||
"carrier acq: drift {} matching {} ampl {}, nearest bin {}",
|
||||
phase,
|
||||
window_offset,
|
||||
ampl,
|
||||
sample_rate.0 as usize * clock_freq_index / fft_size,
|
||||
);
|
||||
} else {
|
||||
window_offset += 1;
|
||||
if window_offset > fft_size {
|
||||
trace!("carrier acq: can't find what i smell. stop");
|
||||
break;
|
||||
}
|
||||
trace!(
|
||||
"carrier acq: carrier ampl {} but phase {}, spinning...",
|
||||
ampl,
|
||||
phase,
|
||||
);
|
||||
}
|
||||
prev_phase = phase;
|
||||
} else {
|
||||
warn!("carrier acq: window overran FFT frame! report this!");
|
||||
break;
|
||||
if phase_error.abs() < 0.01 {
|
||||
ok_in_a_row += 1;
|
||||
if ok_in_a_row >= 25 { carrier.acquired = true; break; }
|
||||
}
|
||||
}
|
||||
|
||||
while carrier.acquired && pcm_buffer.len() >= fft_size {
|
||||
//fft_machine.process(&mut fft_frame);
|
||||
|
||||
/*for (i, s) in fft_frame.iter().enumerate() {
|
||||
let freq = sample_rate.0 as usize * i / fft_size;
|
||||
let ampl = s.norm();
|
||||
let confidence = ampl;
|
||||
if ampl > 50.0 {
|
||||
println!("{:?}: {:?}", freq, ampl);
|
||||
}
|
||||
|
||||
// soon: confidence = ampl / |freq_target - freq_observed|
|
||||
}*/
|
||||
phase_out += alpha * phase_error;
|
||||
freq_out += beta * phase_error;
|
||||
//phase_in += freq_in;
|
||||
phase_out += freq_out;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in a new issue