diff --git a/Makefile b/Makefile index 9199ebe298..0402c31fd0 100644 --- a/Makefile +++ b/Makefile @@ -523,6 +523,7 @@ COMMON_SRC = \ drivers/timer.c \ fc/config.c \ fc/fc_init.c \ + fc/fc_dispatch.c \ fc/fc_hardfaults.c \ fc/fc_main.c \ fc/fc_msp.c \ diff --git a/src/main/fc/fc_dispatch.c b/src/main/fc/fc_dispatch.c new file mode 100644 index 0000000000..c62135c7a9 --- /dev/null +++ b/src/main/fc/fc_dispatch.c @@ -0,0 +1,81 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "drivers/system.h" +#include "fc/fc_dispatch.h" + +#define DISPATCH_QUEUE_SIZE 5 + +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) +{ + if (index == (DISPATCH_QUEUE_SIZE-1)) { + queue[index].ptr = NULL; + next = index; + 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; + } + queue[i].delayedUntil = queue[i+1].delayedUntil; + } +} + +void dispatchProcess(void) +{ + if (queue[0].ptr == NULL) { + 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); + } + } +} + +void dispatchAdd(dispatchFuncPtr ptr, uint32_t delayUs) +{ + if (next < DISPATCH_QUEUE_SIZE) { + queue[next].ptr = ptr; + queue[next].delayedUntil = micros() + delayUs; + next++; + } +} diff --git a/src/main/fc/fc_dispatch.h b/src/main/fc/fc_dispatch.h new file mode 100644 index 0000000000..ced61513fd --- /dev/null +++ b/src/main/fc/fc_dispatch.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +typedef void (*dispatchFuncPtr)(void); + +void dispatchProcess(void); +void dispatchAdd(dispatchFuncPtr ptr, uint32_t delayUs); \ No newline at end of file diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 24448f58a7..1e093ce22c 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,6 +40,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "fc/serial_cli.h" +#include "fc/fc_dispatch.h" #include "flight/pid.h" #include "flight/altitudehold.h" @@ -217,6 +218,13 @@ 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(); @@ -233,6 +241,8 @@ void fcTasksInit(void) setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); + setTaskEnabled(TASK_DISPATCH, true); + #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif @@ -347,6 +357,13 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, + [TASK_DISPATCH] = { + .taskName = "DISPATCH", + .taskFunc = taskDispatch, + .desiredPeriod = TASK_PERIOD_US(100), + .staticPriority = TASK_PRIORITY_REALTIME, + }, + [TASK_BATTERY] = { .taskName = "BATTERY", .taskFunc = taskUpdateBattery, diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index b9f1d625b3..9578eaa18a 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -34,6 +34,7 @@ #include "io/serial.h" #include "fc/config.h" +#include "fc/fc_dispatch.h" #ifdef TELEMETRY #include "telemetry/telemetry.h" @@ -153,8 +154,9 @@ static uint8_t spektrumFrameStatus(void) } } - if (telemetryBufLen) { - srxlRxSendTelemetryData(); + /* only process if 2048, some data in buffer AND servos in phase 0 */ + if (spekHiRes && telemetryBufLen && (spekFrame[2] & 0x80)) { + dispatchAdd(srxlRxSendTelemetryData, 100); } return RX_FRAME_COMPLETE; } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 06014f01bb..255badcaba 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -54,6 +54,7 @@ typedef enum { TASK_ATTITUDE, TASK_RX, TASK_SERIAL, + TASK_DISPATCH, TASK_BATTERY, #ifdef BEEPER TASK_BEEPER,