embedded_dsp/
resampling.rs

1//! Multi-rate digital signal processing routines: Cascaded Integrator-Comb (CIC) decimation/interpolation and linear fractional resampling.
2
3/// Cascaded Integrator-Comb (CIC) Decimator for downsampling signals in integer arithmetic.
4pub struct CicDecimator<const STAGES: usize> {
5    r: usize, // Decimation factor
6    integrator_state: [i32; STAGES],
7    comb_state: [i32; STAGES],
8    sample_counter: usize,
9}
10
11impl<const STAGES: usize> CicDecimator<STAGES> {
12    /// Initialise a new CIC decimator with decimation factor `r`.
13    pub fn new(r: usize) -> Self {
14        Self {
15            r,
16            integrator_state: [0; STAGES],
17            comb_state: [0; STAGES],
18            sample_counter: 0,
19        }
20    }
21
22    /// Process an input sample. Returns `Some(decimated_sample)` every `R` samples.
23    pub fn process_sample(&mut self, input: i32) -> Option<i32> {
24        // Integrator stages running at high sample rate
25        let mut val = input;
26        for i in 0..STAGES {
27            self.integrator_state[i] = self.integrator_state[i].wrapping_add(val);
28            val = self.integrator_state[i];
29        }
30
31        self.sample_counter += 1;
32        if self.sample_counter >= self.r {
33            self.sample_counter = 0;
34
35            // Comb stages running at low sample rate
36            for i in 0..STAGES {
37                let diff = val.wrapping_sub(self.comb_state[i]);
38                self.comb_state[i] = val;
39                val = diff;
40            }
41            Some(val)
42        } else {
43            None
44        }
45    }
46}
47
48/// Cascaded Integrator-Comb (CIC) Interpolator for upsampling signals in integer arithmetic.
49pub struct CicInterpolator<const STAGES: usize> {
50    r: usize, // Interpolation factor
51    comb_state: [i32; STAGES],
52    integrator_state: [i32; STAGES],
53}
54
55impl<const STAGES: usize> CicInterpolator<STAGES> {
56    /// Initialise a new CIC interpolator with interpolation factor `r`.
57    pub fn new(r: usize) -> Self {
58        Self {
59            r,
60            comb_state: [0; STAGES],
61            integrator_state: [0; STAGES],
62        }
63    }
64
65    /// Process a single input sample and populate `out_buf` with `R` interpolated output samples.
66    pub fn process_sample(&mut self, input: i32, out_buf: &mut [i32]) {
67        assert!(
68            out_buf.len() >= self.r,
69            "out_buf must hold at least R samples"
70        );
71
72        // Comb stages at low rate
73        let mut val = input;
74        for i in 0..STAGES {
75            let diff = val.wrapping_sub(self.comb_state[i]);
76            self.comb_state[i] = val;
77            val = diff;
78        }
79
80        // Zero stuffing and integrator stages at high rate
81        for step in 0..self.r {
82            let in_step = if step == 0 { val } else { 0 };
83            let mut stage_val = in_step;
84
85            for i in 0..STAGES {
86                self.integrator_state[i] = self.integrator_state[i].wrapping_add(stage_val);
87                stage_val = self.integrator_state[i];
88            }
89
90            out_buf[step] = stage_val;
91        }
92    }
93}
94
95/// Linear fractional resampler.
96/// Resamples `src` into `dst` according to `ratio` (`src_sample_rate / dst_sample_rate`).
97pub fn resample_linear_f32(src: &[f32], dst: &mut [f32], ratio: f32) {
98    if src.is_empty() || dst.is_empty() || ratio <= 0.0 {
99        return;
100    }
101
102    for i in 0..dst.len() {
103        let src_idx_float = i as f32 * ratio;
104        let idx0 = src_idx_float as usize;
105        let idx1 = (idx0 + 1).min(src.len() - 1);
106
107        if idx0 >= src.len() {
108            dst[i] = src[src.len() - 1];
109            continue;
110        }
111
112        let frac = src_idx_float - idx0 as f32;
113        dst[i] = src[idx0] * (1.0 - frac) + src[idx1] * frac;
114    }
115}
116
117use crate::transform::cfft_f32;
118use crate::types::Status;
119
120/// Spectral (Sinc) 2:1 Interpolator using frequency-domain zero-padding via FFT/IFFT.
121///
122/// `src` length must be a power of 2 (e.g. 16, 32, 64, 128, 256).
123/// `dst` must have length at least `2 * src.len()`.
124pub fn spectral_interpolate_2x_f32(src: &[f32], dst: &mut [f32]) -> Status {
125    let n = src.len();
126    if n < 4 || (n & (n - 1)) != 0 {
127        return Status::ArgumentError;
128    }
129    if dst.len() < 2 * n {
130        return Status::LengthError;
131    }
132    if 4 * n > 1024 {
133        return Status::LengthError; // Max scratch size limit (256-pt input -> 512-pt complex)
134    }
135
136    let mut c_buf = [0.0f32; 1024];
137
138    // Copy src into complex array (size 2 * 2n)
139    for i in 0..n {
140        c_buf[2 * i] = src[i];
141        c_buf[2 * i + 1] = 0.0;
142    }
143
144    // FFT of size n
145    cfft_f32(&mut c_buf[..2 * n], n, 0, 1);
146
147    // Half Nyquist component
148    let nyq_re = 0.5 * c_buf[n];
149    let nyq_im = 0.5 * c_buf[n + 1];
150    c_buf[n] = nyq_re;
151    c_buf[n + 1] = nyq_im;
152
153    // Shift negative frequencies to upper half and zero middle
154    let mut expanded = [0.0f32; 1024];
155    // Copy 0..=N/2
156    for i in 0..=(n / 2) {
157        expanded[2 * i] = c_buf[2 * i];
158        expanded[2 * i + 1] = c_buf[2 * i + 1];
159    }
160    // Nyquist conjugate mirror at 3N/2
161    expanded[2 * (3 * n / 2)] = nyq_re;
162    expanded[2 * (3 * n / 2) + 1] = nyq_im;
163
164    // Negative frequencies
165    for i in (n / 2 + 1)..n {
166        expanded[2 * (i + n)] = c_buf[2 * i];
167        expanded[2 * (i + n) + 1] = c_buf[2 * i + 1];
168    }
169
170    // IFFT of size 2n
171    cfft_f32(&mut expanded[..4 * n], 2 * n, 1, 1);
172
173    // Copy back scaled real part (factor of 2)
174    for i in 0..(2 * n) {
175        dst[i] = 2.0 * expanded[2 * i];
176    }
177
178    Status::Success
179}
embedded_dsp/resampling.rs

embedded_dsp/
resampling.rs
