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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
//! Multi-rate digital signal processing routines: Cascaded Integrator-Comb (CIC) decimation/interpolation and linear fractional resampling.
/// Cascaded Integrator-Comb (CIC) Decimator for downsampling signals in integer arithmetic.
pub struct CicDecimator<const STAGES: usize> {
r: usize, // Decimation factor
integrator_state: [i32; STAGES],
comb_state: [i32; STAGES],
sample_counter: usize,
}
impl<const STAGES: usize> CicDecimator<STAGES> {
/// Initialise a new CIC decimator with decimation factor `r`.
pub fn new(r: usize) -> Self {
Self {
r,
integrator_state: [0; STAGES],
comb_state: [0; STAGES],
sample_counter: 0,
}
}
/// Process an input sample. Returns `Some(decimated_sample)` every `R` samples.
pub fn process_sample(&mut self, input: i32) -> Option<i32> {
// Integrator stages running at high sample rate
let mut val = input;
for i in 0..STAGES {
self.integrator_state[i] = self.integrator_state[i].wrapping_add(val);
val = self.integrator_state[i];
}
self.sample_counter += 1;
if self.sample_counter >= self.r {
self.sample_counter = 0;
// Comb stages running at low sample rate
for i in 0..STAGES {
let diff = val.wrapping_sub(self.comb_state[i]);
self.comb_state[i] = val;
val = diff;
}
Some(val)
} else {
None
}
}
}
/// Cascaded Integrator-Comb (CIC) Interpolator for upsampling signals in integer arithmetic.
pub struct CicInterpolator<const STAGES: usize> {
r: usize, // Interpolation factor
comb_state: [i32; STAGES],
integrator_state: [i32; STAGES],
}
impl<const STAGES: usize> CicInterpolator<STAGES> {
/// Initialise a new CIC interpolator with interpolation factor `r`.
pub fn new(r: usize) -> Self {
Self {
r,
comb_state: [0; STAGES],
integrator_state: [0; STAGES],
}
}
/// Process a single input sample and populate `out_buf` with `R` interpolated output samples.
pub fn process_sample(&mut self, input: i32, out_buf: &mut [i32]) {
assert!(
out_buf.len() >= self.r,
"out_buf must hold at least R samples"
);
// Comb stages at low rate
let mut val = input;
for i in 0..STAGES {
let diff = val.wrapping_sub(self.comb_state[i]);
self.comb_state[i] = val;
val = diff;
}
// Zero stuffing and integrator stages at high rate
for step in 0..self.r {
let in_step = if step == 0 { val } else { 0 };
let mut stage_val = in_step;
for i in 0..STAGES {
self.integrator_state[i] = self.integrator_state[i].wrapping_add(stage_val);
stage_val = self.integrator_state[i];
}
out_buf[step] = stage_val;
}
}
}
/// Linear fractional resampler.
/// Resamples `src` into `dst` according to `ratio` (`src_sample_rate / dst_sample_rate`).
pub fn resample_linear_f32(src: &[f32], dst: &mut [f32], ratio: f32) {
if src.is_empty() || dst.is_empty() || ratio <= 0.0 {
return;
}
for i in 0..dst.len() {
let src_idx_float = i as f32 * ratio;
let idx0 = src_idx_float as usize;
let idx1 = (idx0 + 1).min(src.len() - 1);
if idx0 >= src.len() {
dst[i] = src[src.len() - 1];
continue;
}
let frac = src_idx_float - idx0 as f32;
dst[i] = src[idx0] * (1.0 - frac) + src[idx1] * frac;
}
}