From 525dc6d8f2e607245f18309825a90ca6e089707d Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 1 Oct 2026 23:40:28 +0300 Subject: [PATCH] Vibed more python scripts to log and plot ADC --- servo_controller/scripts/plot_channels.py | 140 +++++++ servo_controller/scripts/send_cmd.py | 1 + servo_controller/scripts/serial_logger.py | 443 ++++++++++++++++++++++ 3 files changed, 584 insertions(+) create mode 100644 servo_controller/scripts/plot_channels.py create mode 100644 servo_controller/scripts/serial_logger.py diff --git a/servo_controller/scripts/plot_channels.py b/servo_controller/scripts/plot_channels.py new file mode 100644 index 0000000..45f5ade --- /dev/null +++ b/servo_controller/scripts/plot_channels.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +"""Plot packed 4-byte-integer channel data from a serial_logger.py CSV, dark mode. + +Expects rows where `data_hex` is N 4-byte little-endian integers packed back +to back (default N=18, matching an 18-channel ADC_READ_ALL-style packet). +X-axis is the device-clock `tick_ms` column by default (or host_time). +""" + +import argparse +import csv +import struct +import sys +from datetime import datetime + +import matplotlib +import matplotlib.pyplot as plt +import matplotlib.dates as mdates + + +def build_parser(): + p = argparse.ArgumentParser( + description="Plot packed 4-byte-integer channels from a serial_logger.py CSV (dark mode).", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog="""\ +examples: + %(prog)s -i adc_log.csv + %(prog)s -i adc_log.csv --command ADC_READ_ALL -o plot.png + %(prog)s -i adc_log.csv --channels 18 --unsigned --x host +""", + ) + + p.add_argument("-i", "--input", required=True, help="input CSV file (from serial_logger.py)") + p.add_argument("-o", "--output", help="save the plot to this file instead of showing it interactively") + + p.add_argument("-c", "--command", help="only use rows with this command name (default: use whatever " + "PACKET rows have the right byte length)") + p.add_argument("-n", "--channels", type=int, default=18, + help="number of 4-byte integer channels packed in data_hex (default: %(default)s)") + p.add_argument("--signed", dest="signed", action="store_true", default=True, + help="interpret channels as signed int32 (default: on)") + p.add_argument("--unsigned", dest="signed", action="store_false", + help="interpret channels as unsigned uint32") + + p.add_argument("--x", choices=["tick", "host"], default="tick", + help="x-axis source: device tick_ms, or host_time (default: %(default)s)") + p.add_argument("--labels", help="comma-separated channel labels (default: ch0..chN-1)") + p.add_argument("--title", default="Channel data", help="plot title (default: %(default)s)") + + return p + + +def load_rows(path, command, channel_bytes): + """Read the CSV and return rows that look like packed-channel PACKET rows.""" + rows = [] + with open(path, newline="", encoding="utf-8") as fh: + reader = csv.DictReader(fh) + for row in reader: + if row.get("type") != "PACKET": + continue + if command and row.get("command") != command: + continue + data_hex = row.get("data_hex", "") + if len(data_hex) != channel_bytes * 2: # 2 hex chars per byte + continue + rows.append(row) + return rows + + +def main(): + parser = build_parser() + args = parser.parse_args() + + channel_bytes = args.channels * 4 + + rows = load_rows(args.input, args.command, channel_bytes) + if not rows: + hint = f" with command={args.command!r}" if args.command else "" + print(f"ERROR: no PACKET rows{hint} with {channel_bytes}-byte payloads " + f"({args.channels} x 4-byte ints) found in {args.input}", file=sys.stderr) + return 1 + + fmt = f"<{args.channels}{'i' if args.signed else 'I'}" + + xs = [] + channels = [[] for _ in range(args.channels)] + + for row in rows: + if args.x == "tick": + xs.append(int(row["tick_ms"])) + else: + xs.append(datetime.fromisoformat(row["host_time"])) + + values = struct.unpack(fmt, bytes.fromhex(row["data_hex"])) + for i, v in enumerate(values): + channels[i].append(v) + + # sort chronologically, just in case the file wasn't strictly ordered + order = sorted(range(len(xs)), key=lambda i: xs[i]) + xs = [xs[i] for i in order] + channels = [[ch[i] for i in order] for ch in channels] + + if args.labels: + labels = [s.strip() for s in args.labels.split(",")] + if len(labels) != args.channels: + parser.error(f"--labels has {len(labels)} entries, expected {args.channels}") + else: + labels = [f"ch{i}" for i in range(args.channels)] + + # --- dark mode plot --- + plt.style.use("dark_background") + fig, ax = plt.subplots(figsize=(13, 7)) + + cmap = matplotlib.colormaps["tab20"].resampled(args.channels) + for i in range(args.channels): + ax.plot(xs, channels[i], label=labels[i], color=cmap(i), linewidth=1.2) + + ax.set_title(args.title, fontsize=14, color="white") + ax.set_xlabel("tick (ms)" if args.x == "tick" else "host time") + ax.set_ylabel("value" + (" (int32)" if args.signed else " (uint32)")) + ax.grid(True, alpha=0.25) + + if args.x == "host": + fig.autofmt_xdate() + ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S")) + + ax.legend(ncol=min(6, args.channels), fontsize=8, loc="upper center", + bbox_to_anchor=(0.5, -0.12), frameon=False) + fig.tight_layout() + + if args.output: + fig.savefig(args.output, dpi=150, facecolor=fig.get_facecolor()) + print(f"Saved plot to {args.output} ({len(xs)} samples, {args.channels} channels)") + else: + plt.show() + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) \ No newline at end of file diff --git a/servo_controller/scripts/send_cmd.py b/servo_controller/scripts/send_cmd.py index bbff51e..d824622 100644 --- a/servo_controller/scripts/send_cmd.py +++ b/servo_controller/scripts/send_cmd.py @@ -44,6 +44,7 @@ COMMANDS = { "ADC_READ": 8, "ADC_READ_ALL": 9, "ADC_SET_READ": 10, + "DIGITAL_IN": 11, } COMMAND_NAMES = {v: k for k, v in COMMANDS.items()} diff --git a/servo_controller/scripts/serial_logger.py b/servo_controller/scripts/serial_logger.py new file mode 100644 index 0000000..aa6f041 --- /dev/null +++ b/servo_controller/scripts/serial_logger.py @@ -0,0 +1,443 @@ +#!/usr/bin/env python3 +"""Open a serial port and log every successful (valid-CRC) packet to a CSV file. + +Standalone: no dependency on any other script in this toolset. + +Packet layout: + + struct command_message_t { + uint8_t prefix; + uint8_t length; + uint8_t id; + uint8_t command; + int64_t tick; // milliseconds, little-endian + uint8_t crc; + uint8_t data[COMMAND_DATA_SIZE]; + } __attribute__((packed)); +""" + +import argparse +import csv +import struct +import sys +import time +from datetime import datetime, timezone +from pathlib import Path + +import serial + +DEFAULT_PORT = "/dev/ttyACM0" +DEFAULT_BAUD = 115200 +DEFAULT_DEVICE_ID = 0 + +COMMAND_PREFIX = 0x69 + +COMMAND_ACK = 0 +COMMAND_NACK = 1 + +# name -> command id +COMMANDS = { + "ACK": 0, + "NACK": 1, + "LED_TOGGLE": 2, + "LED_SET": 3, + "SERVO_SET": 4, + "SERVO_SET_ALL": 5, + "ADC_READ_RAW": 6, + "ADC_READ_RAW_ALL": 7, + "ADC_READ": 8, + "ADC_READ_ALL": 9, + "ADC_SET_READ": 10, + "DIGITAL_IN": 11, +} +COMMAND_NAMES = {v: k for k, v in COMMANDS.items()} + +# struct formats for typed --send-data tokens (all little-endian) +TYPE_FORMATS = { + "u8": " 12 +HEADER_SIZE = CRC_OFFSET + 1 # bytes before data -> 13 + +# Safety valve for the ASCII-log branch: a real firmware log line shouldn't +# be longer than this. If we've buffered more than this with no newline, +# we're desynced (likely sitting inside binary packet data), so drop a byte +# and retry instead of blocking forever waiting for a '\n' that won't come. +MAX_LOG_LINE = 256 + +FIELDNAMES = ["host_time", "tick_ms", "type", "device_id", "command", "length", "data_hex", "text"] + + +# -------------------------------------------------------------------------- +# Packet helpers +# -------------------------------------------------------------------------- + +def calculate_crc(msg: bytes) -> int: + s = sum(msg) & 0xFF + return (-s) & 0xFF + + +def make_packet(command: int, data: bytes = b"", device_id: int = DEFAULT_DEVICE_ID, + tick: int = 0) -> bytes: + pkt = bytearray() + pkt.append(COMMAND_PREFIX) + pkt.append(len(data)) + pkt.append(device_id) + pkt.append(command) + pkt.extend(struct.pack(TICK_FORMAT, tick)) + pkt.append(0) # CRC placeholder + pkt.extend(data) + + pkt[CRC_OFFSET] = calculate_crc(pkt[:CRC_OFFSET] + pkt[CRC_OFFSET + 1:]) + return bytes(pkt) + + +def verify_crc(packet: bytes) -> bool: + crc = packet[CRC_OFFSET] + calc = calculate_crc(packet[:CRC_OFFSET] + packet[CRC_OFFSET + 1:]) + return crc == calc + + +def packet_size(buf: bytes): + if len(buf) < 2: + return None + return HEADER_SIZE + buf[1] + + +def unpack_tick(packet: bytes) -> int: + return struct.unpack(TICK_FORMAT, packet[4:4 + TICK_SIZE])[0] + + +def iso_now() -> str: + return datetime.now(timezone.utc).isoformat(timespec="milliseconds") + + +def drain_input(ser, quiet_period=0.2, max_wait=2.0): + """Actively read and discard bytes until the port stays quiet for + `quiet_period` seconds (or `max_wait` total elapses as a safety cap). + + reset_input_buffer() only clears what the OS has already staged at the + instant it's called. If the device already pushed more bytes down the + wire that hadn't been handed to the OS yet, they show up right after + the flush and get mistaken for new data. Draining until the port is + genuinely idle catches that in-flight backlog too. + """ + ser.reset_input_buffer() + deadline = time.time() + max_wait + quiet_since = time.time() + while time.time() < deadline: + chunk = ser.read(4096) + if chunk: + quiet_since = time.time() + elif time.time() - quiet_since >= quiet_period: + break + + +# -------------------------------------------------------------------------- +# Argument parsing +# -------------------------------------------------------------------------- + +def parse_command(text: str): + """Accept a command name (case-insensitive) or a number. Returns (id, name).""" + key = text.upper() + if key in COMMANDS: + return COMMANDS[key], key + + try: + val = int(text, 0) + except ValueError: + raise argparse.ArgumentTypeError( + f"unknown command '{text}' (use a number, or one of: {', '.join(COMMANDS)})" + ) + if not 0 <= val <= 255: + raise argparse.ArgumentTypeError("command number must be 0-255") + return val, COMMAND_NAMES.get(val, f"#{val}") + + +def parse_data_token(tok: str) -> bytes: + """ + Formats: + 5 -> u8 + u16:500 -> unsigned 16-bit little-endian + i32:-3 -> signed 32-bit little-endian + f32:1.5 -> float32 little-endian + hex:0a0b0c -> raw bytes + """ + if ":" in tok: + kind, value = tok.split(":", 1) + kind = kind.lower() + else: + kind, value = "u8", tok + + if kind == "hex": + return bytes.fromhex(value) + + fmt = TYPE_FORMATS.get(kind) + if fmt is None: + raise ValueError(f"unknown type '{kind}' (use u8/i8/u16/i16/u32/i32/f32/hex)") + + num = float(value) if kind == "f32" else int(value, 0) + return struct.pack(fmt, num) + + +def parse_tick(text: str) -> int: + if text.lower() == "now": + return int(time.time() * 1000) + try: + return int(text, 0) + except ValueError: + raise argparse.ArgumentTypeError("tick must be an integer (ms) or 'now'") + + +def build_parser(): + p = argparse.ArgumentParser( + description="Log every successful (valid-CRC) packet from a serial device to a CSV file.", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog="""\ +CSV columns: + host_time ISO-8601 UTC timestamp when the host received the frame + tick_ms device-clock timestamp from the packet header (blank for log lines) + type PACKET or LOG + device_id, command, length, data_hex set for PACKET rows + text the decoded line, set for LOG rows + +examples: + %(prog)s -o adc_log.csv + %(prog)s -o adc_log.csv --send ADC_SET_READ --send-data u32:100 + %(prog)s -p /dev/ttyUSB0 -b 9600 -o session.csv --duration 60 +""", + ) + + p.add_argument("-p", "--port", default=DEFAULT_PORT, help="serial port (default: %(default)s)") + p.add_argument("-b", "--baud", type=int, default=DEFAULT_BAUD, help="baud rate (default: %(default)s)") + p.add_argument("-d", "--device-id", type=lambda s: int(s, 0), default=None, + help="only log packets from this device id (default: log all)") + + p.add_argument("-o", "--output", + help="CSV file path (default: serial_log_.csv)") + p.add_argument("--append", action="store_true", + help="append to an existing file instead of overwriting (default: off)") + + p.add_argument("--include-logs", dest="include_logs", action="store_true", default=True, + help="also log firmware ASCII log lines as LOG rows (default: on)") + p.add_argument("--no-include-logs", dest="include_logs", action="store_false", + help="packets only, skip firmware ASCII log lines") + + p.add_argument("--boot-delay", type=float, default=0.0, + help="seconds to wait after opening the port before flushing/listening, " + "for boards that reset on connect (default: %(default)s, off)") + p.add_argument("--drain-quiet", type=float, default=0.2, + help="seconds of silence required on the port before logging actually " + "starts, to actively drain any in-flight backlog (default: %(default)s)") + p.add_argument("--no-drain", dest="drain", action="store_false", default=True, + help="skip the active drain, only do a single buffer clear on open") + p.add_argument("--duration", type=float, default=0.0, + help="seconds to log before stopping; 0 = run until Ctrl+C (default: %(default)s)") + p.add_argument("--quiet", action="store_true", + help="don't echo logged rows to the console (default: off)") + + p.add_argument("--allow-unknown-commands", action="store_true", + help="also log packets with a valid CRC but an unrecognized command id " + "(default: off, those are discarded as likely garbage)") + + p.add_argument("--send", type=parse_command, + help="optional command to send once before logging starts (name or number)") + p.add_argument("--send-data", nargs="*", default=[], + help="data tokens for --send: u8/i8/u16/i16/u32/i32/f32: or hex:") + p.add_argument("-t", "--tick", type=parse_tick, default=0, + help="tick value (ms) for the --send packet, or 'now' (default: %(default)s)") + + return p + + +# -------------------------------------------------------------------------- +# Core loop +# -------------------------------------------------------------------------- + +def process_stream(ser, write_row, args, stats): + """Read from `ser`, parse frames, and call write_row(dict) for each valid one. + + Runs until args.duration elapses (0 = forever) or a KeyboardInterrupt. + Bad-CRC frames are discarded, not logged, per "successful packets only". + """ + rx = bytearray() + start = time.time() + + while True: + if args.duration > 0 and (time.time() - start) >= args.duration: + break + + chunk = ser.read(64) + if chunk: + rx.extend(chunk) + + while rx: + if rx[0] == COMMAND_PREFIX: + size = packet_size(rx) + if size is None or len(rx) < size: + break + + pkt = bytes(rx[:size]) + + if not verify_crc(pkt): + stats["bad_crc"] += 1 + # Guessed size may be based on a garbage length byte; + # drop just the leading byte and re-scan. + del rx[:1] + continue + + del rx[:size] + + length = pkt[1] + dev_id = pkt[2] + cmd = pkt[3] + tick = unpack_tick(pkt) + payload = pkt[HEADER_SIZE:HEADER_SIZE + length] + + if args.device_id is not None and dev_id != args.device_id: + continue + + if cmd not in COMMAND_NAMES and not args.allow_unknown_commands: + # Valid CRC but not a recognized command id: more likely + # a chance CRC collision on garbage than a real message. + stats["bad_header"] += 1 + continue + + name = COMMAND_NAMES.get(cmd, str(cmd)) + stats["packets"] += 1 + write_row({ + "host_time": iso_now(), + "tick_ms": tick, + "type": "PACKET", + "device_id": dev_id, + "command": name, + "length": length, + "data_hex": payload.hex(), + "text": "", + }) + else: + idx = rx.find(b"\n") + if idx == -1: + if len(rx) > MAX_LOG_LINE: + del rx[:1] + continue + break + + line = rx[:idx + 1] + del rx[:idx + 1] + + if not args.include_logs: + continue + + try: + text = line.decode().rstrip() + except UnicodeDecodeError: + # Not valid text, e.g. the resync logic caught part of a + # binary frame mid-stream. Discard rather than log garbage. + stats["bad_log"] += 1 + continue + + stats["logs"] += 1 + write_row({ + "host_time": iso_now(), + "tick_ms": "", + "type": "LOG", + "device_id": "", + "command": "", + "length": "", + "data_hex": "", + "text": text, + }) + + +# -------------------------------------------------------------------------- +# Main +# -------------------------------------------------------------------------- + +def main(): + parser = build_parser() + args = parser.parse_args() + + out_path = Path(args.output) if args.output else Path( + f"serial_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" + ) + out_path.parent.mkdir(parents=True, exist_ok=True) + + file_exists = out_path.exists() + mode = "a" if args.append else "w" + + try: + ser = serial.Serial(args.port, args.baud, timeout=0.05) + if args.boot_delay > 0: + time.sleep(args.boot_delay) + if args.drain: + drain_input(ser, quiet_period=args.drain_quiet) + else: + ser.reset_input_buffer() + except serial.SerialException as e: + print(f"ERROR: could not open {args.port}: {e}", file=sys.stderr) + return 1 + + if args.send is not None: + cmd_id, cmd_name = args.send + data = bytearray() + try: + for tok in args.send_data: + data.extend(parse_data_token(tok)) + except (ValueError, struct.error) as e: + parser.error(f"bad --send-data: {e}") + device_id = args.device_id if args.device_id is not None else DEFAULT_DEVICE_ID + packet = make_packet(cmd_id, bytes(data), device_id, args.tick) + print(f"--> TX {cmd_name} (tick={args.tick}ms): {packet.hex(' ')}") + ser.write(packet) + ser.flush() + + stats = {"packets": 0, "logs": 0, "bad_crc": 0, "bad_header": 0, "bad_log": 0} + + with open(out_path, mode, newline="", encoding="utf-8") as fh: + writer = csv.DictWriter(fh, fieldnames=FIELDNAMES) + if mode == "w" or not file_exists: + writer.writeheader() + fh.flush() + + def write_row(row): + writer.writerow(row) + fh.flush() + if not args.quiet: + if row["type"] == "PACKET": + print(f"<-- t={row['tick_ms']}ms {row['command']} len={row['length']} data={row['data_hex']}") + else: + print(f"[LOG] {row['text']}") + + print(f"Logging to {out_path}. Press Ctrl+C to stop.") + try: + if args.send is None: + # Actively drain whatever queued up (or was still in flight) + # while we were opening the output file / writing the + # header, so logging starts clean rather than with a + # backlog of pre-existing traffic. + if args.drain: + drain_input(ser, quiet_period=args.drain_quiet) + else: + ser.reset_input_buffer() + process_stream(ser, write_row, args, stats) + except KeyboardInterrupt: + pass + finally: + ser.close() + + print(f"Done. {stats['packets']} packets, {stats['logs']} log lines, " + f"{stats['bad_crc']} bad CRC discarded, {stats['bad_header']} bad header (unknown command) " + f"discarded, {stats['bad_log']} undecodable log lines discarded -> {out_path}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) \ No newline at end of file