Queue still has issues but its getting somewhere!

Firmware/src/console.h

@@ -3,7 +3,6 @@
 
 #define MAX_COMMAND_LENGTH 256
 #define MAX_ARGS 10
-#define MAX_ARG_LENGTH 32
 
 enum Console
 {

Firmware/src/main.cpp

@@ -1,6 +1,7 @@
 /* Raspberry Pi */
 #include <stdio.h>
 #include "pico/stdlib.h"
+#include "malloc.h"
 
 /* FreeRTOS */
 #include "FreeRTOS.h"
@@ -29,17 +30,61 @@ void led_task(void *pvParams)
     }
 }
 
+void processCommand(Command_t *cmd);
+
+void commandDispatcherTask(void *pvParameters)
+{
+    Command_t *cmd;
+
+    for (;;)
+    {
+        // Technically portMAX_DELAY is not... forever, its like a few months but...
+        if (xQueueReceive(cmdQueue, &cmd, portMAX_DELAY) == pdPASS)
+        {
+            // Process the command
+            processCommand(cmd);
+
+            // Free the allocated memory for the command
+            free(cmd);
+        }
+    }
+}
+
+void processCommand(Command_t *cmd)
+{
+    // Example command processing
+    if (strcmp(cmd->argv[0], "cmd") == 0)
+    {
+        // Handle the command
+        printf("Command: %s\n", cmd->argv[0]);
+        for (int i = 1; i < cmd->argc; i++)
+        {
+            printf("Arg %d: %s\n", i, cmd->argv[i]);
+        }
+    }
+}
+
 int main()
 {
     // From pico SDK, init all
     stdio_init_all();
 
     // FreeRTOS Create queues
-    cmdQueue = xQueueCreate(2, sizeof(Command_t *));
+    cmdQueue = xQueueCreate(4, sizeof(Command_t));
+
+    // Check if the queue was created successfully
+    if (cmdQueue == NULL)
+    {
+        // Handle error
+        ;
+    }
+
+    Command_t testcmd;
 
     // FreeRTOS Create tasks
     xTaskCreate(led_task, "LED_Task", 256, NULL, 1, NULL);
-    xTaskCreate(usb_console, "USB_Console", 512, NULL, 1, NULL);
+    xTaskCreate(commandDispatcherTask, "CmdDispatcher", 512, NULL, 1, NULL);
+    xTaskCreate(usb_console, "USB_Console", 1024, NULL, 1, NULL);
 
     // Begin scheduler
     vTaskStartScheduler();

Firmware/src/usb_console.cpp

@@ -48,14 +48,19 @@ void usb_console(void *pvParams)
                     if (cmd != NULL)
                     {
                         parseCommand(inputBuffer, cmd);
+                        printf("\n [~] %s\n", cmd->command);
 
-                        // Add the command to the queue
-                        if (xQueueSend(cmdQueue, &cmd, portMAX_DELAY) != pdPASS)
+                        printf("%d\n", cmd->argc);
+                        printf("%d\n", cmd->console);
+
+                        // // Add the command to the queue
+                        if (xQueueSend(cmdQueue, &cmd, 200) != pdPASS)
                         {
                             // Handle the error (e.g., queue is full)
-                            free(cmd);
-                            printf("\nCommand in queue.\n");
+                            printf("Could not queue cmd.\n");
                         }
+
+                        free(cmd);
                     }
 
                     bufIndex = 0; // Reset buffer index for the next command