Feat: visualization code implemented

visualise.py

@@ -0,0 +1,327 @@
+import argparse
+import ast
+import csv
+import os
+import re
+import sys
+import time
+
+os.environ.setdefault("MPLCONFIGDIR", "/tmp/matplotlib")
+
+import matplotlib.pyplot as plt
+import numpy as np
+import serial
+
+from pc_reader import PACKET_SIZE, SYNC, decode_packet
+
+
+TEXT_LINE_RE = re.compile(
+    r"#?\s*(?P<count>\d+)\s+"
+    r"(?P<freq>[-+]?\d+(?:\.\d+)?)Hz\s+"
+    r"DIN=(?P<din>\[[^\]]+\])\s+"
+    r"DOUT=(?P<dout>\[[^\]]+\])\s+"
+    r"ACC=(?P<acc>\[[^\]]+\])\s+"
+    r"GYRO=(?P<gyro>\[[^\]]+\])\s+"
+    r"ANGLE=(?P<angle>\[[^\]]+\])"
+)
+
+
+FIELD_NAMES = (
+    "acc_x_g",
+    "acc_y_g",
+    "acc_z_g",
+    "gyro_x_dps",
+    "gyro_y_dps",
+    "gyro_z_dps",
+    "angle_x_deg",
+    "angle_y_deg",
+    "angle_z_deg",
+    "temp_c",
+    "freq_hz",
+)
+
+
+def read_serial_samples(port, baud, seconds=None, max_samples=None, max_gap_ms=1000):
+    samples = []
+    buffer = bytearray()
+    last_t_ms = None
+    elapsed_s = 0.0
+    bad_delta_count = 0
+    start = time.monotonic()
+
+    with serial.Serial(port, baud, timeout=1) as ser:
+        print("Reading {} at {} baud...".format(port, baud), file=sys.stderr)
+
+        while True:
+            if seconds is not None and time.monotonic() - start >= seconds:
+                break
+            if max_samples is not None and len(samples) >= max_samples:
+                break
+
+            chunk = ser.read(ser.in_waiting or 1)
+            if chunk:
+                buffer.extend(chunk)
+
+            while len(buffer) >= PACKET_SIZE:
+                sync_index = buffer.find(SYNC)
+                if sync_index < 0:
+                    del buffer[:-1]
+                    break
+                if sync_index:
+                    del buffer[:sync_index]
+                if len(buffer) < PACKET_SIZE:
+                    break
+
+                packet = bytes(buffer[:PACKET_SIZE])
+                if (sum(packet[:-1]) & 0xFF) != packet[-1]:
+                    del buffer[0]
+                    continue
+
+                del buffer[:PACKET_SIZE]
+                data = decode_packet(packet)
+
+                if last_t_ms is None:
+                    freq_hz = 0.0
+                else:
+                    delta = (data["t_ms"] - last_t_ms) & 0xFFFFFFFF
+                    if 0 < delta <= max_gap_ms:
+                        elapsed_s += delta / 1000.0
+                        freq_hz = 1000.0 / delta
+                    else:
+                        bad_delta_count += 1
+                        freq_hz = 0.0
+
+                last_t_ms = data["t_ms"]
+                samples.append(flatten_packet(data, elapsed_s, freq_hz))
+
+                if max_samples is not None and len(samples) >= max_samples:
+                    break
+
+    if bad_delta_count:
+        print(
+            "Ignored {} abnormal packet time delta(s).".format(bad_delta_count),
+            file=sys.stderr,
+        )
+
+    return samples
+
+
+def flatten_packet(data, t_s, freq_hz):
+    return {
+        "t_s": t_s,
+        "acc_x_g": data["acc_g"][0],
+        "acc_y_g": data["acc_g"][1],
+        "acc_z_g": data["acc_g"][2],
+        "gyro_x_dps": data["gyro_dps"][0],
+        "gyro_y_dps": data["gyro_dps"][1],
+        "gyro_z_dps": data["gyro_dps"][2],
+        "angle_x_deg": data["angle_deg"][0],
+        "angle_y_deg": data["angle_deg"][1],
+        "angle_z_deg": data["angle_deg"][2],
+        "temp_c": data["temp_c"],
+        "freq_hz": freq_hz,
+        "din0": data["din"][0],
+        "din1": data["din"][1],
+        "din2": data["din"][2],
+        "din3": data["din"][3],
+        "dout0": data["dout"][0],
+        "dout1": data["dout"][1],
+    }
+
+
+def read_text_log(path):
+    samples = []
+    t_s = 0.0
+
+    with open(path, "r", encoding="utf-8") as f:
+        for line in f:
+            match = TEXT_LINE_RE.search(line)
+            if not match:
+                continue
+
+            freq_hz = float(match.group("freq"))
+            din = ast.literal_eval(match.group("din"))
+            dout = ast.literal_eval(match.group("dout"))
+            acc = ast.literal_eval(match.group("acc"))
+            gyro = ast.literal_eval(match.group("gyro"))
+            angle = ast.literal_eval(match.group("angle"))
+
+            if samples and freq_hz > 0:
+                t_s += 1.0 / freq_hz
+
+            samples.append(
+                {
+                    "t_s": t_s,
+                    "acc_x_g": acc[0],
+                    "acc_y_g": acc[1],
+                    "acc_z_g": acc[2],
+                    "gyro_x_dps": gyro[0],
+                    "gyro_y_dps": gyro[1],
+                    "gyro_z_dps": gyro[2],
+                    "angle_x_deg": angle[0],
+                    "angle_y_deg": angle[1],
+                    "angle_z_deg": angle[2],
+                    "temp_c": np.nan,
+                    "freq_hz": freq_hz,
+                    "din0": din[0],
+                    "din1": din[1],
+                    "din2": din[2],
+                    "din3": din[3],
+                    "dout0": dout[0],
+                    "dout1": dout[1],
+                }
+            )
+
+    return samples
+
+
+def write_csv(samples, path):
+    if not samples:
+        return
+
+    columns = list(samples[0].keys())
+    with open(path, "w", newline="", encoding="utf-8") as f:
+        writer = csv.DictWriter(f, fieldnames=columns)
+        writer.writeheader()
+        writer.writerows(samples)
+
+
+def print_stats(samples):
+    if not samples:
+        print("No samples parsed.")
+        return
+
+    print("Samples: {}".format(len(samples)))
+    print("Duration: {:.3f}s".format(samples[-1]["t_s"] - samples[0]["t_s"]))
+
+    for group_name, fields in (
+        ("ACC g", ("acc_x_g", "acc_y_g", "acc_z_g")),
+        ("GYRO dps", ("gyro_x_dps", "gyro_y_dps", "gyro_z_dps")),
+        ("ANGLE deg", ("angle_x_deg", "angle_y_deg", "angle_z_deg")),
+    ):
+        print("\n{}".format(group_name))
+        for field in fields:
+            values = np.array([sample[field] for sample in samples], dtype=float)
+            diffs = np.diff(values)
+            diff_std = float(np.std(diffs)) if len(diffs) else 0.0
+            print(
+                "  {:12s} mean={: .6f} std={:.6f} ptp={:.6f} diff_std={:.6f}".format(
+                    field,
+                    float(np.mean(values)),
+                    float(np.std(values)),
+                    float(np.ptp(values)),
+                    diff_std,
+                )
+            )
+
+    freq = np.array([sample["freq_hz"] for sample in samples[1:]], dtype=float)
+    if len(freq):
+        print(
+            "\nFrequency Hz mean={:.3f} std={:.3f} min={:.3f} max={:.3f}".format(
+                float(np.mean(freq)),
+                float(np.std(freq)),
+                float(np.min(freq)),
+                float(np.max(freq)),
+            )
+        )
+
+
+def plot_samples(samples, output=None, show=True):
+    if not samples:
+        raise ValueError("No samples to plot.")
+
+    t = np.array([sample["t_s"] for sample in samples], dtype=float)
+    fig, axes = plt.subplots(4, 1, sharex=True, figsize=(13, 9))
+
+    plot_group(
+        axes[0],
+        t,
+        samples,
+        ("acc_x_g", "acc_y_g", "acc_z_g"),
+        "Acceleration (g)",
+    )
+    plot_group(
+        axes[1],
+        t,
+        samples,
+        ("gyro_x_dps", "gyro_y_dps", "gyro_z_dps"),
+        "Gyro (deg/s)",
+    )
+    plot_group(
+        axes[2],
+        t,
+        samples,
+        ("angle_x_deg", "angle_y_deg", "angle_z_deg"),
+        "Angle (deg)",
+    )
+
+    freq = np.array([sample["freq_hz"] for sample in samples], dtype=float)
+    axes[3].plot(t, freq, label="freq_hz", color="tab:purple", linewidth=1.0)
+    axes[3].set_ylabel("Hz")
+    axes[3].set_xlabel("Time (s)")
+    axes[3].grid(True, alpha=0.3)
+    axes[3].legend(loc="upper right")
+
+    fig.suptitle("IMU Data Quality Overview")
+    fig.tight_layout()
+
+    if output:
+        fig.savefig(output, dpi=160)
+        print("Saved plot to {}".format(output))
+
+    if show:
+        plt.show()
+
+
+def plot_group(axis, t, samples, fields, ylabel):
+    for field in fields:
+        values = np.array([sample[field] for sample in samples], dtype=float)
+        axis.plot(t, values, label=field, linewidth=1.0)
+    axis.set_ylabel(ylabel)
+    axis.grid(True, alpha=0.3)
+    axis.legend(loc="upper right", ncol=3)
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Read ESP IMU data and visualise signal quality/noise."
+    )
+    source = parser.add_mutually_exclusive_group(required=True)
+    source.add_argument("--file", help="pc_reader text log, for example rcd.txt")
+    source.add_argument("--port", help="ESP32 serial port, for example /dev/ttyUSB0")
+    parser.add_argument("--baud", type=int, default=115200)
+    parser.add_argument("--seconds", type=float, default=10.0)
+    parser.add_argument("--samples", type=int)
+    parser.add_argument(
+        "--max-gap-ms",
+        type=int,
+        default=1000,
+        help="Ignore packet timestamp jumps larger than this.",
+    )
+    parser.add_argument("--output", default="imu_quality.png")
+    parser.add_argument("--csv", help="Optional CSV export path.")
+    parser.add_argument("--no-show", action="store_true", help="Save only; do not open a window.")
+    args = parser.parse_args()
+
+    if args.file:
+        samples = read_text_log(args.file)
+    else:
+        samples = read_serial_samples(
+            args.port,
+            args.baud,
+            args.seconds,
+            args.samples,
+            args.max_gap_ms,
+        )
+
+    print_stats(samples)
+
+    if args.csv:
+        write_csv(samples, args.csv)
+        print("Saved CSV to {}".format(args.csv))
+
+    plot_samples(samples, output=args.output, show=not args.no_show)
+
+
+if __name__ == "__main__":
+    main()