diff --git a/docs/force-stream-session.zh.md b/docs/force-stream-session.zh.md index 924175c..8379dbc 100644 --- a/docs/force-stream-session.zh.md +++ b/docs/force-stream-session.zh.md @@ -1,6 +1,6 @@ -# 新力侧会话（T06a/T06b） +# 新力侧会话（T06a/T06b、T07 输出滤波） -本文件是单路 SensorStream 计划 T06a/T06b 的 SDK 合同：新的力侧会话直接消费 T05 的 +本文件是单路 SensorStream 计划 T06a/T06b/T07 的 SDK 合同：新的力侧会话直接消费 T05 的 `ImageMeasurement`（T03 `MeasurementFrame` 头 + 同帧 raw/field），参考版本取帧上的 `reference_generation`。旧 `ForceSession` 仍为 App 保留到 T09/T10 迁移，本会话不提供 `process_measurement`、`request_zero_external_reset`、`tracker_reset_seq` 等旧外部复位接口。 @@ -40,14 +40,39 @@ 迟到、无效和带质量标记的结果不会被丢弃；拒绝帧仍保留 raw 特征。 - **数值等价与滤波。** 零点/标定换算/读数滤波复用已发布算法；同一输入、无复位、同一标定时 与旧 `ForceSession` 逐帧相等（T06a 覆盖标量曲线、无标定、median+lowpass 滤波器；T06b 覆盖 - `context` 与同帧 field）和 FN-only 全局增益模型（`outputs=["fn"]`）；`settings` 为 - 固定设计帧率与显式滤波切换属于 T07，不在本会话实现。 + field 模型与 FN-only 全局增益模型，三类标定都在同一平台同一次运行中对照旧会话）。 - **三类标定与不匹配。** 标量曲线、field、FN-only 各有有效测试；无标定会话保持 raw 与 `uncalibrated` 状态；field 模型缺 `context` 时构造报 `CalibrationError`，基线帧缺 field 或 模型拒绝帧时零点失败并给出 `invalid_input`/原因；FN-only 的 FX/FY 永远保持 NaN + `uncalibrated`，不冒充三轴标定。 +- **输出滤波（T07）。** 设计帧率在构造时固定（`design_sample_rate_hz`），实测帧率漂移和长 + 间隔只作为 `measured_sample_rate_hz`/`sample_rate_irregular` 诊断，不重新设计链路、不清空 + 历史。`request_output_settings(settings)` 可从任意线程发出，只在力线程下一帧边界生效：只 + 重建滤波链并清空其历史，图像参考与已完成的零点不变；相同设置是空操作。每次实际切换发出 + 一个 `output_filter_changed` StreamEvent（规范化设置、指纹、算法版本、生效帧、设计帧率、 + 预热信息，可 JSON 往返），由 `drain_events()` 取出。预热帧数为 + `max(1, median_window) + 2×lowpass_order（低通开启时）`；结果行提供当前设置身份与预热计数。 + 全阶段关闭时输出与输入完全相同（含 NaN 与带符号的零）。已发布 `ForceOutputFilter`/旧会话 + 的自适应调频默认行为不变。 - **线程与只读。** 一个实例只属于一个力侧线程；输入数组只读，输出只含标量/状态/不可变进度。 +## 输出滤波示例 + +```python +from orisys.force_readout import OutputSettings +from orisys.force_stream_session import ForceStreamSession + +session = ForceStreamSession( + sensor_family="box", + settings=OutputSettings(sample_rate_hz=30.0, median_window=3, lowpass_hz=5.0), + design_sample_rate_hz=30.0, +) +new_settings = OutputSettings(sample_rate_hz=30.0, median_window=5, lowpass_hz=10.0) +session.request_output_settings(new_settings) +assert session.output_settings.median_window == 3, "switch waits for a frame boundary" +assert session.design_sample_rate_hz == 30.0 +``` + ## 示例 ```python @@ -72,8 +97,9 @@ assert row.zero.phase in ("resetting", "collecting") ## 后续 -T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价；spatial_gain/接触 -映射模型不在 T06b 验收范围，需要时另立任务。 -T07 才改变输出滤波的帧率与切换语义；T09/T10 迁移 App 后删除旧 `ForceSession`。 +T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价；T07 已交付固定 +设计帧率、显式切换事件与历史/直通合同。spatial_gain/接触映射模型不在 T06/T07 验收范围， +需要时另立任务。 +T08 接单路流入口；T09/T10 迁移 App 后删除旧 `ForceSession`。 另一项后续任务：删除旧 `ForceSession` 之前，先把零点、标定、读数引擎从旧类中抽出来， -避免新会话依赖待删除的类型（T06b 或 T09 之前）。 +避免新会话依赖待删除的类型（T09 之前）。 diff --git a/src/orisys/force_stream_filter.py b/src/orisys/force_stream_filter.py new file mode 100644 index 0000000..3f717ac --- /dev/null +++ b/src/orisys/force_stream_filter.py @@ -0,0 +1,176 @@ +"""Fixed-frame-rate output filter for the new force stream session (T07). + +The published :class:`~orisys.force_readout.ForceOutputFilter` re-designs its +low-pass whenever the measured signal rate drifts more than 10% from the current +design (``ForceOutputFilter._track_signal_rate``). The stage-1 contract (K3.3, +K3.4) instead fixes the design rate when the session starts: later measured-rate +drift and long gaps are *reported*, never used to re-tune or clear the filter. + +:class:`FixedRateOutputFilter` subclasses the published filter, keeping its +median → low-pass → threshold chain (and its exact pass-through/NaN behaviour), +and overrides only: + +* rate tracking: measure and report, never reconfigure; +* :meth:`switch`: explicit, frame-boundary settings replacement that returns + whether the chain changed and starts a documented warm-up counter; +* warm-up and measured-rate diagnostics used by ``ForceStreamOutput``. + +The algorithm version is ``output-filter/1``: its number is part of the settings +identity emitted in ``StreamEvent`` payloads, so T14 can rebuild the same chain +from an event. +""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import asdict, replace + +import numpy as np + +from .force_readout import ForceOutputFilter, OutputSettings + +__all__ = [ + "FIXED_RATE_ALGORITHM", + "FixedRateOutputFilter", + "output_settings_fingerprint", + "warmup_frames_for", +] + +#: Identity of this filter's algorithm; recorded with every settings switch. +FIXED_RATE_ALGORITHM = "output-filter/1" + + +def output_settings_fingerprint(settings: OutputSettings) -> str: + """Content identity of a normalised output-setting group.""" + payload = { + "algorithm": FIXED_RATE_ALGORITHM, + "settings": asdict(settings.normalised()), + } + text = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) + return hashlib.sha256(text.encode("utf-8")).hexdigest() + + +def warmup_frames_for(settings: OutputSettings) -> int: + """Frames needed before a rebuilt chain is reported as warmed up. + + Median: one full window. Low-pass: two orders of magnitude of settling is a + conservative small count (each order costs about one sample time constant at + the configured cutoff); thresholds are memory-less and add nothing. The value + is an explicit contract, not a measured optimum. + """ + resolved = settings.normalised() + frames = max(1, int(resolved.median_window)) + if resolved.lowpass_hz > 0: + frames += 2 * max(1, int(resolved.lowpass_order)) + return int(frames) + + +class FixedRateOutputFilter(ForceOutputFilter): + """Published filter chain with a fixed design rate and explicit switching.""" + + def __init__( + self, + settings: OutputSettings, + *, + design_sample_rate_hz: float | None = None, + ) -> None: + resolved = settings.normalised() + design = ( + float(resolved.sample_rate_hz) + if design_sample_rate_hz is None + else float(design_sample_rate_hz) + ) + if not np.isfinite(design) or design <= 0.0: + raise ValueError("design_sample_rate_hz must be a positive finite number") + # The chain itself is designed once, at the fixed rate. + super().__init__(replace(resolved, sample_rate_hz=design)) + self.design_sample_rate_hz = design + self._measured_sample_rate_hz: float | None = None + self._sample_rate_irregular = False + self._warmup_elapsed_frames = 0 + + # ------------------------------------------------------------ diagnostics + @property + def measured_sample_rate_hz(self) -> float | None: + """Measured signal rate (diagnostic only; never re-tunes the chain).""" + return self._measured_sample_rate_hz + + @property + def sample_rate_irregular(self) -> bool: + """True when the measured rate differs from the design by more than 10%.""" + return bool(self._sample_rate_irregular) + + @property + def settings_fingerprint(self) -> str: + return output_settings_fingerprint(self._settings) + + @property + def warmup_required_frames(self) -> int: + return warmup_frames_for(self._settings) + + @property + def warmup_elapsed_frames(self) -> int: + return int(self._warmup_elapsed_frames) + + @property + def warming_up(self) -> bool: + return self._warmup_elapsed_frames < self.warmup_required_frames + + # ------------------------------------------------------- rate observation + def _track_signal_rate(self, signal_time_s: float | None) -> None: + """Measure the achieved rate for diagnostics only; never re-design.""" + if signal_time_s is None: + return + try: + now = float(signal_time_s) + except (TypeError, ValueError): + return + if not np.isfinite(now): + return + rate = float(self._rate.tick(now)) + if not self._rate.measured: + return + self._measured_sample_rate_hz = rate + self._sample_rate_irregular = bool( + abs(rate - self.design_sample_rate_hz) > 0.1 * self.design_sample_rate_hz + ) + + # ------------------------------------------------------------- processing + def process( + self, raw, newton, *, raw_valid=None, newton_valid=None, signal_time_s=None + ): + result = super().process( + raw, + newton, + raw_valid=raw_valid, + newton_valid=newton_valid, + signal_time_s=signal_time_s, + ) + self._warmup_elapsed_frames += 1 + return result + + # ----------------------------------------------------------------- switching + def switch(self, settings: OutputSettings) -> bool: + """Replace the settings at a caller-controlled frame boundary. + + Returns ``False`` when the normalised settings are unchanged (no chain + rebuild, no history loss, no warm-up restart). An explicit switch is the + only way to change the design: the new group's ``sample_rate_hz`` becomes + the new fixed design rate (the caller only switches when that is intended). + """ + resolved = settings.normalised() + if resolved == self._settings: + return False + self.design_sample_rate_hz = float(resolved.sample_rate_hz) + self._warmup_elapsed_frames = 0 + self._measured_sample_rate_hz = None + self._sample_rate_irregular = False + self.configure(resolved) + return True + + def reset(self) -> None: + super().reset() + self._warmup_elapsed_frames = 0 + self._measured_sample_rate_hz = None + self._sample_rate_irregular = False diff --git a/src/orisys/force_stream_session.py b/src/orisys/force_stream_session.py index c9168ee..da37bd5 100644 --- a/src/orisys/force_stream_session.py +++ b/src/orisys/force_stream_session.py @@ -31,10 +31,12 @@ mutated; outputs carry scalars/statuses/immutable progress, not internal arrays. from __future__ import annotations -from dataclasses import dataclass +import threading +from dataclasses import asdict, dataclass import numpy as np +from .force_readout import OutputSettings from .force_session import ( WORKFLOW_VERSION, ForceOutput, @@ -44,8 +46,12 @@ from .force_session import ( from .force_session import ( FrameStamp as ForceFrameStamp, ) +from .force_stream_filter import ( + FIXED_RATE_ALGORITHM, + FixedRateOutputFilter, +) from .sensor import FrameMeasurement -from .stream_contract import MeasurementFrame +from .stream_contract import MeasurementFrame, StreamEvent __all__ = ["ForceStreamOutput", "ForceStreamSession"] @@ -78,6 +84,15 @@ class ForceStreamOutput: late: bool = False calibration_fingerprint: str = "" workflow_version: str = WORKFLOW_VERSION + # T07: the output-filter identity used for this frame and its warm-up state. + output_settings: OutputSettings | None = None + output_settings_fingerprint: str = "" + design_sample_rate_hz: float | None = None + measured_sample_rate_hz: float | None = None + sample_rate_irregular: bool = False + warmup_required_frames: int = 0 + warmup_elapsed_frames: int = 0 + warming_up: bool = False @property def reference_generation(self) -> int: @@ -140,6 +155,7 @@ class ForceStreamSession: context=None, calibration=None, settings=None, + design_sample_rate_hz: float | None = None, ) -> None: self._source = _PendingMeasurementSource() self._engine = ForceSession( @@ -150,6 +166,17 @@ class ForceStreamSession: calibration=calibration, settings=settings, ) + # T07: the session owns the output filter so the design rate is fixed once + # and explicit switches can be applied at a frame boundary. The engine's + # zero/calibration code keeps calling the same filter interface. + self._filter = FixedRateOutputFilter( + self._engine.output_settings, + design_sample_rate_hz=design_sample_rate_hz, + ) + self._engine._filter = self._filter + self._pending_filter_settings: OutputSettings | None = None + self._filter_lock = threading.Lock() + self._events: list[StreamEvent] = [] self._family = sensor_family self._device_id = device_id self._version: int | None = None @@ -190,10 +217,40 @@ class ForceStreamSession: def output_settings(self): return self._engine.output_settings + @property + def design_sample_rate_hz(self) -> float: + """The fixed filter design rate chosen when this session was created.""" + return float(self._filter.design_sample_rate_hz) + + @property + def output_filter(self) -> FixedRateOutputFilter: + """The session-owned filter (read-only diagnostic access).""" + return self._filter + @property def last_output(self) -> ForceStreamOutput | None: return self._last_output + def request_output_settings(self, settings: OutputSettings) -> None: + """Queue an explicit output-filter switch for the next frame boundary. + + Safe to call from another thread: the request only records the latest + setting group. The force thread applies it at the start of the next + :meth:`process` call (never in the middle of a frame), rebuilds the + filter chain, clears its history and emits one ``output_filter_changed`` + event. Requesting the currently active settings is a no-op. + """ + if not isinstance(settings, OutputSettings): + raise TypeError("settings must be an OutputSettings") + with self._filter_lock: + self._pending_filter_settings = settings.normalised() + + def drain_events(self) -> tuple[StreamEvent, ...]: + """Return the events emitted since the previous drain, in order.""" + events = tuple(self._events) + self._events.clear() + return events + @property def measurement_requirements(self) -> tuple[bool, bool]: """``(want_field, want_contact)`` the installed calibration model asks for. @@ -259,6 +316,36 @@ class ForceStreamSession: ) # ---------------------------------------------------------------- result rows + def _apply_pending_output_filter(self, frame: MeasurementFrame) -> None: + """Apply a queued settings switch at this frame boundary (force thread).""" + with self._filter_lock: + pending = self._pending_filter_settings + self._pending_filter_settings = None + if pending is None: + return + if not self._filter.switch(pending): + return + settings = self._filter.settings + self._events.append( + StreamEvent( + kind="output_filter_changed", + stamp=frame.stamp, + payload={ + "algorithm": FIXED_RATE_ALGORITHM, + "settings": asdict(settings), + "fingerprint": self._filter.settings_fingerprint, + "design_sample_rate_hz": float(self._filter.design_sample_rate_hz), + "effective_frame_id": int(frame.stamp.frame_id), + "effective_source_frame_id": frame.stamp.source_frame_id, + "effective_stamp": frame.stamp.to_dict(), + "warmup": { + "required_frames": int(self._filter.warmup_required_frames), + "elapsed_frames": 0, + }, + }, + ) + ) + def _row( self, frame: MeasurementFrame, @@ -286,6 +373,14 @@ class ForceStreamSession: duplicate=duplicate, late=late, calibration_fingerprint=self._engine.calibration_fingerprint, + output_settings=self._filter.settings, + output_settings_fingerprint=self._filter.settings_fingerprint, + design_sample_rate_hz=self._filter.design_sample_rate_hz, + measured_sample_rate_hz=self._filter.measured_sample_rate_hz, + sample_rate_irregular=self._filter.sample_rate_irregular, + warmup_required_frames=self._filter.warmup_required_frames, + warmup_elapsed_frames=self._filter.warmup_elapsed_frames, + warming_up=self._filter.warming_up, ) self._last_output = output return output @@ -398,12 +493,15 @@ class ForceStreamSession: zero window or readout filter, and without invalidating a completed zero. A reference-version or session change fails the old zero immediately (in any phase) and requires an explicit :meth:`request_zero` for the new one. + An explicit output-filter switch queued by + :meth:`request_output_settings` takes effect at this frame boundary. """ frame = getattr(measurement, "frame", None) if not isinstance(frame, MeasurementFrame): raise TypeError( "ForceStreamSession consumes an ImageMeasurement-like input" ) + self._apply_pending_output_filter(frame) identity = self._identity(frame) version = int(frame.reference_generation) session_id = str(frame.stamp.session_id) diff --git a/tests/test_force_stream_filter.py b/tests/test_force_stream_filter.py new file mode 100644 index 0000000..486e730 --- /dev/null +++ b/tests/test_force_stream_filter.py @@ -0,0 +1,396 @@ +"""T07: fixed-rate output filtering, explicit switching and events. + +The new session's filter is designed once (``design_sample_rate_hz``), reports +measured-rate drift without re-tuning, applies a switch only at a frame boundary, +emits exactly one ``output_filter_changed`` StreamEvent per change, and keeps the +published ``ForceOutputFilter``/old session defaults untouched. +""" + +from __future__ import annotations + +import importlib.util +import json +import threading +from pathlib import Path + +import numpy as np +import pytest + +from orisys.force_readout import ForceOutputFilter, OutputSettings +from orisys.force_stream_filter import ( + FixedRateOutputFilter, + output_settings_fingerprint, + warmup_frames_for, +) +from orisys.force_stream_session import ForceStreamOutput, ForceStreamSession +from orisys.image_pipeline import ImageMeasurement +from orisys.stream_contract import FrameStamp, MeasurementFrame, StreamEvent + +FRAME_NS_30 = 33_333_333 + + +def _load_helpers(): + path = Path(__file__).with_name("test_force_session.py") + spec = importlib.util.spec_from_file_location("_t07_helpers", path) + module = importlib.util.module_from_spec(spec) + assert spec.loader is not None + spec.loader.exec_module(module) + return module + + +helpers = _load_helpers() + + +def _settings(**overrides) -> OutputSettings: + values = { + "sample_rate_hz": 30.0, + "median_window": 3, + "lowpass_hz": 5.0, + "lowpass_order": 2, + } + values.update(overrides) + return OutputSettings(**values) + + +def _measurement( + frame_id: int, + raw: tuple[float, float, float], + *, + version: int = 0, + time_ns: int | None = None, +) -> ImageMeasurement: + stamp = FrameStamp( + sensor_id="TEST-1", + session_id="s1", + frame_id=frame_id, + source_frame_id=frame_id, + capture_time_ns=(frame_id * FRAME_NS_30 if time_ns is None else int(time_ns)), + clock_id="host", + capture_time_basis="recorded_signal", + ) + frame = MeasurementFrame( + stamp=stamp, + source_dt_s=None, + reference_generation=version, + ) + return ImageMeasurement(frame=frame, force_raw=raw, torque=0.0, flow=None) + + +def _raw(frame_id: int) -> tuple[float, float, float]: + return (1000.0 + frame_id, 2000.0 + 0.5 * frame_id, 3000.0 - 0.25 * frame_id) + + +def _session(settings: OutputSettings | None = None, **kwargs) -> ForceStreamSession: + return ForceStreamSession( + sensor_family="box", + settings=settings if settings is not None else _settings(), + **kwargs, + ) + + +def _ready(session: ForceStreamSession, *, count: int = 31) -> ForceStreamOutput: + session.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + results = [ + session.process(_measurement(frame_id, _raw(frame_id))) + for frame_id in range(1, count + 1) + ] + assert results[-1].zero.phase == "ready" + return results[-1] + + +# --------------------------------------------------------------- fixed design rate +def test_fixed_rate_reports_drift_without_redesigning_the_chain() -> None: + settings = _settings(median_window=1, lowpass_hz=5.0) + session = _session(settings, design_sample_rate_hz=30.0) + reference = FixedRateOutputFilter(settings, design_sample_rate_hz=30.0) + actual_hz = 20.0 # -33% from the design rate + old = ForceOutputFilter(settings) + + for frame_id in range(1, 26): + time_ns = int(round(frame_id * 1e9 / actual_hz)) + raw = (1000.0 + frame_id, 2000.0, 3000.0) + newton = (1.0, 2.0, 3.0) + new_out = session.process(_measurement(frame_id, raw, time_ns=time_ns)) + expected_raw, _ = reference.process( + raw, + newton, + raw_valid=(True, True, True), + newton_valid=(True, True, True), + signal_time_s=time_ns / 1e9, + ) + old.process( + raw, + newton, + raw_valid=(True, True, True), + newton_valid=(True, True, True), + signal_time_s=time_ns / 1e9, + ) + assert new_out.raw == pytest.approx(expected_raw), frame_id + + filter_ = session.output_filter + assert session.design_sample_rate_hz == 30.0 + assert filter_.settings.sample_rate_hz == 30.0, "fixed design rate must not change" + assert filter_.measured_sample_rate_hz == pytest.approx(actual_hz, rel=0.02) + assert filter_.sample_rate_irregular is True + assert new_out.design_sample_rate_hz == 30.0 + assert new_out.measured_sample_rate_hz == pytest.approx(actual_hz, rel=0.02) + assert new_out.sample_rate_irregular is True + # The published filter keeps its adaptive behaviour: this is what the new + # fixed-rate contract deliberately does not do. + assert old.settings.sample_rate_hz == pytest.approx(actual_hz, rel=0.02) + assert old.settings.sample_rate_hz != pytest.approx(30.0, rel=0.01) + + +def test_long_gap_does_not_clear_history_or_retune() -> None: + session = _session( + _settings(median_window=3, lowpass_hz=5.0), design_sample_rate_hz=30.0 + ) + reference = FixedRateOutputFilter( + _settings(median_window=3, lowpass_hz=5.0), design_sample_rate_hz=30.0 + ) + outputs = [] + for frame_id in range(1, 16): + # Regular frames, then a 0.5 s gap at frame 10. + time_ns = frame_id * FRAME_NS_30 + (500_000_000 if frame_id >= 10 else 0) + out = session.process(_measurement(frame_id, _raw(frame_id), time_ns=time_ns)) + expected, _ = reference.process( + _raw(frame_id), + (0.0, 0.0, 0.0), + raw_valid=(True, True, True), + newton_valid=(False, False, False), + signal_time_s=time_ns / 1e9, + ) + assert out.raw == pytest.approx(expected), frame_id + outputs.append(out.raw) + assert session.output_filter.settings.sample_rate_hz == 30.0 + assert session.output_filter.measured_sample_rate_hz is not None + assert any(a != b for a, b in zip(outputs[:9], outputs[9:])), ( + "filter history was used" + ) + + +# -------------------------------------------------------------- equality at design +def test_matches_old_session_filter_at_the_design_rate() -> None: + settings = _settings(median_window=3, lowpass_hz=5.0) + source = helpers.FakeMeasurementSource(_raw) + old = helpers.ForceSession( + source=source, + sensor_family="box", + settings=settings, + ) + new = _session(settings) + old.request_zero(reference_id="ref", declared_unloaded=True) + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + old_raw, new_raw = [], [] + for frame_id in range(1, 41): + old_out = helpers.process(old, source, frame_id) + new_out = new.process(_measurement(frame_id, _raw(frame_id))) + old_raw.append(np.asarray(old_out.raw)) + new_raw.append(np.asarray(new_out.raw)) + assert np.array_equal(np.stack(old_raw), np.stack(new_raw), equal_nan=True) + + +# --------------------------------------------------------------- explicit switching +def test_switch_at_frame_boundary_keeps_zero_and_emits_one_event() -> None: + session = _session(_settings(median_window=3, lowpass_hz=5.0)) + _ready(session) + before_zero = session.zero + before_version = session.reference_generation + before_settings = session.output_settings + + session.request_output_settings(_settings(median_window=5, lowpass_hz=10.0)) + assert session.output_settings == before_settings, "not applied before a frame" + assert session.drain_events() == (), "no event before the boundary frame" + + frame = session.process(_measurement(40, _raw(40))) + events = session.drain_events() + assert len(events) == 1 + event = events[0] + assert event.kind == "output_filter_changed" + assert isinstance(event, StreamEvent) + assert session.zero.phase == "ready", "a filter switch must not invalidate the zero" + assert session.reference_generation == before_version + assert session.zero == before_zero + assert frame.output_settings == _settings(median_window=5, lowpass_hz=10.0) + assert frame.output_settings_fingerprint == output_settings_fingerprint( + frame.output_settings + ) + assert event.payload["fingerprint"] == frame.output_settings_fingerprint + assert event.payload["effective_frame_id"] == 40 + assert event.payload["warmup"]["required_frames"] == warmup_frames_for( + frame.output_settings + ) + assert event.payload["warmup"]["elapsed_frames"] == 0 + assert event.stamp.frame_id == 40 + + # The event is a T03 StreamEvent and survives JSON round-trip (T14 input). + encoded = json.loads(json.dumps(event.to_dict())) + assert StreamEvent.from_dict(encoded) == event + + # History starts from the effective frame: the first post-switch outputs match + # a fresh chain fed only the post-switch raw sequence (frame 40 included). + reference = FixedRateOutputFilter( + _settings(median_window=5, lowpass_hz=10.0), design_sample_rate_hz=30.0 + ) + expected, _ = reference.process( + _raw(40), + (0.0, 0.0, 0.0), + raw_valid=(True, True, True), + newton_valid=(False, False, False), + signal_time_s=40 * FRAME_NS_30 / 1e9, + ) + assert frame.raw == pytest.approx(expected) + for frame_id in range(41, 46): + out = session.process(_measurement(frame_id, _raw(frame_id))) + expected, _ = reference.process( + _raw(frame_id), + (0.0, 0.0, 0.0), + raw_valid=(True, True, True), + newton_valid=(False, False, False), + signal_time_s=frame_id * FRAME_NS_30 / 1e9, + ) + assert out.raw == pytest.approx(expected), ( + "post-switch history must start from the effective frame" + ) + + +def test_requesting_the_same_settings_is_a_no_op() -> None: + session = _session(_settings(median_window=3, lowpass_hz=5.0)) + ready = _ready(session) + same = session.output_settings + session.request_output_settings(same) + frame = session.process(_measurement(40, _raw(40))) + assert session.drain_events() == () + assert frame.output_settings == same + # Zero completion already restarted the filter history; a no-op request must + # not restart it again: the warm-up counter simply continues. + assert frame.warmup_elapsed_frames == ready.warmup_elapsed_frames + 1 + + +def test_switch_coalesces_to_the_latest_request_before_the_boundary() -> None: + session = _session(_settings(median_window=3, lowpass_hz=5.0)) + _ready(session) + first = _settings(median_window=5, lowpass_hz=10.0) + latest = _settings(median_window=5, lowpass_hz=12.0) + session.request_output_settings(first) + session.request_output_settings(latest) + frame = session.process(_measurement(40, _raw(40))) + events = session.drain_events() + assert len(events) == 1 + assert frame.output_settings == latest + assert events[0].payload["settings"] == latest.__dict__ + + +def test_warmup_counts_frames_after_a_switch() -> None: + session = _session(_settings(median_window=3, lowpass_hz=5.0)) + _ready(session) + new_settings = _settings(median_window=5, lowpass_hz=10.0) + required = warmup_frames_for(new_settings) + session.request_output_settings(new_settings) + rows = [ + session.process(_measurement(40 + index, _raw(40 + index))) + for index in range(required + 2) + ] + assert rows[0].warmup_elapsed_frames == 1 + assert rows[0].warming_up is True + assert rows[0].warmup_required_frames == required + assert rows[required - 1].warming_up is False + assert rows[required - 1].warmup_elapsed_frames == required + assert rows[-1].warming_up is False + + +# -------------------------------------------------------------- thread boundary +class _GatedSession(ForceStreamSession): + """Hold one frame inside the engine to prove switching waits for a boundary.""" + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + self.entered = threading.Event() + self.release = threading.Event() + self.gate_frame = 40 + + def _feed_engine(self, measurement, frame): + if int(frame.stamp.frame_id) == self.gate_frame: + self.entered.set() + assert self.release.wait(timeout=5.0) + return super()._feed_engine(measurement, frame) + + +def test_threaded_switch_waits_for_the_next_frame_boundary() -> None: + session = _GatedSession( + sensor_family="box", settings=_settings(median_window=3, lowpass_hz=5.0) + ) + _ready(session) + old_settings = session.output_settings + new_settings = _settings(median_window=5, lowpass_hz=10.0) + results: list[ForceStreamOutput] = [] + + def run_frame() -> None: + results.append(session.process(_measurement(40, _raw(40)))) + + worker = threading.Thread(target=run_frame) + worker.start() + try: + assert session.entered.wait(timeout=5.0), "frame never reached the engine gate" + # The frame is provably in flight: the switch request must not touch it. + session.request_output_settings(new_settings) + assert session.output_settings == old_settings, "no mid-frame reconfiguration" + assert session.drain_events() == () + finally: + session.release.set() + worker.join(timeout=5.0) + assert not worker.is_alive() + assert results[0].output_settings == old_settings + + next_frame = session.process(_measurement(41, _raw(41))) + events = session.drain_events() + assert len(events) == 1 + assert next_frame.output_settings == new_settings + assert events[0].payload["effective_frame_id"] == 41 + + +# ---------------------------------------------------------- pass-through and history +def test_all_stages_off_is_an_exact_passthrough_including_nan_and_signed_zero() -> None: + passthrough = OutputSettings(sample_rate_hz=30.0, median_window=1, lowpass_hz=0.0) + filter_ = FixedRateOutputFilter(passthrough, design_sample_rate_hz=30.0) + raw = (0.0, -0.0, float("nan")) + newton = (-0.0, 1.5, float("nan")) + out_raw, out_newton = filter_.process( + raw, + newton, + raw_valid=(True, True, True), + newton_valid=(True, True, True), + signal_time_s=1.0, + ) + assert out_raw[0] == raw[0] and out_raw[1] == raw[1] + assert np.isnan(out_raw[2]) + assert bool(np.signbit(out_raw[1])) is True + assert out_newton[0] == newton[0] and out_newton[1] == newton[1] + assert np.isnan(out_newton[2]) + assert bool(np.signbit(out_newton[0])) is True + + session = _session(passthrough) + _ready(session) + out = session.process(_measurement(40, (0.0, -0.0, float("nan")))) + assert out.raw[0] == 0.0 and bool(np.signbit(out.raw[1])) is True + assert np.isnan(out.raw[2]) + + +def test_published_force_output_filter_default_behaviour_is_unchanged() -> None: + """The old filter still re-designs on rate drift; the new one is opt-in.""" + settings = _settings(median_window=1, lowpass_hz=5.0) + old = ForceOutputFilter(settings) + for frame_id in range(1, 15): + old.process( + (float(frame_id), 0.0, 0.0), + (0.0, 0.0, 0.0), + raw_valid=(True, True, True), + newton_valid=(False, False, False), + signal_time_s=frame_id * 0.05, + ) + assert old.settings.sample_rate_hz == pytest.approx(20.0, rel=0.02) + assert old.settings.sample_rate_hz != settings.sample_rate_hz