Add task scheduler and pool allocator

error_sdr/error_sdr.c

@@ -322,6 +322,7 @@ static void dflt_error_handler
     volatile ERROR_CODE error_code
     )
 {
+__disable_irq();
 led_set_color( LED_RED );
 while(1); /* Control flow trap */
 

error_sdr/error_sdr.h

@@ -115,6 +115,7 @@ typedef enum _ERROR_CODE
     ERROR_LORA_CMD_ERROR               , /* Error with LoRa command           */
     ERROR_IMU_I2C_ERROR                , /* Error with IMU I2C handle         */
     ERROR_BARO_I2C_ERROR               , /* Error with Baro I2C handle        */
+    ERROR_NULL_PTR_ERROR               , /* Error trying to use NULL pointer  */
     } ERROR_CODE;
 
 /* Error callback table entry */

pool_allocator/pool_allocator.c

@@ -0,0 +1,99 @@
+/**
+  ******************************************************************************
+  * @file           : pool_allocator.c
+  * @brief          : TODO
+  ******************************************************************************
+  * @copyright
+  *
+  * Copyright (c) 2025 Sun Devil Rocketry.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is covered under the
+  * BSD-3-Clause.
+  *
+  * https://opensource.org/license/bsd-3-clause
+  *
+  ******************************************************************************
+  */
+
+/*------------------------------------------------------------------------------
+ Standard Includes
+------------------------------------------------------------------------------*/
+
+
+/*------------------------------------------------------------------------------
+ Project Includes
+------------------------------------------------------------------------------*/
+#include "main.h"
+#include "pool_allocator.h"
+#include "error_sdr.h"
+#include "debug_sdr.h"
+
+
+/*------------------------------------------------------------------------------
+ Global Variables  
+------------------------------------------------------------------------------*/
+
+
+/*------------------------------------------------------------------------------
+ API Functions
+------------------------------------------------------------------------------*/
+POOL pool_init
+    (
+    uint8_t* pool_memory,
+    size_t size
+    )
+{
+POOL pool;
+size_t block_count = size / sizeof(POOL_CHUNK);
+pool.free_chunk = (POOL_CHUNK*)pool_memory;
+pool.chunk_arr = pool.free_chunk;
+
+for ( size_t i = 0; i < block_count - 1; i++ )
+    {
+    pool.chunk_arr[i].next = &pool.chunk_arr[i + 1];
+    }
+pool.chunk_arr[block_count - 1].next = NULL;
+
+return pool;
+}
+
+
+void* pool_alloc
+    (
+    POOL* pool
+    )
+{
+if ( pool == NULL || pool->free_chunk == NULL)
+    {
+    return NULL;
+    }
+
+POOL_CHUNK* next_free = pool->free_chunk;
+pool->free_chunk = pool->free_chunk->next;
+
+return next_free;
+
+}
+
+
+void pool_free
+    (
+    POOL* pool, 
+    void* ptr
+    )
+{
+/* ptr can be NULL, but I want to check for now */
+debug_assert(pool != NULL && ptr != NULL, ERROR_NULL_PTR_ERROR);
+POOL_CHUNK* chunk = ptr;
+chunk->next = pool->free_chunk;
+pool->free_chunk = chunk;
+
+}
+
+
+/*******************************************************************************
+* END OF FILE                                                                  *
+*******************************************************************************/

pool_allocator/pool_allocator.h

@@ -0,0 +1,95 @@
+/**
+  ******************************************************************************
+  * @file           : pool_allocator.h
+  * @brief          : TODO
+  ******************************************************************************
+  * @copyright
+  *
+  * Copyright (c) 2025 Sun Devil Rocketry.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is covered under the
+  * BSD-3-Clause.
+  *
+  * https://opensource.org/license/bsd-3-clause
+  *
+  ******************************************************************************
+  @verbatim
+  ==============================================================================
+                      ##### Pool allocator features #####
+  ==============================================================================
+    TODO
+  ******************************************************************************
+  @endverbatim
+  */
+
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef POOL_ALLOCATOR_H 
+#define POOL_ALLOCATOR_H 
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*------------------------------------------------------------------------------
+ Includes 
+------------------------------------------------------------------------------*/
+#include <string.h>
+#include <stdbool.h>
+
+
+/*------------------------------------------------------------------------------
+ Macros
+------------------------------------------------------------------------------*/
+#define CHUNK_SIZE 12 /* Defined as the size of a scheduler task for now */
+
+/*------------------------------------------------------------------------------
+ Typedefs 
+------------------------------------------------------------------------------*/
+
+typedef union POOL_CHUNK POOL_CHUNK;
+union POOL_CHUNK
+    {
+    POOL_CHUNK* next;
+    uint8_t arr[CHUNK_SIZE];
+    };
+
+typedef struct
+    {
+    POOL_CHUNK* free_chunk;
+    POOL_CHUNK* chunk_arr;
+    } POOL;
+
+
+/*------------------------------------------------------------------------------
+ Function Prototypes 
+------------------------------------------------------------------------------*/
+POOL pool_init
+    (
+    uint8_t* pool_memory,
+    size_t size
+    );
+
+void* pool_alloc
+    (
+    POOL* pool
+    );
+
+void pool_free
+    (
+    POOL* pool, 
+    void* ptr
+    );
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* POOL_ALLOCATOR_H */
+
+/*******************************************************************************
+* END OF FILE                                                                  * 
+*******************************************************************************/

scheduler/scheduler.c

@@ -0,0 +1,141 @@
+/**
+  ******************************************************************************
+  * @file           : scheduler.c
+  * @brief          : Asynchronous task scheduler
+  ******************************************************************************
+  * @copyright
+  *
+  * Copyright (c) 2025 Sun Devil Rocketry.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is covered under the
+  * BSD-3-Clause.
+  *
+  * https://opensource.org/license/bsd-3-clause
+  *
+  ******************************************************************************
+  */
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+#include "scheduler.h"
+#include "error_sdr.h"
+#include "debug_sdr.h"
+#include "pool_allocator.h"
+
+/* Static Variables ----------------------------------------------------------*/
+
+/* Memory to be used by scheduler*/
+static uint8_t scheduler_pool_mem[SCHEDULER_POOL_SIZE];
+static POOL scheduler_pool;
+
+/* Dummy head for task list */
+static TASK_LIST task_list_head = { 0 };
+
+
+/* Procedures ----------------------------------------------------------------*/
+
+/**
+ * @brief Initializes the scheduler's memory pool
+ */
+void scheduler_init
+    (
+    void
+    )
+{
+scheduler_pool = pool_init(scheduler_pool_mem, SCHEDULER_POOL_SIZE);
+}
+
+
+/**
+ * @brief Schedules a task.
+ * 
+ * @param task The task to perform.
+ * @param scheduled_systick The systick at which the task will be run.
+ * 
+ * @return The status of the scheduler. 
+ */
+SCHEDULER_STATUS schedule_task
+    (
+    task_callback task,
+    uint32_t scheduled_systick
+    )
+{
+if ( scheduled_systick < HAL_GetTick() )
+    {
+    return SCHEDULER_INVALID_SYSTICK;
+    }
+
+TASK_LIST* new_task = pool_alloc(&scheduler_pool);
+if ( new_task == NULL )
+    {
+    return SCHEDULER_FAIL;
+    }
+new_task->task = task;
+new_task->scheduled_systick = scheduled_systick;
+
+/* Insert new task into the front of the list */
+// potential race condition, but I've been having trouble disabling the TIM6 IRQ
+if ( task_list_head.next != NULL )
+    {
+    new_task->next = task_list_head.next;
+    }
+else
+    {
+    new_task->next = NULL;
+    }
+task_list_head.next = new_task;
+
+
+return SCHEDULER_OK;
+
+}
+
+
+SCHEDULER_STATUS task_check_and_execute
+    (
+    void
+    )
+{
+uint32_t systick_now = HAL_GetTick();
+uint16_t status = SCHEDULER_OK;
+
+TASK_LIST* previous = &task_list_head;
+TASK_LIST* current = task_list_head.next;
+
+while ( current != NULL )
+    {
+    if ( current->scheduled_systick <= systick_now )
+        {
+        current->task(); // this needs error handling somehow
+
+        previous->next = current->next;
+        TASK_LIST* next = current->next;
+
+        pool_free(&scheduler_pool, current);
+        current = next;
+        }
+    else
+        {
+        previous = current;
+        current = current->next;
+        }
+    }
+
+return status;
+}
+
+
+void task_scheduler_IT_handler
+    (
+    void
+    )
+{
+SCHEDULER_STATUS status = task_check_and_execute();
+// if ( status == SCHEDULER_CALLBACK_ERROR )
+//     {
+//     error_fail_fast( ERROR_UNKNOWN_FATAL_ERROR ); // TODO
+//     }
+}