From bbb83752f7de61187369f763abd737dd6e9813df Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 13 Apr 2013 21:50:47 +0200 Subject: [PATCH 1/8] rebased delayed callback scheduler --- flight/Modules/System/systemmod.c | 3 + flight/targets/CopterControl/Makefile | 1 + .../CopterControl/System/inc/openpilot.h | 1 + .../targets/CopterControl/System/pios_board.c | 3 + .../CopterControl/System/pios_board_posix.c | 3 + flight/targets/OSD/Makefile | 1 + flight/targets/OSD/System/inc/openpilot.h | 1 + flight/targets/OSD/System/pios_board.c | 3 + flight/targets/PipXtreme/Makefile | 1 + .../targets/PipXtreme/System/inc/openpilot.h | 1 + flight/targets/PipXtreme/System/pios_board.c | 3 + .../PipXtreme/System/pios_board_posix.c | 3 + flight/targets/RevoMini/Makefile | 1 + flight/targets/RevoMini/Makefile.osx | 1 + .../targets/RevoMini/System/inc/openpilot.h | 1 + flight/targets/RevoMini/System/pios_board.c | 3 + .../targets/RevoMini/System/pios_board_sim.c | 3 + flight/targets/Revolution/Makefile | 1 + flight/targets/Revolution/Makefile.osx | 1 + .../targets/Revolution/System/inc/openpilot.h | 1 + flight/targets/Revolution/System/pios_board.c | 3 + .../Revolution/System/pios_board_sim.c | 3 + flight/targets/SimPosix/Makefile | 1 + .../targets/SimPosix/System/inc/openpilot.h | 1 + flight/targets/SimPosix/System/pios_board.c | 3 + flight/targets/UAVObjects/callbackscheduler.c | 193 ++++++++++++++++++ .../UAVObjects/inc/callbackscheduler.h | 44 ++++ shared/uavobjectdefinition/taskinfo.xml | 3 + 28 files changed, 287 insertions(+) create mode 100644 flight/targets/UAVObjects/callbackscheduler.c create mode 100644 flight/targets/UAVObjects/inc/callbackscheduler.h diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index 8d6e88508..90eb8e127 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -151,6 +151,9 @@ static void systemTask(void *parameters) /* create all modules thread */ MODULE_TASKCREATE_ALL; + /* start the delayed callback scheduler */ + CallbackSchedulerStart(); + if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let diff --git a/flight/targets/CopterControl/Makefile b/flight/targets/CopterControl/Makefile index c2c69871d..3f2a7a6a1 100644 --- a/flight/targets/CopterControl/Makefile +++ b/flight/targets/CopterControl/Makefile @@ -66,6 +66,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(OPUAVOBJ)/callbackscheduler.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/accessorydesired.c diff --git a/flight/targets/CopterControl/System/inc/openpilot.h b/flight/targets/CopterControl/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100644 --- a/flight/targets/CopterControl/System/inc/openpilot.h +++ b/flight/targets/CopterControl/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/CopterControl/System/pios_board.c b/flight/targets/CopterControl/System/pios_board.c index 86ee24309..f8ebe48e3 100644 --- a/flight/targets/CopterControl/System/pios_board.c +++ b/flight/targets/CopterControl/System/pios_board.c @@ -222,6 +222,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); diff --git a/flight/targets/CopterControl/System/pios_board_posix.c b/flight/targets/CopterControl/System/pios_board_posix.c index 30f949830..ec26be804 100644 --- a/flight/targets/CopterControl/System/pios_board_posix.c +++ b/flight/targets/CopterControl/System/pios_board_posix.c @@ -49,6 +49,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ PIOS_COM_Init(); diff --git a/flight/targets/OSD/Makefile b/flight/targets/OSD/Makefile index 2d0deafcd..0148eb2e1 100644 --- a/flight/targets/OSD/Makefile +++ b/flight/targets/OSD/Makefile @@ -52,6 +52,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(OPUAVOBJ)/callbackscheduler.c ## OSD fonts SRC += $(OPSYSTEM)/fonts.c diff --git a/flight/targets/OSD/System/inc/openpilot.h b/flight/targets/OSD/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100644 --- a/flight/targets/OSD/System/inc/openpilot.h +++ b/flight/targets/OSD/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/OSD/System/pios_board.c b/flight/targets/OSD/System/pios_board.c index eef8e6756..0bca98654 100644 --- a/flight/targets/OSD/System/pios_board.c +++ b/flight/targets/OSD/System/pios_board.c @@ -135,6 +135,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); /* IAP System Setup */ PIOS_IAP_Init(); diff --git a/flight/targets/PipXtreme/Makefile b/flight/targets/PipXtreme/Makefile index e0de6fe89..82293a552 100644 --- a/flight/targets/PipXtreme/Makefile +++ b/flight/targets/PipXtreme/Makefile @@ -43,6 +43,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(OPUAVOBJ)/callbackscheduler.c ## UAVObjects SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c diff --git a/flight/targets/PipXtreme/System/inc/openpilot.h b/flight/targets/PipXtreme/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100755 --- a/flight/targets/PipXtreme/System/inc/openpilot.h +++ b/flight/targets/PipXtreme/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/PipXtreme/System/pios_board.c b/flight/targets/PipXtreme/System/pios_board.c index 8db6b8703..b9a931fb7 100644 --- a/flight/targets/PipXtreme/System/pios_board.c +++ b/flight/targets/PipXtreme/System/pios_board.c @@ -115,6 +115,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + #if defined(PIOS_INCLUDE_TIM) /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); diff --git a/flight/targets/PipXtreme/System/pios_board_posix.c b/flight/targets/PipXtreme/System/pios_board_posix.c index 30f949830..ec26be804 100755 --- a/flight/targets/PipXtreme/System/pios_board_posix.c +++ b/flight/targets/PipXtreme/System/pios_board_posix.c @@ -49,6 +49,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ PIOS_COM_Init(); diff --git a/flight/targets/RevoMini/Makefile b/flight/targets/RevoMini/Makefile index 47f631789..5dff7ed97 100644 --- a/flight/targets/RevoMini/Makefile +++ b/flight/targets/RevoMini/Makefile @@ -70,6 +70,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(OPUAVOBJ)/callbackscheduler.c #ifeq ($(DEBUG), YES) SRC += $(OPSYSTEM)/dcc_stdio.c diff --git a/flight/targets/RevoMini/Makefile.osx b/flight/targets/RevoMini/Makefile.osx index 8dec4a2e5..e5df6ebad 100644 --- a/flight/targets/RevoMini/Makefile.osx +++ b/flight/targets/RevoMini/Makefile.osx @@ -141,6 +141,7 @@ SRC += $(OPSYSTEM)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c +SRC += $(OPUAVOBJ)/callbackscheduler.c SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c else ## TESTCODE diff --git a/flight/targets/RevoMini/System/inc/openpilot.h b/flight/targets/RevoMini/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100644 --- a/flight/targets/RevoMini/System/inc/openpilot.h +++ b/flight/targets/RevoMini/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/RevoMini/System/pios_board.c b/flight/targets/RevoMini/System/pios_board.c index b29004192..d0935c8bf 100644 --- a/flight/targets/RevoMini/System/pios_board.c +++ b/flight/targets/RevoMini/System/pios_board.c @@ -370,6 +370,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_3_cfg); diff --git a/flight/targets/RevoMini/System/pios_board_sim.c b/flight/targets/RevoMini/System/pios_board_sim.c index 07ae969fb..03697e46b 100644 --- a/flight/targets/RevoMini/System/pios_board_sim.c +++ b/flight/targets/RevoMini/System/pios_board_sim.c @@ -154,6 +154,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 { diff --git a/flight/targets/Revolution/Makefile b/flight/targets/Revolution/Makefile index 42b17ea00..cea9e6597 100644 --- a/flight/targets/Revolution/Makefile +++ b/flight/targets/Revolution/Makefile @@ -70,6 +70,7 @@ ifndef TESTAPP SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(OPUAVOBJ)/callbackscheduler.c #ifeq ($(DEBUG), YES) SRC += $(OPSYSTEM)/dcc_stdio.c diff --git a/flight/targets/Revolution/Makefile.osx b/flight/targets/Revolution/Makefile.osx index cd1ed731c..3bf375b09 100644 --- a/flight/targets/Revolution/Makefile.osx +++ b/flight/targets/Revolution/Makefile.osx @@ -145,6 +145,7 @@ SRC += $(OPSYSTEM)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c +SRC += $(OPUAVOBJ)/callbackscheduler.c SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c diff --git a/flight/targets/Revolution/System/inc/openpilot.h b/flight/targets/Revolution/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100644 --- a/flight/targets/Revolution/System/inc/openpilot.h +++ b/flight/targets/Revolution/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/Revolution/System/pios_board.c b/flight/targets/Revolution/System/pios_board.c index f68118801..ba020a808 100644 --- a/flight/targets/Revolution/System/pios_board.c +++ b/flight/targets/Revolution/System/pios_board.c @@ -417,6 +417,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); diff --git a/flight/targets/Revolution/System/pios_board_sim.c b/flight/targets/Revolution/System/pios_board_sim.c index 186683df6..0f1b27df6 100644 --- a/flight/targets/Revolution/System/pios_board_sim.c +++ b/flight/targets/Revolution/System/pios_board_sim.c @@ -154,6 +154,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 { diff --git a/flight/targets/SimPosix/Makefile b/flight/targets/SimPosix/Makefile index 47b90c6ec..24ab15f1e 100644 --- a/flight/targets/SimPosix/Makefile +++ b/flight/targets/SimPosix/Makefile @@ -82,6 +82,7 @@ SRC += $(OPSYSTEM)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c +SRC += $(OPUAVOBJ)/callbackscheduler.c SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c SRC += $(FLIGHTLIB)/CoordinateConversions.c diff --git a/flight/targets/SimPosix/System/inc/openpilot.h b/flight/targets/SimPosix/System/inc/openpilot.h index eb3cadcc7..e4ff3a4ce 100644 --- a/flight/targets/SimPosix/System/inc/openpilot.h +++ b/flight/targets/SimPosix/System/inc/openpilot.h @@ -38,6 +38,7 @@ #include "utlist.h" #include "uavobjectmanager.h" #include "eventdispatcher.h" +#include "callbackscheduler.h" #include "alarms.h" #include "taskmonitor.h" #include "uavtalk.h" diff --git a/flight/targets/SimPosix/System/pios_board.c b/flight/targets/SimPosix/System/pios_board.c index 45b078120..fb1b4459e 100644 --- a/flight/targets/SimPosix/System/pios_board.c +++ b/flight/targets/SimPosix/System/pios_board.c @@ -122,6 +122,9 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Configure IO ports */ /* Configure Telemetry port */ diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c new file mode 100644 index 000000000..89e7fd47b --- /dev/null +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -0,0 +1,193 @@ +/** + ****************************************************************************** + * + * @file callbackscheduler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Scheduler to run callback functions from a shared context with given priorities. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" + +// Private constants +#define MAX_QUEUE_SIZE 1 + +#define STACK_SIZE 128 + +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // we run at regular update speed +#define MAX_UPDATE_PERIOD_MS 1000 + +// Private types +/** + * callback information + */ +struct DelayedCallbackInfoStruct { + DelayedCallback cb; + bool volatile waiting; + struct DelayedCallbackInfoStruct *next; +}; + +// Private variables +static DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW+1]; +static DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW+1]; +static xQueueHandle queue; +static xTaskHandle callbackSchedulerTaskHandle; +static xSemaphoreHandle mutex; +static uint32_t stack_size; +static bool scheduler_started; + +// Private functions +static void CallbackSchedulerTask(); +static bool runNextCallback(DelayedCallbackPriority priority); + + +/** + * Initialize the dispatcher + * \return Success (0), failure (-1) + */ +int32_t CallbackSchedulerInitialize() +{ + // Initialize variables + for (DelayedCallbackPriority p=0; p<=CALLBACK_PRIORITY_LOW; p++) callbackQueue[p] = NULL; + + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) + return -1; + + // Create event queue (dummy queue for self signals) + queue = xQueueCreate(1,sizeof(DelayedCallbackInfo*)); + + //minimal stack_size + stack_size = STACK_SIZE; + + scheduler_started = false; + + // Done + return 0; +} + +int32_t CallbackSchedulerStart() +{ + // Create task + scheduler_started = true; + xTaskCreate( CallbackSchedulerTask, (signed char*)"CallbackScheduler", stack_size/4, NULL, TASK_PRIORITY, &callbackSchedulerTaskHandle ); + TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER, callbackSchedulerTaskHandle); + return 0; +} + +/** + * Dispatch an event by invoking the supplied callback. The function + * returns imidiatelly, the callback is invoked from the event task. + * \param[in] cbinfo the callback handle + * \return Success (0), failure (-1) + */ +int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) +{ + // no semaphore needed + cbinfo->waiting=true; + // Push to queue + return xQueueSend(queue, &cbinfo, 0); // will not block if queue is full +} + +/** + * Dispatch an event at periodic intervals. + * \param[in] ev The event to be dispatched + * \param[in] cb The callback to be invoked + * \param[in] periodMs The period the event is generated + * \return Success (0), failure (-1) + */ +DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + if (!scheduler_started && stacksize>stack_size) stack_size=stacksize; // during system initialisation we can adapt to the maximum needed stack + xSemaphoreGiveRecursive(mutex); + + if (stacksize>stack_size) return NULL; // error - not enough memory + + // create process info + DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo)); + info->next = NULL; + info->waiting = false; + info->cb = cb; + + // add to scheduling queue + LL_APPEND(callbackQueue[priority],info); + + return info; +} + +/** + * Scheduler subtask + * \param[in] priority The scheduling priority of the callback to search for + */ +static bool runNextCallback(DelayedCallbackPriority priority) { + // no such queue + if (priority>CALLBACK_PRIORITY_LOW) return false; + + // queue is empty, search a lower priority queue + if (callbackQueue[priority]==NULL) + return runNextCallback(priority+1); + + + DelayedCallbackInfo *current=queueCursor[priority]; + DelayedCallbackInfo *next; + do { + if (current==NULL) { + next=callbackQueue[priority]; // loop around the end of the list + // also attempt to run a callback that has lower priority + // every time the queue is completely traversed + if (runNextCallback(priority+1)) { + queueCursor[priority]=next; + return true; + } + } else { + next=current->next; + if (current->waiting) { + queueCursor[priority]=next; + current->waiting=false; // flag is reset just before execution. + current->cb(); // call the callback + return true; + } + } + current=next; + } while (current!=queueCursor[priority]); + // once the list has been traversed entirely once, abort (nothing to do) + return false; +} + + +/** + * Scheduler task, responsible of invoking callbacks. + */ +static void CallbackSchedulerTask() +{ + static DelayedCallbackInfo *dummy; + while (1) { + + if (!runNextCallback(CALLBACK_PRIORITY_CRITICAL)) { + // queue has no entries, sleep + xQueueReceive(queue, &dummy, MAX_UPDATE_PERIOD_MS/portTICK_RATE_MS); + } + } +} + + + diff --git a/flight/targets/UAVObjects/inc/callbackscheduler.h b/flight/targets/UAVObjects/inc/callbackscheduler.h new file mode 100644 index 000000000..9462a5605 --- /dev/null +++ b/flight/targets/UAVObjects/inc/callbackscheduler.h @@ -0,0 +1,44 @@ +/** + ****************************************************************************** + * + * @file callbackscheduler.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include files of the uavobjectlist library + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef CALLBACKSCHEDULER_H +#define CALLBACKSCHEDULER_H + +// Public types +typedef enum{CALLBACK_PRIORITY_CRITICAL=0,CALLBACK_PRIORITY_REGULAR=1,CALLBACK_PRIORITY_LOW=2} DelayedCallbackPriority; + +typedef void (*DelayedCallback)(void); + +struct DelayedCallbackInfoStruct; +typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo; + +// Public functions +// +int32_t CallbackSchedulerInitialize(); +int32_t CallbackSchedulerStart(); +int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); +DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize); + +#endif // CALLBACKSCHEDULER_H diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 8179ddb89..a34114b67 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -27,6 +27,7 @@ ModemStat Autotune EventDispatcher + CallbackScheduler @@ -55,6 +56,7 @@ ModemStat Autotune EventDispatcher + CallbackScheduler @@ -87,6 +89,7 @@ ModemStat Autotune EventDispatcher + CallbackScheduler From 87ed9a8be254f5e21243e5c550f29e5936b7ef2b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 14 Apr 2013 04:16:34 +0200 Subject: [PATCH 2/8] reimplemented delayed callback scheduler to allow dispatching from ISR's and multiple schedulers with different task priorities --- flight/targets/UAVObjects/callbackscheduler.c | 239 +++++++++++++----- .../UAVObjects/inc/callbackscheduler.h | 3 +- shared/uavobjectdefinition/taskinfo.xml | 15 +- 3 files changed, 193 insertions(+), 64 deletions(-) diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c index 89e7fd47b..4963e654a 100644 --- a/flight/targets/UAVObjects/callbackscheduler.c +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -2,7 +2,7 @@ ****************************************************************************** * * @file callbackscheduler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Scheduler to run callback functions from a shared context with given priorities. * * @see The GNU Public License (GPL) Version 3 @@ -27,69 +27,93 @@ #include "openpilot.h" // Private constants -#define MAX_QUEUE_SIZE 1 - #define STACK_SIZE 128 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // we run at regular update speed -#define MAX_UPDATE_PERIOD_MS 1000 - // Private types +/** + * task information + */ +struct DelayedCallbackTaskStruct { + DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW+1]; + DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW+1]; + xTaskHandle callbackSchedulerTaskHandle; + signed char name[3]; + uint32_t stackSize; + long taskPriority; + xSemaphoreHandle signal; + struct DelayedCallbackTaskStruct *next; +}; + /** * callback information */ struct DelayedCallbackInfoStruct { DelayedCallback cb; bool volatile waiting; + struct DelayedCallbackTaskStruct *task; struct DelayedCallbackInfoStruct *next; }; + + // Private variables -static DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW+1]; -static DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW+1]; -static xQueueHandle queue; -static xTaskHandle callbackSchedulerTaskHandle; +static struct DelayedCallbackTaskStruct *schedulerTasks; static xSemaphoreHandle mutex; -static uint32_t stack_size; -static bool scheduler_started; +static bool schedulerStarted; // Private functions -static void CallbackSchedulerTask(); -static bool runNextCallback(DelayedCallbackPriority priority); +static void CallbackSchedulerTask(void *task); +static bool runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority); /** - * Initialize the dispatcher + * Initialize the scheduler + * must be called before any othyer functions are called * \return Success (0), failure (-1) */ int32_t CallbackSchedulerInitialize() { // Initialize variables - for (DelayedCallbackPriority p=0; p<=CALLBACK_PRIORITY_LOW; p++) callbackQueue[p] = NULL; + schedulerTasks = NULL; + schedulerStarted = false; // Create mutex mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) + if (mutex == NULL) { return -1; - - // Create event queue (dummy queue for self signals) - queue = xQueueCreate(1,sizeof(DelayedCallbackInfo*)); - - //minimal stack_size - stack_size = STACK_SIZE; - - scheduler_started = false; + } // Done return 0; } +/** + * Start the scheduler + * Will + * \return Success (0), failure (-1) + */ int32_t CallbackSchedulerStart() { - // Create task - scheduler_started = true; - xTaskCreate( CallbackSchedulerTask, (signed char*)"CallbackScheduler", stack_size/4, NULL, TASK_PRIORITY, &callbackSchedulerTaskHandle ); - TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER, callbackSchedulerTaskHandle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // only call once + PIOS_Assert(schedulerStarted==false); + + // start tasks + struct DelayedCallbackTaskStruct *cursor; + int t=0; + LL_FOREACH(schedulerTasks,cursor) { + xTaskCreate( CallbackSchedulerTask, cursor->name, cursor->stackSize/4, cursor, cursor->taskPriority, &cursor->callbackSchedulerTaskHandle ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); + } + t++; + } + + schedulerStarted = true; + + xSemaphoreGiveRecursive(mutex); + return 0; } @@ -97,39 +121,130 @@ int32_t CallbackSchedulerStart() * Dispatch an event by invoking the supplied callback. The function * returns imidiatelly, the callback is invoked from the event task. * \param[in] cbinfo the callback handle - * \return Success (0), failure (-1) + * \return Success (-1), failure (0) */ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) { - // no semaphore needed + PIOS_Assert(cbinfo); + + // no semaphore needed for the callback cbinfo->waiting=true; - // Push to queue - return xQueueSend(queue, &cbinfo, 0); // will not block if queue is full + // but the scheduler as a whole needs to be notified + return xSemaphoreGive(cbinfo->task->signal); } /** - * Dispatch an event at periodic intervals. - * \param[in] ev The event to be dispatched + * Dispatch an event by invoking the supplied callback. The function + * returns imidiatelly, the callback is invoked from the event task. + * \param[in] cbinfo the callback handle + * \param[in] pxHigherPriorityTaskWoken + * xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if + * giving the semaphore caused a task to unblock, and the unblocked task has a + * priority higher than the currently running task. If xSemaphoreGiveFromISR() + * sets this value to pdTRUE then a context switch should be requested before + * the interrupt is exited. + * From FreeRTOS Docu: Context switching from an ISR uses port specific syntax. + * Check the demo task for your port to find the syntax required. + * \return Success (-1), failure (0) + */ +int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken) +{ + PIOS_Assert(cbinfo); + + // no semaphore needed for the callback + cbinfo->waiting=true; + // but the scheduler as a whole needs to be notified + return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); +} + +/** + * Register a new callback to be called by a delayed callback scheduler task * \param[in] cb The callback to be invoked - * \param[in] periodMs The period the event is generated + * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. + * \param[in] taskPriority Priority of the scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task alternatingly according to their callback priority. + * \param[in] stacksize The stack requirements of the callback when called by the scheduler. * \return Success (0), failure (-1) */ -DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize) +DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize) { + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - if (!scheduler_started && stacksize>stack_size) stack_size=stacksize; // during system initialisation we can adapt to the maximum needed stack - xSemaphoreGiveRecursive(mutex); + + // find appropriate scheduler task matching taskPriority + struct DelayedCallbackTaskStruct *task=NULL; + int t=0; + LL_FOREACH(schedulerTasks, task) { + if (task->taskPriority == taskPriority) { + break; // found + } + t++; + } + // if scheduler task for given taskPriority does not exist, create it + if (!task) { + // allocate memory if possible + task = (struct DelayedCallbackTaskStruct*)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); + if (!task) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } - if (stacksize>stack_size) return NULL; // error - not enough memory + // initialize structure + for (DelayedCallbackPriority p=0; p<=CALLBACK_PRIORITY_LOW; p++) { + task->callbackQueue[p] = NULL; + task->queueCursor[p] = NULL; + } + task->name[0] = 'C'; + task->name[1] = 'a'+t; + task->name[2] = 0; + task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); + task->taskPriority = taskPriority; + task->next = NULL; - // create process info + // create the signaling semaphore + vSemaphoreCreateBinary( task->signal ); + if (!task->signal) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + + // add to list of scheduler tasks + LL_APPEND(schedulerTasks, task); + + // Previously registered tasks are spawned when CallbackSchedulerStart() is called. + // Tasks registered afterwards need to spawn upon creation. + if (schedulerStarted) { + xTaskCreate( CallbackSchedulerTask, task->name, task->stackSize/4, task, task->taskPriority, &task->callbackSchedulerTaskHandle ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); + } + } + } + + if (!schedulerStarted && stacksize > task->stackSize) { + task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack + } + + + if (stacksize > task->stackSize) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } + + // initialize callback scheduling info DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo)); + if (!info) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } info->next = NULL; info->waiting = false; + info->task = task; info->cb = cb; // add to scheduling queue - LL_APPEND(callbackQueue[priority],info); + LL_APPEND(task->callbackQueue[priority],info); + + xSemaphoreGiveRecursive(mutex); return info; } @@ -137,38 +252,41 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr /** * Scheduler subtask * \param[in] priority The scheduling priority of the callback to search for + * \return Callback has been executed (boolean) */ -static bool runNextCallback(DelayedCallbackPriority priority) { +static bool runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { // no such queue - if (priority>CALLBACK_PRIORITY_LOW) return false; + if (priority > CALLBACK_PRIORITY_LOW) { + return false; + } // queue is empty, search a lower priority queue - if (callbackQueue[priority]==NULL) - return runNextCallback(priority+1); + if (task->callbackQueue[priority] == NULL) { + return runNextCallback(task, priority + 1); + } - - DelayedCallbackInfo *current=queueCursor[priority]; + DelayedCallbackInfo *current = task->queueCursor[priority]; DelayedCallbackInfo *next; do { if (current==NULL) { - next=callbackQueue[priority]; // loop around the end of the list + next=task->callbackQueue[priority]; // loop around the end of the list // also attempt to run a callback that has lower priority // every time the queue is completely traversed - if (runNextCallback(priority+1)) { - queueCursor[priority]=next; + if (runNextCallback(task, priority + 1)) { + task->queueCursor[priority] = next; return true; } } else { - next=current->next; + next = current->next; if (current->waiting) { - queueCursor[priority]=next; - current->waiting=false; // flag is reset just before execution. + task->queueCursor[priority] = next; + current->waiting = false; // flag is reset just before execution. current->cb(); // call the callback return true; } } - current=next; - } while (current!=queueCursor[priority]); + current = next; + } while (current != task->queueCursor[priority]); // once the list has been traversed entirely once, abort (nothing to do) return false; } @@ -176,15 +294,16 @@ static bool runNextCallback(DelayedCallbackPriority priority) { /** * Scheduler task, responsible of invoking callbacks. + * \param[in] task The scheduling task being run */ -static void CallbackSchedulerTask() +static void CallbackSchedulerTask(void *task) { - static DelayedCallbackInfo *dummy; + while (1) { - if (!runNextCallback(CALLBACK_PRIORITY_CRITICAL)) { + if (!runNextCallback((struct DelayedCallbackTaskStruct*) task,CALLBACK_PRIORITY_CRITICAL)) { // queue has no entries, sleep - xQueueReceive(queue, &dummy, MAX_UPDATE_PERIOD_MS/portTICK_RATE_MS); + xSemaphoreTake( ((struct DelayedCallbackTaskStruct*)task)->signal, portMAX_DELAY); } } } diff --git a/flight/targets/UAVObjects/inc/callbackscheduler.h b/flight/targets/UAVObjects/inc/callbackscheduler.h index 9462a5605..78fccd7e3 100644 --- a/flight/targets/UAVObjects/inc/callbackscheduler.h +++ b/flight/targets/UAVObjects/inc/callbackscheduler.h @@ -39,6 +39,7 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo; int32_t CallbackSchedulerInitialize(); int32_t CallbackSchedulerStart(); int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); -DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize); +int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); +DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize); #endif // CALLBACKSCHEDULER_H diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index a34114b67..bd4104f9e 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -27,7 +27,10 @@ ModemStat Autotune EventDispatcher - CallbackScheduler + CallbackScheduler0 + CallbackScheduler1 + CallbackScheduler2 + CallbackScheduler3 @@ -56,7 +59,10 @@ ModemStat Autotune EventDispatcher - CallbackScheduler + CallbackScheduler0 + CallbackScheduler1 + CallbackScheduler2 + CallbackScheduler3 @@ -89,7 +95,10 @@ ModemStat Autotune EventDispatcher - CallbackScheduler + CallbackScheduler0 + CallbackScheduler1 + CallbackScheduler2 + CallbackScheduler3 From 6f1bb9471866afa4d0cfc80368f538ba2f62f0f2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 18 Apr 2013 14:17:29 +0200 Subject: [PATCH 3/8] Delayed Callback Scheduler is now able to schedule delayed execution at an arbitrary time in the future --- flight/targets/UAVObjects/callbackscheduler.c | 136 ++++++++++++++---- .../UAVObjects/inc/callbackscheduler.h | 2 + 2 files changed, 107 insertions(+), 31 deletions(-) diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c index 4963e654a..a309f0c0e 100644 --- a/flight/targets/UAVObjects/callbackscheduler.c +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -28,6 +28,7 @@ // Private constants #define STACK_SIZE 128 +#define MAX_SLEEP 1000 // Private types /** @@ -50,6 +51,7 @@ struct DelayedCallbackTaskStruct { struct DelayedCallbackInfoStruct { DelayedCallback cb; bool volatile waiting; + uint32_t volatile scheduletime; struct DelayedCallbackTaskStruct *task; struct DelayedCallbackInfoStruct *next; }; @@ -63,8 +65,7 @@ static bool schedulerStarted; // Private functions static void CallbackSchedulerTask(void *task); -static bool runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority); - +static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority); /** * Initialize the scheduler @@ -89,7 +90,6 @@ int32_t CallbackSchedulerInitialize() /** * Start the scheduler - * Will * \return Success (0), failure (-1) */ int32_t CallbackSchedulerStart() @@ -109,17 +109,69 @@ int32_t CallbackSchedulerStart() } t++; } - + schedulerStarted = true; xSemaphoreGiveRecursive(mutex); - + return 0; } +/** + * Schedule dispatching a callback at some point in the future. The function returns immediately. + * \param[in] cbinfo the callback handle + * \param[in] milliseconds How far in the future to dispatch the callback + * \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet. + * The options are: + * UPDATEMODE_NONE: An existing schedule will not be touched, the call will have no effect at all if there's an existing schedule. + * UPDATEMODE_SOONER: The callback will be rescheduled only if the new schedule triggers before the original one would have triggered. + * UPDATEMODE_LATER: The callback will be rescheduled only if the new schedule triggers after the original one would have triggered. + * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override) + * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden + */ +int32_t DelayedCallbackSchedule(DelayedCallbackInfo *cbinfo, int32_t milliseconds, DelayedCallbackUpdateMode updatemode) +{ + int32_t result=0; + + PIOS_Assert(cbinfo); + + if (milliseconds<=0) { + milliseconds=0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t + } + + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + uint32_t new = xTaskGetTickCount() + (milliseconds/portTICK_RATE_MS); + if (!new) { + new = 1; // zero has a special meaning, schedule at time 1 instead + } + + int32_t diff = new - cbinfo->scheduletime; + if ( (!cbinfo->scheduletime) + || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff<0) + || ((updatemode & CALLBACK_UPDATEMODE_LATER) && diff>0) + ) { + + // the scheduletime may be updated + if (!cbinfo->scheduletime) { + result=1; + } else { + result=2; + } + cbinfo->scheduletime = new; + + // scheduler needs to be notified to adapt sleep times + xSemaphoreGive(cbinfo->task->signal); + } + + xSemaphoreGiveRecursive(mutex); + + return result; +} + /** * Dispatch an event by invoking the supplied callback. The function - * returns imidiatelly, the callback is invoked from the event task. + * returns immediately, the callback is invoked from the event task. * \param[in] cbinfo the callback handle * \return Success (-1), failure (0) */ @@ -135,7 +187,7 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) /** * Dispatch an event by invoking the supplied callback. The function - * returns imidiatelly, the callback is invoked from the event task. + * returns immediately, the callback is invoked from the event task. * \param[in] cbinfo the callback handle * \param[in] pxHigherPriorityTaskWoken * xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if @@ -161,15 +213,15 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh * Register a new callback to be called by a delayed callback scheduler task * \param[in] cb The callback to be invoked * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. - * \param[in] taskPriority Priority of the scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task alternatingly according to their callback priority. + * \param[in] taskPriority Priority of the scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. * \param[in] stacksize The stack requirements of the callback when called by the scheduler. * \return Success (0), failure (-1) */ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize) { - + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - + // find appropriate scheduler task matching taskPriority struct DelayedCallbackTaskStruct *task=NULL; int t=0; @@ -236,14 +288,15 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr xSemaphoreGiveRecursive(mutex); return NULL; // error - not enough memory } - info->next = NULL; - info->waiting = false; - info->task = task; - info->cb = cb; + info->next = NULL; + info->waiting = false; + info->scheduletime = 0; + info->task = task; + info->cb = cb; // add to scheduling queue LL_APPEND(task->callbackQueue[priority],info); - + xSemaphoreGiveRecursive(mutex); return info; @@ -251,13 +304,18 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr /** * Scheduler subtask + * \param[in] task The scheduler task in question * \param[in] priority The scheduling priority of the callback to search for - * \return Callback has been executed (boolean) + * \return wait time until next scheduled callback is due - 0 if a callback has just been executed */ -static bool runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { +static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { + + int32_t result = MAX_SLEEP; + int32_t diff; + // no such queue if (priority > CALLBACK_PRIORITY_LOW) { - return false; + return result; } // queue is empty, search a lower priority queue @@ -272,38 +330,54 @@ static bool runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallb next=task->callbackQueue[priority]; // loop around the end of the list // also attempt to run a callback that has lower priority // every time the queue is completely traversed - if (runNextCallback(task, priority + 1)) { - task->queueCursor[priority] = next; - return true; + diff = runNextCallback(task, priority + 1); + if (!diff) { + task->queueCursor[priority] = next; // the recursive call has executed a callback + return 0; + } + if (diffnext; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected + if (current->scheduletime) { + diff = next->scheduletime - xTaskGetTickCount(); + if (diff<=0) { + current->waiting = true; + } else if (diffwaiting) { task->queueCursor[priority] = next; - current->waiting = false; // flag is reset just before execution. + current->scheduletime = 0; // any schedules are reset + current->waiting = false; // the flag is reset just before execution. + xSemaphoreGiveRecursive(mutex); current->cb(); // call the callback - return true; + return 0; } + xSemaphoreGiveRecursive(mutex); } current = next; } while (current != task->queueCursor[priority]); - // once the list has been traversed entirely once, abort (nothing to do) - return false; + // once the list has been traversed entirely without finding any to be executed task, abort (nothing to do) + return result; } - /** * Scheduler task, responsible of invoking callbacks. * \param[in] task The scheduling task being run */ static void CallbackSchedulerTask(void *task) { - + uint32_t delay; while (1) { - - if (!runNextCallback((struct DelayedCallbackTaskStruct*) task,CALLBACK_PRIORITY_CRITICAL)) { - // queue has no entries, sleep - xSemaphoreTake( ((struct DelayedCallbackTaskStruct*)task)->signal, portMAX_DELAY); + + delay = runNextCallback((struct DelayedCallbackTaskStruct*) task, CALLBACK_PRIORITY_CRITICAL); + if (delay) { + // nothing to do but sleep + xSemaphoreTake( ((struct DelayedCallbackTaskStruct*)task)->signal, delay); } } } diff --git a/flight/targets/UAVObjects/inc/callbackscheduler.h b/flight/targets/UAVObjects/inc/callbackscheduler.h index 78fccd7e3..840c74516 100644 --- a/flight/targets/UAVObjects/inc/callbackscheduler.h +++ b/flight/targets/UAVObjects/inc/callbackscheduler.h @@ -28,6 +28,7 @@ // Public types typedef enum{CALLBACK_PRIORITY_CRITICAL=0,CALLBACK_PRIORITY_REGULAR=1,CALLBACK_PRIORITY_LOW=2} DelayedCallbackPriority; +typedef enum{CALLBACK_UPDATEMODE_NONE=0,CALLBACK_UPDATEMODE_SOONER=1,CALLBACK_UPDATEMODE_LATER=2,CALLBACK_UPDATEMODE_OVERRIDE=3} DelayedCallbackUpdateMode; typedef void (*DelayedCallback)(void); @@ -38,6 +39,7 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo; // int32_t CallbackSchedulerInitialize(); int32_t CallbackSchedulerStart(); +int32_t DelayedCallbackSchedule(DelayedCallbackInfo *cbinfo, int32_t milliseconds, DelayedCallbackUpdateMode updatemode); int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize); From 17e8268e811efcabb71ba822f6d63027ae5afcea Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 18 Apr 2013 18:35:57 +0200 Subject: [PATCH 4/8] typo fix in callback scheduler --- flight/targets/UAVObjects/callbackscheduler.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c index a309f0c0e..f6ee7bc5d 100644 --- a/flight/targets/UAVObjects/callbackscheduler.c +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -342,7 +342,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa next = current->next; xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected if (current->scheduletime) { - diff = next->scheduletime - xTaskGetTickCount(); + diff = current->scheduletime - xTaskGetTickCount(); if (diff<=0) { current->waiting = true; } else if (diff Date: Thu, 18 Apr 2013 18:58:50 +0200 Subject: [PATCH 5/8] Test suite to test callback scheduler --- flight/Modules/CallbackTest/callbacktest.c | 132 +++++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 flight/Modules/CallbackTest/callbacktest.c diff --git a/flight/Modules/CallbackTest/callbacktest.c b/flight/Modules/CallbackTest/callbacktest.c new file mode 100644 index 000000000..0ef45e9cd --- /dev/null +++ b/flight/Modules/CallbackTest/callbacktest.c @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * + * @file callbacktest.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Example module to be used as a template for actual modules. + * Event callback version. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * + * This is a test suite to test the callback scheduler, + * including its forward scheduling ability + * + */ + +#include "openpilot.h" + +// Private constants +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +// Private types + +//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) +#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex); + +static xSemaphoreHandle mutex; + +// Private variables +static DelayedCallbackInfo *cbinfo[10]; + +static volatile int32_t counter[10]={0}; + +static void DelayedCb0(); +static void DelayedCb1(); +static void DelayedCb2(); +static void DelayedCb3(); +static void DelayedCb4(); +static void DelayedCb5(); +static void DelayedCb6(); +/** + * Initialise the module, called on startup. + * \returns 0 on success or -1 if initialisation failed + */ +int32_t CallbackTestInitialize() +{ + + mutex = xSemaphoreCreateRecursiveMutex(); + + cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); + cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE); + + + return 0; +} +int32_t CallbackTestStart() { + + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + DelayedCallbackDispatch(cbinfo[3]); + DelayedCallbackDispatch(cbinfo[2]); + DelayedCallbackDispatch(cbinfo[1]); + DelayedCallbackDispatch(cbinfo[0]); + // different callback priorities within a taskpriority + DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE); + DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE); + DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE); + // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following + DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); + // delayed counter with 500 ms + DelayedCallbackDispatch(cbinfo[6]); + // high task prio + xSemaphoreGiveRecursive(mutex); + return 0; +} + +static void DelayedCb0() { + DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]); + if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]); +} +static void DelayedCb1() { + DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]); + if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]); +} +static void DelayedCb2() { + DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]); + if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]); +} +static void DelayedCb3() { + DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]); + if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]); +} +static void DelayedCb4() { + DEBUGPRINT("delayed scheduled callback 4 reached!\n"); + exit(0); +} +static void DelayedCb5() { + DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]); + if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); + // it will likely only reach 8 due to cb4 aborting the run +} +static void DelayedCb6() { + DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]); + if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]); +} From 4d1729c415b116b045f8834bef7fd58c5903a453 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Apr 2013 09:07:35 +0200 Subject: [PATCH 6/8] syntax cleanup and betterheader comments --- flight/targets/UAVObjects/callbackscheduler.c | 113 +++++++++++------- .../UAVObjects/inc/callbackscheduler.h | 94 ++++++++++++++- 2 files changed, 159 insertions(+), 48 deletions(-) diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c index f6ee7bc5d..bec932c5d 100644 --- a/flight/targets/UAVObjects/callbackscheduler.c +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -35,8 +35,8 @@ * task information */ struct DelayedCallbackTaskStruct { - DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW+1]; - DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW+1]; + DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1]; + DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1]; xTaskHandle callbackSchedulerTaskHandle; signed char name[3]; uint32_t stackSize; @@ -69,7 +69,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa /** * Initialize the scheduler - * must be called before any othyer functions are called + * must be called before any other functions are called * \return Success (0), failure (-1) */ int32_t CallbackSchedulerInitialize() @@ -89,7 +89,13 @@ int32_t CallbackSchedulerInitialize() } /** - * Start the scheduler + * Start all scheduler tasks + * Will instantiate all scheduler tasks registered so far. Although new + * callbacks CAN be registered beyond that point, any further scheduling tasks + * will be started the moment of instantiation. It is not possible to increase + * the STACK requirements of a scheduler task after this function has been + * run. No callbacks will be run before this function is called, although + * they can be marked for later execution by executing the dispatch function. * \return Success (0), failure (-1) */ int32_t CallbackSchedulerStart() @@ -97,13 +103,20 @@ int32_t CallbackSchedulerStart() xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // only call once - PIOS_Assert(schedulerStarted==false); + PIOS_Assert(schedulerStarted == false); // start tasks - struct DelayedCallbackTaskStruct *cursor; - int t=0; - LL_FOREACH(schedulerTasks,cursor) { - xTaskCreate( CallbackSchedulerTask, cursor->name, cursor->stackSize/4, cursor, cursor->taskPriority, &cursor->callbackSchedulerTaskHandle ); + struct DelayedCallbackTaskStruct *cursor = NULL; + int t = 0; + LL_FOREACH(schedulerTasks, cursor) { + xTaskCreate( + CallbackSchedulerTask, + cursor->name, + cursor->stackSize/4, + cursor, + cursor->taskPriority, + &cursor->callbackSchedulerTaskHandle + ); if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); } @@ -119,7 +132,7 @@ int32_t CallbackSchedulerStart() /** * Schedule dispatching a callback at some point in the future. The function returns immediately. - * \param[in] cbinfo the callback handle + * \param[in] *cbinfo the callback handle * \param[in] milliseconds How far in the future to dispatch the callback * \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet. * The options are: @@ -129,14 +142,18 @@ int32_t CallbackSchedulerStart() * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override) * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden */ -int32_t DelayedCallbackSchedule(DelayedCallbackInfo *cbinfo, int32_t milliseconds, DelayedCallbackUpdateMode updatemode) +int32_t DelayedCallbackSchedule( + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode + ) { - int32_t result=0; + int32_t result = 0; PIOS_Assert(cbinfo); - if (milliseconds<=0) { - milliseconds=0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t + if (milliseconds <= 0) { + milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t } xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -148,15 +165,15 @@ int32_t DelayedCallbackSchedule(DelayedCallbackInfo *cbinfo, int32_t millisecond int32_t diff = new - cbinfo->scheduletime; if ( (!cbinfo->scheduletime) - || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff<0) - || ((updatemode & CALLBACK_UPDATEMODE_LATER) && diff>0) + || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0) + || ((updatemode & CALLBACK_UPDATEMODE_LATER ) && diff > 0) ) { // the scheduletime may be updated if (!cbinfo->scheduletime) { - result=1; + result = 1; } else { - result=2; + result = 2; } cbinfo->scheduletime = new; @@ -180,7 +197,7 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) PIOS_Assert(cbinfo); // no semaphore needed for the callback - cbinfo->waiting=true; + cbinfo->waiting = true; // but the scheduler as a whole needs to be notified return xSemaphoreGive(cbinfo->task->signal); } @@ -204,7 +221,7 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh PIOS_Assert(cbinfo); // no semaphore needed for the callback - cbinfo->waiting=true; + cbinfo->waiting = true; // but the scheduler as a whole needs to be notified return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); } @@ -213,18 +230,23 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh * Register a new callback to be called by a delayed callback scheduler task * \param[in] cb The callback to be invoked * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. - * \param[in] taskPriority Priority of the scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. + * \param[in] taskPriority Priority of the entire scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. * \param[in] stacksize The stack requirements of the callback when called by the scheduler. - * \return Success (0), failure (-1) + * \return CallbackInfo Pointer on success, NULL if failed. */ -DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize) +DelayedCallbackInfo *DelayedCallbackCreate( + DelayedCallback cb, + DelayedCallbackPriority priority, + long taskPriority, + uint32_t stacksize + ) { xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // find appropriate scheduler task matching taskPriority - struct DelayedCallbackTaskStruct *task=NULL; - int t=0; + struct DelayedCallbackTaskStruct *task = NULL; + int t = 0; LL_FOREACH(schedulerTasks, task) { if (task->taskPriority == taskPriority) { break; // found @@ -241,14 +263,14 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr } // initialize structure - for (DelayedCallbackPriority p=0; p<=CALLBACK_PRIORITY_LOW; p++) { + for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) { task->callbackQueue[p] = NULL; - task->queueCursor[p] = NULL; + task->queueCursor[p] = NULL; } task->name[0] = 'C'; task->name[1] = 'a'+t; task->name[2] = 0; - task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); + task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); task->taskPriority = taskPriority; task->next = NULL; @@ -265,7 +287,14 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr // Previously registered tasks are spawned when CallbackSchedulerStart() is called. // Tasks registered afterwards need to spawn upon creation. if (schedulerStarted) { - xTaskCreate( CallbackSchedulerTask, task->name, task->stackSize/4, task, task->taskPriority, &task->callbackSchedulerTaskHandle ); + xTaskCreate( + CallbackSchedulerTask, + task->name, + task->stackSize/4, + task, + task->taskPriority, + &task->callbackSchedulerTaskHandle + ); if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); } @@ -295,7 +324,7 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr info->cb = cb; // add to scheduling queue - LL_APPEND(task->callbackQueue[priority],info); + LL_APPEND(task->callbackQueue[priority], info); xSemaphoreGiveRecursive(mutex); @@ -308,10 +337,11 @@ DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPr * \param[in] priority The scheduling priority of the callback to search for * \return wait time until next scheduled callback is due - 0 if a callback has just been executed */ -static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { +static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) +{ int32_t result = MAX_SLEEP; - int32_t diff; + int32_t diff = 0; // no such queue if (priority > CALLBACK_PRIORITY_LOW) { @@ -326,8 +356,8 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa DelayedCallbackInfo *current = task->queueCursor[priority]; DelayedCallbackInfo *next; do { - if (current==NULL) { - next=task->callbackQueue[priority]; // loop around the end of the list + if (current == NULL) { + next = task->callbackQueue[priority]; // loop around the end of the list // also attempt to run a callback that has lower priority // every time the queue is completely traversed diff = runNextCallback(task, priority + 1); @@ -335,7 +365,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa task->queueCursor[priority] = next; // the recursive call has executed a callback return 0; } - if (diffscheduletime) { diff = current->scheduletime - xTaskGetTickCount(); - if (diff<=0) { + if (diff <= 0) { current->waiting = true; - } else if (diffsignal, delay); + xSemaphoreTake(((struct DelayedCallbackTaskStruct*)task)->signal, delay); } } } - - diff --git a/flight/targets/UAVObjects/inc/callbackscheduler.h b/flight/targets/UAVObjects/inc/callbackscheduler.h index 840c74516..0ffcbe225 100644 --- a/flight/targets/UAVObjects/inc/callbackscheduler.h +++ b/flight/targets/UAVObjects/inc/callbackscheduler.h @@ -27,8 +27,18 @@ #define CALLBACKSCHEDULER_H // Public types -typedef enum{CALLBACK_PRIORITY_CRITICAL=0,CALLBACK_PRIORITY_REGULAR=1,CALLBACK_PRIORITY_LOW=2} DelayedCallbackPriority; -typedef enum{CALLBACK_UPDATEMODE_NONE=0,CALLBACK_UPDATEMODE_SOONER=1,CALLBACK_UPDATEMODE_LATER=2,CALLBACK_UPDATEMODE_OVERRIDE=3} DelayedCallbackUpdateMode; +typedef enum{ + CALLBACK_PRIORITY_CRITICAL = 0, + CALLBACK_PRIORITY_REGULAR = 1, + CALLBACK_PRIORITY_LOW = 2 + } DelayedCallbackPriority; + +typedef enum{ + CALLBACK_UPDATEMODE_NONE = 0, + CALLBACK_UPDATEMODE_SOONER = 1, + CALLBACK_UPDATEMODE_LATER = 2, + CALLBACK_UPDATEMODE_OVERRIDE = 3 + } DelayedCallbackUpdateMode; typedef void (*DelayedCallback)(void); @@ -37,11 +47,83 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo; // Public functions // + +/** + * Initialize the scheduler + * must be called before any other functions are called + * \return Success (0), failure (-1) + */ int32_t CallbackSchedulerInitialize(); + +/** + * Start all scheduler tasks + * Will instantiate all scheduler tasks registered so far. Although new + * callbacks CAN be registered beyond that point, any further scheduling tasks + * will be started the moment of instantiation. It is not possible to increase + * the STACK requirements of a scheduler task after this function has been + * run. No callbacks will be run before this function is called, although + * they can be marked for later execution by executing the dispatch function. + * \return Success (0), failure (-1) + */ int32_t CallbackSchedulerStart(); -int32_t DelayedCallbackSchedule(DelayedCallbackInfo *cbinfo, int32_t milliseconds, DelayedCallbackUpdateMode updatemode); -int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); -int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); -DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, long taskPriority, uint32_t stacksize); + +/** + * Register a new callback to be called by a delayed callback scheduler task. + * If a scheduler task with the specified taskPriority does not exist yet, it + * will be created. + * \param[in] cb The callback to be invoked + * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. + * \param[in] taskPriority Priority of the entire scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. + * \param[in] stacksize The stack requirements of the callback when called by the scheduler. + * \return CallbackInfo Pointer on success, NULL if failed. + */ +DelayedCallbackInfo *DelayedCallbackCreate( + DelayedCallback cb, + DelayedCallbackPriority priority, + long taskPriority, + uint32_t stacksize + ); + +/** + * Schedule dispatching a callback at some point in the future. The function returns immediately. + * \param[in] *cbinfo the callback handle + * \param[in] milliseconds How far in the future to dispatch the callback + * \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet. + * The options are: + * UPDATEMODE_NONE: An existing schedule will not be touched, the call will have no effect at all if there's an existing schedule. + * UPDATEMODE_SOONER: The callback will be rescheduled only if the new schedule triggers before the original one would have triggered. + * UPDATEMODE_LATER: The callback will be rescheduled only if the new schedule triggers after the original one would have triggered. + * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override) + * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden + */ +int32_t DelayedCallbackSchedule( + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode + ); + +/** + * Dispatch an event by invoking the supplied callback. The function + * returns immediately, the callback is invoked from the event task. + * \param[in] *cbinfo the callback handle + * \return Success (-1), failure (0) + */ +int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo ); + +/** + * Dispatch an event by invoking the supplied callback. The function + * returns immediately, the callback is invoked from the event task. + * \param[in] *cbinfo the callback handle + * \param[in] pxHigherPriorityTaskWoken + * xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if + * giving the semaphore caused a task to unblock, and the unblocked task has a + * priority higher than the currently running task. If xSemaphoreGiveFromISR() + * sets this value to pdTRUE then a context switch should be requested before + * the interrupt is exited. + * From FreeRTOS Docu: Context switching from an ISR uses port specific syntax. + * Check the demo task for your port to find the syntax required. + * \return Success (-1), failure (0) + */ +int32_t DelayedCallbackDispatchFromISR( DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken ); #endif // CALLBACKSCHEDULER_H From 28ac515064344be9e75179ea01a284f11ce1b858 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Apr 2013 15:39:12 +0200 Subject: [PATCH 7/8] moved Callback Scheduler startup in front of module task startup --- flight/Modules/System/systemmod.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index 90eb8e127..a64055172 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -148,12 +148,12 @@ MODULE_INITCALL(SystemModInitialize, 0) */ static void systemTask(void *parameters) { - /* create all modules thread */ - MODULE_TASKCREATE_ALL; - /* start the delayed callback scheduler */ CallbackSchedulerStart(); + /* create all modules thread */ + MODULE_TASKCREATE_ALL; + if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let From 6cb2d2b39dcbe7953d259f63436ef533dfe0b49e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Apr 2013 17:25:42 +0200 Subject: [PATCH 8/8] updated in code documentation, use of some enums in callbackscheduler.h --- flight/Modules/CallbackTest/callbacktest.c | 2 +- flight/Modules/Example/examplemodcallback.c | 140 ++++++++++++++++++ flight/targets/UAVObjects/callbackscheduler.c | 24 +-- .../UAVObjects/inc/callbackscheduler.h | 68 ++++++++- 4 files changed, 218 insertions(+), 16 deletions(-) create mode 100644 flight/Modules/Example/examplemodcallback.c diff --git a/flight/Modules/CallbackTest/callbacktest.c b/flight/Modules/CallbackTest/callbacktest.c index 0ef45e9cd..1f5a63b83 100644 --- a/flight/Modules/CallbackTest/callbacktest.c +++ b/flight/Modules/CallbackTest/callbacktest.c @@ -37,7 +37,7 @@ // Private constants #define STACK_SIZE configMINIMAL_STACK_SIZE #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY // Private types //#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) diff --git a/flight/Modules/Example/examplemodcallback.c b/flight/Modules/Example/examplemodcallback.c new file mode 100644 index 000000000..1a050f07e --- /dev/null +++ b/flight/Modules/Example/examplemodcallback.c @@ -0,0 +1,140 @@ +/** + ****************************************************************************** + * + * @file examplemodcallback.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Example module to be used as a template for actual modules. + * Event callback version. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Input objects: ExampleObject1, ExampleSettings + * Output object: ExampleObject2 + * + * This module executes in response to ExampleObject1 updates. When the + * module is triggered it will update the data of ExampleObject2. + * + * No threads are used in this example. + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "openpilot.h" +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) + +// Private constants +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY +// Private types + +// Private variables +static DelayedCallbackInfo *cbinfo; + +// Private functions +static void ObjectUpdatedCb(UAVObjEvent *ev); + +static void DelayedCb(); +/** + * Initialise the module, called on startup. + * \returns 0 on success or -1 if initialisation failed + */ +int32_t ExampleModCallbackInitialize() +{ + // Listen for ExampleObject1 updates, connect a callback function + ExampleObject1ConnectCallback(&ObjectUpdatedCb); + + cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE); + + return 0; +} + +/** + * This function is called each time ExampleObject1 is updated, this could be + * a local update or a remote update from the GCS. In this example the module + * does not have its own thread, the callbacks are executed from within the + * event thread. Because of that the callback execution time must be kept to + * a minimum. + */ +static void ObjectUpdatedCb(UAVObjEvent * ev) +{ + DelayedCallbackDispatch(cbinfo); +} + +/** + * This function is called by the DelayedCallbackScheduler when its execution + * has been requested. Callbacks scheduled for execution are executed in the + * same thread in a round robin fashion. The Dispatch function to reschedule + * execution can be called from within the Callback itself, in which case the + * re-run will be scheduled after all other callback with equal or higher + * priority have been executed. Like event callbacks, delayed callbacks are + * executed in the same thread context one at a time, therefore blocking IO + * functions or very long lasting calculations are prohibited. Unlike Event + * callbacks these callbacks run with a standard (IDLE+1) thread priority and + * do not block regular threads. They are therefore saver to use. + */ +static void DelayedCb(); + ExampleSettingsData settings; + ExampleObject1Data data1; + ExampleObject2Data data2; + int32_t step; + + // Update settings with latest value + ExampleSettingsGet(&settings); + + // Get the input object + ExampleObject1Get(&data1); + + // Determine how to update the output object + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } + + // Update data + data2.Field1 = data1.Field1 + step; + data2.Field2 = data1.Field2 + step; + data2.Field3 = data1.Field3 + step; + data2.Field4[0] = data1.Field4[0] + step; + data2.Field4[1] = data1.Field4[1] + step; + + // Update the ExampleObject2, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data2); + + //call the module again 10 seconds later, + //even if the exampleobject has not been updated + DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE); + +} diff --git a/flight/targets/UAVObjects/callbackscheduler.c b/flight/targets/UAVObjects/callbackscheduler.c index bec932c5d..c8e779305 100644 --- a/flight/targets/UAVObjects/callbackscheduler.c +++ b/flight/targets/UAVObjects/callbackscheduler.c @@ -40,7 +40,7 @@ struct DelayedCallbackTaskStruct { xTaskHandle callbackSchedulerTaskHandle; signed char name[3]; uint32_t stackSize; - long taskPriority; + DelayedCallbackPriorityTask priorityTask; xSemaphoreHandle signal; struct DelayedCallbackTaskStruct *next; }; @@ -114,7 +114,7 @@ int32_t CallbackSchedulerStart() cursor->name, cursor->stackSize/4, cursor, - cursor->taskPriority, + cursor->priorityTask, &cursor->callbackSchedulerTaskHandle ); if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { @@ -227,33 +227,35 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh } /** - * Register a new callback to be called by a delayed callback scheduler task + * Register a new callback to be called by a delayed callback scheduler task. + * If a scheduler task with the specified task priority does not exist yet, it + * will be created. * \param[in] cb The callback to be invoked - * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. - * \param[in] taskPriority Priority of the entire scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. + * \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task. + * \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities * \param[in] stacksize The stack requirements of the callback when called by the scheduler. * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( DelayedCallback cb, DelayedCallbackPriority priority, - long taskPriority, + DelayedCallbackPriorityTask priorityTask, uint32_t stacksize ) { xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // find appropriate scheduler task matching taskPriority + // find appropriate scheduler task matching priorityTask struct DelayedCallbackTaskStruct *task = NULL; int t = 0; LL_FOREACH(schedulerTasks, task) { - if (task->taskPriority == taskPriority) { + if (task->priorityTask == priorityTask) { break; // found } t++; } - // if scheduler task for given taskPriority does not exist, create it + // if given priorityTask does not exist, create it if (!task) { // allocate memory if possible task = (struct DelayedCallbackTaskStruct*)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); @@ -271,7 +273,7 @@ DelayedCallbackInfo *DelayedCallbackCreate( task->name[1] = 'a'+t; task->name[2] = 0; task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); - task->taskPriority = taskPriority; + task->priorityTask = priorityTask; task->next = NULL; // create the signaling semaphore @@ -292,7 +294,7 @@ DelayedCallbackInfo *DelayedCallbackCreate( task->name, task->stackSize/4, task, - task->taskPriority, + task->priorityTask, &task->callbackSchedulerTaskHandle ); if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { diff --git a/flight/targets/UAVObjects/inc/callbackscheduler.h b/flight/targets/UAVObjects/inc/callbackscheduler.h index 0ffcbe225..e989d847f 100644 --- a/flight/targets/UAVObjects/inc/callbackscheduler.h +++ b/flight/targets/UAVObjects/inc/callbackscheduler.h @@ -32,6 +32,51 @@ typedef enum{ CALLBACK_PRIORITY_REGULAR = 1, CALLBACK_PRIORITY_LOW = 2 } DelayedCallbackPriority; +// Use the CallbackPriority to define how frequent a callback needs to be +// called in relation to others in the same callback scheduling task. +// The scheduler will call callbacks waiting for execution with the same +// priority in a round robin way. However one slot in this queue is reserved +// for a chosen member of the next lower priority. This member will also be +// chosen in a round robin way. +// Example: +// Assume you have 6 callbacks in the same PriorityTask, all constantly wanting +// to be executed. +// A and B are priority CRITICAL, +// c and d are priority REGULAR, +// x and y are priority LOW. +// Then the execution schedule will look as follows: +// ...ABcABdABxABcABdAByABcABdABxABcABdABy... +// However if only the 3 callbacks, A, c and x want to execute, you will get: +// ...AcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAx... +// And if onlz A and y need execution it will be: +// ...AyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAy... +// despite their different priority they would get treated equally in this case. +// +// WARNING: Callbacks ALWAYS should return as quickly as possible. Otherwise +// a low priority callback can block a critical one from being executed. +// Callbacks MUST NOT block execution! + +typedef enum{ + CALLBACK_TASK_AUXILIARY = (tskIDLE_PRIORITY + 1), + CALLBACK_TASK_NAVIGATION = (tskIDLE_PRIORITY + 2), + CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3), + CALLBACK_TASK_DEVICEDRIVER = (tskIDLE_PRIORITY + 4), + } DelayedCallbackPriorityTask; +// Use the PriorityTask to define the global importance of callback execution +// compared to other processes in the system. +// Callbacks dispatched in a higher PriorityTasks will halt the execution of +// any lower priority processes, including callbacks and even callback +// scheduling tasks until they are done! +// Assume you have two callbacks: +// A in priorityTask DEVICEDRIVER, +// b and c in priorityTask AUXILIARY, +// Then the execution schedule can look as follows: (| marks a task switch) +// | ... /c>... +// +// WARNING: Any higher priority task can prevent lower priority code from being +// executed! (This does not only apply to callbacks but to all FreeRTOS tasks!) typedef enum{ CALLBACK_UPDATEMODE_NONE = 0, @@ -39,11 +84,26 @@ typedef enum{ CALLBACK_UPDATEMODE_LATER = 2, CALLBACK_UPDATEMODE_OVERRIDE = 3 } DelayedCallbackUpdateMode; +// When scheduling a callback for execution at a time in the future, use the +// update mode to define what should happen if the callback is already +// scheduled. +// With NONE, the schedule will not be updated and the callback will be +// executed at the original time. +// With SOONER, the closer of the two schedules will take precedence +// With LATER, the schedule more distant in the future will be used. +// With OVERRIDE, the original schedule will be discarded. typedef void (*DelayedCallback)(void); +// Use this type for the callback function. struct DelayedCallbackInfoStruct; typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo; +// Use a pointer to DelayedCallbackInfo as a handle to identify registered callbacks. +// be aware that the same callback function can be registered as a callback +// several times, even with different callback priorities and even +// priorityTasks, using different handles and as such different dispatch calls. +// Be aware that using different priorityTasks for the same callback function +// might cause your callback to be executed recursively in different task contexts! // Public functions // @@ -69,18 +129,18 @@ int32_t CallbackSchedulerStart(); /** * Register a new callback to be called by a delayed callback scheduler task. - * If a scheduler task with the specified taskPriority does not exist yet, it + * If a scheduler task with the specified task priority does not exist yet, it * will be created. * \param[in] cb The callback to be invoked - * \param[in] priority Priority of the callback compared to other callbacks called by the same delayed callback scheduler task. - * \param[in] taskPriority Priority of the entire scheduler task. One scheduler task will be spawned for each distinct task priority in use, further callbacks with the same task priority will be handled by the same delayed callback scheduler task, according to their callback priority. + * \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task. + * \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities * \param[in] stacksize The stack requirements of the callback when called by the scheduler. * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( DelayedCallback cb, DelayedCallbackPriority priority, - long taskPriority, + DelayedCallbackPriorityTask priorityTask, uint32_t stacksize );