feat: ForceStreamSession 使用固定设计帧率的输出滤波(T07)

新增 FixedRateOutputFilter:设计帧率在会话创建时固定,实测帧率漂移和长间隔只报告、不重新设计也不清历史;request_output_settings 可跨线程排队,在力线程帧边界切换,只重建滤波链、清空历史,不改零点和参考版本,并发出 output_filter_changed StreamEvent(设置、指纹、生效帧、预热)。结果行带滤波设置身份与预热状态。已发布 ForceReadout 和旧 ForceSession/ForceOutputFilter 默认行为不变;设计帧率下与旧会话逐位一致。回归 391 项、lint 通过。
This commit is contained in:
ykcheungab
2026-10-21 16:27:44 +08:00
parent 11f8dc93e5
commit 26382b359f
4 changed files with 706 additions and 10 deletions
+34 -8
View File
@@ -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 之前)。
+176
View File
@@ -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
+100 -2
View File
@@ -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)
+396
View File
@@ -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