aintgit/hexdec
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Centralized TX messages to command handler to support multiple outputs

Browse Source
This commit is contained in:
Your Name
2026-10-04 16:44:15 +03:00
parent 5d511f3aca
commit 36f406e69d
8 changed files with 99 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
servo_controller/src/adc.c
+2 -2
View File
@@ -1,5 +1,5 @@
#include "adc.h"
#include "usb.h"
#include "command_handler.h"
#include "mux.h"
#include <zephyr/logging/log.h>
@@ -73,7 +73,7 @@ static void adc_thread(void *p1, void *p2, void *p3) {
// Send to USB
command_create_message(adc_msg_buffer_ptr, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
usb_send_command(adc_msg_buffer_ptr);
command_handler_tx(adc_msg_buffer_ptr);
adc_msg_buffer_ptr++;
if (adc_msg_buffer_ptr > &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1]) {
servo_controller/src/command_handler.c
+47 -5
View File
@@ -11,7 +11,26 @@
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;
@@ -71,9 +90,9 @@ int command_handler(struct command_message_t *msg) {
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), msg->command, (uint8_t *)&value);
usb_send_command(reply);
command_handler_tx(reply);
break;
}
@@ -83,9 +102,9 @@ int command_handler(struct command_message_t *msg) {
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), msg->command, (uint8_t *)values);
usb_send_command(reply);
command_handler_tx(reply);
break;
}
@@ -139,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 -7
View File
@@ -4,13 +4,10 @@
#include "command_message.h"
/**
* @brief Process received command message
*
* @param msg Command message to process
* @return 0 on success, negative errno on failure
*/
int command_handler(struct command_message_t *msg);
int command_handler_init();
int command_handler_rx(struct command_message_t *msg);
struct command_message_t* cmd_get_next_tx_buf();
int command_handler_tx(struct command_message_t *msg);
#endif // COMMAND_HANDLER_H
servo_controller/src/digital_in.c
+3 -3
View File
@@ -1,5 +1,5 @@
#include "digital_in.h"
#include "usb.h"
#include "command_handler.h"
#include <zephyr/kernel.h>
#include <zephyr/device.h>
@@ -20,9 +20,9 @@ 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 = usb_get_next_tx_buf();
struct command_message_t *msg = cmd_get_next_tx_buf();
command_create_message(msg, sizeof(pins), DIGITAL_IN, (uint8_t *)&pins);
usb_send_command(msg);
command_handler_tx(msg);
}
int digital_in_pin_init(const struct gpio_dt_spec *gpio, uint32_t *bitmask) {
servo_controller/src/main.c
+9
View File
@@ -5,6 +5,7 @@
#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);
@@ -13,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();
servo_controller/src/power_monitor.c
+6 -2
View File
@@ -1,5 +1,5 @@
#include "power_monitor.h"
#include "usb.h"
#include "command_handler.h"
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
@@ -68,7 +68,7 @@ static void power_monitor_thread(void *p1, void *p2, void *p3) {
// Send to USB
command_create_message(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
usb_send_command(pwr_msg_buffer_ptr);
command_handler_tx(pwr_msg_buffer_ptr);
pwr_msg_buffer_ptr++;
if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
@@ -84,6 +84,10 @@ int power_monitor_init() {
// 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");
servo_controller/src/usb.c
+26 -27
View File
@@ -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];
struct command_message_t *usb_tx_buf_ptr;
char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE + 2) * sizeof(struct command_message_t *)];
char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE) * 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
@@ -121,28 +123,28 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
if (calculated_crc != msg.crc) {
if (RETURN_ACK) {
// Send NACK
usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
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_tx_buffer[TX_BUFFER_SIZE].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE]);
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_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
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);
}
}
@@ -205,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]);
command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
// CREATE ACK/NACK
command_create_ack(&ack);
command_create_nack(&nack);
int ret;
@@ -270,20 +272,17 @@ int usb_init() {
return -ENOMEM;
}
usb_device_ready = 1;
return ret;
}
struct command_message_t* usb_get_next_tx_buf() {
struct command_message_t *buf = usb_tx_buf_ptr;
int usb_is_tx_enabled() {
return usb_tx_enabled && usb_device_ready;
}
// Increment the buffer pointer
usb_tx_buf_ptr++;
if (usb_tx_buf_ptr > &usb_tx_buffer[TX_BUFFER_SIZE-1]) {
usb_tx_buf_ptr = usb_tx_buffer;
}
return buf;
void usb_set_tx_state(int state) {
usb_tx_enabled = state ? 1 : 0;
}
int usb_send_command(struct command_message_t *msg) {
servo_controller/src/usb.h
+2 -1
View File
@@ -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);