hexdec/servo_controller/scripts/serial_logger.py

#!/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,
	"DIGITAL_LEG_OUT": 12,
	"POWER_MONITOR_SET_READ": 13,
	"POWER_MONITOR_READ": 14,
}
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())