Centralized TX messages to command handler to support multiple outputs

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