From aea013305e99eb4c184e98dee5fa9247323ecfc0 Mon Sep 17 00:00:00 2001 From: Ano-sys Date: Wed, 3 Jun 2026 14:23:27 +0200 Subject: [PATCH] Implemented task based command handling via fixed tasks, notify and queue --- PicoSDKandFreeRtos/main.cpp | 217 ++++++++++++++++++++++++++++++++---- 1 file changed, 197 insertions(+), 20 deletions(-) diff --git a/PicoSDKandFreeRtos/main.cpp b/PicoSDKandFreeRtos/main.cpp index 7dd33f9..a35a3f7 100644 --- a/PicoSDKandFreeRtos/main.cpp +++ b/PicoSDKandFreeRtos/main.cpp @@ -1,21 +1,146 @@ #include -#include "pico/stdlib.h" +#include +#include +#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: ":\n" +*/ \ No newline at end of file