Your Name
133 lines
3.7 KiB
C
133 lines
3.7 KiB
C
#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;
|
|
} |