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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
//! Quadrature demod, the core of an FM demodulator.
use crate::block::{Block, BlockRet};
use crate::stream::{ReadStream, WriteStream};
use crate::{Complex, Float, Result};
/// Quadrature demod, the core of an FM demodulator.
///
/// Quadrature demodulation works is best done by thinking of the samples
/// as vectors going out of the origin on the complex plane.
///
/// A zero frequency means no "spinning" around the origin, but with all
/// samples just being on a vector, with the same angle, though possibly
/// varying magnitude.
///
/// Negative frequency means the vector is spinning counter
/// clockwise. Positive frequency means spinning clockwise.
///
/// Quadrature demodulation discards the magnitude of the vector, and just
/// looks at the angle between the current sample, and the previous
/// sample.
///
/// Because magnitude is discarded, this block is only useful for decoding
/// frequency changes (FM, FSK, …), not things like QAM.
///
/// [This article][vectorized] gives some good illustrations.
///
/// Enabling the `fast-math` feature (dependency) speeds up
/// `QuadratureDemod` by about 4x.
///
/// [vectorized]: https://mazzo.li/posts/vectorized-atan2.html
#[derive(rustradio_macros::Block)]
#[rustradio(crate, new)]
pub struct QuadratureDemod {
gain: Float,
#[rustradio(in)]
src: ReadStream<Complex>,
#[rustradio(out)]
dst: WriteStream<Float>,
#[rustradio(default)]
tmp: Vec<Complex>,
}
impl Block for QuadratureDemod {
fn work(&mut self) -> Result<BlockRet<'_>> {
loop {
let (inp, _) = self.src.read_buf()?;
if inp.len() < 2 {
return Ok(BlockRet::WaitForStream(&self.src, 2));
}
let mut out = self.dst.write_buf()?;
if out.is_empty() {
return Ok(BlockRet::WaitForStream(&self.dst, 1));
}
let n = inp.len().min(out.len());
let n1 = n - 1;
let o = &mut out.slice()[..n1];
let i = &inp.slice()[..n];
if self.tmp.len() < n {
self.tmp.resize(n, Complex::default());
}
// Conjugate.
#[cfg(feature = "volk")]
volk::volk_32fc_x2_multiply_conjugate_32fc(&mut self.tmp[..n1], &i[1..n], &i[..n1]);
#[cfg(not(feature = "volk"))]
{
for t in 0..n1 {
self.tmp[t] = i[t].conj() * i[t + 1];
}
}
// atan2
#[cfg(feature = "fast-math")]
{
for (i, item) in o.iter_mut().enumerate().take(n1) {
*item = self.gain * fast_math::atan2(self.tmp[i].im, self.tmp[i].re);
}
if false {
// Maybe this can be faster in some circumstances, but not yet in my
// testing.
use rayon::iter::IndexedParallelIterator;
use rayon::iter::IntoParallelRefIterator;
use rayon::iter::IntoParallelRefMutIterator;
use rayon::iter::ParallelIterator;
o.par_iter_mut()
.zip(self.tmp.par_iter())
.for_each(|(a, b)| {
*a = self.gain * fast_math::atan2(b.im, b.re);
});
}
}
#[cfg(not(feature = "fast-math"))]
{
// This is way slower than fast-math. Fast-math atan2 is just that
// fast. Maybe one day it'll be in volk.
//
// https://mazzo.li/posts/vectorized-atan2.html
#[cfg(feature = "volk")]
volk::volk_32fc_s32f_atan2_32f(&mut out.slice()[..n1], &self.tmp[..n1], self.gain);
#[cfg(not(feature = "volk"))]
o.iter_mut().zip(self.tmp.iter()).for_each(|(a, b)| {
*a = self.gain * b.im.atan2(b.re);
});
}
inp.consume(n1);
out.produce(n1, &[]);
}
}
}
/// A faster version of FM demodulation, that makes some assumptions.
///
/// This block can be used instead of a `QuadratureDemod` block, for
/// performance. It's much faster (~4x compared to the fast-math
/// version of `QuadratureDemod`), but it's less good.
///
/// The algorithm is taken from Lyons, Understanding Digital Signal
/// Processing, third edition, page 760.
///
/// This is the faster version of the two, which assumes all
/// frequencies are constant amplitude. This means it can be used to
/// e.g. demodulate an FM carrier for 1200bps AX.25, but *not* to
/// decode the preemphasized bell 202 inside.
///
/// You could deemphasize, if you know all transmitters preemp
/// parameters.
///
/// For 9600bps AX.25 it works fine, if the sample rate is high
/// enough. At 50ksps `QuadratureDemod` works well, but `FastFM` does
/// not. At 500ksps `FastFM` performs just as well in my tests. But
/// `FastFM` at 500ksps is about half the speed of `QuadratureDemod` at
/// 50ksps.
///
/// Really, just use `QuadratureDemod` unless it's shown to be too slow
/// for your use case.
///
/// Lyons has an more general version of this algorithm, also on page
/// 760, but it's not implemented here.
#[derive(rustradio_macros::Block)]
#[rustradio(crate, new, sync)]
pub struct FastFM {
#[rustradio(in)]
src: ReadStream<Complex>,
#[rustradio(out)]
dst: WriteStream<Float>,
#[rustradio(default)]
q1: Complex,
#[rustradio(default)]
q2: Complex,
}
impl FastFM {
fn process_sync(&mut self, s: Complex) -> Float {
let top = (s.im - self.q2.im) * self.q1.re;
let bottom = (s.re - self.q2.re) * self.q1.im;
self.q2 = self.q1;
self.q1 = s;
top - bottom
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::blocks::VectorSource;
use crate::tests::assert_almost_equal_float;
#[test]
fn quad_nulls() -> Result<()> {
let (mut b, prev) = VectorSource::new(vec![Complex::default(); 4]);
b.work()?;
let (mut b, out) = QuadratureDemod::new(prev, 1.0);
b.work()?;
let (o, _) = out.read_buf()?;
assert_eq!(o.slice(), vec![0.0f32; 3]);
Ok(())
}
#[test]
fn quad_cw() -> Result<()> {
let (mut b, prev) = VectorSource::new(vec![
Complex::new(1.0, 0.0),
Complex::new(0.707, -0.707),
Complex::new(0.0, -1.0),
Complex::new(-1.0, 0.0),
]);
b.work()?;
let (mut b, out) = QuadratureDemod::new(prev, 1.0);
b.work()?;
let (o, _) = out.read_buf()?;
assert_almost_equal_float(
o.slice(),
&[
-std::f32::consts::PI / 4.0,
-std::f32::consts::PI / 4.0,
-std::f32::consts::PI / 2.0,
],
);
Ok(())
}
#[test]
fn quad_ccw() -> Result<()> {
let (mut b, prev) = VectorSource::new(vec![
Complex::new(1.0, 0.0),
Complex::new(0.707, 0.707),
Complex::new(0.0, 1.0),
Complex::new(-1.0, 0.0),
]);
b.work()?;
let (mut b, out) = QuadratureDemod::new(prev, 1.0);
b.work()?;
let (o, _) = out.read_buf()?;
assert_almost_equal_float(
o.slice(),
&[
std::f32::consts::PI / 4.0,
std::f32::consts::PI / 4.0,
std::f32::consts::PI / 2.0,
],
);
Ok(())
}
}