Vibed more python scripts to log and plot ADC

servo_controller/CMakeLists.txt

@@ -6,5 +6,7 @@ set(BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
 find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 project(servo_controller)
 
+add_subdirectory(drivers)
+
 FILE(GLOB app_sources src/*.c)
 target_sources(app PRIVATE ${app_sources})

servo_controller/Kconfig

@@ -3,6 +3,8 @@
 
 source "Kconfig.zephyr"
 
+rsource "drivers/Kconfig"
+
 menu "USB options"
 	depends on USB_DEVICE_STACK_NEXT
 

servo_controller/drivers/CMakeLists.txt

@@ -0,0 +1,2 @@
+
+add_subdirectory(multiplexer)

servo_controller/drivers/Kconfig

@@ -0,0 +1,5 @@
+menu "Servo2350 Device Drivers"
+
+rsource "multiplexer/Kconfig"
+
+endmenu

servo_controller/drivers/multiplexer/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory_ifdef(CONFIG_CD74HC4067 cd74hc4067)

servo_controller/drivers/multiplexer/Kconfig

@@ -0,0 +1,5 @@
+menu "Multiplexer Drivers"
+
+rsource "cd74hc4067/Kconfig"
+
+endmenu

servo_controller/drivers/multiplexer/cd74hc4067/CMakeLists.txt

@@ -0,0 +1,4 @@
+
+target_sources(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}/cd74hc4067.c)
+
+target_include_directories(app PRIVATE ${CMAKE_CURRENT_LIST_DIR})

servo_controller/drivers/multiplexer/cd74hc4067/Kconfig

@@ -0,0 +1,23 @@
+menuconfig CD74HC4067
+	bool "TI CD74HC4067 analog multiplexer"
+	depends on GPIO
+	help
+	  Enable support for the TI CD74HC4067 16-channel analog
+	  multiplexer controlled through GPIO selection pins.
+
+if CD74HC4067
+
+config CD74HC4067_INIT_PRIORITY
+	int "Initialization priority"
+	default 80
+	help
+	  Device initialization priority for the CD74HC4067 driver.
+
+
+# Creates "CONFIG_CD74HC4067_LOG_LEVEL"
+module = CD74HC4067
+module-str = cd74hc4067
+source "subsys/logging/Kconfig.template.log_config"
+
+endif # CD74HC4067
+

servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c

@@ -0,0 +1,191 @@
+#define DT_DRV_COMPAT cd74hc4067
+
+#include <errno.h>
+#include <zephyr/logging/log.h>
+
+#include "cd74hc4067.h"
+
+LOG_MODULE_REGISTER(cd74hc4067);
+
+//-------------------------------------------------------------------------
+// Declarations
+
+static int cd74hc4067_init(const struct device *dev);
+static int cd74hc4067_enable_pin(const struct device *dev, bool state);
+
+//-------------------------------------------------------------------------
+// Private
+
+
+static int cd74hc4067_init(const struct device *dev) {
+	int ret;
+
+	const struct cd74hc4067_cfg *cfg = dev->config;
+	struct cd74hc4067_data *data = dev->data;
+
+	k_mutex_init(&data->lock);
+	data->current_channel = 0U;
+	data->enabled = !cfg->has_enable;
+
+	// Init the select pins
+	for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) {
+		const struct gpio_dt_spec *pin = &(cfg->select[i]);
+		if (!gpio_is_ready_dt(pin)) {
+			LOG_ERR("Select pin %d not ready.\r\n", i);
+			return -ENODEV;
+		}
+
+		ret = gpio_pin_configure_dt(pin, GPIO_OUTPUT_INACTIVE);
+		if (ret < 0) {
+			LOG_ERR("Could not configure select pin %d as output\r\n", i);
+			return -ENODEV;
+		}
+	}
+
+	if (cfg->has_enable) {
+		// Init the enable pin
+		const struct gpio_dt_spec *enable = &cfg->enable;
+		LOG_DBG("Initializing enable pin.\r\n");
+
+		if (!gpio_is_ready_dt(enable)) {
+			LOG_ERR("Enable pin not ready.\r\n");
+			return -ENODEV;
+		}
+
+		ret = gpio_pin_configure_dt(enable, GPIO_OUTPUT_INACTIVE);
+		if (ret < 0) {
+			LOG_ERR("Could not configure ENABLE pin as output\r\n");
+			return -ENODEV;
+		}
+	}
+
+	return 0;
+}
+
+static int cd74hc4067_enable_pin(const struct device *dev, bool state) {
+	const struct cd74hc4067_cfg *cfg = dev->config;
+	struct cd74hc4067_data *data = dev->data;
+	int ret = 0;
+
+	if (!cfg->has_enable) {
+		if (!state) {
+			return -ENOTSUP;
+		}
+
+		data->enabled = true;
+		return 0;
+	}
+
+	if (!device_is_ready(cfg->enable.port)) {
+		LOG_ERR("%s enable GPIO not ready", dev->name);
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_set_dt(&cfg->enable, state ? 0 : 1);
+	if (ret == 0) {
+		data->enabled = state;
+	}
+
+	return ret;
+}
+
+//-------------------------------------------------------------------------
+// Public
+
+int cd74hc4067_enable(const struct device *dev) {
+	struct cd74hc4067_data *data = dev->data;
+	int ret;
+
+	ret = k_mutex_lock(&data->lock, K_FOREVER);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = cd74hc4067_enable_pin(dev, true);
+
+	k_mutex_unlock(&data->lock);
+	return ret;
+}
+
+int cd74hc4067_disable(const struct device *dev) {
+	struct cd74hc4067_data *data = dev->data;
+	int ret;
+
+	ret = k_mutex_lock(&data->lock, K_FOREVER);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = cd74hc4067_enable_pin(dev, false);
+
+	k_mutex_unlock(&data->lock);
+	return ret;
+}
+
+int cd74hc4067_select_channel(const struct device *dev, uint8_t channel) {
+	if (channel >= CD74HC4067_SELECT_CHANNEL_COUNT) {
+		LOG_ERR("Channel not found.\r\n");
+		return -ENODEV;
+	}
+	
+	int ret;
+	const struct cd74hc4067_cfg *cfg = dev->config;
+	struct cd74hc4067_data *data = dev->data;
+
+	ret = k_mutex_lock(&data->lock, K_FOREVER);
+	if (ret < 0) {
+		LOG_ERR("Mutex error");
+		return ret;
+	}
+
+	for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) {
+		bool state = (channel >> i) & 0x1;
+		ret = gpio_pin_set_dt(&cfg->select[i], state);
+		if (ret < 0) {
+			LOG_ERR("Unable to set channel pin: %d", ret);
+			goto out;
+		}
+	}
+
+	data->current_channel = channel;
+
+out:
+	k_mutex_unlock(&data->lock);
+	return ret;
+}
+
+//-------------------------------------------------------------------------
+// Devicetree
+
+// static const struct cd74hc4067_api cd74hc4067_api_funcs = {
+// 	.enable = cd74hc4067_enable,
+// 	.disable = cd74hc4067_disable,
+// 	.select_channel = cd74hc4067_select_channel,
+// };
+
+
+#define CD74HC4067_DEFINE(inst)												\
+																			\
+	static const struct cd74hc4067_cfg cd74hc4067_cfg_##inst = {			\
+		.select = {															\
+			GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 0),	\
+			GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 1),	\
+			GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 2),	\
+			GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 3),	\
+		},																	\
+		.enable = COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, enable_gpio),		\
+				      (GPIO_DT_SPEC_GET(DT_DRV_INST(inst), enable_gpio)),	\
+				      ((struct gpio_dt_spec){0})),							\
+		.has_enable = DT_INST_NODE_HAS_PROP(inst, enable_gpio),				\
+	};																		\
+	static struct cd74hc4067_data cd74hc4067_data_##inst;					\
+	DEVICE_DT_INST_DEFINE(	inst, 											\
+							cd74hc4067_init, 								\
+							NULL, 											\
+							&cd74hc4067_data_##inst,						\
+			      			&cd74hc4067_cfg_##inst,							\
+							POST_KERNEL,									\
+			      			CONFIG_CD74HC4067_INIT_PRIORITY, 				\
+							NULL);
+
+DT_INST_FOREACH_STATUS_OKAY(CD74HC4067_DEFINE)

servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h

@@ -0,0 +1,45 @@
+#ifndef SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_
+#define SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_
+
+#include <stdint.h>
+
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/gpio.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// struct cd74hc4067_api {
+// 	int (*enable)(const struct device *dev);
+// 	int (*disable)(const struct device *dev);
+// 	int (*select_channel)(const struct device *dev, uint8_t channel);
+// };
+
+struct cd74hc4067_data {
+	struct k_mutex lock;
+	uint8_t current_channel;
+	bool enabled;
+};
+
+#define CD74HC4067_SELECT_CHANNEL_COUNT 16U
+#define CD74HC4067_SELECT_PIN_COUNT 4U
+struct cd74hc4067_cfg {
+    struct gpio_dt_spec select[CD74HC4067_SELECT_PIN_COUNT];
+	struct gpio_dt_spec enable;
+	bool has_enable;
+};
+
+int cd74hc4067_enable(const struct device *dev);
+int cd74hc4067_disable(const struct device *dev);
+int cd74hc4067_select_channel(const struct device *dev, uint8_t channel);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_ */
+

servo_controller/dts/bindings/cd74hc4067.yaml

@@ -0,0 +1,23 @@
+description: |
+  TI CD74HC4067 16-channel analog multiplexer.
+
+compatible: "cd74hc4067"
+
+include: base.yaml
+
+properties:
+  select-gpios:
+    type: phandle-array
+    required: true
+    description: |
+      GPIO pins for channel selection (S0-S3).
+      Array must contain exactly 4 GPIOs:
+      - Index 0: S0
+      - Index 1: S1
+      - Index 2: S2
+      - Index 3: S3
+
+  enable-gpio:
+    type: phandle
+    description: |
+      Optional GPIO for common enable pin (E).

servo_controller/prj.conf

@@ -2,6 +2,9 @@ CONFIG_GPIO=y
 CONFIG_PWM=y
 CONFIG_ADC=y
 
+# Mux
+CONFIG_CD74HC4067=y
+
 # Serial
 CONFIG_SERIAL=y
 CONFIG_CONSOLE=y
@@ -29,4 +32,6 @@ CONFIG_LOG_MODE_IMMEDIATE=y
 # DEBUG
 CONFIG_DEBUG_THREAD_INFO=y
 # CONFIG_DEBUG=y
-# CONFIG_DEBUG_OPTIMIZATIONS=y
+# CONFIG_DEBUG_OPTIMIZATIONS=y
+CONFIG_OUTPUT_DISASSEMBLY=n		# Create disassmebly files
+#CONFIG_OUTPUT_DISASSEMBLE_ALL=n

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/read_adc.py

@@ -16,8 +16,10 @@ LED_TOGGLE = 2
 LED_SET = 3
 SERVO_SET = 4
 SERVO_SET_ALL = 5
-ADC_READ = 6
+ADC_READ_RAW = 6
-ADC_READ_ALL = 7
+ADC_READ_RAW_ALL = 7
+ADC_READ = 8
+ADC_READ_ALL = 9
 
 DEVICE_ID = 0
 
@@ -114,7 +116,8 @@ def main():
 	threading.Thread(target=reader, args=(ser,), daemon=True).start()
 
 	data = bytearray()
-	data.append(17)
+	# Data needs to be a bitmask of channels
+	data.append(0b11011)
 
 	ser.write(make_packet(ADC_READ, data))
 

servo_controller/scripts/read_adc_all.py

@@ -16,8 +16,10 @@ LED_TOGGLE = 2
 LED_SET = 3
 SERVO_SET = 4
 SERVO_SET_ALL = 5
-ADC_READ = 6
+ADC_READ_RAW = 6
-ADC_READ_ALL = 7
+ADC_READ_RAW_ALL = 7
+ADC_READ = 8
+ADC_READ_ALL = 9
 
 DEVICE_ID = 0
 

servo_controller/scripts/send_cmd.py

@@ -0,0 +1,568 @@
+#!/usr/bin/env python3
+"""Send binary commands to a device over a serial port, optionally receiving and/or requiring an ACK.
+
+Packet layout matches:
+
+	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 struct
+import sys
+import threading
+import time
+
+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()}
+
+# Commands for which the data argument is OPTIONAL.
+# Every command not listed here REQUIRES data.
+# >>> Edit this list to match your firmware. <<<
+COMMANDS_DATA_OPTIONAL = {
+	"ACK",
+	"NACK",
+	"LED_TOGGLE",
+	"ADC_READ_RAW_ALL",
+	"ADC_READ_ALL",
+}
+
+# struct formats for typed data tokens (all little-endian)
+TYPE_FORMATS = {
+	"u8": "<B", "i8": "<b",
+	"u16": "<H", "i16": "<h",
+	"u32": "<I", "i32": "<i",
+	"f32": "<f",
+}
+
+# Exit codes
+EXIT_OK = 0
+EXIT_ERROR = 1
+EXIT_NO_ACK = 2
+EXIT_NACK = 3
+
+# 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
+
+
+# --------------------------------------------------------------------------
+# 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]
+
+
+# --------------------------------------------------------------------------
+# Receiver thread
+# --------------------------------------------------------------------------
+
+class Receiver(threading.Thread):
+	"""Parses incoming packets / ASCII logs.
+
+	`verbose` gates whether anything is printed at all. `mode` picks the
+	print format for received command packets:
+	  normal    - name, tick, length, then the raw hex line (default)
+	  debug     - the full raw hex bitstream of every packet/line, nothing else
+	  data      - only the data payload bytes
+	  timestamp - tick + data payload bytes ("-l")
+
+	Stats for --telemetry are always collected, regardless of `verbose`.
+	"""
+
+	def __init__(self, ser, verbose=False, mode="normal"):
+		super().__init__(daemon=True)
+		self.ser = ser
+		self.verbose = verbose
+		self.mode = mode
+		self.stop_event = threading.Event()
+		self.ack_event = threading.Event()
+		self.result = None  # "ACK" or "NACK" once received
+
+		# telemetry
+		self.start_time = None
+		self.end_time = None
+		self.command_counts = {}
+		self.ack_count = 0
+		self.nack_count = 0
+		self.bad_crc_count = 0
+		self.log_line_count = 0
+		self.data_byte_count = 0
+		self.packet_count = 0
+		# device-clock (tick, ms) timestamps of every CRC-valid packet, in
+		# arrival order, used to compute real RX frequency. Kept per name
+		# and combined across all types.
+		self.tick_history = {}
+		self.all_ticks = []
+
+	def _print(self, *args):
+		if self.verbose:
+			print(*args)
+
+	def _print_packet(self, pkt, length, dev_id, cmd, tick, payload):
+		name = COMMAND_NAMES.get(cmd, str(cmd))
+
+		if self.mode == "debug":
+			self._print(f"<-- RAW: {pkt.hex(' ')}")
+		elif self.mode == "data":
+			self._print(f"<-- DATA: {payload.hex(' ')}")
+		elif self.mode == "timestamp":
+			self._print(f"<-- t={tick}ms  DATA: {payload.hex(' ')}")
+		else:  # normal
+			self._print(f"<-- {name} (device={dev_id}, tick={tick}ms) len={length}")
+			self._print(f"<-- RX: {pkt.hex(' ')}")
+
+	def _print_ack_nack(self, pkt, dev_id, tick, name):
+		if self.mode == "debug":
+			self._print(f"<-- RAW: {pkt.hex(' ')}")
+		elif self.mode == "data":
+			pass  # no data payload to show
+		elif self.mode == "timestamp":
+			self._print(f"<-- t={tick}ms  {name}")
+		else:  # normal
+			self._print(f"<-- {name} (device={dev_id}, tick={tick}ms)")
+
+	def run(self):
+		self.start_time = time.time()
+		rx = bytearray()
+
+		while not self.stop_event.is_set():
+			data = self.ser.read(64)
+			if not data:
+				continue
+
+			rx.extend(data)
+
+			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):
+						self.bad_crc_count += 1
+						# The CRC is what's wrong, not necessarily the tick
+						# field's position, so still surface it - useful for
+						# spotting exactly this kind of firmware bug.
+						bad_tick = unpack_tick(pkt)
+						if self.mode == "debug":
+							self._print(f"RX: Bad CRC: {pkt.hex(' ')}")
+						else:
+							self._print(f"RX: Bad CRC (tick={bad_tick}ms): {pkt.hex(' ')}")
+						# The guessed size was based on a length byte that
+						# might itself be garbage (stale/misaligned bytes).
+						# Drop just the leading prefix byte and re-scan,
+						# rather than eating `size` bytes and risking a
+						# cascade of misalignment.
+						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]
+
+					self.packet_count += 1
+
+					if cmd == COMMAND_ACK:
+						self.ack_count += 1
+						self.result = "ACK"
+						self._print_ack_nack(pkt, dev_id, tick, "ACK")
+						self.tick_history.setdefault("ACK", []).append(tick)
+						self.all_ticks.append(tick)
+						self.ack_event.set()
+					elif cmd == COMMAND_NACK:
+						self.nack_count += 1
+						self.result = "NACK"
+						self._print_ack_nack(pkt, dev_id, tick, "NACK")
+						self.tick_history.setdefault("NACK", []).append(tick)
+						self.all_ticks.append(tick)
+						self.ack_event.set()
+					else:
+						name = COMMAND_NAMES.get(cmd, str(cmd))
+						self.command_counts[name] = self.command_counts.get(name, 0) + 1
+						self.data_byte_count += length
+						self._print_packet(pkt, length, dev_id, cmd, tick, payload)
+						self.tick_history.setdefault(name, []).append(tick)
+						self.all_ticks.append(tick)
+				else:
+					idx = rx.find(b"\n")
+					if idx == -1:
+						if len(rx) > MAX_LOG_LINE:
+							# Not really a log line: desynced, likely sitting
+							# inside binary packet data. Shed a byte and
+							# keep looking for the next prefix instead of
+							# stalling forever waiting for '\n'.
+							del rx[:1]
+							continue
+						break
+
+					line = rx[:idx + 1]
+					del rx[:idx + 1]
+
+					self.log_line_count += 1
+
+					if self.mode == "data":
+						continue  # data-only stream: skip firmware log text
+
+					try:
+						decoded = line.decode().rstrip()
+						if self.mode == "debug":
+							self._print(f"[RAW] {line.hex(' ')}")
+						else:
+							self._print("[LOG]", decoded)
+					except UnicodeDecodeError:
+						self._print("[RAW]", line.hex())
+
+		self.end_time = time.time()
+
+	def print_telemetry(self):
+		start = self.start_time or time.time()
+		end = self.end_time or time.time()
+		elapsed = max(end - start, 1e-9)
+		total = self.packet_count + self.log_line_count
+
+		def hz(ticks):
+			"""Frequency in Hz from device-clock (tick, ms) deltas between
+			the first and last sample. None if there aren't enough points
+			or the ticks didn't advance (e.g. all identical / rolled over)."""
+			if len(ticks) < 2:
+				return None
+			span_ms = ticks[-1] - ticks[0]
+			if span_ms <= 0:
+				return None
+			return (len(ticks) - 1) * 1000.0 / span_ms
+
+		print("--- Telemetry ---")
+		print(f"elapsed:        {elapsed:.3f} s")
+		print(f"packets:        {self.packet_count} ({self.packet_count / elapsed:.2f} pkt/s host-side)")
+		print(f"data bytes:     {self.data_byte_count} ({self.data_byte_count / elapsed:.2f} B/s)")
+		print(f"log lines:      {self.log_line_count}")
+		print(f"ACK / NACK:     {self.ack_count} / {self.nack_count}")
+		print(f"bad CRC:        {self.bad_crc_count}")
+		print(f"total frames:   {total} ({total / elapsed:.2f} frames/s)")
+
+		overall_hz = hz(self.all_ticks)
+		if overall_hz is not None:
+			print(f"RX frequency:   {overall_hz:.2f} Hz (from device tick deltas, all valid packets)")
+		else:
+			print("RX frequency:   n/a (need >=2 CRC-valid packets with advancing tick)")
+
+		if self.command_counts:
+			print("by command (count, Hz from tick deltas):")
+			for name, count in sorted(self.command_counts.items()):
+				f = hz(self.tick_history.get(name, []))
+				f_str = f"{f:.2f} Hz" if f is not None else "n/a"
+				print(f"  {name:<18} {count:>6}   {f_str}")
+
+
+# --------------------------------------------------------------------------
+# 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 --list-commands to see them)"
+		)
+	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():
+	commands_help = ", ".join(f"{name}={cid}" for name, cid in COMMANDS.items())
+	optional_help = ", ".join(sorted(COMMANDS_DATA_OPTIONAL)) or "(none)"
+
+	p = argparse.ArgumentParser(
+		description="Send a binary command to a serial device.",
+		formatter_class=argparse.RawDescriptionHelpFormatter,
+		epilog=f"""\
+commands:
+  {commands_help}
+
+commands where data is optional:
+  {optional_help}
+
+data tokens (all multi-byte values are little-endian):
+  5           u8 (default type)
+  u16:500     also: u8 i8 u16 i16 u32 i32 f32
+  hex:0a0b    raw bytes
+
+RX display modes (pick at most one; require --rx, --debug, --data or -l to receive at all):
+  (none)      name, tick, length, and raw hex per packet
+  --debug     raw hex bitstream of every packet/line, nothing decoded
+  --data      only the data payload bytes of received commands
+  -l          tick timestamp + data payload bytes
+
+examples:
+  %(prog)s LED_TOGGLE
+  %(prog)s ADC_SET_READ u32:1 --require-ack
+  %(prog)s SERVO_SET 0 u16:1500 --rx --rx-time 5
+  %(prog)s ADC_READ_ALL -l --telemetry
+  %(prog)s -p /dev/ttyUSB0 -b 9600 ADC_READ_ALL --debug
+""",
+	)
+
+	p.add_argument("command", nargs="?", type=parse_command,
+	               help="command name or number")
+	p.add_argument("data", nargs="*", help="command data tokens (see below)")
+
+	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=DEFAULT_DEVICE_ID,
+	               help="device id in the packet header (default: %(default)s)")
+	p.add_argument("-t", "--tick", type=parse_tick, default=0,
+	               help="tick value in ms for the outgoing packet header, or 'now' (default: %(default)s)")
+
+	p.add_argument("--rx", action="store_true",
+	               help="receive and print incoming packets/logs in the default format (default: off)")
+	p.add_argument("--rx-time", type=float, default=10.0,
+	               help="seconds to keep listening after sending, when receiving (default: %(default)s)")
+
+	rx_format = p.add_mutually_exclusive_group()
+	rx_format.add_argument("--debug", action="store_true",
+	                        help="RX mode: print the raw hex bitstream of every packet/line (default: off)")
+	rx_format.add_argument("--data", dest="data_only", action="store_true",
+	                        help="RX mode: print only the data payload of received commands (default: off)")
+	rx_format.add_argument("-l", "--timestamp", action="store_true",
+	                        help="RX mode: print data payload with its tick timestamp (default: off)")
+
+	p.add_argument("--require-ack", action="store_true",
+	               help="wait for an ACK; exit 2 on timeout, 3 on NACK (default: off)")
+	p.add_argument("--ack-timeout", type=float, default=1.0,
+	               help="seconds to wait for ACK (default: %(default)s)")
+
+	p.add_argument("--boot-delay", type=float, default=0.0,
+	               help="seconds to wait after opening the port before flushing/sending, "
+	                    "for boards that reset on connect (default: %(default)s, off)")
+
+	p.add_argument("--telemetry", action="store_true",
+	               help="print an RX telemetry summary (packet/byte rates, counts) at exit (default: off)")
+
+	p.add_argument("--list-commands", action="store_true",
+	               help="list known commands and exit")
+	return p
+
+
+# --------------------------------------------------------------------------
+# Main
+# --------------------------------------------------------------------------
+
+def main():
+	parser = build_parser()
+	args = parser.parse_args()
+
+	if args.list_commands:
+		for name, cid in COMMANDS.items():
+			opt = "data optional" if name in COMMANDS_DATA_OPTIONAL else "data required"
+			print(f"{cid:3d}  {name:<18} {opt}")
+		return EXIT_OK
+
+	if args.command is None:
+		parser.error("the following arguments are required: command")
+
+	cmd_id, cmd_name = args.command
+
+	if not args.data and cmd_name not in COMMANDS_DATA_OPTIONAL:
+		parser.error(f"command {cmd_name} requires data")
+
+	data = bytearray()
+	try:
+		for tok in args.data:
+			data.extend(parse_data_token(tok))
+	except (ValueError, struct.error) as e:
+		parser.error(f"bad data: {e}")
+
+	if len(data) > 255:
+		parser.error(f"data too long ({len(data)} bytes, max 255)")
+
+	packet = make_packet(cmd_id, bytes(data), args.device_id, args.tick)
+
+	# --debug/--data/-l/--telemetry imply listening for the rx-time window,
+	# same as --rx, even if --rx itself wasn't passed.
+	listen = args.rx or args.debug or args.data_only or args.timestamp or args.telemetry
+	need_receiver = listen or args.require_ack
+
+	if args.debug:
+		mode = "debug"
+	elif args.data_only:
+		mode = "data"
+	elif args.timestamp:
+		mode = "timestamp"
+	else:
+		mode = "normal"
+
+	try:
+		ser = serial.Serial(args.port, args.baud, timeout=0.05)
+		if args.boot_delay > 0:
+			time.sleep(args.boot_delay)  # let a board that resets on connect finish booting
+		ser.reset_input_buffer()  # discard stale/boot-garbage bytes before we start
+	except serial.SerialException as e:
+		print(f"ERROR: could not open {args.port}: {e}", file=sys.stderr)
+		return EXIT_ERROR
+
+	receiver = None
+	exit_code = EXIT_OK
+
+	try:
+		# Start the receiver BEFORE sending so a fast ACK isn't missed.
+		if need_receiver:
+			receiver = Receiver(ser, verbose=listen, mode=mode)
+			receiver.start()
+
+		print(f"--> TX {cmd_name} (tick={args.tick}ms): {packet.hex(' ')}")
+		ser.write(packet)
+		ser.flush()
+
+		if args.require_ack:
+			if not receiver.ack_event.wait(args.ack_timeout):
+				print(f"ERROR: no ACK within {args.ack_timeout}s", file=sys.stderr)
+				exit_code = EXIT_NO_ACK
+			elif receiver.result == "NACK":
+				print("ERROR: device replied NACK", file=sys.stderr)
+				exit_code = EXIT_NACK
+			else:
+				print("ACK received.")
+
+		if listen:
+			time.sleep(args.rx_time)
+
+	except KeyboardInterrupt:
+		pass
+	finally:
+		if receiver:
+			receiver.stop_event.set()
+			receiver.join(timeout=1)
+		ser.close()
+
+	if args.telemetry and receiver:
+		receiver.print_telemetry()
+
+	print("Done.")
+	return exit_code
+
+
+if __name__ == "__main__":
+	sys.exit(main())

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

servo_controller/scripts/test_adc_set_interval.py

@@ -0,0 +1,129 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_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
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+					print(f"<-- RX: {pkt.hex(' ')} ")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+
+	data = bytearray()
+	data.extend((1).to_bytes(4, byteorder="little"))
+
+	ser.write(make_packet(ADC_SET_READ, data))
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	print("Done.")
+	time.sleep(10)
+
+
+if __name__ == "__main__":
+	main()

servo_controller/src/adc.c

@@ -1,7 +1,11 @@
 #include "adc.h"
+#include "command_message.h"
+#include "usb.h"
+#include "mux.h"
 
 #include <zephyr/logging/log.h>
 #include <zephyr/drivers/adc.h>
+#include <zephyr/kernel.h>
 
 LOG_MODULE_REGISTER(adc, LOG_LEVEL_INF);
 
@@ -17,14 +21,74 @@ struct adc_sequence sequence = {
     .buffer_size = sizeof(adc_buffer),
 };
 
+// ADC THREAD
+static struct k_thread adc_thread_data;
+static k_tid_t adc_thread_id = NULL;
+#define ADC_THREAD_STACK_SIZE 2048
+#define ADC_THREAD_PRIORITY 5
+K_THREAD_STACK_DEFINE(adc_thread_stack, ADC_THREAD_STACK_SIZE);
+
+// ADC MASK BUFFER
+#define ADC_MASK_BUFFER_SIZE 10
+char adc_mask_msgq_buffer[ADC_MASK_BUFFER_SIZE * sizeof(uint32_t)];
+struct k_msgq adc_mask_msgq;
+
+// ADC TIMER
+struct k_timer adc_timer;
+
+
+static void adc_timer_handler(struct k_timer *timer) {
+    // Timer trigger
+	adc_add_mask(0b111111111111111111);
+}
+
+static void adc_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+    while (1) {
+		uint32_t mask = 0;
+        k_msgq_get(&adc_mask_msgq, &mask, K_FOREVER);
+
+		// Read ADC
+		int values[NUM_ADC_CHANNELS];
+		for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
+			int bit = (mask >> i) & 0x1;
+			
+			if (bit) {
+				values[i] = adc_read_id(i);
+			}
+			else {
+				values[i] = 0;
+			}
+
+			// TODO: int to angles
+			// TODO: normalize
+		}
+		
+		// Send to USB
+		struct command_message_t *msg = usb_get_next_tx_buf();
+		command_create_message(msg, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+		usb_send_command(msg);
+	}
+}
+
 int adc_init_all(void) {
 	int ret;
 
-	// ret = mux_init();
+	// MSGQ
-	// if (ret != 0) {
+	k_msgq_init(&adc_mask_msgq, adc_mask_msgq_buffer, sizeof(uint32_t), ADC_MASK_BUFFER_SIZE);
-	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
+
-	// 	return ret;
+	// TIMER
-	// }
+	k_timer_init(&adc_timer, adc_timer_handler, NULL);
+
+	// MUX init
+	ret = mux_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to setup multiplexer: %d", ret);
+		return ret;
+	}
 
 	if (!adc_is_ready_dt(&adc_mux_spec)) {
 		LOG_ERR("ADC mux channel not ready");
@@ -59,6 +123,18 @@ int adc_init_all(void) {
 	adc_sequence_init_dt(&adc_ch16_spec, &sequence);
 	adc_sequence_init_dt(&adc_ch17_spec, &sequence);
 
+	// THREAD
+	adc_thread_id = k_thread_create(
+		&adc_thread_data,
+		adc_thread_stack,
+		K_THREAD_STACK_SIZEOF(adc_thread_stack),
+		adc_thread,
+		NULL, NULL, NULL,
+		ADC_THREAD_PRIORITY,
+		0,
+		K_NO_WAIT
+	);
+
     return 0;
 }
 
@@ -72,12 +148,11 @@ int adc_read_id(int id) {
 		// Read muxed channels
 		(void)adc_sequence_init_dt(&adc_mux_spec, &sequence);
 
-		// Set mux TODO:
+		ret = mux_select_channel(id);
-		// ret = mux_select_channel(id);
+		if (ret < 0) {
-		// if (ret < 0) {
+			LOG_ERR("Could not set multiplexer (%d)\n", ret);
-		// 	LOG_ERR("Could not set multiplexer (%d)\n", ret);
+			return 0;
-		// 	return 0;
+		}
-		// }
 		
 		ret = adc_read_dt(&adc_mux_spec, &sequence);
 		if (ret < 0) {
@@ -117,4 +192,22 @@ int adc_read_id(int id) {
 
     
 	return val_mv;
+}
+
+int adc_add_mask(uint32_t mask) {
+	k_msgq_put(&adc_mask_msgq, &mask, K_NO_WAIT);
+
+	return 0;
+}
+
+int adc_timer_set(int interval_ms) {
+
+	if (interval_ms > 0) {
+		k_timer_start(&adc_timer, K_MSEC(interval_ms), K_MSEC(interval_ms));
+	}
+	else {
+		k_timer_stop(&adc_timer);
+	}
+	
+	return 0;
 }

servo_controller/src/adc.h

@@ -2,12 +2,16 @@
 #define ADC_H
 
 
-#define MUX_CHANNELS 16
+#include <stdint.h>
+
+
 #define NUM_ADC_CHANNELS 18
 
 
 int adc_init_all();
 int adc_read_id(int id);
+int adc_add_mask(uint32_t mask);
+int adc_timer_set(int interval_ms);
 
 
 #endif // ADC_H

servo_controller/src/command_handler.c

@@ -65,29 +65,55 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
-		case ADC_READ: {
+		case ADC_READ_RAW: {
 			int channel = msg->data[0];
 			int value = adc_read_id(channel);
 
 			struct command_message_t *reply = usb_get_next_tx_buf();
-			command_create_message(reply, sizeof(value), ADC_READ, (uint8_t *)&value);
+			command_create_message(reply, sizeof(value), msg->command, (uint8_t *)&value);
 			usb_send_command(reply);
 
 			break;
 		}
-		case ADC_READ_ALL: {
+		case ADC_READ_RAW_ALL: {
 			int values[NUM_ADC_CHANNELS];
 			for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
 				values[i] = adc_read_id(i);
 			}
 
 			struct command_message_t *reply = usb_get_next_tx_buf();
-			command_create_message(reply, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+			command_create_message(reply, sizeof(values), msg->command, (uint8_t *)values);
 			usb_send_command(reply);
 			
 			break;
 		}
 
+		case ADC_READ: {
+			uint32_t mask;
+			memcpy(&mask, &msg->data[0], sizeof(uint32_t));
+
+			adc_add_mask(mask);
+
+			break;
+		}
+
+		case ADC_READ_ALL: {
+			uint32_t mask = 0b1111111111111111;
+
+			adc_add_mask(mask);
+			
+			break;
+		}
+
+		case ADC_SET_READ: {
+			int interval_ms;
+			memcpy(&interval_ms, &msg->data[0], sizeof(int));
+
+			adc_timer_set(interval_ms);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;

servo_controller/src/command_message.c

@@ -2,6 +2,7 @@
 
 #include <string.h>
 #include <zephyr/logging/log.h>
+#include <zephyr/kernel.h>
 
 LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
 
@@ -9,6 +10,7 @@ void command_message_init(struct command_message_t *msg) {
 	memset(msg, 0, sizeof(struct command_message_t));
 	msg->prefix = COMMAND_PREFIX;
 	msg->id = COMMAND_ID;
+	msg->tick = 0;
 }
 
 void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]) {
@@ -20,6 +22,7 @@ void command_create_message(struct command_message_t *msg, uint8_t length, comma
 	command_message_init(msg);
 	msg->length = length;
 	msg->command = command;
+	msg->tick = k_uptime_get();
 
 	// Copy the data
 	if (data != NULL) {
@@ -38,7 +41,7 @@ uint8_t command_calculate_crc(struct command_message_t *msg) {
 	int loop_length = (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length);
 
 	for (int i = 0; i < loop_length; i++) {
-		if (i == 4) { continue; }
+		if (i == (COMMAND_HEADER_SIZE - 1)) { continue; }
 		sum += byte_ptr[i];
 	}
 

servo_controller/src/command_message.h

@@ -8,7 +8,7 @@
 #define COMMAND_PREFIX 0x69
 #define COMMAND_ID 0x00
 #define COMMAND_DATA_SIZE 160
-#define COMMAND_HEADER_SIZE 5
+#define COMMAND_HEADER_SIZE 13
 
 typedef enum {
 	COMMAND_ACK,
@@ -17,8 +17,12 @@ typedef enum {
 	LED_SET,
 	SERVO_SET,
 	SERVO_SET_ALL,
+	ADC_READ_RAW,
+	ADC_READ_RAW_ALL,
 	ADC_READ,
 	ADC_READ_ALL,
+	ADC_SET_READ,
+	DIGITAL_IN,
 
 	// Keep last
 	NUM_COMMANDS,
@@ -29,6 +33,7 @@ struct command_message_t {
 	uint8_t length;
 	uint8_t id;
 	uint8_t command;
+	int64_t tick;
 	uint8_t crc;
 	uint8_t data[COMMAND_DATA_SIZE];
 } __attribute__((packed));

servo_controller/src/digital_in.c

@@ -0,0 +1,64 @@
+#include "digital_in.h"
+#include "usb.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/gpio.h>
+
+LOG_MODULE_REGISTER(digital_in, LOG_LEVEL_INF);
+
+
+#define DIGITAL_IN_MACRO(node_id) GPIO_DT_SPEC_GET(node_id, gpios),
+
+static const struct gpio_dt_spec digital_inputs[] = {
+    DT_FOREACH_CHILD(DT_NODELABEL(digital_inputs), DIGITAL_IN_MACRO)
+};
+static struct gpio_callback digital_in_cb_data;
+
+
+void digital_in_irq_handler(const struct device *dev, struct gpio_callback *cb, uint32_t pins) {
+	// TODO: what happens
+	struct command_message_t *msg = usb_get_next_tx_buf();
+	command_create_message(msg, sizeof(pins), DIGITAL_IN, (uint8_t *)&pins);
+	usb_send_command(msg);
+}
+
+int digital_in_pin_init(const struct gpio_dt_spec *gpio, uint32_t *bitmask) {
+	int ret;
+
+	if (!gpio_is_ready_dt(gpio)) {
+		LOG_ERR("GPIO device %s is not ready\n", gpio->port->name);
+		return -EBUSY;
+	}
+
+	ret = gpio_pin_configure_dt(gpio, GPIO_INPUT);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	ret = gpio_pin_interrupt_configure_dt(gpio, GPIO_INT_EDGE_TO_ACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure interrupt on %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	*bitmask |= BIT(gpio->pin);
+
+	return ret;
+}
+
+int digital_in_init() {
+	uint32_t bitmask = 0;
+
+	for (int i = 0; i < DIGITAL_IN_CHANNELS; i++) {
+		digital_in_pin_init(&digital_inputs[i], &bitmask);
+	}
+
+	gpio_init_callback(&digital_in_cb_data, digital_in_irq_handler, bitmask);
+	gpio_add_callback(digital_inputs[0].port, &digital_in_cb_data);
+
+	return 0;
+}

servo_controller/src/digital_in.h

@@ -0,0 +1,10 @@
+#ifndef DIGITAL_IN_H
+#define DIGITAL_IN_H
+
+
+#define DIGITAL_IN_CHANNELS 6
+
+int digital_in_init();
+
+
+#endif // DIGITAL_IN_H

servo_controller/src/main.c

@@ -2,6 +2,7 @@
 #include "usb.h"
 #include "servo.h"
 #include "adc.h"
+#include "digital_in.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -38,5 +39,12 @@ int main(void) {
 		return 0;
 	}
 
+	// DIGITAL IN init
+	ret = digital_in_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable DIGITAL IN");
+		return 0;
+	}
+
 	return 0;
 }

servo_controller/src/mux.c

@@ -0,0 +1,44 @@
+#include "mux.h"
+
+#include <zephyr/logging/log.h>
+#include <stdlib.h>
+
+LOG_MODULE_REGISTER(mux);
+
+#define MUX_NODE DT_ALIAS(mux)
+
+static const struct device *mux_dev = DEVICE_DT_GET(MUX_NODE);
+
+int mux_init(void) {
+	int ret;
+
+	if (!DT_NODE_HAS_STATUS(MUX_NODE, okay)) {
+		LOG_ERR("mux alias missing or disabled in devicetree");
+		return -ENODEV;
+	}
+
+	if (!device_is_ready(mux_dev)) {
+		LOG_ERR("Multiplexer device not ready");
+		return -ENODEV;
+	}
+
+	ret = cd74hc4067_enable(mux_dev);
+	if ((ret != 0) && (ret != -ENOTSUP)) {
+		LOG_ERR("Failed to enable multiplexer: %d", ret);
+		return ret;
+	}
+
+	ret = cd74hc4067_select_channel(mux_dev, 0U);
+	if (ret != 0) {
+		LOG_ERR("Failed to select initial channel: %d", ret);
+		return ret;
+	}
+
+	LOG_INF("Multiplexer HAL initialized");
+
+	return 0;
+}
+
+int mux_select_channel(int channel) {
+    return cd74hc4067_select_channel(mux_dev, channel);
+}

servo_controller/src/mux.h

@@ -0,0 +1,13 @@
+#ifndef MUX_H
+#define MUX_H
+
+
+#include "cd74hc4067.h"
+
+#define MUX_CHANNELS CD74HC4067_SELECT_CHANNEL_COUNT
+
+int mux_init(void);
+int mux_select_channel(int channel);
+
+
+#endif // MUX_H

servo_controller/src/usb.c

@@ -104,13 +104,14 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
 			len = ring_buf_get(&ringbuf, &buf_prefix, 1);
 
 			if (len && (buf_prefix == COMMAND_PREFIX)) {
-				uint8_t buf_header[4];
+				uint8_t buf_header[COMMAND_HEADER_SIZE];
-				len = ring_buf_get(&ringbuf, buf_header, 4);
+				len = ring_buf_get(&ringbuf, buf_header, (COMMAND_HEADER_SIZE - 1));
 
-				if ((len == 4) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
+				if ((len == (COMMAND_HEADER_SIZE - 1)) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
 					msg.length = buf_header[0];
 					msg.command = buf_header[2];
-					msg.crc = buf_header[3];
+					// Ignore the tick for now
+					msg.crc = buf_header[COMMAND_HEADER_SIZE - 2];
 
 					if (msg.length) {
 						len = ring_buf_get(&ringbuf, msg.data, msg.length);
@@ -120,6 +121,8 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
 					if (calculated_crc != msg.crc) {
 						if (RETURN_ACK) {
 							// Send NACK
+							usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
+							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 						}
 						continue;
@@ -129,12 +132,16 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
 					if (ret == 0) {
 						if (RETURN_ACK) {
 							// Send ACK
+							usb_tx_buffer[TX_BUFFER_SIZE].tick = k_uptime_get();
+							usb_tx_buffer[TX_BUFFER_SIZE].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE]);
 							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
 						}
 					}
 					else {
 						if (RETURN_ACK) {
 							// Send NACK
+							usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
+							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 						}
 					}