First implementation of INA3221 power monitor

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/prj.conf

@@ -1,6 +1,11 @@
 CONFIG_GPIO=y
 CONFIG_PWM=y
 CONFIG_ADC=y
+CONFIG_I2C=y
+
+# INA3221
+CONFIG_SENSOR=y
+CONFIG_INA3221=y
 
 # Mux
 CONFIG_CD74HC4067=y
@@ -33,5 +38,5 @@ CONFIG_LOG_MODE_IMMEDIATE=y
 CONFIG_DEBUG_THREAD_INFO=y
 # CONFIG_DEBUG=y
 # CONFIG_DEBUG_OPTIMIZATIONS=y
-CONFIG_OUTPUT_DISASSEMBLY=n		# Create disassmebly files
+# CONFIG_OUTPUT_DISASSEMBLY=y		# Create disassmebly files
 #CONFIG_OUTPUT_DISASSEMBLE_ALL=n

servo_controller/scripts/plot_channels.py

@@ -1,135 +1,366 @@
 #!/usr/bin/env python3
-"""Plot packed 4-byte-integer channel data from a serial_logger.py CSV, dark mode.
+"""Plot decoded command payloads from a serial_logger.py CSV, dark mode.
 
-Expects rows where `data_hex` is N 4-byte little-endian integers packed back
+Each command gets its own subplot, stacked vertically in one window, in the
-to back (default N=18, matching an 18-channel ADC_READ_ALL-style packet).
+order given by --commands (default: ADC_READ_ALL on top, POWER_MONITOR_READ
-X-axis is the device-clock `tick_ms` column by default (or host_time).
+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
 
 
-def build_parser():
+# --------------------------------------------------------------------------
-	p = argparse.ArgumentParser(
+# Data formats: command name -> how to decode its payload for plotting.
-		description="Plot packed 4-byte-integer channels from a serial_logger.py CSV (dark mode).",
+# This is the "field" to edit when a new command/payload format shows up.
-		formatter_class=argparse.RawDescriptionHelpFormatter,
+# --------------------------------------------------------------------------
-		epilog="""\
+
-examples:
+@dataclass
-  %(prog)s -i adc_log.csv
+class DataFormat:
-  %(prog)s -i adc_log.csv --command ADC_READ_ALL -o plot.png
+	byte_length: int                        # expected payload length in bytes
-  %(prog)s -i adc_log.csv --channels 18 --unsigned --x host
+	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)",
 	)
 
-	p.add_argument("-i", "--input", required=True, help="input CSV file (from serial_logger.py)")
-	p.add_argument("-o", "--output", help="save the plot to this file instead of showing it interactively")
 
-	p.add_argument("-c", "--command", help="only use rows with this command name (default: use whatever "
+# --------------------------------------------------------------------------
-	                                        "PACKET rows have the right byte length)")
+# CSV loading
-	p.add_argument("-n", "--channels", type=int, default=18,
+# --------------------------------------------------------------------------
-	               help="number of 4-byte integer channels packed in data_hex (default: %(default)s)")
-	p.add_argument("--signed", dest="signed", action="store_true", default=True,
-	               help="interpret channels as signed int32 (default: on)")
-	p.add_argument("--unsigned", dest="signed", action="store_false",
-	               help="interpret channels as unsigned uint32")
 
-	p.add_argument("--x", choices=["tick", "host"], default="tick",
+def load_rows(path: str, command: str):
-	               help="x-axis source: device tick_ms, or host_time (default: %(default)s)")
-	p.add_argument("--labels", help="comma-separated channel labels (default: ch0..chN-1)")
-	p.add_argument("--title", default="Channel data", help="plot title (default: %(default)s)")
-
-	return p
-
-
-def load_rows(path, command, channel_bytes):
-	"""Read the CSV and return rows that look like packed-channel PACKET rows."""
 	rows = []
 	with open(path, newline="", encoding="utf-8") as fh:
 		reader = csv.DictReader(fh)
 		for row in reader:
 			if row.get("type") != "PACKET":
 				continue
-			if command and row.get("command") != command:
+			if row.get("command") != command:
-				continue
-			data_hex = row.get("data_hex", "")
-			if len(data_hex) != channel_bytes * 2:  # 2 hex chars per byte
 				continue
 			rows.append(row)
 	return rows
 
 
+def 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()
 
-	channel_bytes = args.channels * 4
+	commands = [c.strip() for c in args.commands.split(",") if c.strip()]
+	if not commands:
+		parser.error("--commands gave no command names")
 
-	rows = load_rows(args.input, args.command, channel_bytes)
+	panels = []  # (command, fmt, xs, channels)
-	if not rows:
+	for command in commands:
-		hint = f" with command={args.command!r}" if args.command else ""
+		rows = load_rows(args.input, command)
-		print(f"ERROR: no PACKET rows{hint} with {channel_bytes}-byte payloads "
+		if not rows:
-		      f"({args.channels} x 4-byte ints) found in {args.input}", file=sys.stderr)
+			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
 
-	fmt = f"<{args.channels}{'i' if args.signed else 'I'}"
-
-	xs = []
-	channels = [[] for _ in range(args.channels)]
-
-	for row in rows:
-		if args.x == "tick":
-			xs.append(int(row["tick_ms"]))
-		else:
-			xs.append(datetime.fromisoformat(row["host_time"]))
-
-		values = struct.unpack(fmt, bytes.fromhex(row["data_hex"]))
-		for i, v in enumerate(values):
-			channels[i].append(v)
-
-	# sort chronologically, just in case the file wasn't strictly ordered
-	order = sorted(range(len(xs)), key=lambda i: xs[i])
-	xs = [xs[i] for i in order]
-	channels = [[ch[i] for i in order] for ch in channels]
-
-	if args.labels:
-		labels = [s.strip() for s in args.labels.split(",")]
-		if len(labels) != args.channels:
-			parser.error(f"--labels has {len(labels)} entries, expected {args.channels}")
-	else:
-		labels = [f"ch{i}" for i in range(args.channels)]
-
-	# --- dark mode plot ---
 	plt.style.use("dark_background")
-	fig, ax = plt.subplots(figsize=(13, 7))
+	fig, axes = plt.subplots(len(panels), 1, figsize=(13, 5 * len(panels)), sharex=True)
+	if len(panels) == 1:
+		axes = [axes]
 
-	cmap = matplotlib.colormaps["tab20"].resampled(args.channels)
+	for ax, (command, fmt, xs, channels) in zip(axes, panels):
-	for i in range(args.channels):
+		plot_subplot(ax, xs, channels, fmt, command, args.x)
-		ax.plot(xs, channels[i], label=labels[i], color=cmap(i), linewidth=1.2)
-
-	ax.set_title(args.title, fontsize=14, color="white")
-	ax.set_xlabel("tick (ms)" if args.x == "tick" else "host time")
-	ax.set_ylabel("value" + (" (int32)" if args.signed else " (uint32)"))
-	ax.grid(True, alpha=0.25)
 
+	axes[-1].set_xlabel("tick (ms)" if args.x == "tick" else "host time")
 	if args.x == "host":
 		fig.autofmt_xdate()
-		ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S"))
 
-	ax.legend(ncol=min(6, args.channels), fontsize=8, loc="upper center",
+	fig.suptitle(args.title, fontsize=15, color="white")
-	          bbox_to_anchor=(0.5, -0.12), frameon=False)
+	fig.tight_layout(rect=(0, 0, 1, 0.97))
-	fig.tight_layout()
 
 	if args.output:
 		fig.savefig(args.output, dpi=150, facecolor=fig.get_facecolor())
-		print(f"Saved plot to {args.output} ({len(xs)} samples, {args.channels} channels)")
+		summary = ", ".join(f"{c} ({len(xs)} samples)" for c, _, xs, _ in panels)
+		print(f"Saved plot to {args.output}: {summary}")
 	else:
 		plt.show()
 

servo_controller/scripts/send_cmd.py

@@ -46,6 +46,8 @@ COMMANDS = {
 	"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()}
 

servo_controller/scripts/serial_logger.py

@@ -50,6 +50,8 @@ COMMANDS = {
 	"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()}
 

servo_controller/src/adc.c

@@ -1,5 +1,4 @@
 #include "adc.h"
-#include "command_message.h"
 #include "usb.h"
 #include "mux.h"
 

servo_controller/src/command_handler.c

@@ -5,6 +5,7 @@
 #include "adc.h"
 #include "usb.h"
 #include "digital_out.h"
+#include "power_monitor.h"
 
 #include <zephyr/logging/log.h>
 
@@ -123,6 +124,15 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case POWER_MONITOR_SET_READ: {
+			int interval_ms;
+			memcpy(&interval_ms, &msg->data[0], sizeof(int));
+
+			power_monitor_timer_set(interval_ms);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;

servo_controller/src/command_message.h

@@ -24,6 +24,8 @@ typedef enum {
 	ADC_SET_READ,
 	DIGITAL_IN,
 	DIGITAL_LEG_OUT,
+	POWER_MONITOR_SET_READ,
+	POWER_MONITOR_READ,
 
 	// Keep last
 	NUM_COMMANDS,

servo_controller/src/main.c

@@ -4,6 +4,7 @@
 #include "adc.h"
 #include "digital_in.h"
 #include "digital_out.h"
+#include "power_monitor.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -54,5 +55,12 @@ int main(void) {
 		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/power_monitor.c

@@ -0,0 +1,109 @@
+#include "power_monitor.h"
+#include "usb.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);
+
+// ADC TIMER
+struct k_timer power_monitor_timer;
+struct k_sem power_monitor_semaphore;
+
+
+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
+		struct command_message_t *msg = usb_get_next_tx_buf();
+		command_create_message(msg, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
+		usb_send_command(msg);
+	}
+}
+
+int power_monitor_init() {
+
+	k_sem_init(&power_monitor_semaphore, 0, 1);
+	
+	// TIMER
+	k_timer_init(&power_monitor_timer, power_monitor_timer_handler, NULL);
+
+	// 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;
+}

servo_controller/src/power_monitor.h

@@ -0,0 +1,9 @@
+#ifndef POWER_MONITOR_H
+#define POWER_MONITOR_H
+
+
+int power_monitor_init();
+int power_monitor_timer_set(int interval_ms);
+
+
+#endif // POWER_MONITOR_H