goertzel_nostd/
lib.rs

1// This (implicitly/naively) uses a rectangular window.  Some way to set up a window function
2// will be needed probably -- if mutating your samples before calling this isn't sufficient.
3
4// allow std in tests
5#![cfg_attr(not(test), no_std)]
6
7use core::{borrow::Borrow, f32::consts::PI};
8
9/// Set up parameters (and some precomputed values for those).
10#[derive(Clone, Copy)]
11pub struct Parameters {
12	// Parameters we're working with
13	window_size: usize,
14	// Precomputed value:
15	sine: f32,
16	cosine: f32,
17	term_coefficient: f32,
18}
19
20pub struct Partial {
21	params: Parameters,
22	count: usize,
23	prev: f32,
24	prevprev: f32,
25}
26
27impl Parameters {
28	pub fn new(target_freq: f32, sample_rate: u32, window_size: usize) -> Self {
29		let k = target_freq * (window_size as f32) / (sample_rate as f32);
30		let omega = (PI * 2. * k) / (window_size as f32);
31		let cosine = libm::cosf(omega);
32		Parameters {
33			window_size,
34			sine: libm::sinf(omega),
35			cosine,
36			term_coefficient: 2. * cosine,
37		}
38	}
39
40	pub fn start(self) -> Partial {
41		Partial {
42			params: self,
43			count: 0,
44			prev: 0.,
45			prevprev: 0.,
46		}
47	}
48
49	pub fn mag<V: Borrow<f32>, I: Iterator<Item = V>>(self, samples: I) -> f32 {
50		self.start().add_samples(samples).finish_mag()
51	}
52
53	pub fn mag_squared<V: Borrow<f32>, I: Iterator<Item = V>>(self, samples: I) -> f32 {
54		self.start().add_samples(samples).finish_mag_squared()
55	}
56}
57
58impl Partial {
59	pub fn add_samples<V: Borrow<f32>, I: Iterator<Item = V>>(mut self, samples: I) -> Self {
60		for sample in samples {
61			let this = self.params.term_coefficient * self.prev - self.prevprev + sample.borrow();
62			self.prevprev = self.prev;
63			self.prev = this;
64			self.count += 1;
65		}
66
67		self
68	}
69	pub fn finish(self) -> (f32, f32) {
70		assert_eq!(self.count, self.params.window_size);
71		let real = self.prev - self.prevprev * self.params.cosine;
72		let imag = self.prevprev * self.params.sine;
73		(real, imag)
74	}
75
76	pub fn finish_mag(self) -> f32 {
77		libm::sqrtf(self.finish_mag_squared())
78	}
79
80	pub fn finish_mag_squared(self) -> f32 {
81		let (real, imag) = self.finish();
82		real * real + imag * imag
83	}
84}
85
86#[test]
87fn zero_data() {
88	let p = Parameters::new(1800., 8000, 256);
89	assert!(p.start().add_samples([0.0; 256].iter()).finish_mag() == 0.);
90	assert!(
91		p.start()
92			.add_samples([0.0; 128].iter())
93			.add_samples([0.0; 128].iter())
94			.finish_mag()
95			== 0.
96	);
97}
98
99#[test]
100fn sine() {
101	let mut buf = [0.0; 8000];
102	for &freq in [697., 1200., 1800., 1633.].iter() {
103		// Generate a 1 second sine wave at freq hz
104		let step = 1. / 8000.;
105		for (i, v) in buf.iter_mut().enumerate() {
106			let time = i as f32 * step;
107			*v = (time * freq * PI * 2.).sin();
108		}
109
110		let p = Parameters::new(freq, 8000, 8000);
111		let mag = p.start().add_samples(buf[..].iter()).finish_mag();
112		for testfreq in (0..30).map(|x| (x * 100) as f32) {
113			let p = Parameters::new(testfreq, 8000, 8000);
114			let testmag = p.mag(buf[..].iter());
115			println!("{testfreq:4}: {testmag:12.3}");
116			if (freq - testfreq).abs() > 100. {
117				println!("{mag} > 10*{testmag}");
118				assert!(mag > 10. * testmag);
119			}
120		}
121	}
122}
goertzel_nostd/lib.rs

goertzel_nostd/
lib.rs
