-
Notifications
You must be signed in to change notification settings - Fork 14
/
fmd.hh
144 lines (134 loc) · 2.8 KB
/
fmd.hh
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
/*
FM Demodulation
Copyright 2019 Ahmet Inan <[email protected]>
*/
#pragma once
namespace DSP {
template <typename TYPE>
class FMD1
{
typedef TYPE complex_type;
typedef typename complex_type::value_type value_type;
value_type prev;
value_type scale;
value_type wrap_around(value_type angle)
{
if (angle < -DSP::Const<value_type>::Pi())
return angle + DSP::Const<value_type>::TwoPi();
if (angle > DSP::Const<value_type>::Pi())
return angle - DSP::Const<value_type>::TwoPi();
return angle;
}
public:
constexpr FMD1() : prev(0), scale(0)
{
}
void bandwidth(value_type bw)
{
scale = value_type(1) / (bw * DSP::Const<value_type>::Pi());
}
value_type operator()(complex_type input)
{
value_type phase = arg(input);
value_type delta = phase - prev;
delta = wrap_around(delta);
prev = phase;
return scale * delta;
}
};
template <typename TYPE>
class FMD2
{
typedef TYPE complex_type;
typedef typename complex_type::value_type value_type;
complex_type prev;
value_type scale;
public:
constexpr FMD2() : prev(1, 0), scale(0)
{
}
void bandwidth(value_type bw)
{
scale = value_type(1) / (bw * DSP::Const<value_type>::Pi());
}
value_type operator()(complex_type input)
{
value_type phase = arg(input / prev);
prev = input;
return scale * phase;
}
};
template <typename TYPE>
class FMD3
{
typedef TYPE complex_type;
typedef typename complex_type::value_type value_type;
complex_type prev;
value_type scale;
public:
constexpr FMD3() : prev(1, 0), scale(0)
{
}
void bandwidth(value_type bw)
{
scale = value_type(1) / (bw * DSP::Const<value_type>::Pi());
}
value_type operator()(complex_type input)
{
value_type phase = arg(input * conj(prev));
prev = input;
return scale * phase;
}
};
template <typename TYPE>
class FMD4
{
typedef TYPE complex_type;
typedef typename complex_type::value_type value_type;
complex_type prev;
value_type scale;
public:
constexpr FMD4() : prev(1, 0), scale(0)
{
}
void bandwidth(value_type bw)
{
scale = value_type(1) / (bw * DSP::Const<value_type>::Pi());
}
value_type operator()(complex_type input)
{
value_type phase =
prev.real() * input.imag() -
prev.imag() * input.real();
phase /= norm(input);
prev = input;
return scale * phase;
}
};
template <typename TYPE>
class FMD5
{
typedef TYPE complex_type;
typedef typename complex_type::value_type value_type;
complex_type prev1, prev2;
value_type scale;
public:
constexpr FMD5() : prev1(1, 0), prev2(1, 0), scale(0)
{
}
void bandwidth(value_type bw)
{
scale = value_type(1) / (bw * DSP::Const<value_type>::TwoPi());
}
value_type operator()(complex_type input)
{
complex_type diff = input - prev2;
value_type phase =
prev1.real() * diff.imag() -
prev1.imag() * diff.real();
phase /= norm(prev1);
prev2 = prev1; prev1 = input;
return scale * phase;
}
};
}