Implemented task based command handling via fixed tasks, notify and queue

PicoSDKandFreeRtos/main.cpp

@@ -1,21 +1,146 @@
 #include <stdio.h>
-#include "pico/stdlib.h"
+#include <stdlib.h>
+#include <string.h>
 
+#include "pico/stdlib.h"
 #include "FreeRTOS.h"
 #include "task.h"
+#include "queue.h"
+#include "semphr.h"
 
-static void hello_task(void *params)
-{
-    (void)params;
+#define PING_COMMAND "ping"
+#define PING_RESPONSE "PONG!"
 
-    while (true) {
-        printf("Hello from FreeRTOS\n");
-        vTaskDelay(pdMS_TO_TICKS(1000));
+#define MAX_COMMAND_LENGTH 64
+
+#define distilleCore(core) (1 << (core))
+
+QueueHandle_t serial_queue;
+QueueHandle_t command_handler_task_queue;
+
+typedef struct {
+    int id;
+    char command[MAX_COMMAND_LENGTH];
+    TaskHandle_t task_handle;
+} CommandTask;
+
+static SemaphoreHandle_t serial_out_mutex;
+
+static void serial_command_handler(CommandTask *cmd_task){
+    const char *cmd_str = (const char *)cmd_task->command;
+    if(strstr(cmd_str, PING_COMMAND) == cmd_str){
+        if(xSemaphoreTake(serial_out_mutex, portMAX_DELAY) == pdTRUE){
+            printf("%d:%s\n", cmd_task->id, PING_RESPONSE);
+            xSemaphoreGive(serial_out_mutex);
+        }
     }
 }
 
-static void led_task(void *params)
-{
+static void read_serial_line_task(void *params){
+    (void)params;
+
+    char buf[MAX_COMMAND_LENGTH];
+    size_t it = 0;
+
+    while(true){
+        int c = getchar_timeout_us(0);
+
+        if(c == PICO_ERROR_TIMEOUT){
+            vTaskDelay(pdMS_TO_TICKS(10));
+            continue;
+        }
+
+        if(c == '\r'){
+            continue;
+        }
+
+        if(it < MAX_COMMAND_LENGTH - 1){
+            buf[it++] = (char)c;
+        }
+        else{
+            printf("Command too long, discarding\n");
+            it = 0;
+        }
+        
+        if(c == '\n'){
+            buf[it] = '\0';
+            it = 0;
+
+            xQueueSendToBack(serial_queue, buf, portMAX_DELAY);
+        }
+    }
+}
+
+bool is_valid_command(const char *command){
+    return strstr(command, PING_COMMAND) == command;
+}
+
+void parse_command(CommandTask *dest, const char *command){
+    char *delim = strchr(command, ':');
+    if(delim == nullptr){
+        dest->id = -1;
+        dest->command[0] = '\0';
+        return;
+    }
+    
+    char *id_str = strndup(command, delim - command);
+
+    dest->id = (int)strtol(id_str, nullptr, 10);
+    free(id_str);
+
+    strncpy(dest->command, delim + 1, MAX_COMMAND_LENGTH - 1);
+    dest->command[MAX_COMMAND_LENGTH - 1] = '\0';
+    return;
+}
+
+void clear_command_task(CommandTask *cmd_task){
+    cmd_task->id = -1;
+    cmd_task->command[0] = '\0';
+}
+
+void command_handler_task(void *params){
+    CommandTask *cmd_task = (CommandTask*)params;
+
+    while(true){
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+        serial_command_handler(cmd_task);
+
+        clear_command_task(cmd_task);
+        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
+    }
+}
+
+static void serial_processing_task(void *params){
+    (void)params;
+
+    char buf[MAX_COMMAND_LENGTH];
+    while(true){
+        if(xQueueReceive(serial_queue, buf, portMAX_DELAY)){
+            CommandTask* cmd_task_p = nullptr;
+            xQueueReceive(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
+            if(cmd_task_p == nullptr){
+                printf("Failed to receive command task from queue\n");
+                continue;
+            }
+
+            parse_command(cmd_task_p, buf);
+            
+            if(cmd_task_p->id == -1  
+               || cmd_task_p->command[0] == '\0'
+               || !is_valid_command(cmd_task_p->command)){
+                printf("Invalid command format: %s\n", buf);
+                clear_command_task(cmd_task_p);
+                xQueueSendToBack(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
+                continue;
+            }
+            
+            xTaskNotifyGive(cmd_task_p->task_handle);
+        }
+    }
+}
+
+static void led_task(void *params){
     (void)params;
 
     int led_pin = 25;
@@ -23,7 +148,7 @@ static void led_task(void *params)
     gpio_init(led_pin);
     gpio_set_dir(led_pin, GPIO_OUT);
 
-    while (true) {
+    while(true){
         gpio_put(led_pin, 1);
         vTaskDelay(pdMS_TO_TICKS(500));
         gpio_put(led_pin, 0);
@@ -32,28 +157,76 @@ static void led_task(void *params)
 }
 
 // needs to be defined to avoid linker errors, see FreeRTOSConfig.h configCHECK_FOR_STACK_OVERFLOW
-extern "C" void vApplicationStackOverflowHook(TaskHandle_t task, char *task_name)
-{
+extern "C" void vApplicationStackOverflowHook(TaskHandle_t task, char *task_name){
     (void)task;
     (void)task_name;
 
     taskDISABLE_INTERRUPTS();
-    while (true) {
+    while(true){
         tight_loop_contents();
     }
 }
 
-int distilleCore(int core_id)
-{
-    return 1u << core_id;
+void init_serial_queue(){
+    serial_queue = xQueueCreate(10, sizeof(char[64]));
+    if (serial_queue == NULL) {
+        printf("Failed to create serial queue\n");
+        while(true){
+            tight_loop_contents();
+        }
+    }
 }
 
-int main()
-{
+void init_command_handler(){
+    command_handler_task_queue = xQueueCreate(10, sizeof(CommandTask*));
+    if (command_handler_task_queue == NULL) {
+        printf("Failed to create command handler task queue\n");
+        while(true){
+            tight_loop_contents();
+        }
+    }
+
+    for(int i = 0; i < 10; i++){
+        CommandTask *cmd_task = (CommandTask*)malloc(sizeof(CommandTask));
+        if(cmd_task == nullptr){
+            printf("Failed to allocate memory for command task\n");
+            while(true){
+                tight_loop_contents();
+            }
+        }
+
+        if(xTaskCreateAffinitySet(command_handler_task, "CmdHandler", 512, cmd_task, 2, distilleCore(0), &cmd_task->task_handle) != pdPASS){
+            printf("Failed to create command handler task\n");
+            while(true){
+                tight_loop_contents();
+            }
+        }
+
+        clear_command_task(cmd_task);
+        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
+    }
+}
+
+void init_serial_out_mutex(){
+    serial_out_mutex = xSemaphoreCreateMutex();
+    if (serial_out_mutex == NULL) {
+        printf("Failed to create serial output mutex\n");
+        while(true){
+            tight_loop_contents();
+        }
+    }
+}
+
+int main(){
     stdio_init_all();
 
-    xTaskCreate(hello_task, "Hello Task", 512, nullptr, 1, nullptr);
-    xTaskCreateAffinitySet(led_task, "LED Task", 512, nullptr, 1, distilleCore(1), nullptr);
+    init_serial_queue();
+    init_command_handler();
+    init_serial_out_mutex();
+
+    xTaskCreateAffinitySet(led_task, "LED", 512, nullptr, 1, distilleCore(0), nullptr);
+    xTaskCreateAffinitySet(serial_processing_task, "Serial Processing", 512, nullptr, 1, distilleCore(0), nullptr);
+    xTaskCreateAffinitySet(read_serial_line_task, "Serial Receiver", 512, nullptr, 1, distilleCore(0), nullptr);
 
     vTaskStartScheduler();
 
@@ -61,3 +234,7 @@ int main()
         tight_loop_contents();
     }
 }
+
+/*
+Serial Command Layout: "<id>:<command>\n"
+*/