Updated project plan

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/app.overlay

@@ -0,0 +1,9 @@
+&i2c0 {
+	power_monitor: ina3221@40 {
+		compatible = "ti,ina3221";
+		reg = <0x40>;
+		status = "okay";
+
+		shunt-resistors = <100>, <100>, <100>;
+	};
+};

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

@@ -1,6 +1,14 @@
 CONFIG_GPIO=y
 CONFIG_PWM=y
 CONFIG_ADC=y
+CONFIG_I2C=y
+
+# INA3221
+CONFIG_SENSOR=y
+CONFIG_INA3221=y
+
+# Mux
+CONFIG_CD74HC4067=y
 
 # Serial
 CONFIG_SERIAL=y
@@ -29,4 +37,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=y		# Create disassmebly files
+#CONFIG_OUTPUT_DISASSEMBLE_ALL=n

servo_controller/scripts/plot_channels.py

@@ -0,0 +1,371 @@
+#!/usr/bin/env python3
+"""Plot decoded command payloads from a serial_logger.py CSV, dark mode.
+
+Each command gets its own subplot, stacked vertically in one window, in the
+order given by --commands (default: ADC_READ_ALL on top, POWER_MONITOR_READ
+below it). X-axis is the device-clock `tick_ms` column by default, or
+host_time.
+
+--------------------------------------------------------------------------
+TO ADD OR CHANGE A DATA FORMAT: edit the FORMATS dict below. Each entry is
+a DataFormat(...) describing how to turn that command's raw data_hex bytes
+into a list of (label, value) channels for one subplot. See the two
+existing entries for examples: a plain packed-int array (ADC_READ_ALL) and
+a decoded struct with scaling (POWER_MONITOR_READ, Zephyr sensor_value).
+--------------------------------------------------------------------------
+"""
+
+import argparse
+import csv
+import struct
+import sys
+from dataclasses import dataclass, field, replace
+from datetime import datetime
+from typing import Callable, List, Sequence
+
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.dates as mdates
+
+
+# --------------------------------------------------------------------------
+# Data formats: command name -> how to decode its payload for plotting.
+# This is the "field" to edit when a new command/payload format shows up.
+# --------------------------------------------------------------------------
+
+@dataclass
+class DataFormat:
+	byte_length: int                        # expected payload length in bytes
+	decode: Callable[[bytes], Sequence[float]]  # raw payload -> one float per channel
+	labels: List[str]                       # one label per decoded value, same order as decode()
+	ylabel: str = "value"
+	title: str = ""                         # subplot title; defaults to the command name
+	# Optional: if the labels are `group_count` groups of `group_size` each
+	# (e.g. 3 power channels x 3 metrics, grouped channel-major), the plot
+	# uses one color per group and a different line style per item within
+	# a group, instead of a flat colormap over every label.
+	group_count: int = 0
+	group_size: int = 0
+
+
+def decode_int32_array(n: int, signed: bool = True) -> Callable[[bytes], Sequence[float]]:
+	"""N packed little-endian 4-byte integers, back to back."""
+	fmt = f"<{n}{'i' if signed else 'I'}"
+
+	def _decode(payload: bytes) -> Sequence[float]:
+		return struct.unpack(fmt, payload)
+
+	return _decode
+
+
+def decode_sensor_value_grid(num_channels: int, metrics: List[str]) -> Callable[[bytes], Sequence[float]]:
+	"""Decode `struct sensor_value values[num_channels][len(metrics)]`,
+	memcpy'd channel-major (matches `values[0][0], values[0][1], ...,
+	values[1][0], ...` in the C code). Each sensor_value is
+	`{int32_t val1; int32_t val2;}`; real value = val1 + val2 * 1e-6
+	(val2 is the fractional part, in millionths)."""
+	n = num_channels * len(metrics)
+
+	def _decode(payload: bytes) -> Sequence[float]:
+		raw = struct.unpack(f"<{n * 2}i", payload)
+		return [raw[2 * i] + raw[2 * i + 1] * 1e-6 for i in range(n)]
+
+	return _decode
+
+
+def grid_labels(num_channels: int, metrics: List[str]) -> List[str]:
+	return [f"Ch{ch + 1} {metric}" for ch in range(num_channels) for metric in metrics]
+
+
+POWER_MONITOR_METRICS = ["Voltage (V)", "Current (A)", "Power (W)"]
+POWER_MONITOR_CHANNELS = 3
+
+FORMATS = {
+	"ADC_READ_ALL": DataFormat(
+		byte_length=18 * 4,
+		decode=decode_int32_array(18, signed=True),
+		labels=[f"angle{i}" for i in range(18)],
+		ylabel="value (int32)",
+		title="Angles",
+	),
+	"POWER_MONITOR_READ": DataFormat(
+		# 3 channels x 3 sensor_value structs x 8 bytes each (int32 val1 + int32 val2)
+		byte_length=POWER_MONITOR_CHANNELS * len(POWER_MONITOR_METRICS) * 8,
+		decode=decode_sensor_value_grid(POWER_MONITOR_CHANNELS, POWER_MONITOR_METRICS),
+		labels=grid_labels(POWER_MONITOR_CHANNELS, POWER_MONITOR_METRICS),
+		ylabel="value",
+		title="Power Monitor",
+		group_count=POWER_MONITOR_CHANNELS,
+		group_size=len(POWER_MONITOR_METRICS),
+	),
+}
+
+DEFAULT_COMMANDS = ["ADC_READ_ALL", "POWER_MONITOR_READ"]
+
+
+def fallback_format(byte_length: int, signed: bool = True) -> DataFormat:
+	"""Used for a --commands entry with no FORMATS registry entry: assume a
+	plain packed int32 array and size it from the data actually seen."""
+	n = byte_length // 4
+	return DataFormat(
+		byte_length=byte_length,
+		decode=decode_int32_array(n, signed=signed),
+		labels=[f"ch{i}" for i in range(n)],
+		ylabel="value (int32)" if signed else "value (uint32)",
+	)
+
+
+# --------------------------------------------------------------------------
+# CSV loading
+# --------------------------------------------------------------------------
+
+def load_rows(path: str, command: str):
+	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 row.get("command") != command:
+				continue
+			rows.append(row)
+	return rows
+
+
+def build_series(rows, fmt: DataFormat, x_source: str):
+	xs = []
+	channels = [[] for _ in fmt.labels]
+
+	for row in rows:
+		data_hex = row.get("data_hex", "")
+		if len(data_hex) != fmt.byte_length * 2:  # 2 hex chars per byte
+			continue  # skip rows that don't match this format's expected size
+
+		if x_source == "tick":
+			xs.append(int(row["tick_ms"]))
+		else:
+			xs.append(datetime.fromisoformat(row["host_time"]))
+
+		values = fmt.decode(bytes.fromhex(data_hex))
+		for i, v in enumerate(values):
+			channels[i].append(v)
+
+	if not xs:
+		return xs, channels
+
+	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]
+	return xs, channels
+
+
+# --------------------------------------------------------------------------
+# Plotting
+# --------------------------------------------------------------------------
+
+LINESTYLES = ["-", "--", ":", "-."]
+
+
+def plot_subplot(ax, xs, channels, fmt: DataFormat, command: str, x_source: str):
+	if fmt.group_count and fmt.group_size and fmt.group_count * fmt.group_size == len(fmt.labels):
+		# One color per group (e.g. per channel), one line style per item
+		# within the group (e.g. per metric), so related lines are easy
+		# to tell apart at a glance instead of 9 near-identical hues.
+		# Index the discrete tab10 swatches directly (not .resampled(),
+		# which interpolates between them and can produce near-duplicate
+		# muddy colors for small group counts).
+		tab10 = matplotlib.colormaps["tab10"].colors
+		for i, label in enumerate(fmt.labels):
+			g, m = divmod(i, fmt.group_size)
+			ax.plot(xs, channels[i], label=label, color=tab10[g % len(tab10)],
+			        linestyle=LINESTYLES[m % len(LINESTYLES)], linewidth=1.3)
+	else:
+		cmap = matplotlib.colormaps["tab20"].resampled(max(len(fmt.labels), 1))
+		for i, label in enumerate(fmt.labels):
+			ax.plot(xs, channels[i], label=label, color=cmap(i), linewidth=1.2)
+
+	ax.set_title(fmt.title or command, fontsize=12, color="white", loc="left")
+	ax.set_ylabel(fmt.ylabel)
+	ax.grid(True, alpha=0.25)
+
+	if x_source == "host":
+		ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S"))
+
+	if fmt.group_count and fmt.group_size and fmt.group_count * fmt.group_size == len(fmt.labels):
+		ncol = fmt.group_size  # one column per metric, one row per channel/group
+	else:
+		ncol = min(6, max(len(fmt.labels), 1))
+	ax.legend(ncol=ncol, fontsize=8, loc="upper center",
+	          bbox_to_anchor=(0.5, -0.15), frameon=False)
+
+
+def parse_index_list(text: str) -> List[int]:
+	"""'0,2,5' or '0-3,7' -> [0,2,5] / [0,1,2,3,7]."""
+	indices = []
+	for part in text.split(","):
+		part = part.strip()
+		if not part:
+			continue
+		if "-" in part:
+			a, b = part.split("-", 1)
+			indices.extend(range(int(a), int(b) + 1))
+		else:
+			indices.append(int(part))
+	return indices
+
+
+def select_channels(fmt: DataFormat, channels, keep_indices: List[int]) -> "tuple[DataFormat, list]":
+	"""Return a (fmt, channels) pair restricted to keep_indices, preserving
+	group_count/group_size for the grouped coloring in plot_subplot if the
+	selection still divides evenly (e.g. picking a subset of metrics but
+	keeping every channel)."""
+	labels2 = [fmt.labels[i] for i in keep_indices]
+	channels2 = [channels[i] for i in keep_indices]
+
+	group_count, group_size = fmt.group_count, fmt.group_size
+	if group_count and group_size:
+		# If we kept the same subset of positions-within-group for every
+		# group (e.g. "voltage only" for all 3 channels), grouping still
+		# applies with a smaller group_size. Otherwise drop grouping.
+		per_group = [[] for _ in range(group_count)]
+		ok = True
+		for i in keep_indices:
+			g, m = divmod(i, group_size)
+			if g >= group_count:
+				ok = False
+				break
+			per_group[g].append(m)
+		if ok and len(set(tuple(p) for p in per_group if p)) <= 1 and all(per_group[0] == p for p in per_group if p):
+			group_size = len(per_group[0]) if per_group[0] else 0
+		else:
+			group_count, group_size = 0, 0
+	return replace(fmt, labels=labels2, group_count=group_count, group_size=group_size), channels2
+
+
+def build_parser():
+	p = argparse.ArgumentParser(
+		description="Plot decoded command payloads from a serial_logger.py CSV (dark mode).",
+		formatter_class=argparse.RawDescriptionHelpFormatter,
+		epilog="""\
+To add/change how a command's data is decoded, edit the FORMATS dict at the
+top of this script (see decode_int32_array / decode_power_monitor).
+
+examples:
+  %(prog)s -i log.csv
+  %(prog)s -i log.csv -o plot.png
+  %(prog)s -i log.csv --commands ADC_READ_ALL
+  %(prog)s -i log.csv --commands POWER_MONITOR_READ,ADC_READ_ALL --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("--commands", default=",".join(DEFAULT_COMMANDS),
+	               help="comma-separated command names to plot, top to bottom "
+	                    "(default: %(default)s)")
+	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("--title", default="Serial data", help="overall figure title (default: %(default)s)")
+
+	p.add_argument("--unsigned", action="store_true",
+	               help="for a command with no FORMATS entry, decode its ints as unsigned "
+	                    "(default: signed)")
+
+	p.add_argument("--adc-channels",
+	               help="only plot these ADC_READ_ALL channel indices, e.g. '0,2,5' or '0-3,7' "
+	                    "(default: all)")
+
+	power_group = p.add_argument_group(
+		"power monitor metrics",
+		"which POWER_MONITOR_READ metrics to plot, for every channel (default: all three)")
+	power_group.add_argument("--voltage", action="store_true", help="show voltage")
+	power_group.add_argument("--current", action="store_true", help="show current")
+	power_group.add_argument("--power", action="store_true", help="show power")
+
+	return p
+
+
+def main():
+	parser = build_parser()
+	args = parser.parse_args()
+
+	commands = [c.strip() for c in args.commands.split(",") if c.strip()]
+	if not commands:
+		parser.error("--commands gave no command names")
+
+	panels = []  # (command, fmt, xs, channels)
+	for command in commands:
+		rows = load_rows(args.input, command)
+		if not rows:
+			print(f"WARNING: no PACKET rows for command={command!r} in {args.input}, skipping", file=sys.stderr)
+			continue
+
+		fmt = FORMATS.get(command)
+		if fmt is None:
+			# No registry entry: fall back to a plain packed-int32 array,
+			# sized from whatever payload length actually shows up.
+			byte_length = len(rows[0]["data_hex"]) // 2
+			fmt = fallback_format(byte_length, signed=not args.unsigned)
+			print(f"NOTE: {command!r} has no FORMATS entry, treating its {byte_length}-byte "
+			      f"payload as {byte_length // 4} packed int32 values. Add a FORMATS entry "
+			      f"for proper decoding.", file=sys.stderr)
+
+		xs, channels = build_series(rows, fmt, args.x)
+		if not xs:
+			print(f"WARNING: rows for command={command!r} didn't match the expected "
+			      f"{fmt.byte_length}-byte payload, skipping", file=sys.stderr)
+			continue
+
+		if command == "ADC_READ_ALL" and args.adc_channels:
+			try:
+				keep = parse_index_list(args.adc_channels)
+			except ValueError:
+				parser.error(f"--adc-channels: couldn't parse {args.adc_channels!r}")
+			bad = [i for i in keep if not (0 <= i < len(fmt.labels))]
+			if bad:
+				parser.error(f"--adc-channels: index out of range (0-{len(fmt.labels) - 1}): {bad}")
+			fmt, channels = select_channels(fmt, channels, keep)
+
+		if command == "POWER_MONITOR_READ" and fmt.group_count and fmt.group_size:
+			selected_metrics = [m for flag, m in
+			                     [(args.voltage, "Voltage"), (args.current, "Current"), (args.power, "Power")]
+			                     if flag]
+			if selected_metrics:
+				keep = [i for i, label in enumerate(fmt.labels)
+				        if any(m in label for m in selected_metrics)]
+				fmt, channels = select_channels(fmt, channels, keep)
+
+		panels.append((command, fmt, xs, channels))
+
+	if not panels:
+		print("ERROR: nothing to plot", file=sys.stderr)
+		return 1
+
+	plt.style.use("dark_background")
+	fig, axes = plt.subplots(len(panels), 1, figsize=(13, 5 * len(panels)), sharex=True)
+	if len(panels) == 1:
+		axes = [axes]
+
+	for ax, (command, fmt, xs, channels) in zip(axes, panels):
+		plot_subplot(ax, xs, channels, fmt, command, args.x)
+
+	axes[-1].set_xlabel("tick (ms)" if args.x == "tick" else "host time")
+	if args.x == "host":
+		fig.autofmt_xdate()
+
+	fig.suptitle(args.title, fontsize=15, color="white")
+	fig.tight_layout(rect=(0, 0, 1, 0.97))
+
+	if args.output:
+		fig.savefig(args.output, dpi=150, facecolor=fig.get_facecolor())
+		summary = ", ".join(f"{c} ({len(xs)} samples)" for c, _, xs, _ in panels)
+		print(f"Saved plot to {args.output}: {summary}")
+	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,571 @@
+#!/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,
+	"DIGITAL_LEG_OUT": 12,
+	"POWER_MONITOR_SET_READ": 13,
+	"POWER_MONITOR_READ": 14,
+}
+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,446 @@
+#!/usr/bin/env python3
+"""Open a serial port and log every successful (valid-CRC) packet to a CSV file.
+
+Standalone: no dependency on any other script in this toolset.
+
+Packet layout:
+
+	struct command_message_t {
+		uint8_t prefix;
+		uint8_t length;
+		uint8_t id;
+		uint8_t command;
+		int64_t tick;      // milliseconds, little-endian
+		uint8_t crc;
+		uint8_t data[COMMAND_DATA_SIZE];
+	} __attribute__((packed));
+"""
+
+import argparse
+import csv
+import struct
+import sys
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+
+import serial
+
+DEFAULT_PORT = "/dev/ttyACM0"
+DEFAULT_BAUD = 115200
+DEFAULT_DEVICE_ID = 0
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+
+# name -> command id
+COMMANDS = {
+	"ACK": 0,
+	"NACK": 1,
+	"LED_TOGGLE": 2,
+	"LED_SET": 3,
+	"SERVO_SET": 4,
+	"SERVO_SET_ALL": 5,
+	"ADC_READ_RAW": 6,
+	"ADC_READ_RAW_ALL": 7,
+	"ADC_READ": 8,
+	"ADC_READ_ALL": 9,
+	"ADC_SET_READ": 10,
+	"DIGITAL_IN": 11,
+	"DIGITAL_LEG_OUT": 12,
+	"POWER_MONITOR_SET_READ": 13,
+	"POWER_MONITOR_READ": 14,
+}
+COMMAND_NAMES = {v: k for k, v in COMMANDS.items()}
+
+# struct formats for typed --send-data tokens (all little-endian)
+TYPE_FORMATS = {
+	"u8": "<B", "i8": "<b",
+	"u16": "<H", "i16": "<h",
+	"u32": "<I", "i32": "<i",
+	"f32": "<f",
+}
+
+# Fixed header layout: prefix, length, id, command, tick(int64), crc
+TICK_FORMAT = "<q"                         # signed 64-bit little-endian, milliseconds
+TICK_SIZE = struct.calcsize(TICK_FORMAT)   # 8
+CRC_OFFSET = 4 + TICK_SIZE                 # index of the crc byte -> 12
+HEADER_SIZE = CRC_OFFSET + 1               # bytes before data -> 13
+
+# Safety valve for the ASCII-log branch: a real firmware log line shouldn't
+# be longer than this. If we've buffered more than this with no newline,
+# we're desynced (likely sitting inside binary packet data), so drop a byte
+# and retry instead of blocking forever waiting for a '\n' that won't come.
+MAX_LOG_LINE = 256
+
+FIELDNAMES = ["host_time", "tick_ms", "type", "device_id", "command", "length", "data_hex", "text"]
+
+
+# --------------------------------------------------------------------------
+# Packet helpers
+# --------------------------------------------------------------------------
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"", device_id: int = DEFAULT_DEVICE_ID,
+                 tick: int = 0) -> bytes:
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(len(data))
+	pkt.append(device_id)
+	pkt.append(command)
+	pkt.extend(struct.pack(TICK_FORMAT, tick))
+	pkt.append(0)  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[CRC_OFFSET] = calculate_crc(pkt[:CRC_OFFSET] + pkt[CRC_OFFSET + 1:])
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[CRC_OFFSET]
+	calc = calculate_crc(packet[:CRC_OFFSET] + packet[CRC_OFFSET + 1:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return HEADER_SIZE + buf[1]
+
+
+def unpack_tick(packet: bytes) -> int:
+	return struct.unpack(TICK_FORMAT, packet[4:4 + TICK_SIZE])[0]
+
+
+def iso_now() -> str:
+	return datetime.now(timezone.utc).isoformat(timespec="milliseconds")
+
+
+def drain_input(ser, quiet_period=0.2, max_wait=2.0):
+	"""Actively read and discard bytes until the port stays quiet for
+	`quiet_period` seconds (or `max_wait` total elapses as a safety cap).
+
+	reset_input_buffer() only clears what the OS has already staged at the
+	instant it's called. If the device already pushed more bytes down the
+	wire that hadn't been handed to the OS yet, they show up right after
+	the flush and get mistaken for new data. Draining until the port is
+	genuinely idle catches that in-flight backlog too.
+	"""
+	ser.reset_input_buffer()
+	deadline = time.time() + max_wait
+	quiet_since = time.time()
+	while time.time() < deadline:
+		chunk = ser.read(4096)
+		if chunk:
+			quiet_since = time.time()
+		elif time.time() - quiet_since >= quiet_period:
+			break
+
+
+# --------------------------------------------------------------------------
+# Argument parsing
+# --------------------------------------------------------------------------
+
+def parse_command(text: str):
+	"""Accept a command name (case-insensitive) or a number. Returns (id, name)."""
+	key = text.upper()
+	if key in COMMANDS:
+		return COMMANDS[key], key
+
+	try:
+		val = int(text, 0)
+	except ValueError:
+		raise argparse.ArgumentTypeError(
+			f"unknown command '{text}' (use a number, or one of: {', '.join(COMMANDS)})"
+		)
+	if not 0 <= val <= 255:
+		raise argparse.ArgumentTypeError("command number must be 0-255")
+	return val, COMMAND_NAMES.get(val, f"#{val}")
+
+
+def parse_data_token(tok: str) -> bytes:
+	"""
+	Formats:
+	  5            -> u8
+	  u16:500      -> unsigned 16-bit little-endian
+	  i32:-3       -> signed 32-bit little-endian
+	  f32:1.5      -> float32 little-endian
+	  hex:0a0b0c   -> raw bytes
+	"""
+	if ":" in tok:
+		kind, value = tok.split(":", 1)
+		kind = kind.lower()
+	else:
+		kind, value = "u8", tok
+
+	if kind == "hex":
+		return bytes.fromhex(value)
+
+	fmt = TYPE_FORMATS.get(kind)
+	if fmt is None:
+		raise ValueError(f"unknown type '{kind}' (use u8/i8/u16/i16/u32/i32/f32/hex)")
+
+	num = float(value) if kind == "f32" else int(value, 0)
+	return struct.pack(fmt, num)
+
+
+def parse_tick(text: str) -> int:
+	if text.lower() == "now":
+		return int(time.time() * 1000)
+	try:
+		return int(text, 0)
+	except ValueError:
+		raise argparse.ArgumentTypeError("tick must be an integer (ms) or 'now'")
+
+
+def build_parser():
+	p = argparse.ArgumentParser(
+		description="Log every successful (valid-CRC) packet from a serial device to a CSV file.",
+		formatter_class=argparse.RawDescriptionHelpFormatter,
+		epilog="""\
+CSV columns:
+  host_time   ISO-8601 UTC timestamp when the host received the frame
+  tick_ms     device-clock timestamp from the packet header (blank for log lines)
+  type        PACKET or LOG
+  device_id, command, length, data_hex   set for PACKET rows
+  text        the decoded line, set for LOG rows
+
+examples:
+  %(prog)s -o adc_log.csv
+  %(prog)s -o adc_log.csv --send ADC_SET_READ --send-data u32:100
+  %(prog)s -p /dev/ttyUSB0 -b 9600 -o session.csv --duration 60
+""",
+	)
+
+	p.add_argument("-p", "--port", default=DEFAULT_PORT, help="serial port (default: %(default)s)")
+	p.add_argument("-b", "--baud", type=int, default=DEFAULT_BAUD, help="baud rate (default: %(default)s)")
+	p.add_argument("-d", "--device-id", type=lambda s: int(s, 0), default=None,
+	               help="only log packets from this device id (default: log all)")
+
+	p.add_argument("-o", "--output",
+	               help="CSV file path (default: serial_log_<timestamp>.csv)")
+	p.add_argument("--append", action="store_true",
+	               help="append to an existing file instead of overwriting (default: off)")
+
+	p.add_argument("--include-logs", dest="include_logs", action="store_true", default=True,
+	               help="also log firmware ASCII log lines as LOG rows (default: on)")
+	p.add_argument("--no-include-logs", dest="include_logs", action="store_false",
+	               help="packets only, skip firmware ASCII log lines")
+
+	p.add_argument("--boot-delay", type=float, default=0.0,
+	               help="seconds to wait after opening the port before flushing/listening, "
+	                    "for boards that reset on connect (default: %(default)s, off)")
+	p.add_argument("--drain-quiet", type=float, default=0.2,
+	               help="seconds of silence required on the port before logging actually "
+	                    "starts, to actively drain any in-flight backlog (default: %(default)s)")
+	p.add_argument("--no-drain", dest="drain", action="store_false", default=True,
+	               help="skip the active drain, only do a single buffer clear on open")
+	p.add_argument("--duration", type=float, default=0.0,
+	               help="seconds to log before stopping; 0 = run until Ctrl+C (default: %(default)s)")
+	p.add_argument("--quiet", action="store_true",
+	               help="don't echo logged rows to the console (default: off)")
+
+	p.add_argument("--allow-unknown-commands", action="store_true",
+	               help="also log packets with a valid CRC but an unrecognized command id "
+	                    "(default: off, those are discarded as likely garbage)")
+
+	p.add_argument("--send", type=parse_command,
+	               help="optional command to send once before logging starts (name or number)")
+	p.add_argument("--send-data", nargs="*", default=[],
+	               help="data tokens for --send: u8/i8/u16/i16/u32/i32/f32:<value> or hex:<bytes>")
+	p.add_argument("-t", "--tick", type=parse_tick, default=0,
+	               help="tick value (ms) for the --send packet, or 'now' (default: %(default)s)")
+
+	return p
+
+
+# --------------------------------------------------------------------------
+# Core loop
+# --------------------------------------------------------------------------
+
+def process_stream(ser, write_row, args, stats):
+	"""Read from `ser`, parse frames, and call write_row(dict) for each valid one.
+
+	Runs until args.duration elapses (0 = forever) or a KeyboardInterrupt.
+	Bad-CRC frames are discarded, not logged, per "successful packets only".
+	"""
+	rx = bytearray()
+	start = time.time()
+
+	while True:
+		if args.duration > 0 and (time.time() - start) >= args.duration:
+			break
+
+		chunk = ser.read(64)
+		if chunk:
+			rx.extend(chunk)
+
+		while rx:
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+
+				if not verify_crc(pkt):
+					stats["bad_crc"] += 1
+					# Guessed size may be based on a garbage length byte;
+					# drop just the leading byte and re-scan.
+					del rx[:1]
+					continue
+
+				del rx[:size]
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+				tick = unpack_tick(pkt)
+				payload = pkt[HEADER_SIZE:HEADER_SIZE + length]
+
+				if args.device_id is not None and dev_id != args.device_id:
+					continue
+
+				if cmd not in COMMAND_NAMES and not args.allow_unknown_commands:
+					# Valid CRC but not a recognized command id: more likely
+					# a chance CRC collision on garbage than a real message.
+					stats["bad_header"] += 1
+					continue
+
+				name = COMMAND_NAMES.get(cmd, str(cmd))
+				stats["packets"] += 1
+				write_row({
+					"host_time": iso_now(),
+					"tick_ms": tick,
+					"type": "PACKET",
+					"device_id": dev_id,
+					"command": name,
+					"length": length,
+					"data_hex": payload.hex(),
+					"text": "",
+				})
+			else:
+				idx = rx.find(b"\n")
+				if idx == -1:
+					if len(rx) > MAX_LOG_LINE:
+						del rx[:1]
+						continue
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				if not args.include_logs:
+					continue
+
+				try:
+					text = line.decode().rstrip()
+				except UnicodeDecodeError:
+					# Not valid text, e.g. the resync logic caught part of a
+					# binary frame mid-stream. Discard rather than log garbage.
+					stats["bad_log"] += 1
+					continue
+
+				stats["logs"] += 1
+				write_row({
+					"host_time": iso_now(),
+					"tick_ms": "",
+					"type": "LOG",
+					"device_id": "",
+					"command": "",
+					"length": "",
+					"data_hex": "",
+					"text": text,
+				})
+
+
+# --------------------------------------------------------------------------
+# Main
+# --------------------------------------------------------------------------
+
+def main():
+	parser = build_parser()
+	args = parser.parse_args()
+
+	out_path = Path(args.output) if args.output else Path(
+		f"serial_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
+	)
+	out_path.parent.mkdir(parents=True, exist_ok=True)
+
+	file_exists = out_path.exists()
+	mode = "a" if args.append else "w"
+
+	try:
+		ser = serial.Serial(args.port, args.baud, timeout=0.05)
+		if args.boot_delay > 0:
+			time.sleep(args.boot_delay)
+		if args.drain:
+			drain_input(ser, quiet_period=args.drain_quiet)
+		else:
+			ser.reset_input_buffer()
+	except serial.SerialException as e:
+		print(f"ERROR: could not open {args.port}: {e}", file=sys.stderr)
+		return 1
+
+	if args.send is not None:
+		cmd_id, cmd_name = args.send
+		data = bytearray()
+		try:
+			for tok in args.send_data:
+				data.extend(parse_data_token(tok))
+		except (ValueError, struct.error) as e:
+			parser.error(f"bad --send-data: {e}")
+		device_id = args.device_id if args.device_id is not None else DEFAULT_DEVICE_ID
+		packet = make_packet(cmd_id, bytes(data), device_id, args.tick)
+		print(f"--> TX {cmd_name} (tick={args.tick}ms): {packet.hex(' ')}")
+		ser.write(packet)
+		ser.flush()
+
+	stats = {"packets": 0, "logs": 0, "bad_crc": 0, "bad_header": 0, "bad_log": 0}
+
+	with open(out_path, mode, newline="", encoding="utf-8") as fh:
+		writer = csv.DictWriter(fh, fieldnames=FIELDNAMES)
+		if mode == "w" or not file_exists:
+			writer.writeheader()
+			fh.flush()
+
+		def write_row(row):
+			writer.writerow(row)
+			fh.flush()
+			if not args.quiet:
+				if row["type"] == "PACKET":
+					print(f"<-- t={row['tick_ms']}ms {row['command']} len={row['length']} data={row['data_hex']}")
+				else:
+					print(f"[LOG] {row['text']}")
+
+		print(f"Logging to {out_path}. Press Ctrl+C to stop.")
+		try:
+			if args.send is None:
+				# Actively drain whatever queued up (or was still in flight)
+				# while we were opening the output file / writing the
+				# header, so logging starts clean rather than with a
+				# backlog of pre-existing traffic.
+				if args.drain:
+					drain_input(ser, quiet_period=args.drain_quiet)
+				else:
+					ser.reset_input_buffer()
+			process_stream(ser, write_row, args, stats)
+		except KeyboardInterrupt:
+			pass
+		finally:
+			ser.close()
+
+	print(f"Done. {stats['packets']} packets, {stats['logs']} log lines, "
+	      f"{stats['bad_crc']} bad CRC discarded, {stats['bad_header']} bad header (unknown command) "
+	      f"discarded, {stats['bad_log']} undecodable log lines discarded -> {out_path}")
+	return 0
+
+
+if __name__ == "__main__":
+	sys.exit(main())

servo_controller/scripts/set_legs.py

@@ -0,0 +1,137 @@
+#!/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
+DIGITAL_LEG_OUT = 12
+
+DEVICE_ID = 0
+
+# Header layout: prefix, length, id, command, tick(int64 LE), 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
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"", tick: int = 0) -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	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 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]
+				tick = unpack_tick(pkt)
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id}, tick={tick}ms)")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id}, tick={tick}ms)")
+
+				else:
+					print(f"<-- Command {cmd} (tick={tick}ms) 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(0x3f):
+		data = struct.pack("<B", i)  # explicitly uint8_t, raises if i is out of 0-255 range
+
+		ser.write(make_packet(DIGITAL_LEG_OUT, data, tick=0))
+		time.sleep(0.1)
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	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,10 @@
 #include "adc.h"
+#include "command_handler.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 +20,87 @@ 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;
+
+// ADC BUFFER
+#define ADC_MSG_BUFFER_SIZE 10
+struct command_message_t adc_msg_buffer[ADC_MSG_BUFFER_SIZE];
+struct command_message_t *adc_msg_buffer_ptr;
+
+
+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
+		command_create_message(adc_msg_buffer_ptr, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+		command_handler_tx(adc_msg_buffer_ptr);
+		adc_msg_buffer_ptr++;
+
+		if (adc_msg_buffer_ptr > &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1]) {
+			adc_msg_buffer_ptr = adc_msg_buffer;
+		}
+	}
+}
+
 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);
+
+	// BUFFER
+	memset(adc_msg_buffer, 0, sizeof(adc_msg_buffer));
+	adc_msg_buffer_ptr = adc_msg_buffer;
+
+	// 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,10 +135,21 @@ 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;
 }
 
-
 int adc_read_id(int id) {
 	int ret;
     int val_mv = 0;
@@ -72,12 +159,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 +203,33 @@ 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;
+}
+
+void* adc_get_buffer() {
+	struct command_message_t *ptr = adc_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &adc_msg_buffer[0]) {
+		ptr = &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
 }

servo_controller/src/adc.h

@@ -2,12 +2,17 @@
 #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);
+void* adc_get_buffer();
 
 
 #endif // ADC_H

servo_controller/src/command_handler.c

@@ -4,12 +4,33 @@
 #include "servo.h"
 #include "adc.h"
 #include "usb.h"
+#include "digital_out.h"
+#include "power_monitor.h"
 
 #include <zephyr/logging/log.h>
 
 LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
 
-int command_handler(struct command_message_t *msg) {
+
+// BUFFER (add ack and nack at the and as static)
+#define CMD_MSG_BUFFER_SIZE 10
+struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2];
+struct command_message_t *cmd_msg_buffer_ptr;
+
+
+int command_handler_init() {
+	// BUFFER
+	memset(cmd_msg_buffer, 0, sizeof(cmd_msg_buffer));
+	cmd_msg_buffer_ptr = cmd_msg_buffer;
+
+	// CREATE ACK/NACK
+	command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]);
+	command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]);
+
+	return 0;
+}
+
+int command_handler_rx(struct command_message_t *msg) {
 	if (msg == NULL) {
 		LOG_ERR("Received NULL message pointer");
 		return -EINVAL;
@@ -65,25 +86,68 @@ 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();
+			struct command_message_t *reply = cmd_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);
+			command_handler_tx(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();
+			struct command_message_t *reply = cmd_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);
+			command_handler_tx(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 = 0b111111111111111111;
+
+			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;
+		}
+
+		case DIGITAL_LEG_OUT: {
+			uint8_t mask = msg->data[0];
+
+			digital_out_set_leg(mask);
+			
+			break;
+		}
+
+		case POWER_MONITOR_SET_READ: {
+			int interval_ms;
+			memcpy(&interval_ms, &msg->data[0], sizeof(int));
+
+			power_monitor_timer_set(interval_ms);
 			
 			break;
 		}
@@ -94,5 +158,28 @@ int command_handler(struct command_message_t *msg) {
 		}
 	}
 
+	return 0;
+}
+
+struct command_message_t* cmd_get_next_tx_buf() {
+	struct command_message_t *buf = cmd_msg_buffer_ptr;
+
+	// Increment the buffer pointer
+	cmd_msg_buffer_ptr++;
+
+	if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) {
+		cmd_msg_buffer_ptr = cmd_msg_buffer;
+	}
+
+	return buf;
+}
+
+int command_handler_tx(struct command_message_t *msg) {
+
+	// Send over USB if enabled and ready
+	if (usb_is_tx_enabled()) {
+		usb_send_command(msg);
+	}
+
 	return 0;
 }

servo_controller/src/command_handler.h

@@ -4,13 +4,10 @@
 
 #include "command_message.h"
 
-/**
+int command_handler_init();
- * @brief Process received command message
+int command_handler_rx(struct command_message_t *msg);
- * 
+struct command_message_t* cmd_get_next_tx_buf();
- * @param msg Command message to process
+int command_handler_tx(struct command_message_t *msg);
- * @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

@@ -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,15 @@ 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,
+	DIGITAL_LEG_OUT,
+	POWER_MONITOR_SET_READ,
+	POWER_MONITOR_READ,
 
 	// Keep last
 	NUM_COMMANDS,
@@ -29,6 +36,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));
@@ -75,7 +83,7 @@ void command_create_nack(struct command_message_t *msg);
 /**
  * @brief Print the command with LOG
  * 
- * @param msg Message to calculate CRC for
+ * @param msg Message to LOG
  */
 void command_log(struct command_message_t *msg);
 

servo_controller/src/digital_in.c

@@ -0,0 +1,64 @@
+#include "digital_in.h"
+#include "command_handler.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 = cmd_get_next_tx_buf();
+	command_create_message(msg, sizeof(pins), DIGITAL_IN, (uint8_t *)&pins);
+	command_handler_tx(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 -ENODEV;
+	}
+
+	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/digital_out.c

@@ -0,0 +1,61 @@
+#include "digital_out.h"
+
+#include <zephyr/devicetree.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/gpio.h>
+
+LOG_MODULE_REGISTER(digital_out, LOG_LEVEL_INF);
+
+
+#define DIGITAL_OUT_MACRO(node_id) GPIO_DT_SPEC_GET(node_id, gpios),
+
+static const struct gpio_dt_spec digital_leg_outputs[] = {
+    DT_FOREACH_CHILD(DT_NODELABEL(digital_outputs), DIGITAL_OUT_MACRO)
+};
+
+
+static int digital_out_pin_init(const struct gpio_dt_spec *gpio) {
+	int ret;
+
+	if (!device_is_ready(gpio->port)) {
+		LOG_ERR("GPIO device %s is not ready\n", gpio->port->name);
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_configure_dt(gpio, GPIO_OUTPUT_INACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	// Turn it off
+	ret = gpio_pin_set_dt(gpio, 0);
+	if (ret != 0) {
+		LOG_ERR("Failed to initialize %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+int digital_out_init() {
+	// Initialize the LEG enable pins
+	for (int i = 0; i < DIGITAL_OUT_LEG_CHANNELS; i++) {
+		digital_out_pin_init(&digital_leg_outputs[i]);
+	}
+
+	return 0;
+}
+
+int digital_out_set_leg(uint8_t mask) {
+	for (int i = 0; i < DIGITAL_OUT_LEG_CHANNELS; i++) {
+		if ((mask >> i) & 0x1) {
+			gpio_pin_set_dt(&digital_leg_outputs[i], 1);
+		}
+		else {
+			gpio_pin_set_dt(&digital_leg_outputs[i], 0);
+		}
+	}
+
+	return 0;
+}

servo_controller/src/digital_out.h

@@ -0,0 +1,14 @@
+#ifndef DIGITAL_OUT_H
+#define DIGITAL_OUT_H
+
+
+#include <stdint.h>
+
+
+#define DIGITAL_OUT_LEG_CHANNELS 6
+
+int digital_out_init();
+int digital_out_set_leg(uint8_t mask);
+
+
+#endif // DIGITAL_OUT_H

servo_controller/src/main.c

@@ -2,6 +2,10 @@
 #include "usb.h"
 #include "servo.h"
 #include "adc.h"
+#include "digital_in.h"
+#include "digital_out.h"
+#include "power_monitor.h"
+#include "command_handler.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -10,12 +14,20 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
 int main(void) {
 	int ret;
 
+	// COMMAND HANDLER init
+	ret = command_handler_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable COMMAND HANDLER");
+		return 0;
+	}
+
 	// USB init
 	ret = usb_init();
 	if (ret != 0) {
 		LOG_ERR("Failed to enable USB");
 		return 0;
 	}
+	usb_set_tx_state(1);
 
 	// LED init
 	ret = led_init();
@@ -38,5 +50,26 @@ int main(void) {
 		return 0;
 	}
 
+	// DIGITAL IN init
+	ret = digital_in_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable DIGITAL IN");
+		return 0;
+	}
+
+	// DIGITAL OUT init
+	ret = digital_out_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable DIGITAL OUT");
+		return 0;
+	}
+
+	// POWER MONITOR init
+	ret = power_monitor_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable POWER MONITOR");
+		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/power_monitor.c

@@ -0,0 +1,133 @@
+#include "power_monitor.h"
+#include "command_handler.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/sensor.h>
+
+LOG_MODULE_REGISTER(power_monitor, LOG_LEVEL_INF);
+
+// DEVICE
+#define POWER_MONITOR_NODE DT_NODELABEL(power_monitor)
+const struct device *power_monitor_dt = DEVICE_DT_GET(POWER_MONITOR_NODE);
+
+
+// THREAD
+static struct k_thread power_monitor_thread_data;
+static k_tid_t power_monitor_thread_id = NULL;
+#define POWER_MONITOR_THREAD_STACK_SIZE 2048
+#define POWER_MONITOR_THREAD_PRIORITY 5
+K_THREAD_STACK_DEFINE(power_monitor_thread_stack, POWER_MONITOR_THREAD_STACK_SIZE);
+
+// TIMER
+struct k_timer power_monitor_timer;
+struct k_sem power_monitor_semaphore;
+
+// BUFFER
+#define PWR_MSG_BUFFER_SIZE 10
+struct command_message_t pwr_msg_buffer[PWR_MSG_BUFFER_SIZE];
+struct command_message_t *pwr_msg_buffer_ptr;
+
+
+static void power_monitor_timer_handler(struct k_timer *timer) {
+    // Timer trigger
+	k_sem_give(&power_monitor_semaphore);
+}
+
+static void power_monitor_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+	while (1) {
+		k_sem_take(&power_monitor_semaphore, K_FOREVER);
+
+        struct sensor_value values[3][3];
+        sensor_sample_fetch(power_monitor_dt);
+
+		// Read channel 1
+		struct sensor_value value = {1, 0};
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[0][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[0][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[0][2]);
+
+		// Read channel 2
+		value.val1 = 2;
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[1][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[1][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[1][2]);
+
+		// Read channel 3
+		value.val1 = 3;
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[2][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[2][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[2][2]);
+
+		// Send to USB
+		command_create_message(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
+		command_handler_tx(pwr_msg_buffer_ptr);
+		pwr_msg_buffer_ptr++;
+
+		if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
+			pwr_msg_buffer_ptr = pwr_msg_buffer;
+		}
+	}
+}
+
+int power_monitor_init() {
+
+	k_sem_init(&power_monitor_semaphore, 0, 1);
+	
+	// TIMER
+	k_timer_init(&power_monitor_timer, power_monitor_timer_handler, NULL);
+
+	// BUFFER
+	memset(pwr_msg_buffer, 0, sizeof(pwr_msg_buffer));
+	pwr_msg_buffer_ptr = pwr_msg_buffer;
+
+	// DEVICE
+    if (!device_is_ready(power_monitor_dt)) {
+        LOG_ERR("INA3221 not ready\n");
+        return 0;
+    }
+	
+	// THREAD
+	power_monitor_thread_id = k_thread_create(
+		&power_monitor_thread_data,
+		power_monitor_thread_stack,
+		K_THREAD_STACK_SIZEOF(power_monitor_thread_stack),
+		power_monitor_thread,
+		NULL, NULL, NULL,
+		POWER_MONITOR_THREAD_PRIORITY,
+		0,
+		K_NO_WAIT
+	);
+
+	return 0;
+}
+
+int power_monitor_timer_set(int interval_ms) {
+
+	if (interval_ms > 0) {
+		k_timer_start(&power_monitor_timer, K_MSEC(interval_ms), K_MSEC(interval_ms));
+	}
+	else {
+		k_timer_stop(&power_monitor_timer);
+	}
+	
+	return 0;
+}
+
+void* power_monitor_get_buffer() {
+	struct command_message_t *ptr = pwr_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &pwr_msg_buffer[0]) {
+		ptr = &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
+}

servo_controller/src/power_monitor.h

@@ -0,0 +1,10 @@
+#ifndef POWER_MONITOR_H
+#define POWER_MONITOR_H
+
+
+int power_monitor_init();
+int power_monitor_timer_set(int interval_ms);
+void* power_monitor_get_buffer();
+
+
+#endif // POWER_MONITOR_H

servo_controller/src/usb.c

@@ -16,6 +16,8 @@ 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;
+int usb_device_ready = 0;
+int usb_tx_enabled = 0;
 
 // RX THREAD
 static struct k_thread usb_rx_thread_data;
@@ -35,12 +37,12 @@ static k_tid_t usb_tx_thread_id = NULL;
 #define USB_TX_THREAD_STACK_SIZE 512
 K_THREAD_STACK_DEFINE(usb_tx_thread_stack, USB_TX_THREAD_STACK_SIZE);
 
-// TX BUFFER (add ack and nack at the and as static)
+// TX MSG BUFFER
 #define TX_BUFFER_SIZE 10
-struct command_message_t usb_tx_buffer[TX_BUFFER_SIZE + 2];
+char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE) * sizeof(struct command_message_t *)];
-struct command_message_t *usb_tx_buf_ptr;
-char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE + 2) * sizeof(struct command_message_t *)];
 struct k_msgq usb_tx_ptr_msgq;
+struct command_message_t ack;
+struct command_message_t nack;
 
 // ACK / NACK messages
 #define RETURN_ACK true
@@ -104,13 +106,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,22 +123,28 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
 					if (calculated_crc != msg.crc) {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.tick = k_uptime_get();
+							nack.crc = command_calculate_crc(&nack);
+							usb_send_command(&nack);
 						}
 						continue;
 					}
 
-					int ret = command_handler(&msg);
+					int ret = command_handler_rx(&msg);
 					if (ret == 0) {
 						if (RETURN_ACK) {
 							// Send ACK
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
+							ack.tick = k_uptime_get();
+							ack.crc = command_calculate_crc(&ack);
+							usb_send_command(&ack);
 						}
 					}
 					else {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.tick = k_uptime_get();
+							nack.crc = command_calculate_crc(&nack);
+							usb_send_command(&nack);
 						}
 					}
 
@@ -198,11 +207,11 @@ static void usb_tx_thread(void *p1, void *p2, void *p3) {
 int usb_init() {
 	ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
 	k_sem_init(&rx_semaphore, 0, 1);
-	usb_tx_buf_ptr = usb_tx_buffer;
 	k_msgq_init(&usb_tx_ptr_msgq, usb_tx_ptr_msgq_buffer, sizeof(struct command_message_t *), TX_BUFFER_SIZE);
 
-	command_create_ack(&usb_tx_buffer[TX_BUFFER_SIZE]);
+	// CREATE ACK/NACK
-	command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+	command_create_ack(&ack);
+	command_create_nack(&nack);
 
 	int ret;
 
@@ -263,20 +272,17 @@ int usb_init() {
 		return -ENOMEM;
 	}
 
+	usb_device_ready = 1;
+
 	return ret;
 }
 
-struct command_message_t* usb_get_next_tx_buf() {
+int usb_is_tx_enabled() {
-	struct command_message_t *buf = usb_tx_buf_ptr;
+	return usb_tx_enabled && usb_device_ready;
+}
 
-	// Increment the buffer pointer
+void usb_set_tx_state(int state) {
-	usb_tx_buf_ptr++;
+	usb_tx_enabled = state ? 1 : 0;
-
-	if (usb_tx_buf_ptr > &usb_tx_buffer[TX_BUFFER_SIZE-1]) {
-		usb_tx_buf_ptr = usb_tx_buffer;
-	}
-
-	return buf;
 }
 
 int usb_send_command(struct command_message_t *msg) {

servo_controller/src/usb.h

@@ -6,7 +6,8 @@
 
 
 int usb_init();
-struct command_message_t* usb_get_next_tx_buf();
+int usb_is_tx_enabled();
+void usb_set_tx_state(int state);
 int usb_send_command(struct command_message_t *msg);