fix path errors

2026-08-26 19:22:37 +08:00
parent 2cb152b76e
commit d59e1cb7fb
35 changed files with 546 additions and 69 deletions
@@ -1,11 +1,18 @@
-"""Signal helpers for force filtering and normal-force calibration.
+"""Signal helpers for force filtering and force calibration.

 Batch ``lowpass_filter`` is the zero-phase Butterworth used by example charts.
 ``LowpassFilter`` is a causal, stateful filter for per-frame ``Sensor`` outputs.
+
+Force calibration resolution order (when register support arrives):
+  1. Sensor register / EEPROM calibration blob
+  2. ``sensors/<name>/force_calibration.json``
+  3. Hardcoded linear defaults below
 """

 from __future__ import annotations

+from dataclasses import dataclass
+
 import numpy as np
 from scipy.signal import butter, filtfilt, sosfilt, sosfilt_zi

@@ -18,28 +25,241 @@ DEFAULT_SAMPLE_RATE_HZ = 30.0
 DEFAULT_LOWPASS_ORDER = 2


-def _load_normal_force_coeffs(sensor_name: str | None = None) -> tuple[float, float]:
-    if sensor_name:
-        try:
-            from .configs import load_force_calibration
+def _parse_calibration_point(point) -> tuple[float, float]:
+    if isinstance(point, dict):
+        raw = point.get("fnormal", point.get("raw", point.get("x")))
+        force_n = point.get("force_n", point.get("n", point.get("y")))
+        if raw is None or force_n is None:
+            raise ValueError(f"invalid calibration point: {point!r}")
+        return float(raw), float(force_n)
+    if isinstance(point, (list, tuple)) and len(point) >= 2:
+        return float(point[0]), float(point[1])
+    raise ValueError(f"invalid calibration point: {point!r}")

-            data = load_force_calibration(sensor_name)
-        except Exception:
-            data = None
-        if isinstance(data, dict):
-            block = data.get("normal_force", data)
-            if isinstance(block, dict):
-                slope = block.get("slope")
-                offset = block.get("offset")
-                if slope is not None and offset is not None:
-                    return float(slope), float(offset)
+
+@dataclass(frozen=True)
+class ForceChannelCalibration:
+    """One force channel: linear coeffs and/or piecewise-linear breakpoints."""
+
+    method: str = "linear"
+    slope: float | None = None
+    offset: float | None = None
+    points: tuple[tuple[float, float], ...] = ()
+    extrapolate: bool = True
+    clamp_output_min: float | None = 0.0
+    input_mode: str = "magnitude"
+    signed: bool = False
+    unit: str = "N"
+
+    @classmethod
+    def linear(cls, slope: float, offset: float, *, unit: str = "N") -> ForceChannelCalibration:
+        return cls(method="linear", slope=float(slope), offset=float(offset), unit=unit)
+
+    @classmethod
+    def from_channel_dict(
+        cls,
+        block: dict,
+        *,
+        unit: str = "N",
+        default_method: str = "piecewise_linear",
+    ) -> ForceChannelCalibration | None:
+        if not isinstance(block, dict):
+            return None
+
+        method = str(block.get("method", block.get("model", default_method))).lower()
+        if method in ("linear", "affine"):
+            slope = block.get("slope")
+            offset = block.get("offset")
+            if slope is None or offset is None:
+                return None
+            return cls.linear(float(slope), float(offset), unit=str(block.get("unit", unit)))
+
+        if method != "piecewise_linear":
+            return None
+
+        raw_points = block.get("points", [])
+        if not isinstance(raw_points, (list, tuple)) or len(raw_points) < 2:
+            return None
+        try:
+            points = tuple(sorted(_parse_calibration_point(p) for p in raw_points))
+        except (TypeError, ValueError):
+            return None
+        if len(points) < 2:
+            return None
+
+        return cls(
+            method="piecewise_linear",
+            points=points,
+            extrapolate=bool(block.get("extrapolate", True)),
+            clamp_output_min=block.get("clamp_output_min", 0.0),
+            input_mode=str(block.get("input", "magnitude")).lower(),
+            signed=bool(block.get("signed", False)),
+            unit=str(block.get("unit", unit)),
+        )
+
+    def convert(self, raw: float) -> float:
+        raw = float(raw)
+        if self.method == "linear":
+            slope = NORMAL_FORCE_CAL_SLOPE if self.slope is None else float(self.slope)
+            offset = NORMAL_FORCE_CAL_OFFSET if self.offset is None else float(self.offset)
+            return slope * raw + offset
+
+        points = self.points
+        if len(points) < 2:
+            return raw
+
+        if raw <= points[0][0]:
+            segment = (points[0], points[1])
+            if not self.extrapolate:
+                return self._clamp(points[0][1])
+        elif raw >= points[-1][0]:
+            segment = (points[-2], points[-1])
+            if not self.extrapolate:
+                return self._clamp(points[-1][1])
+        else:
+            segment = None
+            for left, right in zip(points, points[1:]):
+                if left[0] <= raw <= right[0]:
+                    segment = (left, right)
+                    break
+            if segment is None:
+                return self._clamp(points[-1][1])
+
+        (raw0, n0), (raw1, n1) = segment
+        if raw1 == raw0:
+            value = n0
+        else:
+            ratio = (raw - raw0) / (raw1 - raw0)
+            value = n0 + ratio * (n1 - n0)
+        return self._clamp(value)
+
+    def convert_component(self, raw: float) -> float:
+        if not self.signed:
+            return self.convert(raw)
+        raw = float(raw)
+        sign = -1.0 if raw < 0.0 else 1.0
+        return sign * self.convert(abs(raw))
+
+    def _clamp(self, value: float) -> float:
+        if self.clamp_output_min is not None:
+            return max(float(self.clamp_output_min), float(value))
+        return float(value)
+
+
+_DEFAULT_NORMAL = ForceChannelCalibration.linear(
+    NORMAL_FORCE_CAL_SLOPE,
+    NORMAL_FORCE_CAL_OFFSET,
+)
+
+
+def _load_force_calibration_data(sensor_name: str | None) -> dict | None:
+    if not sensor_name:
+        return None
+    try:
+        from .configs import load_force_calibration
+
+        data = load_force_calibration(sensor_name)
+    except Exception:
+        return None
+    return data if isinstance(data, dict) else None
+
+
+def _channel_from_data(data: dict | None, channel: str) -> ForceChannelCalibration | None:
+    if not isinstance(data, dict):
+        return None
+
+    unit = str(data.get("unit", "N"))
+    channels = data.get("channels")
+    if isinstance(channels, dict):
+        block = channels.get(channel)
+        cal = ForceChannelCalibration.from_channel_dict(block, unit=unit)
+        if cal is not None:
+            return cal
+
+    # Legacy single-channel file: {"normal_force": {"model": "linear", ...}}
+    if channel == "normal":
+        block = data.get("normal_force", data)
+        cal = ForceChannelCalibration.from_channel_dict(
+            block, unit=unit, default_method="linear"
+        )
+        if cal is not None:
+            return cal
+        if isinstance(block, dict):
+            slope = block.get("slope")
+            offset = block.get("offset")
+            if slope is not None and offset is not None:
+                return ForceChannelCalibration.linear(
+                    float(slope),
+                    float(offset),
+                    unit=str(block.get("unit", unit)),
+                )
+    return None
+
+
+def _normal_calibration(sensor_name: str | None = None) -> ForceChannelCalibration:
+    cal = _channel_from_data(_load_force_calibration_data(sensor_name), "normal")
+    return cal if cal is not None else _DEFAULT_NORMAL
+
+
+def _shear_calibration(sensor_name: str | None = None) -> ForceChannelCalibration | None:
+    return _channel_from_data(_load_force_calibration_data(sensor_name), "shear")
+
+
+def _load_normal_force_coeffs(sensor_name: str | None = None) -> tuple[float, float]:
+    """Legacy helper: linear slope/offset only (piecewise returns hardcoded defaults)."""
+    cal = _normal_calibration(sensor_name)
+    if cal.method == "linear" and cal.slope is not None and cal.offset is not None:
+        return float(cal.slope), float(cal.offset)
    return NORMAL_FORCE_CAL_SLOPE, NORMAL_FORCE_CAL_OFFSET


 def calibrate_normal_force(raw_value: float, *, sensor_name: str | None = None) -> float:
-    """Convert a raw normal-force reading to Newtons using y = mx + c."""
-    slope, offset = _load_normal_force_coeffs(sensor_name)
-    return slope * float(raw_value) + offset
+    """Convert a raw normal-force reading to Newtons.
+
+    Uses ``sensors/<name>/force_calibration.json`` when present (piecewise or linear).
+    Falls back to hardcoded linear defaults when missing.
+    """
+    return _normal_calibration(sensor_name).convert(raw_value)
+
+
+def calibrate_shear_force(raw_value: float, *, sensor_name: str | None = None) -> float:
+    """Convert a raw shear magnitude to Newtons using the shear channel curve.
+
+    Returns ``float(raw_value)`` unchanged when no shear calibration is available.
+    """
+    cal = _shear_calibration(sensor_name)
+    if cal is None:
+        return float(raw_value)
+    return cal.convert(raw_value)
+
+
+def calibrate_shear_components(
+    shear_x: float,
+    shear_y: float,
+    *,
+    sensor_name: str | None = None,
+) -> tuple[float, float]:
+    """Convert raw shear components to Newtons.
+
+    When the shear channel uses ``input: components`` and ``signed: true``, each
+    axis is calibrated on absolute magnitude then re-signed. Otherwise the
+    magnitude is calibrated and components are scaled proportionally.
+    """
+    sx = float(shear_x)
+    sy = float(shear_y)
+    cal = _shear_calibration(sensor_name)
+    if cal is None:
+        return sx, sy
+
+    if cal.input_mode == "components":
+        return cal.convert_component(sx), cal.convert_component(sy)
+
+    magnitude = (sx * sx + sy * sy) ** 0.5
+    if magnitude <= 0.0:
+        return 0.0, 0.0
+    calibrated = cal.convert(magnitude)
+    scale = calibrated / magnitude
+    return sx * scale, sy * scale


 def _nyquist_hz(sample_rate_hz: float) -> float: