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
//! Phase-Locked Loops (PLL) and carrier recovery for power electronics, motor resolvers, and SDR.
//!
//! Includes:
//! - [`SogiPll`]: Second-Order Generalized Integrator PLL for single-phase grid synchronization (solar inverters, UPS) and resolver angle tracking.
//! - [`CostasLoop`]: Carrier phase and frequency recovery loop for BPSK/QPSK demodulation.
#[allow(unused_imports)]
use crate::math::FloatMath;
/// Second-Order Generalized Integrator Phase-Locked Loop (SOGI-PLL).
///
/// Implements orthogonal signal generation ($v_\alpha, v_\beta$) from a single-phase input $v(t)$
/// and tracks fundamental frequency and phase in real-time.
///
/// Discretized using trapezoidal (Tustin) integration for zero frequency warping at the center frequency.
#[derive(Debug, Clone, Copy)]
pub struct SogiPll {
// SOGI internal filter states and coefficients
sample_rate_hz: f32,
omega_center: f32, // Nominal center frequency in rad/s
k_sogi: f32, // SOGI damping factor (typically sqrt(2) ≈ 1.414)
v_alpha: f32, // In-phase filtered orthogonal component
v_beta: f32, // Quadrature (90 deg lagging) filtered component
x1: f32, // Integrator 1 state
x2: f32, // Integrator 2 state
// Loop filter (PI) & NCO states
kp: f32, // Proportional gain
ki: f32, // Integral gain
phase: f32, // Estimated phase θ in [-π, π]
omega_est: f32, // Estimated frequency in rad/s
integrator_pi: f32,// PI controller accumulator
}
impl SogiPll {
/// Creates a new SOGI-PLL tuned to `center_freq_hz` at `sample_rate_hz`.
///
/// - `k_sogi`: SOGI damping factor (default `1.414`).
/// - `kp`: Loop filter proportional gain (e.g. `60.0`).
/// - `ki`: Loop filter integral gain (e.g. `1400.0`).
pub fn new(center_freq_hz: f32, sample_rate_hz: f32, k_sogi: f32, kp: f32, ki: f32) -> Self {
let omega_center = 2.0 * core::f32::consts::PI * center_freq_hz;
Self {
sample_rate_hz,
omega_center,
k_sogi,
v_alpha: 0.0,
v_beta: 0.0,
x1: 0.0,
x2: 0.0,
kp,
ki,
phase: 0.0,
omega_est: omega_center,
integrator_pi: 0.0,
}
}
/// Process a single input sample and return the tracked instantaneous phase $\theta \in [-\pi, \pi]$.
pub fn process(&mut self, input: f32) -> f32 {
let ts = 1.0 / self.sample_rate_hz;
let half_ts = 0.5 * ts;
// 1. SOGI Orthogonal Signal Generation (Tustin integration)
let err = input - self.v_alpha;
let k_err = self.k_sogi * err;
let w = self.omega_est;
// State update for SOGI
let d_x1 = (k_err - self.v_beta) * w;
let d_x2 = self.v_alpha * w;
let x1_new = self.x1 + half_ts * d_x1;
let x2_new = self.x2 + half_ts * d_x2;
self.v_alpha = x1_new;
self.v_beta = x2_new;
self.x1 += ts * (k_err - self.v_beta) * w;
self.x2 += ts * self.v_alpha * w;
// 2. Park Transform Phase Detector: q-axis error = -v_alpha * sin(θ) + v_beta * cos(θ)
let sin_p = self.phase.sin();
let cos_p = self.phase.cos();
let v_q = -self.v_alpha * sin_p + self.v_beta * cos_p;
// 3. Loop Filter (PI controller on v_q)
self.integrator_pi += self.ki * ts * v_q;
let delta_omega = self.kp * v_q + self.integrator_pi;
self.omega_est = self.omega_center + delta_omega;
// 4. Integrator NCO -> Phase update
self.phase += self.omega_est * ts;
// Wrap phase to [-π, π]
let pi = core::f32::consts::PI;
let two_pi = 2.0 * pi;
while self.phase > pi {
self.phase -= two_pi;
}
while self.phase < -pi {
self.phase += two_pi;
}
self.phase
}
/// Returns the estimated fundamental frequency in Hz.
#[inline(always)]
pub fn frequency_hz(&self) -> f32 {
self.omega_est / (2.0 * core::f32::consts::PI)
}
/// Returns the filtered orthogonal components `(v_alpha, v_beta)`.
#[inline(always)]
pub fn orthogonal_components(&self) -> (f32, f32) {
(self.v_alpha, self.v_beta)
}
/// Returns the instantaneous phase $\theta \in [-\pi, \pi]$.
#[inline(always)]
pub fn phase(&self) -> f32 {
self.phase
}
/// Reset PLL internal states.
pub fn reset(&mut self) {
self.v_alpha = 0.0;
self.v_beta = 0.0;
self.x1 = 0.0;
self.x2 = 0.0;
self.phase = 0.0;
self.omega_est = self.omega_center;
self.integrator_pi = 0.0;
}
}
/// Costas Loop for BPSK / QPSK carrier phase and frequency tracking.
#[derive(Debug, Clone, Copy)]
pub struct CostasLoop {
sample_rate_hz: f32,
phase: f32,
freq_rad_per_sample: f32,
center_freq_rad: f32,
alpha: f32, // Proportional loop filter parameter
beta: f32, // Integral loop filter parameter
}
impl CostasLoop {
/// Create a new Costas Loop.
pub fn new(center_freq_hz: f32, sample_rate_hz: f32, loop_bandwidth_hz: f32, damping: f32) -> Self {
let center_freq_rad = 2.0 * core::f32::consts::PI * center_freq_hz / sample_rate_hz;
let theta = 2.0 * core::f32::consts::PI * loop_bandwidth_hz / sample_rate_hz;
let d = 1.0 + 2.0 * damping * theta + theta * theta;
let alpha = (4.0 * damping * theta) / d;
let beta = (4.0 * theta * theta) / d;
Self {
sample_rate_hz,
phase: 0.0,
freq_rad_per_sample: center_freq_rad,
center_freq_rad,
alpha,
beta,
}
}
/// Process a modulated carrier sample and return the demodulated baseband in-phase (I) sample.
pub fn process_sample(&mut self, sample: f32) -> (f32, f32) {
let cos_val = self.phase.cos();
let sin_val = (-self.phase).sin();
let i_arm = sample * cos_val;
let q_arm = sample * sin_val;
// BPSK phase error detector: e = I * sign(Q) or e = I * Q
let error = (i_arm * q_arm).clamp(-1.0, 1.0);
// Loop filter update
self.freq_rad_per_sample += self.beta * error;
self.phase += self.freq_rad_per_sample + self.alpha * error;
// Wrap phase to [-π, π]
let pi = core::f32::consts::PI;
while self.phase > pi {
self.phase -= 2.0 * pi;
}
while self.phase < -pi {
self.phase += 2.0 * pi;
}
(i_arm, q_arm)
}
/// Current tracked carrier frequency in Hz.
#[inline(always)]
pub fn frequency_hz(&self) -> f32 {
self.freq_rad_per_sample * self.sample_rate_hz / (2.0 * core::f32::consts::PI)
}
/// Nominal center frequency in Hz.
#[inline(always)]
pub fn center_frequency_hz(&self) -> f32 {
self.center_freq_rad * self.sample_rate_hz / (2.0 * core::f32::consts::PI)
}
}