Updated project plan

pins.drawio

@@ -1,6 +1,6 @@
 <mxfile host="Electron" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/29.2.9 Chrome/140.0.7339.249 Electron/38.7.2 Safari/537.36" pages="3">
   <diagram name="Blocks" id="qTBlaR7WWd7gh2COTHJy">
-    <mxGraphModel dx="1285" dy="882" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0">
+    <mxGraphModel grid="1" page="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0">
       <root>
         <mxCell id="0" />
         <mxCell id="1" parent="0" />
@@ -896,9 +896,6 @@
         <mxCell id="gFHrDWAFEzCY88qi_DX8-10" edge="1" parent="1" source="xkMDcVt49qT7mFQ4EbYJ-9" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;" target="gFHrDWAFEzCY88qi_DX8-8">
           <mxGeometry relative="1" as="geometry" />
         </mxCell>
-        <mxCell id="xkMDcVt49qT7mFQ4EbYJ-9" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="ADC thread" vertex="1">
-          <mxGeometry height="60" width="90" x="1950" y="410" as="geometry" />
-        </mxCell>
         <mxCell id="xkMDcVt49qT7mFQ4EbYJ-17" parent="1" style="rounded=0;whiteSpace=wrap;html=1;fontSize=10;" value="GPIO" vertex="1">
           <mxGeometry height="40" width="50" x="1560" y="480" as="geometry" />
         </mxCell>
@@ -908,14 +905,6 @@
         <mxCell id="xkMDcVt49qT7mFQ4EbYJ-10" edge="1" parent="1" source="COvur8ckIkHQRD2Fb0JY-94" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;" target="xkMDcVt49qT7mFQ4EbYJ-9">
           <mxGeometry relative="1" as="geometry" />
         </mxCell>
-        <mxCell id="gFHrDWAFEzCY88qi_DX8-15" edge="1" parent="1" source="xkMDcVt49qT7mFQ4EbYJ-26" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.25;entryDx=0;entryDy=0;" target="gFHrDWAFEzCY88qi_DX8-14">
-          <mxGeometry relative="1" as="geometry">
-            <Array as="points">
-              <mxPoint x="1830" y="290" />
-              <mxPoint x="1830" y="305" />
-            </Array>
-          </mxGeometry>
-        </mxCell>
         <mxCell id="xkMDcVt49qT7mFQ4EbYJ-26" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="Digital in controller" vertex="1">
           <mxGeometry height="60" width="90" x="1720" y="260" as="geometry" />
         </mxCell>
@@ -936,17 +925,6 @@
             </Array>
           </mxGeometry>
         </mxCell>
-        <mxCell id="gFHrDWAFEzCY88qi_DX8-62" edge="1" parent="1" source="xkMDcVt49qT7mFQ4EbYJ-68" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;" target="gFHrDWAFEzCY88qi_DX8-14">
-          <mxGeometry relative="1" as="geometry">
-            <Array as="points">
-              <mxPoint x="1830" y="450" />
-              <mxPoint x="1830" y="375" />
-            </Array>
-          </mxGeometry>
-        </mxCell>
-        <mxCell id="xkMDcVt49qT7mFQ4EbYJ-68" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="Power monitor thread" vertex="1">
-          <mxGeometry height="60" width="90" x="1720" y="420" as="geometry" />
-        </mxCell>
         <mxCell id="xkMDcVt49qT7mFQ4EbYJ-71" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="Leg power switch" vertex="1">
           <mxGeometry height="80" width="120" x="1240" y="300" as="geometry" />
         </mxCell>
@@ -1072,22 +1050,19 @@
           <mxGeometry height="300" width="250.14" x="1150" y="770" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-1" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="USB RX thread" vertex="1">
-          <mxGeometry height="30" width="90" x="1720" y="720" as="geometry" />
+          <mxGeometry height="30" width="90" x="1720" y="760" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-33" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-2" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=1;entryY=0.25;entryDx=0;entryDy=0;" target="COvur8ckIkHQRD2Fb0JY-124">
           <mxGeometry relative="1" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-2" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="USB TX thread" vertex="1">
-          <mxGeometry height="30" width="90" x="1720" y="690" as="geometry" />
+          <mxGeometry height="30" width="90" x="1720" y="730" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-9" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-8" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" target="COvur8ckIkHQRD2Fb0JY-105">
           <mxGeometry relative="1" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-8" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="MUX controller" vertex="1">
-          <mxGeometry height="60" width="90" x="1950" y="330" as="geometry" />
+          <mxGeometry height="60" width="90" x="1950" y="300" as="geometry" />
-        </mxCell>
-        <mxCell id="gFHrDWAFEzCY88qi_DX8-14" parent="1" style="rounded=0;whiteSpace=wrap;html=1;fillColor=light-dark(#1BBCE2,#1035C4);fontColor=default;strokeColor=#006EAF;" value="ZBUS" vertex="1">
-          <mxGeometry height="140" width="70" x="1845" y="270" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-21" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-20" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.25;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;" target="COvur8ckIkHQRD2Fb0JY-174">
           <mxGeometry relative="1" as="geometry">
@@ -1117,11 +1092,16 @@
         <mxCell id="gFHrDWAFEzCY88qi_DX8-56" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;startArrow=classic;startFill=1;" target="gFHrDWAFEzCY88qi_DX8-47" value="">
           <mxGeometry relative="1" as="geometry" />
         </mxCell>
-        <mxCell id="gFHrDWAFEzCY88qi_DX8-30" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="Command handler&amp;nbsp;&lt;div&gt;thread&lt;/div&gt;" vertex="1">
+        <mxCell id="gFHrDWAFEzCY88qi_DX8-30" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="Command handler" vertex="1">
-          <mxGeometry height="60" width="90" x="1720" y="610" as="geometry" />
+          <mxGeometry height="60" width="90" x="1720" y="650" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-32" edge="1" parent="1" source="COvur8ckIkHQRD2Fb0JY-124" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.75;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" target="gFHrDWAFEzCY88qi_DX8-1">
-          <mxGeometry relative="1" as="geometry" />
+          <mxGeometry relative="1" as="geometry">
+            <Array as="points">
+              <mxPoint x="1650" y="730" />
+              <mxPoint x="1650" y="775" />
+            </Array>
+          </mxGeometry>
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-40" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-39" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" target="COvur8ckIkHQRD2Fb0JY-152">
           <mxGeometry relative="1" as="geometry">
@@ -1143,13 +1123,18 @@
           <mxGeometry height="80" width="120" x="1240" y="540" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-46" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="IRQ" vertex="1">
-          <mxGeometry height="30" width="50" x="1720" y="500" as="geometry" />
+          <mxGeometry height="30" width="50" x="1720" y="540" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-49" edge="1" parent="1" source="gFHrDWAFEzCY88qi_DX8-47" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=1;entryY=0.5;entryDx=0;entryDy=0;startArrow=classic;startFill=1;" target="8JtIVVyV2YErFim5cGSA-2">
-          <mxGeometry relative="1" as="geometry" />
+          <mxGeometry relative="1" as="geometry">
+            <Array as="points">
+              <mxPoint x="1640" y="600" />
+              <mxPoint x="1640" y="560" />
+            </Array>
+          </mxGeometry>
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-47" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="LoRa&lt;br&gt;&lt;div&gt;thread&lt;/div&gt;" vertex="1">
-          <mxGeometry height="60" width="90" x="1720" y="530" as="geometry" />
+          <mxGeometry height="60" width="90" x="1720" y="570" as="geometry" />
         </mxCell>
         <mxCell id="gFHrDWAFEzCY88qi_DX8-59" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="IRQ" vertex="1">
           <mxGeometry height="30" width="50" x="1720" y="230" as="geometry" />
@@ -1164,25 +1149,49 @@
           <mxGeometry height="60" width="90" x="1950" y="610" as="geometry" />
         </mxCell>
         <mxCell id="pz70zx9ZGhry34nHQQIc-1" connectable="0" parent="1" style="group" value="" vertex="1">
-          <mxGeometry height="330" width="105" x="1827.5" y="610" as="geometry" />
+          <mxGeometry height="370" width="105" x="1828" y="570" as="geometry" />
         </mxCell>
-        <mxCell id="gFHrDWAFEzCY88qi_DX8-16" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="STATE&lt;div&gt;controller&lt;/div&gt;" vertex="1">
+        <mxCell id="gFHrDWAFEzCY88qi_DX8-16" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="STATE&lt;div&gt;controller&lt;/div&gt;&lt;div&gt;thread&lt;/div&gt;" vertex="1">
-          <mxGeometry height="330" width="105" as="geometry" />
+          <mxGeometry height="370" width="105" as="geometry" />
         </mxCell>
         <mxCell id="GHP3okiXW9Laa2N-MtJK-4" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="BOOTING" vertex="1">
-          <mxGeometry height="40" width="70" x="17.5" y="45" as="geometry" />
+          <mxGeometry height="44.848484848484844" width="70" x="17.5" y="50.45454545454545" as="geometry" />
         </mxCell>
         <mxCell id="AYLe-5JP3gpOVYFiu9-b-1" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="MOTOR INIT" vertex="1">
-          <mxGeometry height="40" width="70" x="17.5" y="100" as="geometry" />
+          <mxGeometry height="44.848484848484844" width="70" x="17.5" y="112.12121212121212" as="geometry" />
         </mxCell>
         <mxCell id="AYLe-5JP3gpOVYFiu9-b-2" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="READY" vertex="1">
-          <mxGeometry height="40" width="70" x="17.5" y="158" as="geometry" />
+          <mxGeometry height="44.848484848484844" width="70" x="17.5" y="177.15151515151516" as="geometry" />
         </mxCell>
         <mxCell id="AYLe-5JP3gpOVYFiu9-b-3" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="IDLE" vertex="1">
-          <mxGeometry height="40" width="70" x="17.5" y="216" as="geometry" />
+          <mxGeometry height="44.848484848484844" width="70" x="17.5" y="242.18181818181816" as="geometry" />
         </mxCell>
         <mxCell id="AYLe-5JP3gpOVYFiu9-b-4" parent="pz70zx9ZGhry34nHQQIc-1" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="SLEEP" vertex="1">
-          <mxGeometry height="40" width="70" x="17.5" y="274" as="geometry" />
+          <mxGeometry height="44.848484848484844" width="70" x="17.5" y="307.2121212121212" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-2" connectable="0" parent="1" style="group" value="" vertex="1">
+          <mxGeometry height="60" width="90" x="1950" y="380" as="geometry" />
+        </mxCell>
+        <mxCell id="xkMDcVt49qT7mFQ4EbYJ-9" parent="0XTEijxMU6OlM955Z0Kw-2" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="ADC&amp;nbsp;&lt;div&gt;thread&lt;/div&gt;" vertex="1">
+          <mxGeometry height="60" width="90" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-1" parent="0XTEijxMU6OlM955Z0Kw-2" style="rounded=0;whiteSpace=wrap;html=1;" value="msg buffer" vertex="1">
+          <mxGeometry height="20" width="90" y="40" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-4" connectable="0" parent="1" style="group" value="" vertex="1">
+          <mxGeometry height="60" width="90" x="1720" y="420" as="geometry" />
+        </mxCell>
+        <mxCell id="xkMDcVt49qT7mFQ4EbYJ-68" parent="0XTEijxMU6OlM955Z0Kw-4" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" value="Power monitor thread" vertex="1">
+          <mxGeometry height="60" width="90" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-3" parent="0XTEijxMU6OlM955Z0Kw-4" style="rounded=0;whiteSpace=wrap;html=1;" value="msg buffer" vertex="1">
+          <mxGeometry height="20" width="90" y="40" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-5" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="TIMER" vertex="1">
+          <mxGeometry height="30" width="50" x="1760" y="480" as="geometry" />
+        </mxCell>
+        <mxCell id="0XTEijxMU6OlM955Z0Kw-6" parent="1" style="rounded=0;whiteSpace=wrap;html=1;" value="TIMER" vertex="1">
+          <mxGeometry height="30" width="50" x="1950" y="440" as="geometry" />
         </mxCell>
       </root>
     </mxGraphModel>

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>
@@ -35,6 +35,11 @@ struct k_msgq adc_mask_msgq;
 // ADC TIMER
 struct k_timer adc_timer;
 
+// ADC BUFFER
+#define ADC_MSG_BUFFER_SIZE 10
+struct command_message_t adc_msg_buffer[ADC_MSG_BUFFER_SIZE];
+struct command_message_t *adc_msg_buffer_ptr;
+
 
 static void adc_timer_handler(struct k_timer *timer) {
     // Timer trigger
@@ -67,9 +72,13 @@ static void adc_thread(void *p1, void *p2, void *p3) {
 		}
 		
 		// Send to USB
-		struct command_message_t *msg = usb_get_next_tx_buf();
+		command_create_message(adc_msg_buffer_ptr, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
-		command_create_message(msg, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+		command_handler_tx(adc_msg_buffer_ptr);
-		usb_send_command(msg);
+		adc_msg_buffer_ptr++;
+
+		if (adc_msg_buffer_ptr > &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1]) {
+			adc_msg_buffer_ptr = adc_msg_buffer;
+		}
 	}
 }
 
@@ -82,6 +91,10 @@ int adc_init_all(void) {
 	// TIMER
 	k_timer_init(&adc_timer, adc_timer_handler, NULL);
 
+	// BUFFER
+	memset(adc_msg_buffer, 0, sizeof(adc_msg_buffer));
+	adc_msg_buffer_ptr = adc_msg_buffer;
+
 	// MUX init
 	ret = mux_init();
 	if (ret != 0) {
@@ -137,7 +150,6 @@ int adc_init_all(void) {
     return 0;
 }
 
-
 int adc_read_id(int id) {
 	int ret;
     int val_mv = 0;
@@ -210,3 +222,14 @@ int adc_timer_set(int interval_ms) {
 	
 	return 0;
 }
+
+void* adc_get_buffer() {
+	struct command_message_t *ptr = adc_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &adc_msg_buffer[0]) {
+		ptr = &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
+}

servo_controller/src/adc.h

@@ -12,6 +12,7 @@ int adc_init_all();
 int adc_read_id(int id);
 int adc_add_mask(uint32_t mask);
 int adc_timer_set(int interval_ms);
+void* adc_get_buffer();
 
 
 #endif // ADC_H

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/command_message.h

@@ -83,7 +83,7 @@ void command_create_nack(struct command_message_t *msg);
 /**
  * @brief Print the command with LOG
  * 
- * @param msg Message to calculate CRC for
+ * @param msg Message to LOG
  */
 void command_log(struct command_message_t *msg);
 

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>
@@ -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(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
-		command_create_message(msg, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
+		command_handler_tx(pwr_msg_buffer_ptr);
-		usb_send_command(msg);
+		pwr_msg_buffer_ptr++;
+
+		if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
+			pwr_msg_buffer_ptr = pwr_msg_buffer;
+		}
 	}
 }
 
@@ -75,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");
@@ -107,3 +120,14 @@ 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;
+}

servo_controller/src/power_monitor.h

@@ -4,6 +4,7 @@
 
 int power_monitor_init();
 int power_monitor_timer_set(int interval_ms);
+void* power_monitor_get_buffer();
 
 
 #endif // POWER_MONITOR_H

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
+void usb_set_tx_state(int state) {
-	usb_tx_buf_ptr++;
+	usb_tx_enabled = state ? 1 : 0;
-
-	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) {

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