forked from ShunFang-cart/ESP
328 lines
9.4 KiB
Python
328 lines
9.4 KiB
Python
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()
|