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

Added thread safety to USB TX, servo control thread, primitive ADC thread, python test codes

Browse Source
This commit is contained in:
Your Name
2026-09-22 13:01:31 +03:00
parent 1d1416d5d8
commit dd7e6d9456
16 changed files with 944 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
servo_controller/src/usb.c
+88 -32
View File
@@ -17,11 +17,11 @@ LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
const struct device *const uart_dev = DEVICE_DT_GET_ONE(zephyr_cdc_acm_uart);
static struct usbd_context *usb_context;
// THREAD
static struct k_thread usb_thread_data;
static k_tid_t usb_thread_id = NULL;
#define USB_THREAD_STACK_SIZE 2048
K_THREAD_STACK_DEFINE(usb_thread_stack, USB_THREAD_STACK_SIZE);
// RX THREAD
static struct k_thread usb_rx_thread_data;
static k_tid_t usb_rx_thread_id = NULL;
#define USB_RX_THREAD_STACK_SIZE 2048
K_THREAD_STACK_DEFINE(usb_rx_thread_stack, USB_RX_THREAD_STACK_SIZE);
// RX BUFFER
#define RING_BUF_SIZE 255
@@ -29,10 +29,21 @@ static uint8_t ring_buffer[RING_BUF_SIZE];
static struct ring_buf ringbuf;
struct k_sem rx_semaphore;
// TX THREAD
static struct k_thread usb_tx_thread_data;
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)
#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 *)];
struct k_msgq usb_tx_ptr_msgq;
// ACK / NACK messages
#define RETURN_ACK true
struct command_message_t ack_msg;
struct command_message_t nack_msg;
static void interrupt_handler(const struct device *dev, void *user_data) {
@@ -73,7 +84,7 @@ static void interrupt_handler(const struct device *dev, void *user_data) {
}
}
static void usb_thread(void *p1, void *p2, void *p3) {
static void usb_rx_thread(void *p1, void *p2, void *p3) {
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
@@ -109,7 +120,7 @@ static void usb_thread(void *p1, void *p2, void *p3) {
if (calculated_crc != msg.crc) {
if (RETURN_ACK) {
// Send NACK
usb_send_command(&nack_msg);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
}
continue;
}
@@ -118,13 +129,13 @@ static void usb_thread(void *p1, void *p2, void *p3) {
if (ret == 0) {
if (RETURN_ACK) {
// Send ACK
usb_send_command(&ack_msg);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
}
}
else {
if (RETURN_ACK) {
// Send NACK
usb_send_command(&nack_msg);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
}
}
@@ -162,12 +173,36 @@ static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *ms
}
}
static void usb_tx_thread(void *p1, void *p2, void *p3) {
struct command_message_t *data;
while (1) {
k_msgq_get(&usb_tx_ptr_msgq, &data, K_FOREVER);
if (!device_is_ready(uart_dev)) {
// FIXME: Don't drop packages
continue;
}
// Message size: prefix + length + id + command + crc + data
size_t msg_size = COMMAND_HEADER_SIZE + data->length;
uint8_t *msg_bytes = (uint8_t *)data;
/* uart_poll_out blocks until sent, ensuring data integrity */
for (size_t i = 0; i < msg_size; i++) {
uart_poll_out(uart_dev, msg_bytes[i]);
}
}
}
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(&ack_msg);
command_create_nack(&nack_msg);
command_create_ack(&usb_tx_buffer[TX_BUFFER_SIZE]);
command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
int ret;
@@ -194,38 +229,59 @@ int usb_init() {
uart_irq_callback_set(uart_dev, interrupt_handler);
uart_irq_rx_enable(uart_dev);
usb_thread_id = k_thread_create(
&usb_thread_data,
usb_thread_stack,
K_THREAD_STACK_SIZEOF(usb_thread_stack),
usb_thread,
// RX THREAD
usb_rx_thread_id = k_thread_create(
&usb_rx_thread_data,
usb_rx_thread_stack,
K_THREAD_STACK_SIZEOF(usb_rx_thread_stack),
usb_rx_thread,
NULL, NULL, NULL,
5,
0,
K_NO_WAIT
);
if (usb_thread_id == NULL) {
LOG_ERR("Failed to create USB thread");
if (usb_rx_thread_id == NULL) {
LOG_ERR("Failed to create USB RX thread");
return -ENOMEM;
}
// TX THREAD
usb_tx_thread_id = k_thread_create(
&usb_tx_thread_data,
usb_tx_thread_stack,
K_THREAD_STACK_SIZEOF(usb_tx_thread_stack),
usb_tx_thread,
NULL, NULL, NULL,
5,
0,
K_NO_WAIT
);
if (usb_tx_thread_id == NULL) {
LOG_ERR("Failed to create USB TX thread");
return -ENOMEM;
}
return ret;
}
struct command_message_t* usb_get_next_tx_buf() {
struct command_message_t *buf = usb_tx_buf_ptr;
// 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;
}
int usb_send_command(struct command_message_t *msg) {
if (!device_is_ready(uart_dev)) {
return -ENODEV;
}
// Message size: prefix + length + id + command + crc + data
size_t msg_size = 5 + msg->length;
uint8_t *msg_bytes = (uint8_t *)msg;
/* uart_poll_out blocks until sent, ensuring data integrity */
for (size_t i = 0; i < msg_size; i++) {
uart_poll_out(uart_dev, msg_bytes[i]);
}
k_msgq_put(&usb_tx_ptr_msgq, &msg, K_NO_WAIT);
return 0;
}