embedded_dsp/
const_generics.rs

1//! Const generic safe wrappers for compile-time sized FIR filters, Biquads, and Matrices.
2
3use crate::filtering::{biquad_cascade_df1_f32, fir_f32, BiquadCascadeInstanceF32, FirInstanceF32};
4use crate::matrix::{
5    mat_add_f32, mat_mult_f32, mat_scale_f32, mat_sub_f32, mat_trans_f32, MatrixInstance,
6    MatrixInstanceMut,
7};
8
9/// Compile-time fixed-size FIR filter holding its own state buffer.
10#[derive(Debug, Clone)]
11#[cfg_attr(feature = "defmt", derive(defmt::Format))]
12pub struct FirFilter<const TAPS: usize> {
13    pub coeffs: [f32; TAPS],
14    state: [f32; TAPS],
15}
16
17impl<const TAPS: usize> FirFilter<TAPS> {
18    /// Create a new FIR filter with given coefficients.
19    pub fn new(coeffs: [f32; TAPS]) -> Self {
20        Self {
21            coeffs,
22            state: [0.0; TAPS],
23        }
24    }
25
26    /// Process input slice `src` into output slice `dst`.
27    pub fn process(&mut self, src: &[f32], dst: &mut [f32]) {
28        let mut instance = FirInstanceF32 {
29            num_taps: TAPS as u16,
30            coeffs: &self.coeffs,
31            state: &mut self.state,
32        };
33        fir_f32(&mut instance, src, dst);
34    }
35
36    /// Reset filter state buffer.
37    pub fn reset(&mut self) {
38        self.state.fill(0.0);
39    }
40}
41
42/// Compile-time fixed-size Biquad Cascade Direct Form I filter holding its state buffer.
43#[derive(Debug, Clone)]
44#[cfg_attr(feature = "defmt", derive(defmt::Format))]
45pub struct BiquadCascade<const COEFFS_LEN: usize, const STATE_LEN: usize> {
46    pub coeffs: [f32; COEFFS_LEN],
47    pub state: [f32; STATE_LEN],
48    num_stages: u8,
49}
50
51impl<const COEFFS_LEN: usize, const STATE_LEN: usize> BiquadCascade<COEFFS_LEN, STATE_LEN> {
52    /// Create a new Biquad cascade filter given coefficients and number of stages.
53    pub fn new(coeffs: [f32; COEFFS_LEN]) -> Self {
54        let num_stages = (COEFFS_LEN / 5) as u8;
55        Self {
56            coeffs,
57            state: [0.0; STATE_LEN],
58            num_stages,
59        }
60    }
61
62    /// Process input slice `src` into output slice `dst`.
63    pub fn process(&mut self, src: &[f32], dst: &mut [f32]) {
64        let mut instance = BiquadCascadeInstanceF32 {
65            num_stages: self.num_stages,
66            coeffs: &self.coeffs,
67            state: &mut self.state,
68        };
69        biquad_cascade_df1_f32(&mut instance, src, dst);
70    }
71
72    /// Reset internal filter delay state.
73    pub fn reset(&mut self) {
74        self.state.fill(0.0);
75    }
76}
77
78/// Compile-time fixed-size 2D matrix structure.
79#[derive(Debug, Clone, Copy, PartialEq)]
80#[cfg_attr(feature = "defmt", derive(defmt::Format))]
81pub struct Matrix<const R: usize, const C: usize, const N: usize> {
82    pub data: [f32; N],
83}
84
85impl<const R: usize, const C: usize, const N: usize> Matrix<R, C, N> {
86    /// Create matrix from array.
87    pub const fn new(data: [f32; N]) -> Self {
88        Self { data }
89    }
90
91    /// Matrix zero constructor.
92    pub fn zeros() -> Self {
93        Self { data: [0.0; N] }
94    }
95
96    /// Matrix addition: `self + rhs`.
97    pub fn add(&self, rhs: &Self) -> Self {
98        let mut out = Self::zeros();
99        let a_inst = MatrixInstance::new(R as u16, C as u16, &self.data);
100        let b_inst = MatrixInstance::new(R as u16, C as u16, &rhs.data);
101        let mut out_inst = MatrixInstanceMut::new(R as u16, C as u16, &mut out.data);
102        mat_add_f32(&a_inst, &b_inst, &mut out_inst);
103        out
104    }
105
106    /// Matrix subtraction: `self - rhs`.
107    pub fn sub(&self, rhs: &Self) -> Self {
108        let mut out = Self::zeros();
109        let a_inst = MatrixInstance::new(R as u16, C as u16, &self.data);
110        let b_inst = MatrixInstance::new(R as u16, C as u16, &rhs.data);
111        let mut out_inst = MatrixInstanceMut::new(R as u16, C as u16, &mut out.data);
112        mat_sub_f32(&a_inst, &b_inst, &mut out_inst);
113        out
114    }
115
116    /// Matrix scaling: `self * scale`.
117    pub fn scale(&self, scale: f32) -> Self {
118        let mut out = Self::zeros();
119        let a_inst = MatrixInstance::new(R as u16, C as u16, &self.data);
120        let mut out_inst = MatrixInstanceMut::new(R as u16, C as u16, &mut out.data);
121        mat_scale_f32(&a_inst, scale, &mut out_inst);
122        out
123    }
124
125    /// Matrix transpose.
126    pub fn transpose(&self) -> Matrix<C, R, N> {
127        let mut out = Matrix::<C, R, N>::zeros();
128        let a_inst = MatrixInstance::new(R as u16, C as u16, &self.data);
129        let mut out_inst = MatrixInstanceMut::new(C as u16, R as u16, &mut out.data);
130        mat_trans_f32(&a_inst, &mut out_inst);
131        out
132    }
133
134    /// Matrix multiplication: `self * rhs`.
135    pub fn mul_mat<const C2: usize, const N2: usize, const N_OUT: usize>(
136        &self,
137        rhs: &Matrix<C, C2, N2>,
138    ) -> Matrix<R, C2, N_OUT> {
139        let mut out = Matrix::<R, C2, N_OUT>::zeros();
140        let a_inst = MatrixInstance::new(R as u16, C as u16, &self.data);
141        let b_inst = MatrixInstance::new(C as u16, C2 as u16, &rhs.data);
142        let mut out_inst = MatrixInstanceMut::new(R as u16, C2 as u16, &mut out.data);
143        mat_mult_f32(&a_inst, &b_inst, &mut out_inst);
144        out
145    }
146}
embedded_dsp/const_generics.rs

embedded_dsp/
const_generics.rs
