Centralized TX messages to command handler to support multiple outputs

servo_controller/src/adc.c

@@ -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

@@ -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;
 		}
@@ -141,3 +160,26 @@ 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,13 +4,10 @@
 
 #include "command_message.h"
 
-/**
+int command_handler_init();
- * @brief Process received command message
+int command_handler_rx(struct command_message_t *msg);
- * 
+struct command_message_t* cmd_get_next_tx_buf();
- * @param msg Command message to process
+int command_handler_tx(struct command_message_t *msg);
- * @return 0 on success, negative errno on failure
- */
-int command_handler(struct command_message_t *msg);
 
 
 #endif // COMMAND_HANDLER_H

servo_controller/src/digital_in.c

@@ -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

@@ -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

@@ -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

@@ -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];
+char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE) * sizeof(struct command_message_t *)];
-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;
+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();
+							nack.tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.crc = command_calculate_crc(&nack);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							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();
+							ack.tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE]);
+							ack.crc = command_calculate_crc(&ack);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
+							usb_send_command(&ack);
 						}
 					}
 					else {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
+							nack.tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.crc = command_calculate_crc(&nack);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							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]);
+	// CREATE ACK/NACK
-	command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+	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() {
+int usb_is_tx_enabled() {
-	struct command_message_t *buf = usb_tx_buf_ptr;
+	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

@@ -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);