init

2026-06-08 17:52:48 +08:00
commit 7a15bd8e14
189 changed files with 192687 additions and 0 deletions
@@ -0,0 +1,111 @@
+from dataclasses import dataclass
+
+
+DEFAULT_FORCE_CALIBRATION_POINTS = [
+    {"raw": 1000.0, "force_n": 0.0},
+    {"raw": 10000.0, "force_n": 0.377},
+    {"raw": 30000.0, "force_n": 1.377},
+    {"raw": 62000.0, "force_n": 2.377},
+]
+
+DEFAULT_NORMAL_FORCE_CALIBRATION = {
+    "enabled": True,
+    "method": "piecewise_linear",
+    "extrapolate": True,
+    "clamp_output_min": 0.0,
+    "points": [
+        {"fnormal": 1000.0, "force_n": 0.0},
+        {"fnormal": 10000.0, "force_n": 0.377},
+        {"fnormal": 30000.0, "force_n": 1.377},
+        {"fnormal": 62000.0, "force_n": 2.377},
+    ],
+}
+
+DEFAULT_SHEAR_FORCE_CALIBRATION = {
+    "enabled": True,
+    "method": "piecewise_linear",
+    "extrapolate": True,
+    "clamp_output_min": 0.0,
+    "points": DEFAULT_FORCE_CALIBRATION_POINTS,
+}
+
+
+def parse_calibration_point(point):
+    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")))
+        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}")
+
+
+@dataclass(frozen=True)
+class ForceConverter:
+    points: tuple
+    unit: str = "N"
+    enabled: bool = True
+    extrapolate: bool = True
+    clamp_output_min: float | None = 0.0
+
+    @classmethod
+    def from_config(cls, calibration, config_name):
+        if calibration is None:
+            calibration = DEFAULT_NORMAL_FORCE_CALIBRATION
+        if not calibration.get("enabled", True):
+            return cls(points=(), unit="raw", enabled=False)
+
+        method = str(calibration.get("method", "piecewise_linear")).lower()
+        if method != "piecewise_linear":
+            raise ValueError(f"unsupported {config_name} method: {method}")
+
+        points = tuple(sorted(
+            parse_calibration_point(point)
+            for point in calibration.get("points", [])
+        ))
+        if len(points) < 2:
+            raise ValueError(f"{config_name}.points must contain at least two points")
+
+        return cls(
+            points=points,
+            enabled=True,
+            extrapolate=bool(calibration.get("extrapolate", True)),
+            clamp_output_min=calibration.get("clamp_output_min", 0.0),
+        )
+
+    def convert(self, raw):
+        if not self.enabled:
+            return float(raw)
+
+        raw = float(raw)
+        points = self.points
+        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 _clamp(self, value):
+        if self.clamp_output_min is not None:
+            value = max(float(self.clamp_output_min), value)
+        return value
+