Add STEVAL-MKI217V1 SPI logger and replay

soc/software/fusion/fusion_eskf.py

@@ -1,9 +1,12 @@
 #!/usr/bin/env python3
-"""Offline GPS/ICM-4268x fusion for the stationary Genesys2 capture.
+"""Offline GPS/IMU fusion for the stationary Genesys2 capture.
 
 The two UART logs are captured sequentially, so their elapsed-time axes are
 replayed together.  That is valid for the explicitly stationary experiment,
 but is not a substitute for a synchronous moving-platform capture.
+
+The IMU parser accepts both the original averaged ICM-4268x report and the
+timestamped raw LSM6DSOX stream from ``steval_mki217v1_test``.
 """
 
 from __future__ import annotations
@@ -98,8 +101,69 @@ def parse_gps(path: Path) -> list[GpsSample]:
 
 def parse_imu(path: Path) -> list[ImuSample]:
     samples: list[ImuSample] = []
+    raw_meta: dict[str, float] | None = None
+    first_raw_tick: int | None = None
     for row in read_uart_csv(path):
-        match = IMU_RE.search(row["uart_line"])
+        line = row["uart_line"]
+
+        if line.startswith("IMU_META,LSM6DSOX,"):
+            fields = line.split(",")[2:]
+            values: dict[str, float] = {}
+            for field in fields:
+                key, separator, value = field.partition("=")
+                if not separator:
+                    continue
+                values[key] = float(value)
+            required = {
+                "tick_hz",
+                "odr_hz",
+                "accel_ug_lsb",
+                "gyro_udps_lsb",
+                "temp_lsb_c",
+                "temp_offset_millic",
+            }
+            missing = required.difference(values)
+            if missing:
+                raise ValueError(
+                    f"LSM6DSOX metadata missing {sorted(missing)} in {path}"
+                )
+            if values["tick_hz"] <= 0.0 or values["temp_lsb_c"] <= 0.0:
+                raise ValueError(f"invalid LSM6DSOX metadata in {path}")
+            raw_meta = values
+            continue
+
+        if line.startswith("IMU_RAW,LSM6DSOX,"):
+            if raw_meta is None:
+                raise ValueError(f"LSM6DSOX raw sample precedes metadata in {path}")
+            fields = line.split(",")
+            if len(fields) != 11:
+                raise ValueError(f"malformed LSM6DSOX raw sample in {path}: {line}")
+            tick = int(fields[3])
+            if first_raw_tick is None:
+                first_raw_tick = tick
+            raw_accel = np.array([int(x) for x in fields[4:7]], dtype=float)
+            raw_gyro = np.array([int(x) for x in fields[7:10]], dtype=float)
+            raw_temperature = int(fields[10])
+            samples.append(
+                ImuSample(
+                    t=(tick - first_raw_tick) / raw_meta["tick_hz"],
+                    unix_s=float(row["host_unix_s"]),
+                    samples=1,
+                    temperature_c=(
+                        raw_meta["temp_offset_millic"] / 1000.0
+                        + raw_temperature / raw_meta["temp_lsb_c"]
+                    ),
+                    accel_mps2=(
+                        G * raw_accel * raw_meta["accel_ug_lsb"] * 1e-6
+                    ),
+                    gyro_rps=np.deg2rad(
+                        raw_gyro * raw_meta["gyro_udps_lsb"] * 1e-6
+                    ),
+                )
+            )
+            continue
+
+        match = IMU_RE.search(line)
         if not match:
             continue
         samples.append(
@@ -119,9 +183,10 @@ def parse_imu(path: Path) -> list[ImuSample]:
         )
     if len(samples) < 4:
         raise ValueError(f"only {len(samples)} IMU windows found in {path}")
-    t0 = samples[0].t
-    for sample in samples:
-        sample.t -= t0
+    if first_raw_tick is None:
+        t0 = samples[0].t
+        for sample in samples:
+            sample.t -= t0
     return samples