diff --git a/.gitignore b/.gitignore index fdebf0dd..0034ef30 100644 --- a/.gitignore +++ b/.gitignore @@ -148,3 +148,4 @@ src/ezmsg/sigproc/__version__.py uv.lock *.local.json tmp/ +.codegraph/ \ No newline at end of file diff --git a/src/ezmsg/sigproc/adaptive_lnc.py b/src/ezmsg/sigproc/adaptive_lnc.py index 4a4b6f6d..b2f4761e 100644 --- a/src/ezmsg/sigproc/adaptive_lnc.py +++ b/src/ezmsg/sigproc/adaptive_lnc.py @@ -149,6 +149,12 @@ class AdaptiveLNCSettings(ez.Settings): loops do not fight. Independent of chunk size (the per-update gain is derived from elapsed time). Read live each chunk.""" + max_freq_deviation: float | None = 2.0 + """Maximum tracked deviation from ``line_freq`` in Hz. The default ±2 Hz + range is deliberately generous for mains tracking while preventing a weak + or absent line estimate from random-walking the FLL toward DC. ``None`` + permits unbounded tracking.""" + cancel_method: CancelMethod = CancelMethod.NOTCH """How to remove the line; see :class:`CancelMethod`. ``NOTCH`` (default) applies the SOS notch cascade -- a perfect null that also removes any signal @@ -170,6 +176,12 @@ class AdaptiveLNCState: omega: float = 0.0 """Current NCO angular frequency in rad/sample (tracked by the FLL).""" + omega_min: float | None = None + """Lower FLL bound in rad/sample, or ``None`` when tracking is unbounded.""" + + omega_max: float | None = None + """Upper FLL bound in rad/sample, or ``None`` when tracking is unbounded.""" + phase: float = 0.0 """NCO phase (rad) for the first sample of the next chunk; accumulates so the demodulation reference is continuous across chunk boundaries.""" @@ -288,6 +300,17 @@ def _reset_state(self, message: AxisArray) -> None: fs = 1.0 / message.axes[self.settings.axis].gain # Seed the NCO at the nominal normalised frequency; the FLL refines it. self._state.omega = 2.0 * np.pi * self.settings.line_freq / fs + max_deviation = self.settings.max_freq_deviation + if max_deviation is not None: + if max_deviation < 0: + raise ValueError("max_freq_deviation must be non-negative or None") + min_freq = max(0.0, self.settings.line_freq - max_deviation) + max_freq = min(fs / 2.0, self.settings.line_freq + max_deviation) + self._state.omega_min = 2.0 * np.pi * min_freq / fs + self._state.omega_max = 2.0 * np.pi * max_freq / fs + else: + self._state.omega_min = None + self._state.omega_max = None self._state.phase = 0.0 # Frequency-update window: one mains period, inferred from line_freq/fs. @@ -366,7 +389,10 @@ def _fll_step(self, z_fund: np.ndarray, beta: float) -> None: if st.z_phasor_prev is not None: cross = np.sum(z_fund * np.conj(st.z_phasor_prev)) if cross != 0: - st.omega = st.omega + beta * float(np.angle(cross)) / st.block_len + omega = st.omega + beta * float(np.angle(cross)) / st.block_len + if st.omega_min is not None and st.omega_max is not None: + omega = float(np.clip(omega, st.omega_min, st.omega_max)) + st.omega = omega st.z_phasor_prev = z_fund def _freq_update(self, beta: float) -> None: diff --git a/tests/unit/test_adaptive_lnc.py b/tests/unit/test_adaptive_lnc.py index 5d8c3900..447cf47e 100644 --- a/tests/unit/test_adaptive_lnc.py +++ b/tests/unit/test_adaptive_lnc.py @@ -146,6 +146,7 @@ def adaptive_lnc( num_harmonics: int = 1, adapt_time_constant: float = 0.1, freq_time_constant: float | None = 0.5, + max_freq_deviation: float | None = 2.0, cancel_method: str = "notch", axis: str = "time", ) -> AdaptiveLNCTransformer: @@ -156,6 +157,7 @@ def adaptive_lnc( num_harmonics=num_harmonics, adapt_time_constant=adapt_time_constant, freq_time_constant=freq_time_constant, + max_freq_deviation=max_freq_deviation, cancel_method=cancel_method, axis=axis, ) @@ -246,6 +248,28 @@ def test_line_freq_change_reseeds_nco(): assert omega_50 == pytest.approx(2 * np.pi * 50.0 / FS) +def test_frequency_tracking_is_bounded_around_nominal(): + max_deviation = 0.25 + proc = adaptive_lnc( + line_freq=LINE_FREQ, + freq_time_constant=0.1, + max_freq_deviation=max_deviation, + ) + proc(_make_axisarray(np.zeros((1, 1), dtype=np.float32))) + + # A quarter-cycle phasor rotation would request a many-Hz correction in a + # single FLL step. Both directions must clamp at the configured bounds. + proc._state.z_phasor_prev = np.ones(1, dtype=np.complex128) + proc._fll_step(np.full(1, 1j, dtype=np.complex128), beta=1.0) + freq = proc._state.omega * FS / (2.0 * np.pi) + assert freq == pytest.approx(LINE_FREQ + max_deviation) + + proc._state.z_phasor_prev = np.ones(1, dtype=np.complex128) + proc._fll_step(np.full(1, -1j, dtype=np.complex128), beta=1.0) + freq = proc._state.omega * FS / (2.0 * np.pi) + assert freq == pytest.approx(LINE_FREQ - max_deviation) + + # --------------------------------------------------------------------------- # # Frequency tracking (FLL enabled) # # --------------------------------------------------------------------------- #