Added power monitor its own message buffer

servo_controller/src/power_monitor.c

@@ -19,10 +19,15 @@ static k_tid_t power_monitor_thread_id = NULL;
 #define POWER_MONITOR_THREAD_PRIORITY 5
 K_THREAD_STACK_DEFINE(power_monitor_thread_stack, POWER_MONITOR_THREAD_STACK_SIZE);
 
-// ADC TIMER
+// TIMER
 struct k_timer power_monitor_timer;
 struct k_sem power_monitor_semaphore;
 
+// BUFFER
+#define PWR_MSG_BUFFER_SIZE 10
+struct command_message_t pwr_msg_buffer[PWR_MSG_BUFFER_SIZE];
+struct command_message_t *pwr_msg_buffer_ptr;
+
 
 static void power_monitor_timer_handler(struct k_timer *timer) {
     // Timer trigger
@@ -62,9 +67,13 @@ static void power_monitor_thread(void *p1, void *p2, void *p3) {
         sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[2][2]);
 
 		// Send to USB
-		struct command_message_t *msg = usb_get_next_tx_buf();
-		command_create_message(msg, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
-		usb_send_command(msg);
+		command_create_message(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
+		usb_send_command(pwr_msg_buffer_ptr);
+		pwr_msg_buffer_ptr++;
+
+		if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
+			pwr_msg_buffer_ptr = pwr_msg_buffer;
+		}
 	}
 }
 
@@ -106,4 +115,15 @@ int power_monitor_timer_set(int interval_ms) {
 	}
 	
 	return 0;
+}
+
+void* power_monitor_get_buffer() {
+	struct command_message_t *ptr = pwr_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &pwr_msg_buffer[0]) {
+		ptr = &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
 }