First

servo_controller/.gitignore

@@ -0,0 +1,4 @@
+build
+tmp
+boards
+.vscode

servo_controller/CMakeLists.txt

@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+set(BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(servo_controller)
+
+FILE(GLOB app_sources src/*.c)
+target_sources(app PRIVATE ${app_sources})

servo_controller/Kconfig

@@ -0,0 +1,60 @@
+# Copyright (c) 2023 Nordic Semiconductor ASA
+# SPDX-License-Identifier: Apache-2.0
+
+source "Kconfig.zephyr"
+
+menu "USB options"
+	depends on USB_DEVICE_STACK_NEXT
+
+config USBD_MANUFACTURER
+	string "USB device manufacturer string"
+	default "Zephyr Project"
+	help
+	  USB device manufacturer string.
+
+config USBD_PRODUCT
+	string "USB device product string"
+	default "USBD"
+	help
+	  USB device product stringa.
+
+config USBD_VID
+	hex "USB device Vendor ID"
+	default 0x2fe3
+	help
+	  USB device Vendor ID. The default id (0x2fe3) is associated to
+	  Zephyr Project, you must use your own VID and applications
+	  outside of Zephyr Project.
+
+config USBD_PID
+	hex "USB device Product ID"
+	default 0x0001
+	help
+	  USB device Product ID. You must use your own PID
+	  and applications outside of Zephyr Project.
+
+config USBD_SELF_POWERED
+	bool "USB device Self-powered attribute"
+	default y
+	help
+	  Set the Self-powered attribute in the configuration.
+
+config USBD_REMOTE_WAKEUP
+	bool "USB device Remote Wakeup attribute"
+	help
+	  Set the Remote Wakeup attribute in the configuration.
+
+config USBD_MAX_POWER
+	int "USB device bMaxPower value"
+	default 125
+	range 0 250
+	help
+	  bMaxPower value in the configuration in 2 mA units.
+
+config USBD_20_EXTENSION_DESC
+	bool "Use default USB 2.0 Extension Descriptor"
+	depends on USBD_BOS_SUPPORT
+	help
+	  Set bcdUSB value to 0201 and use default USB 2.0 Extension Descriptor.
+
+endmenu

servo_controller/README.md

@@ -0,0 +1,56 @@
+# Zephyr USB Command Handler
+
+This repository contains the implementation of a USB command handler for the Zephyr RTOS. The system allows devices to communicate via USB using custom command messages.
+
+### Components
+
+1. **command_handler.c/h**: Handles incoming commands, processes them, and performs actions based on the command type.
+2. **command_message.c/h**: Manages the creation, validation, and logging of command messages, including CRC calculation.
+3. **usb.c/h**: Manages USB initialization, data reception via UART, and sending responses. Includes a USB thread.
+4. **usb_conf.c/h**: Configures the USB device.
+
+### ACK
+
+Every command returns either an ACK or NACK.
+
+### Command Structure
+
+```c
+struct command_message_t {
+    uint8_t prefix;     // 0x69
+    uint8_t length;
+    uint8_t id;
+    uint8_t command;
+    uint8_t crc;
+    uint8_t data[160];
+} __attribute__((packed));
+```
+
+### CRC Calculation Function
+
+```c
+uint8_t calculate_crc(struct command_message_t *msg) {
+    uint32_t sum = 0;
+    uint8_t crc = 0;
+
+    uint8_t *byte_ptr = (uint8_t *)msg;
+
+    for (int i = 0; i < (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length) - 1; i++) {
+        sum += byte_ptr[i];
+    }
+
+    crc = 0x100 - (sum & 0xff);
+
+    return crc;
+}
+```
+
+### Python test script (AI generated)
+
+**Command Usage:**
+
+```bash
+python3 scripts/led_blink.py
+```
+
+This script sends LED toggle commands to the device connected via USB.

servo_controller/prj.conf

@@ -0,0 +1,30 @@
+CONFIG_GPIO=y
+CONFIG_PWM=y
+
+# Serial
+CONFIG_SERIAL=y
+CONFIG_CONSOLE=y
+CONFIG_UART_CONSOLE=y
+CONFIG_STDOUT_CONSOLE=y
+CONFIG_UART_LINE_CTRL=y
+
+# USB
+CONFIG_USB_DEVICE_STACK_NEXT=y
+CONFIG_CDC_ACM_SERIAL_INITIALIZE_AT_BOOT=n
+CONFIG_USBD_VID=0xffff
+CONFIG_USBD_PID=0x0420
+CONFIG_USBD_MANUFACTURER="Some Dumbass"
+CONFIG_USBD_PRODUCT="The Thing"
+CONFIG_USBD_SELF_POWERED=y
+CONFIG_USBD_MAX_POWER=125
+
+# LOG
+CONFIG_LOG=n
+CONFIG_USBD_CDC_ACM_LOG_LEVEL_OFF=y  # This removes a pointless warning
+CONFIG_LOG_DEFAULT_LEVEL=3
+CONFIG_LOG_MODE_IMMEDIATE=y
+
+# DEBUG
+CONFIG_DEBUG_THREAD_INFO=y
+# CONFIG_DEBUG=y
+# CONFIG_DEBUG_OPTIMIZATIONS=y

servo_controller/scripts/led_blink.py

@@ -0,0 +1,123 @@
+#!/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 = 2
+
+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})")
+
+                elif cmd == COMMAND_ERROR:
+                    print(f"<-- ERROR (device={dev_id})")
+
+                else:
+                    print(f"<-- Command {cmd} len={length}")
+
+            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)
+
+    threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+    for i in range(10):
+        print(f"--> Sending LED command {i + 1}")
+        ser.write(make_packet(LED))
+        time.sleep(0.5)
+
+    print("Done.")
+    time.sleep(2)
+
+
+if __name__ == "__main__":
+    main()

servo_controller/scripts/speed_test.py

@@ -0,0 +1,263 @@
+#!/usr/bin/env python3
+
+import serial
+import threading
+import time
+
+# ==========================
+# Configuration
+# ==========================
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED = 2
+
+DEVICE_ID = 0
+
+# Test parameters
+TEST_DURATION = 2.0  # seconds per stage
+LOSS_THRESHOLD = 1.0  # percent
+
+# Rates to test (packets/second)
+RATES = [
+    100,
+    200,
+    500,
+    1000,
+    2000,
+    4000,
+    8000,
+    16000,
+]
+
+# ==========================
+# Statistics
+# ==========================
+
+lock = threading.Lock()
+
+tx_packets = 0
+tx_bytes = 0
+
+rx_packets = 0
+rx_ack = 0
+rx_nack = 0
+rx_error = 0
+
+
+# ==========================
+# Packet helpers
+# ==========================
+
+def calculate_crc(msg: bytes) -> int:
+    s = sum(msg) & 0xFF
+    return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+    pkt = bytearray()
+
+    pkt.append(COMMAND_PREFIX)
+    pkt.append(len(data))
+    pkt.append(DEVICE_ID)
+    pkt.append(command)
+    pkt.append(0)
+
+    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]
+
+
+# ==========================
+# Receiver
+# ==========================
+
+def reader(ser):
+    global rx_packets, rx_ack, rx_nack, rx_error
+
+    rx = bytearray()
+
+    while True:
+        data = ser.read(4096)
+        if not data:
+            continue
+
+        rx.extend(data)
+
+        while True:
+            if not rx:
+                break
+
+            # ASCII log output
+            if rx[0] != COMMAND_PREFIX:
+                nl = rx.find(b"\n")
+                if nl == -1:
+                    rx.clear()
+                    break
+
+                line = rx[: nl + 1]
+                del rx[: nl + 1]
+
+                try:
+                    print("[LOG]", line.decode().rstrip())
+                except UnicodeDecodeError:
+                    pass
+
+                continue
+
+            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):
+                continue
+
+            cmd = pkt[3]
+
+            with lock:
+                rx_packets += 1
+
+                if cmd == COMMAND_ACK:
+                    rx_ack += 1
+                elif cmd == COMMAND_NACK:
+                    rx_nack += 1
+                elif cmd == COMMAND_ERROR:
+                    rx_error += 1
+
+
+# ==========================
+# Benchmark
+# ==========================
+
+def run_stage(ser, packet, target_pps):
+    global tx_packets, tx_bytes
+    global rx_packets, rx_ack, rx_nack, rx_error
+
+    with lock:
+        tx_packets = 0
+        tx_bytes = 0
+        rx_packets = 0
+        rx_ack = 0
+        rx_nack = 0
+        rx_error = 0
+
+    interval = 1.0 / target_pps
+
+    start = time.perf_counter()
+    end = start + TEST_DURATION
+    next_tx = start
+
+    while True:
+        now = time.perf_counter()
+
+        if now >= end:
+            break
+
+        if now >= next_tx:
+            ser.write(packet)
+
+            with lock:
+                tx_packets += 1
+                tx_bytes += len(packet)
+
+            next_tx += interval
+        else:
+            sleep = next_tx - now
+            if sleep > 0:
+                time.sleep(min(sleep, 0.0005))
+
+    # Allow final ACKs to arrive
+    time.sleep(0.25)
+
+    elapsed = time.perf_counter() - start
+
+    with lock:
+        tx = tx_packets
+        ack = rx_ack
+        nack = rx_nack
+        err = rx_error
+        bytes_sent = tx_bytes
+
+    loss = 0.0
+    if tx:
+        loss = (tx - ack) / tx * 100.0
+
+    print()
+    print("=" * 60)
+    print(f"Target Rate : {target_pps:>7} pkt/s")
+    print(f"Elapsed     : {elapsed:.3f} s")
+    print(f"Sent        : {tx}")
+    print(f"ACK         : {ack}")
+    print(f"NACK        : {nack}")
+    print(f"ERROR       : {err}")
+    print(f"Loss        : {loss:.2f}%")
+    print(f"Actual TX   : {tx / elapsed:.0f} pkt/s")
+    print(f"Throughput  : {bytes_sent / elapsed / 1024:.2f} KiB/s")
+
+    return loss
+
+
+# ==========================
+# Main
+# ==========================
+
+def main():
+    packet = make_packet(LED)
+
+    print(f"Opening {PORT} @ {BAUDRATE} baud...")
+    ser = serial.Serial(PORT, BAUDRATE, timeout=0.01)
+
+    threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+    print("\nStarting communication benchmark...\n")
+
+    previous_rate = None
+
+    for rate in RATES:
+        loss = run_stage(ser, packet, rate)
+
+        if loss > LOSS_THRESHOLD:
+            print()
+            print(
+                f"Link became unreliable (> {LOSS_THRESHOLD:.1f}% loss)."
+            )
+
+            if previous_rate is not None:
+                print(f"Maximum reliable rate ≈ {previous_rate} pkt/s")
+
+            break
+
+        previous_rate = rate
+
+    else:
+        print()
+        print("Completed all test stages.")
+        print(f"Reliable up to at least {RATES[-1]} pkt/s.")
+
+    ser.close()
+
+
+if __name__ == "__main__":
+    main()

servo_controller/scripts/speed_test2.py

@@ -0,0 +1,224 @@
+#!/usr/bin/env python3
+
+import serial
+import threading
+import time
+
+# ==========================================================
+# Configuration
+# ==========================================================
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED = 2
+
+DEVICE_ID = 0
+
+TEST_DURATION = 5.0
+ACK_TIMEOUT = 1.0
+
+# ==========================================================
+# Packet helpers
+# ==========================================================
+
+def calculate_crc(msg: bytes) -> int:
+    return (-sum(msg)) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+    pkt = bytearray()
+
+    pkt.append(COMMAND_PREFIX)
+    pkt.append(len(data))
+    pkt.append(DEVICE_ID)
+    pkt.append(command)
+    pkt.append(0)
+
+    pkt.extend(data)
+
+    pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+    return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+    return packet[4] == calculate_crc(packet[:4] + packet[5:])
+
+
+# ==========================================================
+# ACK synchronization
+# ==========================================================
+
+ack_event = threading.Event()
+
+ack_count = 0
+nack_count = 0
+error_count = 0
+timeout_count = 0
+
+running = True
+
+
+# ==========================================================
+# Receiver
+# ==========================================================
+
+def reader(ser):
+    global ack_count
+    global nack_count
+    global error_count
+
+    rx = bytearray()
+
+    while running:
+
+        data = ser.read(4096)
+
+        if not data:
+            continue
+
+        rx.extend(data)
+
+        while True:
+
+            if len(rx) < 5:
+                break
+
+            if rx[0] != COMMAND_PREFIX:
+
+                nl = rx.find(b"\n")
+
+                if nl == -1:
+                    rx.clear()
+                    break
+
+                try:
+                    print("[LOG]", rx[:nl].decode().rstrip())
+                except UnicodeDecodeError:
+                    pass
+
+                del rx[: nl + 1]
+                continue
+
+            length = rx[1]
+            size = 5 + length
+
+            if len(rx) < size:
+                break
+
+            pkt = bytes(rx[:size])
+            del rx[:size]
+
+            if not verify_crc(pkt):
+                continue
+
+            cmd = pkt[3]
+
+            if cmd == COMMAND_ACK:
+                ack_count += 1
+                ack_event.set()
+
+            elif cmd == COMMAND_NACK:
+                nack_count += 1
+                ack_event.set()
+
+            elif cmd == COMMAND_ERROR:
+                error_count += 1
+                ack_event.set()
+
+
+# ==========================================================
+# Benchmark
+# ==========================================================
+
+def benchmark(ser):
+
+    global timeout_count
+
+    packet = make_packet(LED)
+
+    sent = 0
+
+    rtt_sum = 0.0
+    rtt_min = float("inf")
+    rtt_max = 0.0
+
+    start = time.perf_counter()
+    end = start + TEST_DURATION
+
+    while time.perf_counter() < end:
+
+        ack_event.clear()
+
+        t0 = time.perf_counter()
+
+        ser.write(packet)
+
+        if not ack_event.wait(ACK_TIMEOUT):
+            timeout_count += 1
+            continue
+
+        t1 = time.perf_counter()
+
+        rtt = t1 - t0
+
+        sent += 1
+        rtt_sum += rtt
+        rtt_min = min(rtt_min, rtt)
+        rtt_max = max(rtt_max, rtt)
+
+    elapsed = time.perf_counter() - start
+
+    print()
+    print("=" * 60)
+    print(f"Elapsed      : {elapsed:.3f} s")
+    print(f"Sent         : {sent}")
+    print(f"ACK          : {ack_count}")
+    print(f"NACK         : {nack_count}")
+    print(f"ERROR        : {error_count}")
+    print(f"Timeouts     : {timeout_count}")
+    print(f"Packet Rate  : {sent / elapsed:.1f} pkt/s")
+
+    if sent:
+        print(f"Mean RTT     : {1000*rtt_sum/sent:.3f} ms")
+        print(f"Min RTT      : {1000*rtt_min:.3f} ms")
+        print(f"Max RTT      : {1000*rtt_max:.3f} ms")
+
+
+# ==========================================================
+# Main
+# ==========================================================
+
+def main():
+
+    global running
+
+    print(f"Opening {PORT}")
+
+    ser = serial.Serial(
+        PORT,
+        BAUDRATE,
+        timeout=0.01,
+    )
+
+    threading.Thread(
+        target=reader,
+        args=(ser,),
+        daemon=True,
+    ).start()
+
+    print("Running benchmark...")
+
+    benchmark(ser)
+
+    running = False
+
+    ser.close()
+
+
+if __name__ == "__main__":
+    main()

servo_controller/scripts/speed_test3.py

@@ -0,0 +1,384 @@
+#!/usr/bin/env python3
+"""
+max_command_speed_test.py
+
+Finds the maximum stable command rate for the UART command protocol described
+by `struct command_message_t`, using the LED command (sent with length=0,
+i.e. no data bytes -- only the 5-byte header) as the test payload.
+
+Wire format (matches the firmware's ring-buffer parser):
+
+    [prefix:1][length:1][id:1][command:1][crc:1][data:length]
+
+Note the firmware only ever reads `length` bytes of data off the wire (see
+`usb_thread`), NOT the full 160-byte `data[]` array from the struct -- the
+160 bytes only exist in RAM. So we must only transmit `length` bytes.
+
+CRC: sum of prefix+length+id+command+0(crc placeholder)+data[0..length-1],
+mod 256, then crc = (0x100 - sum) & 0xff. This matches
+`command_calculate_crc()`, which runs while msg->crc is still 0 (it's set
+by the caller only *after* this function returns).
+
+Two test modes:
+
+  roundtrip  - send one command, wait for ACK/NACK, repeat. Measures the
+               sustainable rate when the host waits for each reply
+               (safest / simplest way to drive the device).
+
+  flood      - send a whole burst of commands back-to-back at a fixed
+               inter-command delay, then collect all the ACK/NACK replies
+               afterwards. This stresses the UART IRQ handler + ring buffer
+               directly and finds the max raw throughput the firmware can
+               absorb without dropping/desyncing bytes.
+
+The script sweeps delays (roundtrip) or a binary search on delay (flood) to
+find the smallest stable delay (i.e. highest command rate) that still gets
+a correct ACK for every command sent.
+
+Requires: pip install pyserial
+"""
+
+import argparse
+import struct
+import sys
+import time
+
+import serial
+
+# ---- Protocol constants (from firmware) -----------------------------------
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+COMMAND_DATA_SIZE = 160
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED = 2
+
+HEADER_FMT = "<BBBBB"  # prefix, length, id, command, crc
+HEADER_LEN = struct.calcsize(HEADER_FMT)
+
+
+def calc_crc(length: int, command: int, data: bytes) -> int:
+    """Replicates command_calculate_crc(): sum over
+    [prefix, length, id, command, crc(=0 at calc time), data[:length]]"""
+    s = COMMAND_PREFIX + length + COMMAND_ID + command + 0
+    s += sum(data[:length])
+    return (0x100 - (s & 0xFF)) & 0xFF
+
+
+def build_packet(command: int, data: bytes = b"") -> bytes:
+    length = len(data)
+    if length > COMMAND_DATA_SIZE - 1:
+        raise ValueError(f"data too long: {length} > {COMMAND_DATA_SIZE - 1}")
+    crc = calc_crc(length, command, data)
+    return struct.pack(HEADER_FMT, COMMAND_PREFIX, length, COMMAND_ID, command, crc) + data
+
+
+def build_led_packet() -> bytes:
+    """LED command with no payload (length=0). Only the 5-byte header
+    [prefix][length=0][id][command][crc] is sent -- no data bytes at all."""
+    return build_packet(LED, b"")
+
+
+class ResponseError(Exception):
+    pass
+
+
+def read_response(ser: serial.Serial, timeout: float):
+    """Read one full response packet (ACK/NACK) from the wire, honoring
+    `timeout` seconds total. Returns dict with command/length/data/crc_ok,
+    or None on timeout."""
+    ser.timeout = timeout
+    deadline = time.monotonic() + timeout
+
+    # Scan for prefix byte
+    while True:
+        remaining = deadline - time.monotonic()
+        if remaining <= 0:
+            return None
+        ser.timeout = remaining
+        b = ser.read(1)
+        if not b:
+            return None
+        if b[0] == COMMAND_PREFIX:
+            break
+
+    remaining = deadline - time.monotonic()
+    if remaining <= 0:
+        return None
+    ser.timeout = remaining
+    hdr = ser.read(4)  # length, id, command, crc
+    if len(hdr) < 4:
+        return None
+    length, resp_id, command, crc = hdr
+
+    data = b""
+    if length:
+        remaining = deadline - time.monotonic()
+        if remaining <= 0:
+            return None
+        ser.timeout = remaining
+        data = ser.read(length)
+        if len(data) < length:
+            return None
+
+    expected_crc = calc_crc(length, command, data)
+    return {
+        "id": resp_id,
+        "command": command,
+        "length": length,
+        "data": data,
+        "crc": crc,
+        "crc_ok": crc == expected_crc,
+    }
+
+
+def send_led(ser: serial.Serial):
+    ser.write(build_led_packet())
+
+
+# ---- Test modes -------------------------------------------------------------
+
+def test_roundtrip(ser, delay, n, timeout):
+    """Send n commands, waiting for a valid ACK after each before sending
+    the next (with an *additional* `delay` seconds between send and next
+    send, on top of whatever the round-trip itself costs). Returns
+    (success_count, n, elapsed_seconds), where elapsed_seconds is the
+    *measured* wall-clock time for the whole trial -- not derived from
+    `delay`. This is what "rate" should be computed from, since at
+    delay=0 the round-trip latency (write + firmware processing + read)
+    is the real limiter, not some divide-by-zero fiction."""
+    ser.reset_input_buffer()
+    ok = 0
+    t_start = time.monotonic()
+    for i in range(n):
+        send_led(ser)
+        resp = read_response(ser, timeout)
+        if resp and resp["crc_ok"] and resp["command"] == COMMAND_ACK:
+            ok += 1
+        if delay:
+            time.sleep(delay)
+    elapsed = time.monotonic() - t_start
+    return ok, n, elapsed
+
+
+def test_flood(ser, delay, n, timeout):
+    """Send n commands back-to-back with only `delay` seconds between
+    sends (no waiting for replies in between), then collect n replies
+    afterward. Returns (success_count, n, elapsed_seconds).
+
+    elapsed_seconds covers the whole trial (send phase + collecting all
+    replies) as actually measured -- at delay=0 this still takes real,
+    nonzero time (write() syscalls, USB bulk transfer framing, the
+    firmware's IRQ handler + ring buffer + usb_thread all take time), so
+    this is the number that should be used to compute cmd/s, never
+    `1/delay`."""
+    ser.reset_input_buffer()
+    t_start = time.monotonic()
+    for i in range(n):
+        send_led(ser)
+        if delay:
+            time.sleep(delay)
+    send_duration = time.monotonic() - t_start
+
+    ok = 0
+    # give it time proportional to what we sent, plus per-reply timeout
+    end_deadline = time.monotonic() + timeout + send_duration
+    for i in range(n):
+        remaining = end_deadline - time.monotonic()
+        if remaining <= 0:
+            break
+        resp = read_response(ser, remaining)
+        if resp and resp["crc_ok"] and resp["command"] == COMMAND_ACK:
+            ok += 1
+        elif resp is None:
+            break
+    elapsed = time.monotonic() - t_start
+    return ok, n, elapsed
+
+
+def achieved_rate(ok, elapsed):
+    """cmd/s actually measured, based on successful commands over real
+    wall-clock time. Never derived from the requested delay."""
+    if elapsed <= 0:
+        return 0.0
+    return ok / elapsed
+
+
+# ---- Sweep / search logic ----------------------------------------------------
+
+def find_max_rate(ser, mode, n, timeout, success_threshold, start_delay, min_delay, verbose):
+    """Binary search the smallest stable inter-command delay that still
+    achieves >= success_threshold success ratio. Returns
+    (best_delay, measured_rate_at_best_delay).
+
+    Rate is always the *measured* cmd/s from the trial, never 1/delay --
+    at delay=0 the requested delay tells you nothing about the actual
+    ceiling, which is set by write()/USB overhead and firmware processing
+    time, not by our sleep() calls."""
+    test_fn = test_roundtrip if mode == "roundtrip" else test_flood
+    last_rate = 0.0
+
+    def trial(delay):
+        nonlocal last_rate
+        ok, total, elapsed = test_fn(ser, delay, n, timeout)
+        ratio = ok / total if total else 0.0
+        rate = achieved_rate(ok, elapsed)
+        last_rate = rate
+        if verbose:
+            print(f"  delay={delay*1000:8.3f} ms  measured_rate={rate:9.1f} cmd/s  "
+                  f"success={ok}/{total} ({ratio*100:5.1f}%)  elapsed={elapsed*1000:.1f} ms")
+        return ratio >= success_threshold
+
+    lo, hi = min_delay, start_delay
+    if not trial(hi):
+        print(f"WARNING: even the slow starting delay ({hi*1000:.3f} ms) failed "
+              f"the success threshold. Try increasing --start-delay.")
+        return hi, last_rate
+    hi_rate = last_rate
+
+    if trial(lo):
+        # Even the fastest requested delay was stable. That does NOT mean
+        # the rate is infinite -- it means we've hit the real ceiling
+        # (write()/USB/firmware), and last_rate is the measured number
+        # for it. Report that instead of pretending it's unbounded.
+        print(f"NOTE: even the fastest delay ({lo*1000:.3f} ms) passed, at a "
+              f"measured {last_rate:.1f} cmd/s. That's likely the true ceiling "
+              f"(write()/USB overhead + firmware processing), not an artifact "
+              f"of --min-delay. Run --soak to confirm it holds over a longer run.")
+        return lo, last_rate
+
+    # Binary search between lo (fails) and hi (passes) for smallest passing delay
+    best_rate = hi_rate
+    for _ in range(20):
+        mid = (lo + hi) / 2.0
+        if trial(mid):
+            hi = mid
+            best_rate = last_rate
+        else:
+            lo = mid
+        if hi - lo < 1e-5:  # 0.01 ms resolution
+            break
+
+    return hi, best_rate
+
+
+def soak_test(ser, mode, delay, duration_s, timeout):
+    """Run continuously at a fixed delay for `duration_s` seconds and
+    report the measured sustained cmd/s and success ratio. Use this to
+    confirm a delay=0 (or any) result actually holds up over time, since
+    a short burst can pass while a longer run reveals ring-buffer
+    overflow or drift."""
+    test_fn = test_roundtrip if mode == "roundtrip" else test_flood
+    t_start = time.monotonic()
+    total_ok = 0
+    total_n = 0
+    # run in chunks so we can report progress and stop at duration_s
+    chunk = 200
+    while time.monotonic() - t_start < duration_s:
+        ok, n, elapsed = test_fn(ser, delay, chunk, timeout)
+        total_ok += ok
+        total_n += n
+        rate = achieved_rate(total_ok, time.monotonic() - t_start)
+        print(f"  soak: {total_ok}/{total_n} ok so far, "
+              f"sustained rate={rate:.1f} cmd/s, "
+              f"t={time.monotonic() - t_start:5.1f}s / {duration_s}s")
+    elapsed = time.monotonic() - t_start
+    ratio = total_ok / total_n if total_n else 0.0
+    rate = achieved_rate(total_ok, elapsed)
+    return total_ok, total_n, ratio, rate
+
+
+def sweep(ser, mode, n, timeout, delays, verbose=True):
+    test_fn = test_roundtrip if mode == "roundtrip" else test_flood
+    results = []
+    print(f"\n{'delay (ms)':>12} {'measured cmd/s':>16} {'success':>10} {'ratio':>8}")
+    print("-" * 52)
+    for delay in delays:
+        ok, total, elapsed = test_fn(ser, delay, n, timeout)
+        ratio = ok / total if total else 0.0
+        rate = achieved_rate(ok, elapsed)  # measured, never 1/delay
+        print(f"{delay*1000:12.3f} {rate:16.1f} {ok:>4}/{total:<5} {ratio*100:7.1f}%")
+        results.append((delay, ok, total, ratio, rate))
+    return results
+
+
+def main():
+    ap = argparse.ArgumentParser(description=__doc__,
+                                  formatter_class=argparse.RawDescriptionHelpFormatter)
+    ap.add_argument("--port", default="/dev/ttyACM0")
+    ap.add_argument("--baud", type=int, default=115200,
+                     help="Baud rate (often ignored by USB CDC-ACM, but set for compatibility)")
+    ap.add_argument("--mode", choices=["roundtrip", "flood"], default="roundtrip",
+                     help="roundtrip: wait for ACK after each send. "
+                          "flood: send a burst, then collect replies (tests raw UART/ring-buffer throughput)")
+    ap.add_argument("-n", type=int, default=200, help="Commands per trial")
+    ap.add_argument("--timeout", type=float, default=0.5, help="Per-response read timeout (s)")
+    ap.add_argument("--success-threshold", type=float, default=1.0,
+                     help="Required success ratio to call a rate 'stable' (0-1)")
+    ap.add_argument("--start-delay", type=float, default=0.02,
+                     help="Slow starting inter-command delay in seconds for the search (known-good)")
+    ap.add_argument("--min-delay", type=float, default=0.0,
+                     help="Fastest inter-command delay to try, in seconds (0 = back-to-back)")
+    ap.add_argument("--sweep", action="store_true",
+                     help="Also print a full table sweeping delays geometrically "
+                          "from --start-delay down to --min-delay-floor")
+    ap.add_argument("--min-delay-floor", type=float, default=0.0005,
+                     help="Smallest delay used for --sweep table (s)")
+    ap.add_argument("--sweep-steps", type=int, default=12)
+    ap.add_argument("--soak", type=float, default=0.0,
+                     help="After the search, run this many seconds at the best "
+                          "delay found to confirm the rate holds up over time "
+                          "(recommended, especially when the best delay is 0)")
+    args = ap.parse_args()
+
+    print(f"Opening {args.port} @ {args.baud} baud, mode={args.mode}, n={args.n}/trial")
+    ser = serial.Serial(args.port, args.baud, timeout=args.timeout)
+    time.sleep(0.2)  # let the port settle
+    ser.reset_input_buffer()
+    ser.reset_output_buffer()
+
+    try:
+        if args.sweep:
+            delays = []
+            hi, lo = args.start_delay, args.min_delay_floor
+            steps = max(args.sweep_steps, 1)
+            for i in range(steps):
+                frac = i / (steps - 1) if steps > 1 else 0
+                # geometric interpolation from hi -> lo
+                d = hi * ((lo / hi) ** frac) if hi > 0 else 0
+                delays.append(d)
+            sweep(ser, args.mode, args.n, args.timeout, delays)
+            print()
+
+        print("Binary-searching for max stable command rate...")
+        best_delay, best_rate = find_max_rate(
+            ser, args.mode, args.n, args.timeout,
+            args.success_threshold, args.start_delay, args.min_delay,
+            verbose=True,
+        )
+
+        print("\n=== Result ===")
+        print(f"Max stable inter-command delay: {best_delay*1000:.3f} ms")
+        print(f"Measured rate at that delay: {best_rate:.1f} commands/sec "
+              f"(measured from actual elapsed time over {args.n} commands, "
+              f"not derived from the delay)")
+        print(f"(mode={args.mode}, n={args.n}, success_threshold={args.success_threshold*100:.0f}%)")
+
+        if args.soak > 0:
+            print(f"\nRunning {args.soak:.0f}s soak test at delay={best_delay*1000:.3f} ms "
+                  f"to confirm this holds up over time...")
+            ok, total, ratio, rate = soak_test(ser, args.mode, best_delay, args.soak, args.timeout)
+            print(f"\nSoak result: {ok}/{total} ok ({ratio*100:.1f}%), "
+                  f"sustained rate={rate:.1f} cmd/s over {args.soak:.0f}s")
+            if ratio < args.success_threshold:
+                print("WARNING: the short trial passed but the soak test did NOT hold up -- "
+                      "the real stable rate is lower than reported above. Try a slower "
+                      "--start-delay / larger --min-delay and re-run.")
+
+    finally:
+        ser.close()
+
+
+if __name__ == "__main__":
+    main()

servo_controller/src/command_handler.c

@@ -0,0 +1,47 @@
+#include "command_handler.h"
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
+
+int command_handler(struct command_message_t *msg) {
+	if (msg == NULL) {
+		LOG_ERR("Received NULL message pointer");
+		return -EINVAL;
+	}
+
+	LOG_DBG("Processing command: %d, length: %d", msg->command, msg->length);
+
+	switch (msg->command) {
+		case LED_TOGGLE: {
+			// Toggle LED
+			int led = msg->data[0];
+
+			if (!led) {
+				led0_toggle();
+			}
+			else {
+				led1_toggle();
+			}
+			break;
+		}
+
+		case LED_SET: {
+			// Set LED
+			int led = msg->data[0];
+			int state = msg->data[1];
+
+			led_set(led, state);
+
+			break;
+		}
+
+		default: {
+			LOG_WRN("Unknown command received: %d", msg->command);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}

servo_controller/src/command_handler.h

@@ -0,0 +1,16 @@
+#ifndef COMMAND_HANDLER_H
+#define COMMAND_HANDLER_H
+
+
+#include "command_message.h"
+
+/**
+ * @brief Process received command message
+ * 
+ * @param msg Command message to process
+ * @return 0 on success, negative errno on failure
+ */
+int command_handler(struct command_message_t *msg);
+
+
+#endif // COMMAND_HANDLER_H

servo_controller/src/command_message.c

@@ -0,0 +1,75 @@
+#include "command_message.h"
+
+#include <string.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
+
+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;
+}
+
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
+	// Ensure length doesn't exceed available space
+	if (length > sizeof(msg->data) - 1) {
+		return;
+	}
+
+	command_message_init(msg);
+	msg->length = length;
+	msg->command = command;
+
+	// Copy the data
+	if (data != NULL) {
+		for (int i = 0; i < msg->length; i++) {
+			msg->data[i] = data[i];
+		}
+	}
+	msg->crc = command_calculate_crc(msg);
+}
+
+uint8_t command_calculate_crc(struct command_message_t *msg) {
+	uint32_t sum = 0;
+	uint8_t crc = 0;
+
+	uint8_t *byte_ptr = (uint8_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; }
+		sum += byte_ptr[i];
+	}
+
+	crc = 0x100 - (sum & 0xff);
+
+	return crc;
+}
+
+void command_create_ack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_ACK, NULL);
+}
+
+void command_create_nack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_NACK, NULL);
+}
+
+void command_log(struct command_message_t *msg) {
+	if (msg->length > sizeof(msg->data) - 1) {
+		LOG_ERR("Message length too long: %d", msg->length);
+		return;
+	}
+
+	LOG_INF("Prefix: %d\n\r", msg->prefix);
+	LOG_INF("Length: %d\n\r", msg->length);
+	LOG_INF("COMMAND_ID: %d\n\r", msg->id);
+	LOG_INF("Command: %d\n\r", msg->command);
+	LOG_INF("Data:\n\r");
+
+	for (int i = 0; i < msg->length; i++) {
+		LOG_INF("%d", msg->data[i]);
+	}
+
+	LOG_INF("CRC: %d\n\r", msg->crc);
+}

servo_controller/src/command_message.h

@@ -0,0 +1,75 @@
+#ifndef COMMAND_MESSAGE_H
+#define COMMAND_MESSAGE_H
+
+
+#include <stdint.h>
+
+
+#define COMMAND_PREFIX 0x69
+#define COMMAND_ID 0x00
+#define COMMAND_DATA_SIZE 160
+
+typedef enum {
+	COMMAND_ACK,
+	COMMAND_NACK,
+	LED_TOGGLE,
+	LED_SET,
+} commands_e;
+
+struct command_message_t {
+	uint8_t prefix;
+	uint8_t length;
+	uint8_t id;
+	uint8_t command;
+	uint8_t crc;
+	uint8_t data[COMMAND_DATA_SIZE];
+} __attribute__((packed));
+
+/**
+ * @brief Initialize command message to default state
+ * 
+ * @param msg Message to initialize
+ */
+void command_message_init(struct command_message_t *msg);
+
+/**
+ * @brief Create command message with data and CRC
+ * 
+ * @param msg Message to populate
+ * @param length Data length in bytes
+ * @param command Command type
+ * @param data Data payload
+ */
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]);
+
+/**
+ * @brief Calculate CRC for command message
+ * 
+ * @param msg Message to calculate CRC for
+ * @return CRC value
+ */
+uint8_t command_calculate_crc(struct command_message_t *msg);
+
+/**
+ * @brief Create ACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_ack(struct command_message_t *msg);
+
+/**
+ * @brief Create NACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_nack(struct command_message_t *msg);
+
+/**
+ * @brief Print the command with LOG
+ * 
+ * @param msg Message to calculate CRC for
+ */
+void command_log(struct command_message_t *msg);
+
+
+#endif // COMMAND_MESSAGE_H

servo_controller/src/led.c

@@ -0,0 +1,81 @@
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/gpio.h>
+
+LOG_MODULE_REGISTER(led, LOG_LEVEL_INF);
+
+static const struct gpio_dt_spec led0_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
+static const struct gpio_dt_spec led1_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
+static int led_states[2] = {0, 0};
+
+static int pin_init(const struct gpio_dt_spec *dt) {
+	int ret;
+
+	if (!device_is_ready(dt->port)) {
+		LOG_ERR("LED0 GPIO device not ready");
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_configure_dt(dt, GPIO_OUTPUT_INACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure LED0 GPIO: %d", ret);
+		return ret;
+	}
+
+	// Turn it off
+	ret = gpio_pin_set_dt(dt, 0);
+	if (ret != 0) {
+		LOG_ERR("Failed to initialize LED");
+		return ret;
+	}
+
+	LOG_INF("LED driver initialized (GPIO %d)", dt->pin);
+
+	return 0;
+}
+
+int led_init() {
+	int ret;
+
+	ret = pin_init(&led0_gpio);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = pin_init(&led1_gpio);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+int led_set(int led, int state) {
+	int ret;
+	int set_led = led ? 1 : 0;
+	led_states[set_led] = state ? 1 : 0;
+
+	if (!set_led) {
+		ret = gpio_pin_set_dt(&led0_gpio, led_states[set_led]);
+		if (ret != 0) {
+			LOG_ERR("Failed to set LED0: %d", ret);
+		}
+	}
+	else {
+		ret = gpio_pin_set_dt(&led1_gpio, led_states[set_led]);
+		if (ret != 0) {
+			LOG_ERR("Failed to set LED1: %d", ret);
+		}
+	}
+
+	return ret;
+}
+
+int led0_toggle() {
+	return led_set(0, !led_states[0]);
+}
+
+int led1_toggle() {
+	return led_set(1, !led_states[1]);
+}

servo_controller/src/led.h

@@ -0,0 +1,11 @@
+#ifndef LED_H
+#define LED_H
+
+
+int led_init();
+int led_set(int led, int state);
+int led0_toggle();
+int led1_toggle();
+
+
+#endif // LED_H

servo_controller/src/main.c

@@ -0,0 +1,24 @@
+#include "led.h"
+#include "usb.h"
+
+#include <zephyr/logging/log.h>
+LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
+
+
+int main(void) {
+	int ret;
+
+	ret = usb_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable USB");
+		return 0;
+	}
+
+	ret = led_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable LED");
+		return 0;
+	}
+
+	return 0;
+}

servo_controller/src/servo.c

@@ -0,0 +1,37 @@
+#include "servo.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/pwm.h>
+
+LOG_MODULE_REGISTER(servo, LOG_LEVEL_INF);
+
+// ----------- SERVOS -----------
+static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
+	PWM_DT_SPEC_GET(DT_ALIAS(servo1)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo2)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo3)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo4)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo5)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo6)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo7)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo8)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo9)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo10)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo11)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo12)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo13)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo14)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo15)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo16)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo17)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo18)),
+};
+
+
+static int pin_init(const ) {
+
+}
+
+int servo_init() {
+
+}

servo_controller/src/servo.h

@@ -0,0 +1,13 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+
+
+#define NUM_SERVO_CHANNELS 18
+
+
+
+int servo_init();
+
+
+#endif // SERVO_H

servo_controller/src/usb.c

@@ -0,0 +1,231 @@
+#include "usb.h"
+#include "usb_conf.h"
+#include "command_handler.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/uart.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/ring_buffer.h>
+
+// PICO-SDK
+#include "pico/bootrom.h"
+
+LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
+
+// DEVICE
+const struct device *const uart_dev = DEVICE_DT_GET_ONE(zephyr_cdc_acm_uart);
+static struct usbd_context *usb_context;
+
+// THREAD
+static struct k_thread usb_thread_data;
+static k_tid_t usb_thread_id = NULL;
+#define USB_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(usb_thread_stack, USB_THREAD_STACK_SIZE);
+
+// RX BUFFER
+#define RING_BUF_SIZE 255
+static uint8_t ring_buffer[RING_BUF_SIZE];
+static struct ring_buf ringbuf;
+struct k_sem rx_semaphore;
+
+// ACK / NACK messages
+#define RETURN_ACK true
+struct command_message_t ack_msg;
+struct command_message_t nack_msg;
+
+
+static void interrupt_handler(const struct device *dev, void *user_data) {
+	ARG_UNUSED(user_data);
+
+	while (true) {
+		uart_irq_update(dev);
+
+		if (uart_irq_is_pending(dev) <= 0) {
+			break;
+		}
+
+		if (uart_irq_rx_ready(dev)) {
+			int recv_len, rb_len;
+			uint8_t buffer[64];
+			size_t len = MIN(ring_buf_space_get(&ringbuf), sizeof(buffer));
+
+			if (len == 0) {
+				// ring buffer full, drops package(s)
+			    uart_irq_rx_disable(dev);
+				k_sem_give(&rx_semaphore);
+				break;
+			}
+
+			recv_len = uart_fifo_read(dev, buffer, len);
+			if (recv_len < 0) {
+				LOG_ERR("Failed to read UART FIFO");
+				recv_len = 0;
+			};
+
+			rb_len = ring_buf_put(&ringbuf, buffer, recv_len);
+			if (rb_len < recv_len) {
+				LOG_ERR("Drop %u bytes", recv_len - rb_len);
+			}
+
+			k_sem_give(&rx_semaphore);
+		}
+	}
+}
+
+static void usb_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	struct command_message_t msg;
+	command_message_init(&msg);
+	
+	LOG_INF("USB command processing thread started");
+	
+	while (1) {
+		k_sem_take(&rx_semaphore, K_FOREVER);
+
+		int len;
+
+		// While ring buffer has data
+		do {
+			uint8_t buf_prefix;
+			len = ring_buf_get(&ringbuf, &buf_prefix, 1);
+
+			if (len && (buf_prefix == COMMAND_PREFIX)) {
+				uint8_t buf_header[4];
+				len = ring_buf_get(&ringbuf, buf_header, 4);
+
+				if ((len == 4) && (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];
+
+					if (msg.length) {
+						len = ring_buf_get(&ringbuf, msg.data, msg.length);
+					}
+
+					uint8_t calculated_crc = command_calculate_crc(&msg);
+					if (calculated_crc != msg.crc) {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+						continue;
+					}
+
+					int ret = command_handler(&msg);
+					if (ret == 0) {
+						if (RETURN_ACK) {
+							// Send ACK
+							usb_send_command(&ack_msg);
+						}
+					}
+					else {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+					}
+
+				}
+				else {
+					// Command_id did not match, ignore
+					continue;
+				}
+			}
+			else {
+				// Prefix did not match, ignore
+				continue;
+			}
+		} while (len > 0);
+
+		uart_irq_rx_enable(uart_dev);
+	}
+	
+	LOG_INF("USB command processing thread exiting");
+}
+
+static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *msg) {
+	if (msg->type == USBD_MSG_CDC_ACM_LINE_CODING) {
+		// Jump to BOOTSEL when baudrate changes to 1200
+		uint32_t baudrate;
+
+		if (uart_line_ctrl_get(msg->dev, UART_LINE_CTRL_BAUD_RATE, &baudrate) == 0) {
+			LOG_INF("Baudrate %u", baudrate);
+
+			if (baudrate == 1200) {
+				LOG_INF("Entering BOOTSEL...");
+				reset_usb_boot(0, 0);
+			}
+		}
+	}
+}
+
+int usb_init() {
+	ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
+	k_sem_init(&rx_semaphore, 0, 1);
+
+	command_create_ack(&ack_msg);
+	command_create_nack(&nack_msg);
+
+	int ret;
+
+	if (!device_is_ready(uart_dev)) {
+		LOG_ERR("CDC ACM device not ready");
+		return -ENODEV;
+	}
+
+	usb_context = usb_device_init(usb_msg_cb);
+	if (usb_context == NULL) {
+		LOG_ERR("Failed to initialize USB device");
+		return -ENODEV;
+	}
+
+	if (!usbd_can_detect_vbus(usb_context)) {
+		ret = usbd_enable(usb_context);
+		if (ret) {
+			LOG_ERR("Failed to enable device support");
+			return ret;
+		}
+	}
+	k_msleep(100);
+
+	uart_irq_callback_set(uart_dev, interrupt_handler);
+	uart_irq_rx_enable(uart_dev);
+
+	usb_thread_id = k_thread_create(
+		&usb_thread_data,
+		usb_thread_stack,
+		K_THREAD_STACK_SIZEOF(usb_thread_stack),
+		usb_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (usb_thread_id == NULL) {
+		LOG_ERR("Failed to create USB thread");
+		return -ENOMEM;
+	}
+
+	return ret;
+}
+
+int usb_send_command(struct command_message_t *msg) {
+	if (!device_is_ready(uart_dev)) {
+		return -ENODEV;
+	}
+
+	// Message size: prefix + length + id + command + crc + data
+	size_t msg_size = 5 + msg->length;
+	uint8_t *msg_bytes = (uint8_t *)msg;
+
+	/* uart_poll_out blocks until sent, ensuring data integrity */
+	for (size_t i = 0; i < msg_size; i++) {
+		uart_poll_out(uart_dev, msg_bytes[i]);
+	}
+
+	return 0;
+}

servo_controller/src/usb.h

@@ -0,0 +1,12 @@
+#ifndef USB_H
+#define USB_H
+
+
+#include "command_message.h"
+
+
+int usb_init();
+int usb_send_command(struct command_message_t *msg);
+
+
+#endif // USB_H

servo_controller/src/usb_conf.c

@@ -0,0 +1,184 @@
+#include "usb_conf.h"
+
+#include <stdint.h>
+#include <zephyr/device.h>
+#include <zephyr/usb/usbd.h>
+#include <zephyr/usb/bos.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(usb_conf, LOG_LEVEL_DBG);
+
+/* By default, do not register the USB DFU class DFU mode instance. */
+static const char *const blocklist[] = {
+	"dfu_dfu",
+	NULL,
+};
+
+/*
+ * Instantiate a context named my_usb_context using the default USB device
+ * controller, the Zephyr project vendor ID, and the sample product ID.
+ * Zephyr project vendor ID must not be used outside of Zephyr samples.
+ */
+USBD_DEVICE_DEFINE(my_usb_context,
+		   DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)),
+		   CONFIG_USBD_VID, CONFIG_USBD_PID);
+
+USBD_DESC_LANG_DEFINE(my_usb_lang);
+USBD_DESC_MANUFACTURER_DEFINE(my_usb_mfr, CONFIG_USBD_MANUFACTURER);
+USBD_DESC_PRODUCT_DEFINE(my_usb_product, CONFIG_USBD_PRODUCT);
+IF_ENABLED(CONFIG_HWINFO, (USBD_DESC_SERIAL_NUMBER_DEFINE(my_usb_serial)));
+
+USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration");
+USBD_DESC_CONFIG_DEFINE(hs_cfg_desc, "HS Configuration");
+
+static const uint8_t attributes = (IS_ENABLED(CONFIG_USBD_SELF_POWERED) ?
+				   USB_SCD_SELF_POWERED : 0) |
+				  (IS_ENABLED(CONFIG_USBD_REMOTE_WAKEUP) ?
+				   USB_SCD_REMOTE_WAKEUP : 0);
+
+/* Full speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_fs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &fs_cfg_desc);
+
+/* High speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_hs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &hs_cfg_desc);
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+/*
+ * This does not yet provide valuable information, but rather serves as an
+ * example, and will be improved in the future.
+ */
+static const struct usb_bos_capability_lpm bos_cap_lpm = {
+	.bLength = sizeof(struct usb_bos_capability_lpm),
+	.bDescriptorType = USB_DESC_DEVICE_CAPABILITY,
+	.bDevCapabilityType = USB_BOS_CAPABILITY_EXTENSION,
+	.bmAttributes = 0UL,
+};
+
+USBD_DESC_BOS_DEFINE(my_usb_usbext, sizeof(bos_cap_lpm), &bos_cap_lpm);
+#endif
+
+static void usb_fix_code_triple(struct usbd_context *uds_ctx, const enum usbd_speed speed) {
+	/* Always use class code information from Interface Descriptors */
+	if (IS_ENABLED(CONFIG_USBD_CDC_ACM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_ECM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_NCM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_MIDI2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_AUDIO2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_VIDEO_CLASS)) {
+		/*
+		 * Class with multiple interfaces have an Interface
+		 * Association Descriptor available, use an appropriate triple
+		 * to indicate it.
+		 */
+		usbd_device_set_code_triple(uds_ctx, speed,
+					    USB_BCC_MISCELLANEOUS, 0x02, 0x01);
+	} else {
+		usbd_device_set_code_triple(uds_ctx, speed, 0, 0, 0);
+	}
+}
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_lang);
+	if (err) {
+		LOG_ERR("Failed to initialize language descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_mfr);
+	if (err) {
+		LOG_ERR("Failed to initialize manufacturer descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_product);
+	if (err) {
+		LOG_ERR("Failed to initialize product descriptor (%d)", err);
+		return NULL;
+	}
+
+	IF_ENABLED(CONFIG_HWINFO, (
+		err = usbd_add_descriptor(&my_usb_context, &my_usb_serial);
+	))
+	if (err) {
+		LOG_ERR("Failed to initialize SN descriptor (%d)", err);
+		return NULL;
+	}
+
+	if (USBD_SUPPORTS_HIGH_SPEED &&
+	    usbd_caps_speed(&my_usb_context) == USBD_SPEED_HS) {
+		err = usbd_add_configuration(&my_usb_context, USBD_SPEED_HS,
+					     &sample_hs_config);
+		if (err) {
+			LOG_ERR("Failed to add High-Speed configuration");
+			return NULL;
+		}
+
+		err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_HS, 1,
+						blocklist);
+		if (err) {
+			LOG_ERR("Failed to add register classes");
+			return NULL;
+		}
+
+		usb_fix_code_triple(&my_usb_context, USBD_SPEED_HS);
+	}
+
+	err = usbd_add_configuration(&my_usb_context, USBD_SPEED_FS,
+				     &sample_fs_config);
+	if (err) {
+		LOG_ERR("Failed to add Full-Speed configuration");
+		return NULL;
+	}
+
+	err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_FS, 1, blocklist);
+	if (err) {
+		LOG_ERR("Failed to add register classes");
+		return NULL;
+	}
+
+	usb_fix_code_triple(&my_usb_context, USBD_SPEED_FS);
+	usbd_self_powered(&my_usb_context, attributes & USB_SCD_SELF_POWERED);
+
+	if (msg_cb != NULL) {
+		err = usbd_msg_register_cb(&my_usb_context, msg_cb);
+		if (err) {
+			LOG_ERR("Failed to register message callback");
+			return NULL;
+		}
+	}
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_FS, 0x0201);
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_HS, 0x0201);
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_usbext);
+	if (err) {
+		LOG_ERR("Failed to add USB 2.0 Extension Descriptor");
+		return NULL;
+	}
+#endif
+
+	return &my_usb_context;
+}
+
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	if (usb_device_setup(msg_cb) == NULL) {
+		return NULL;
+	}
+
+	err = usbd_init(&my_usb_context);
+	if (err) {
+		LOG_ERR("Failed to initialize device support");
+		return NULL;
+	}
+
+	return &my_usb_context;
+}

servo_controller/src/usb_conf.h

@@ -0,0 +1,11 @@
+#ifndef USB_CONF_H
+#define USB_CONF_H
+
+
+#include <zephyr/usb/usbd.h>
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb);
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb);
+
+
+#endif // USB_CONF_H