diff --git a/imu_quality.png b/imu_quality.png new file mode 100644 index 0000000..30003e6 Binary files /dev/null and b/imu_quality.png differ diff --git a/visualise.py b/visualise.py new file mode 100644 index 0000000..5dd34ae --- /dev/null +++ b/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\d+)\s+" + r"(?P[-+]?\d+(?:\.\d+)?)Hz\s+" + r"DIN=(?P\[[^\]]+\])\s+" + r"DOUT=(?P\[[^\]]+\])\s+" + r"ACC=(?P\[[^\]]+\])\s+" + r"GYRO=(?P\[[^\]]+\])\s+" + r"ANGLE=(?P\[[^\]]+\])" +) + + +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()