diff --git a/src/main/fc/fc_dispatch.c b/src/main/fc/fc_dispatch.c index c62135c7a9..d850bb057c 100644 --- a/src/main/fc/fc_dispatch.c +++ b/src/main/fc/fc_dispatch.c @@ -21,61 +21,62 @@ #include +#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; } diff --git a/src/main/fc/fc_dispatch.h b/src/main/fc/fc_dispatch.h index ced61513fd..773f6953ea 100644 --- a/src/main/fc/fc_dispatch.h +++ b/src/main/fc/fc_dispatch.h @@ -19,5 +19,13 @@ typedef void (*dispatchFuncPtr)(void); -void dispatchProcess(void); -void dispatchAdd(dispatchFuncPtr ptr, uint32_t delayUs); \ No newline at end of file +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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 1e093ce22c..8b9a270356 100644 --- a/src/main/fc/fc_tasks.c +++ b/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] = { diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 9578eaa18a..7e6fdde6a2 100644 --- a/src/main/rx/spektrum.c +++ b/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; }