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

/*! 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
*/
use anyhow::Result;

use crate::{map_block_convert_macro, Complex, Float};

/// Quadrature demod, the core of an FM demodulator.
pub struct QuadratureDemod {
    gain: Float,
    last: Complex,
}

impl QuadratureDemod {
    /// Create new QuadratureDemod block.
    ///
    /// Gain is just used to scale the value, and can be set to 1.0 if
    /// you don't care about the scale.
    pub fn new(gain: Float) -> Self {
        Self {
            gain,
            last: Complex::default(),
        }
    }
    fn process_one(&mut self, s: Complex) -> Float {
        let t = s * self.last.conj();
        self.last = s;

        #[cfg(feature = "fast-math")]
        return self.gain * fast_math::atan2(t.im, t.re);

        #[cfg(not(feature = "fast-math"))]
        return self.gain * t.im.atan2(t.re);
    }
}
map_block_convert_macro![QuadratureDemod];