From 36f406e69dd64d2135165e4691af78d531d032f2 Mon Sep 17 00:00:00 2001 From: Your Name Date: Sun, 4 Oct 2026 16:44:15 +0300 Subject: [PATCH] Centralized TX messages to command handler to support multiple outputs --- servo_controller/src/adc.c | 4 +- servo_controller/src/command_handler.c | 52 ++++++++++++++++++++++--- servo_controller/src/command_handler.h | 11 ++---- servo_controller/src/digital_in.c | 6 +-- servo_controller/src/main.c | 9 +++++ servo_controller/src/power_monitor.c | 8 +++- servo_controller/src/usb.c | 53 +++++++++++++------------- servo_controller/src/usb.h | 3 +- 8 files changed, 99 insertions(+), 47 deletions(-) diff --git a/servo_controller/src/adc.c b/servo_controller/src/adc.c index a5f6d81..e4850de 100644 --- a/servo_controller/src/adc.c +++ b/servo_controller/src/adc.c @@ -1,5 +1,5 @@ #include "adc.h" -#include "usb.h" +#include "command_handler.h" #include "mux.h" #include @@ -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]) { diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c index 0d0f406..88217c1 100644 --- a/servo_controller/src/command_handler.c +++ b/servo_controller/src/command_handler.c @@ -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; } \ No newline at end of file diff --git a/servo_controller/src/command_handler.h b/servo_controller/src/command_handler.h index 9e0f904..2f450e6 100644 --- a/servo_controller/src/command_handler.h +++ b/servo_controller/src/command_handler.h @@ -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 \ No newline at end of file diff --git a/servo_controller/src/digital_in.c b/servo_controller/src/digital_in.c index 1f047d0..2c966dc 100644 --- a/servo_controller/src/digital_in.c +++ b/servo_controller/src/digital_in.c @@ -1,5 +1,5 @@ #include "digital_in.h" -#include "usb.h" +#include "command_handler.h" #include #include @@ -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) { diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c index 1baf400..a7cb106 100644 --- a/servo_controller/src/main.c +++ b/servo_controller/src/main.c @@ -5,6 +5,7 @@ #include "digital_in.h" #include "digital_out.h" #include "power_monitor.h" +#include "command_handler.h" #include 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(); diff --git a/servo_controller/src/power_monitor.c b/servo_controller/src/power_monitor.c index 8ae364f..d581971 100644 --- a/servo_controller/src/power_monitor.c +++ b/servo_controller/src/power_monitor.c @@ -1,5 +1,5 @@ #include "power_monitor.h" -#include "usb.h" +#include "command_handler.h" #include #include @@ -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"); diff --git a/servo_controller/src/usb.c b/servo_controller/src/usb.c index cd3c7fa..f1bce71 100644 --- a/servo_controller/src/usb.c +++ b/servo_controller/src/usb.c @@ -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) { diff --git a/servo_controller/src/usb.h b/servo_controller/src/usb.h index 08b9a96..d1d25b7 100644 --- a/servo_controller/src/usb.h +++ b/servo_controller/src/usb.h @@ -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);