Although I haven’t been able to make much progress on the project due to school over the past weeks I did manage to make some substantial project mostly with the scheduler, allowing for fairly good performance on my STM Nucleo boards, rather than it just crashing instantly.

1. Issues with the Old Design

The flight controller’s RTOS scheduler wasn’t working under real hardware timing because global variables like currTaskIndex, topOfStack, and excReturn were being modified from multiple entry points simultaneously. Yield calls, interrupt handlers, and the main task-selection logic were all making independent writes to the same shared task control blocks. Whenever a hardware interrupt fired mid-write, stack pointers and exception return values were saved into the wrong taskControlBlocks slot. This created edge cases where preempted tasks were silently double-queued, state tracking completely desynchronized, and time-critical hardware events were dropped during flight.

2. Centralizing Context Switches in PendSV

To eliminate these race conditions, all state-selection decisions were moved out of individual execution contexts and consolidated into decideNextTask(), which now runs exclusively inside the PendSV assembly handler. Functions like yieldCurrentTask(), decideNextInterruptTask(), and decideNextScheduledTask() no longer touch global scheduler state or choose which task runs next; they simply update local status flags like TaskState::BLOCKED or TaskState::READY inside brief __disable_irq() critical sections and trigger PendSV using SCB_ICSR_PENDSVSET_Msk. When PendSV fires, decideNextTask() saves outgoingSP and outgoingExcReturn to the active block, sets RUNNING tasks to READY, and runs a two-pass check over taskControlBlocks[]—evaluating high-priority TaskType::INTERRUPT tasks first, then TaskType::SCHEDULED tasks, and falling back to index 0 for idle. Since PendSV is the only place allowed to modify currTaskIndex, concurrent state corruption is structurally impossible.