Updated in response to feedback.

Now using linked list.

src/main/fc/fc_dispatch.c

@@ -21,61 +21,62 @@
 
 #include <platform.h>
 
+#include "common/utils.h"
+
 #include "drivers/system.h"
 #include "fc/fc_dispatch.h"
 
-#define DISPATCH_QUEUE_SIZE     5
+static dispatchTask_t *head = NULL;
 
-typedef struct {
-    dispatchFuncPtr ptr;
-    uint32_t delayedUntil;
-} dispatchItem_t;
-
-static dispatchItem_t queue[DISPATCH_QUEUE_SIZE];
-static int next = 0;
-
-static void dispatchRemove(int index)
+void dispatchProcess(uint32_t currentTime)
 {
-    if (index == (DISPATCH_QUEUE_SIZE-1)) {
-        queue[index].ptr = NULL;
-        next = index;
+    if (!head || currentTime < head->delayedUntil) {
         return;
     }
 
-    for (int i = index; i < DISPATCH_QUEUE_SIZE-1; i++) {
-        queue[i].ptr = queue[i+1].ptr;
-
-        if (queue[i].ptr == NULL) {
-            next = i;
-            break;
+    dispatchTask_t *current = head;
+    dispatchTask_t *previous = NULL;
+    while (current && current->delayedUntil < currentTime) {
+        if (current->ptr) {
+            (*current->ptr)();
         }
-        queue[i].delayedUntil = queue[i+1].delayedUntil;
+
+        /* remove item from list */
+        if (previous) {
+            previous->next = current->next;
+        } else {
+            head = current->next;
+        }     
+        current->delayedUntil = 0;
+        current = current->next;
     }
 }
 
-void dispatchProcess(void)
+void dispatchAdd(dispatchTask_t *task)
 {
-    if (queue[0].ptr == NULL) {
+    if (!task || task->delayedUntil) {
+        /* invalid or already in the list */
         return;
     }
 
-    for (int i = 0; i < DISPATCH_QUEUE_SIZE; i++) {
-        if (queue[i].ptr == NULL) {
-            break;
-        }
-        if (queue[i].delayedUntil < micros()) {
-            (*queue[i].ptr)();
-            queue[i].ptr = NULL;
-            dispatchRemove(i);
-        }
-    }
-}
+    task->next = NULL;
+    task->delayedUntil = micros() + task->minimumDelayUs;
 
-void dispatchAdd(dispatchFuncPtr ptr, uint32_t delayUs)
-{
-    if (next < DISPATCH_QUEUE_SIZE) {
-        queue[next].ptr = ptr;
-        queue[next].delayedUntil = micros() + delayUs;
-        next++;
+    if (!head) {
+        head = task;
+        return;
     }
+
+    if (task->delayedUntil < head->delayedUntil) {
+        task->next = head;
+        head = task;
+        return;
+    }
+
+    dispatchTask_t *pos = head;
+    while (pos->next && pos->next->delayedUntil < task->delayedUntil) {
+        pos = pos->next;
+    }
+    task->next = pos->next;
+    pos->next = task;
 }

src/main/fc/fc_dispatch.h

@@ -19,5 +19,13 @@
 
 typedef void (*dispatchFuncPtr)(void);
 
-void dispatchProcess(void);
-void dispatchAdd(dispatchFuncPtr ptr, uint32_t delayUs);
+typedef struct dispatchTask_s {
+    dispatchFuncPtr ptr;
+    uint16_t minimumDelayUs;
+
+    uint32_t delayedUntil;
+    struct dispatchTask_s *next;
+} dispatchTask_t;
+
+void dispatchProcess(uint32_t currentTime);
+void dispatchAdd(dispatchTask_t *task);

src/main/fc/fc_tasks.c

@@ -218,13 +218,6 @@ void taskVtxControl(uint32_t currentTime)
 }
 #endif
 
-/* simplified task for dispatching a call to *(void x(void)) after a set period of time */
-void taskDispatch(uint32_t currentTime)
-{
-    UNUSED(currentTime);
-    dispatchProcess();
-}
-
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -359,9 +352,9 @@ cfTask_t cfTasks[TASK_COUNT] = {
 
     [TASK_DISPATCH] = {
         .taskName = "DISPATCH",
-        .taskFunc = taskDispatch,
-        .desiredPeriod = TASK_PERIOD_US(100),
-        .staticPriority = TASK_PRIORITY_REALTIME,
+        .taskFunc = dispatchProcess,
+        .desiredPeriod = TASK_PERIOD_HZ(1000),
+        .staticPriority = TASK_PRIORITY_HIGH,
     },
 
     [TASK_BATTERY] = {

src/main/rx/spektrum.c

@@ -113,6 +113,7 @@ static void spektrumDataReceive(uint16_t c)
 }
 
 static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
+static dispatchTask_t srxlTelemetryTask = { .ptr = srxlRxSendTelemetryData, .minimumDelayUs = 100 };
 
 static uint8_t spektrumFrameStatus(void)
 {
@@ -156,7 +157,7 @@ static uint8_t spektrumFrameStatus(void)
 
     /* only process if 2048, some data in buffer AND servos in phase 0 */
     if (spekHiRes && telemetryBufLen && (spekFrame[2] & 0x80)) {
-        dispatchAdd(srxlRxSendTelemetryData, 100);
+        dispatchAdd(&srxlTelemetryTask);
     }
     return RX_FRAME_COMPLETE;
 }