diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 9967e7ebf..a16273e2f 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -169,12 +169,14 @@ MODULE_INITCALL(SystemModInitialize, 0); */ static void systemTask(__attribute__((unused)) void *parameters) { - /* start the delayed callback scheduler */ - CallbackSchedulerStart(); uint8_t cycleCount = 0; + /* 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/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index e3ac9f653..194387fdc 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -197,6 +197,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -237,9 +240,6 @@ void PIOS_Board_Init(void) AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } - /* 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/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c index 5750bed21..7487a17aa 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c @@ -38,6 +38,9 @@ void PIOS_Board_Init(void) /* Delay system */ PIOS_DELAY_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -50,9 +53,6 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - /* Initialize the PiOS library */ PIOS_COM_Init(); } diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 210c825b9..ce097f581 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -105,6 +105,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -145,9 +148,6 @@ void PIOS_Board_Init(void) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ - /* 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/boards/oplinkmini/firmware/pios_board_posix.c b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c index 5750bed21..7487a17aa 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c @@ -38,6 +38,9 @@ void PIOS_Board_Init(void) /* Delay system */ PIOS_DELAY_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -50,9 +53,6 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - /* Initialize the PiOS library */ PIOS_COM_Init(); } diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index 308999c51..ffaa1dd36 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -177,6 +177,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -191,9 +194,6 @@ void PIOS_Board_Init(void) /* Initialize the alarms library */ AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 83b5c60fe..9ee08bf64 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -403,6 +403,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -415,9 +418,6 @@ void PIOS_Board_Init(void) /* Initialize the alarms library */ AlarmsInitialize(); - /* 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/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 9def851ae..4a9453efb 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -131,6 +131,9 @@ void PIOS_Board_Init(void) /* Delay system */ PIOS_DELAY_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -150,9 +153,6 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 { diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 2c2cea409..b3a76c386 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -443,6 +443,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -452,9 +455,6 @@ void PIOS_Board_Init(void) /* Initialize the alarms library */ AlarmsInitialize(); - /* 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/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 9b4c60bc0..401d4be57 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -131,6 +131,9 @@ void PIOS_Board_Init(void) /* Delay system */ PIOS_DELAY_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -151,9 +154,6 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 { diff --git a/flight/targets/boards/simposix/firmware/pios_board.c b/flight/targets/boards/simposix/firmware/pios_board.c index 51c800df6..9645c753d 100644 --- a/flight/targets/boards/simposix/firmware/pios_board.c +++ b/flight/targets/boards/simposix/firmware/pios_board.c @@ -123,6 +123,9 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -134,9 +137,6 @@ void PIOS_Board_Init(void) /* Initialize the alarms library */ AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - /* Configure IO ports */ /* Configure Telemetry port */ diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 503e9b1e5..9fc78a21c 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -41,7 +41,8 @@ #define STACK_SIZE configMINIMAL_STACK_SIZE #endif /* PIOS_EVENTDISPATCHER_STACK_SIZE */ -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL +#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define MAX_UPDATE_PERIOD_MS 1000 // Private types @@ -70,7 +71,7 @@ typedef struct PeriodicObjectListStruct PeriodicObjectList; // Private variables static PeriodicObjectList *mObjList; static xQueueHandle mQueue; -static xTaskHandle mEventTaskHandle; +static DelayedCallbackInfo *eventSchedulerCallback; static xSemaphoreHandle mMutex; static EventStats mStats; @@ -101,8 +102,8 @@ int32_t EventDispatcherInitialize() // Create event queue mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); - // Create task - xTaskCreate(eventTask, (signed char *)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); + // Create callback + eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4); // Done return 0; @@ -145,7 +146,9 @@ int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb) evInfo.cb = cb; evInfo.queue = 0; // Push to queue - return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full + int32_t result = xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full + DelayedCallbackDispatch(eventSchedulerCallback); + return result; } /** @@ -276,38 +279,33 @@ static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQue } /** - * Event task, responsible of invoking callbacks. + * Delayed event callback, responsible of invoking (event) callbacks. */ static void eventTask() { - uint32_t timeToNextUpdateMs; - uint32_t delayMs; + static uint32_t timeToNextUpdateMs = 0; EventCallbackInfo evInfo; - /* Must do this in task context to ensure that TaskMonitor has already finished its init */ - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); + // Wait for queue message + int limit = MAX_QUEUE_SIZE; - // Initialize time - timeToNextUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS; - - // Loop forever - while (1) { - // Calculate delay time - delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); - - // Wait for queue message - if (xQueueReceive(mQueue, &evInfo, delayMs / portTICK_RATE_MS) == pdTRUE) { - // Invoke callback, if one - if (evInfo.cb != 0) { - evInfo.cb(&evInfo.ev); // the function is expected to copy the event information - } + while (xQueueReceive(mQueue, &evInfo, 0) == pdTRUE) { + // Invoke callback, if any + if (evInfo.cb != 0) { + evInfo.cb(&evInfo.ev); // the function is expected to copy the event information } - - // Process periodic updates - if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) { - timeToNextUpdateMs = processPeriodicUpdates(); + // limit loop to max queue size to slightly reduce the impact of recursive events + if (!--limit) { + break; } } + + // Process periodic updates + if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) { + timeToNextUpdateMs = processPeriodicUpdates(); + } + + DelayedCallbackSchedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER); } /**