From 5bba799bdd56335bacf5dcf7868b538749058d54 Mon Sep 17 00:00:00 2001 From: Your Name Date: Sat, 3 Oct 2026 20:50:51 +0300 Subject: [PATCH] First implementation of INA3221 power monitor --- servo_controller/app.overlay | 9 + servo_controller/prj.conf | 7 +- servo_controller/scripts/plot_channels.py | 403 +++++++++++++++++----- servo_controller/scripts/send_cmd.py | 2 + servo_controller/scripts/serial_logger.py | 2 + servo_controller/src/adc.c | 1 - servo_controller/src/command_handler.c | 10 + servo_controller/src/command_message.h | 2 + servo_controller/src/main.c | 8 + servo_controller/src/power_monitor.c | 109 ++++++ servo_controller/src/power_monitor.h | 9 + 11 files changed, 474 insertions(+), 88 deletions(-) create mode 100644 servo_controller/src/power_monitor.c create mode 100644 servo_controller/src/power_monitor.h diff --git a/servo_controller/app.overlay b/servo_controller/app.overlay index e69de29..c233346 100644 --- a/servo_controller/app.overlay +++ b/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>; + }; +}; diff --git a/servo_controller/prj.conf b/servo_controller/prj.conf index 5caec1c..e704802 100644 --- a/servo_controller/prj.conf +++ b/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 \ No newline at end of file diff --git a/servo_controller/scripts/plot_channels.py b/servo_controller/scripts/plot_channels.py index 45f5ade..0c0bca0 100644 --- a/servo_controller/scripts/plot_channels.py +++ b/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 -to back (default N=18, matching an 18-channel ADC_READ_ALL-style packet). -X-axis is the device-clock `tick_ms` column by default (or host_time). +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 -def build_parser(): - p = argparse.ArgumentParser( - description="Plot packed 4-byte-integer channels from a serial_logger.py CSV (dark mode).", - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog="""\ -examples: - %(prog)s -i adc_log.csv - %(prog)s -i adc_log.csv --command ADC_READ_ALL -o plot.png - %(prog)s -i adc_log.csv --channels 18 --unsigned --x host -""", +# -------------------------------------------------------------------------- +# 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)", ) - p.add_argument("-i", "--input", required=True, help="input CSV file (from serial_logger.py)") - p.add_argument("-o", "--output", help="save the plot to this file instead of showing it interactively") - p.add_argument("-c", "--command", help="only use rows with this command name (default: use whatever " - "PACKET rows have the right byte length)") - p.add_argument("-n", "--channels", type=int, default=18, - help="number of 4-byte integer channels packed in data_hex (default: %(default)s)") - p.add_argument("--signed", dest="signed", action="store_true", default=True, - help="interpret channels as signed int32 (default: on)") - p.add_argument("--unsigned", dest="signed", action="store_false", - help="interpret channels as unsigned uint32") +# -------------------------------------------------------------------------- +# CSV loading +# -------------------------------------------------------------------------- - p.add_argument("--x", choices=["tick", "host"], default="tick", - help="x-axis source: device tick_ms, or host_time (default: %(default)s)") - p.add_argument("--labels", help="comma-separated channel labels (default: ch0..chN-1)") - p.add_argument("--title", default="Channel data", help="plot title (default: %(default)s)") - - return p - - -def load_rows(path, command, channel_bytes): - """Read the CSV and return rows that look like packed-channel PACKET rows.""" +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 command and row.get("command") != command: - continue - data_hex = row.get("data_hex", "") - if len(data_hex) != channel_bytes * 2: # 2 hex chars per byte + 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() - 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) - if not rows: - hint = f" with command={args.command!r}" if args.command else "" - print(f"ERROR: no PACKET rows{hint} with {channel_bytes}-byte payloads " - f"({args.channels} x 4-byte ints) found in {args.input}", file=sys.stderr) + 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 - 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 i in range(args.channels): - ax.plot(xs, channels[i], label=labels[i], color=cmap(i), linewidth=1.2) - - ax.set_title(args.title, fontsize=14, color="white") - ax.set_xlabel("tick (ms)" if args.x == "tick" else "host time") - ax.set_ylabel("value" + (" (int32)" if args.signed else " (uint32)")) - ax.grid(True, alpha=0.25) + 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() - ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S")) - ax.legend(ncol=min(6, args.channels), fontsize=8, loc="upper center", - bbox_to_anchor=(0.5, -0.12), frameon=False) - fig.tight_layout() + 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()) - 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() diff --git a/servo_controller/scripts/send_cmd.py b/servo_controller/scripts/send_cmd.py index 8c58c4f..60c590b 100644 --- a/servo_controller/scripts/send_cmd.py +++ b/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()} diff --git a/servo_controller/scripts/serial_logger.py b/servo_controller/scripts/serial_logger.py index 6afe9a0..b4f9fec 100644 --- a/servo_controller/scripts/serial_logger.py +++ b/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()} diff --git a/servo_controller/src/adc.c b/servo_controller/src/adc.c index 63b09c5..7f0ff74 100644 --- a/servo_controller/src/adc.c +++ b/servo_controller/src/adc.c @@ -1,5 +1,4 @@ #include "adc.h" -#include "command_message.h" #include "usb.h" #include "mux.h" diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c index e694d1e..0d0f406 100644 --- a/servo_controller/src/command_handler.c +++ b/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 @@ -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; diff --git a/servo_controller/src/command_message.h b/servo_controller/src/command_message.h index a65fe24..6d23180 100644 --- a/servo_controller/src/command_message.h +++ b/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, diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c index 7030421..1baf400 100644 --- a/servo_controller/src/main.c +++ b/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 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; } \ No newline at end of file diff --git a/servo_controller/src/power_monitor.c b/servo_controller/src/power_monitor.c new file mode 100644 index 0000000..35d7a8b --- /dev/null +++ b/servo_controller/src/power_monitor.c @@ -0,0 +1,109 @@ +#include "power_monitor.h" +#include "usb.h" + +#include +#include +#include + +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; +} \ No newline at end of file diff --git a/servo_controller/src/power_monitor.h b/servo_controller/src/power_monitor.h new file mode 100644 index 0000000..e1bbf13 --- /dev/null +++ b/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 \ No newline at end of file