First implementation of INA3221 power monitor

2026-10-03 20:50:51 +03:00
parent 3e23ddfa3d
commit 5bba799bdd
11 changed files with 474 additions and 88 deletions
@@ -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;
+}