1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
use num_complex::Complex32 as C32;
use crate::core::{Block, WorkReport};
use crate::dsp::LpCascade;
use crate::util::atan2_approx;
/// PM demodulator via quadrature (phase difference) + post LPF
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct PmQuadratureDemod {
fs: f32, // sample rate (kept for future use; ok if unused)
k: f32, // gain/sensitivity applied to phase difference
post_lp: LpCascade,
prev: C32, // previous complex sample for quadrature detector
}
impl PmQuadratureDemod {
/// `audio_bw_hz` is the post-demod audio bandwidth (low-pass cutoff).
/// `k` is a scaling constant (1.0 is fine; adjust per modulator).
pub fn new(fs: f32, k: f32, audio_bw_hz: f32) -> Self {
// Gentle transition band (25% of cutoff); tweak as needed.
let lp = LpCascade::design(fs, audio_bw_hz * 0.9);
Self {
fs,
k,
post_lp: lp,
prev: C32::new(1.0, 0.0),
}
}
}
impl Block for PmQuadratureDemod {
type In = C32;
type Out = f32;
#[inline(always)]
fn process(&mut self, input: &[Self::In], output: &mut [Self::Out]) -> WorkReport {
let n = input.len().min(output.len());
if n == 0 {
return WorkReport { in_read: 0, out_written: 0 };
}
// 1) Quadrature discriminator: angle( z[n] * conj(z[n-1]) )
// This yields Δphase; for PM this is proportional to d/dt of message.
// If your PM modulator is symmetric (no extra integration), this
// matches the “quadrature PM” path used in your tests.
let mut prev = self.prev;
for i in 0..n {
let z = input[i];
let w = z * prev.conj();
output[i] = self.post_lp.process(self.k * atan2_approx(w.im, w.re));
prev = z;
}
self.prev = prev;
WorkReport { in_read: n, out_written: n }
}
}