Vibed more python scripts to log and plot ADC

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())

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()}
 

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": "<B", "i8": "<b",
+	"u16": "<H", "i16": "<h",
+	"u32": "<I", "i32": "<i",
+	"f32": "<f",
+}
+
+# Fixed header layout: prefix, length, id, command, tick(int64), crc
+TICK_FORMAT = "<q"                         # signed 64-bit little-endian, milliseconds
+TICK_SIZE = struct.calcsize(TICK_FORMAT)   # 8
+CRC_OFFSET = 4 + TICK_SIZE                 # index of the crc byte -> 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_<timestamp>.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:<value> or hex:<bytes>")
+	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())